diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt new file mode 100644 index 0000000..f11d6cf --- /dev/null +++ b/src/controllers/mdog_simple_controller/CMakeLists.txt @@ -0,0 +1,111 @@ +cmake_minimum_required(VERSION 3.8) +project(mdog_simple_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(ament_cmake_gmock REQUIRED) +find_package(controller_manager REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(ros2_control_test_assets REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# Add mdog_simple_controller library related compile commands +generate_parameter_library(mdog_simple_controller_parameters + src/mdog_simple_controller.yaml + include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp +) +add_library( + mdog_simple_controller + SHARED + src/mdog_simple_controller.cpp +) +target_include_directories(mdog_simple_controller PUBLIC + "$" + "$") +target_link_libraries(mdog_simple_controller mdog_simple_controller_parameters) +ament_target_dependencies(mdog_simple_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(mdog_simple_controller PRIVATE "MDOG_SIMPLE_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface mdog_simple_controller.xml) + +install( + TARGETS + mdog_simple_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + + ament_add_gmock(test_load_mdog_simple_controller test/test_load_mdog_simple_controller.cpp) + target_include_directories(test_load_mdog_simple_controller PRIVATE include) + ament_target_dependencies( + test_load_mdog_simple_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock(test_mdog_simple_controller test/test_mdog_simple_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mdog_simple_controller_params.yaml) + target_include_directories(test_mdog_simple_controller PRIVATE include) + target_link_libraries(test_mdog_simple_controller mdog_simple_controller) + ament_target_dependencies( + test_mdog_simple_controller + controller_interface + hardware_interface + ) + + add_rostest_with_parameters_gmock(test_mdog_simple_controller_preceeding test/test_mdog_simple_controller_preceeding.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/mdog_simple_controller_preceeding_params.yaml) + target_include_directories(test_mdog_simple_controller_preceeding PRIVATE include) + target_link_libraries(test_mdog_simple_controller_preceeding mdog_simple_controller) + ament_target_dependencies( + test_mdog_simple_controller_preceeding + controller_interface + hardware_interface + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + mdog_simple_controller +) + +ament_package() diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp new file mode 100644 index 0000000..0cb7113 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp @@ -0,0 +1,117 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_ +#define MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "mdog_simple_controller_parameters.hpp" +#include "mdog_simple_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +namespace mdog_simple_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + +// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) +enum class control_mode_type : std::uint8_t +{ + FAST = 0, + SLOW = 1, +}; + +class MdogSimpleController : public controller_interface::ControllerInterface +{ +public: + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + MdogSimpleController(); + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // TODO(anyone): replace the state and command message types + using ControllerReferenceMsg = control_msgs::msg::JointJog; + using ControllerModeSrvType = std_srvs::srv::SetBool; + using ControllerStateMsg = control_msgs::msg::JointControllerState; + +protected: + std::shared_ptr param_listener_; + mdog_simple_controller::Params params_; + + std::vector state_joints_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_ref_; + + rclcpp::Service::SharedPtr set_slow_control_mode_service_; + realtime_tools::RealtimeBuffer control_mode_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + +private: + // callback for topic interface + MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL + void reference_callback(const std::shared_ptr msg); +}; + +} // namespace mdog_simple_controller + +#endif // MDOG_SIMPLE_CONTROLLER__MDOG_SIMPLE_CONTROLLER_HPP_ diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp new file mode 100644 index 0000000..1e09499 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/validate_mdog_simple_controller_parameters.hpp @@ -0,0 +1,43 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef MDOG_SIMPLE_CONTROLLER__VALIDATE_MDOG_SIMPLE_CONTROLLER_PARAMETERS_HPP_ +#define MDOG_SIMPLE_CONTROLLER__VALIDATE_MDOG_SIMPLE_CONTROLLER_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" + +namespace parameter_traits +{ +Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) +{ + auto const & interface_name = parameter.as_string(); + + if (interface_name.rfind("blup_", 0) == 0) + { + return ERROR("'interface_name' parameter can not start with 'blup_'"); + } + + return OK; +} + +} // namespace parameter_traits + +#endif // MDOG_SIMPLE_CONTROLLER__VALIDATE_MDOG_SIMPLE_CONTROLLER_PARAMETERS_HPP_ diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h new file mode 100644 index 0000000..eb6a7a1 --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/visibility_control.h @@ -0,0 +1,54 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef MDOG_SIMPLE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef MDOG_SIMPLE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT +#else +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT +#endif +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC_TYPE MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL +#else +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_LOCAL +#endif +#define MDOG_SIMPLE_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // MDOG_SIMPLE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/src/controllers/mdog_simple_controller/mdog_simple_controller.xml b/src/controllers/mdog_simple_controller/mdog_simple_controller.xml new file mode 100644 index 0000000..654bf58 --- /dev/null +++ b/src/controllers/mdog_simple_controller/mdog_simple_controller.xml @@ -0,0 +1,28 @@ + + + + + + MdogSimpleController ros2_control controller. + + + diff --git a/src/controllers/mdog_simple_controller/package.xml b/src/controllers/mdog_simple_controller/package.xml new file mode 100644 index 0000000..787dbc0 --- /dev/null +++ b/src/controllers/mdog_simple_controller/package.xml @@ -0,0 +1,39 @@ + + + + mdog_simple_controller + 0.0.0 + TODO: Package description + robofish + TODO: License declaration + + ament_cmake + + generate_parameter_library + + control_msgs + + controller_interface + + hardware_interface + + pluginlib + + rclcpp + + rclcpp_lifecycle + + realtime_tools + + std_srvs + + ament_lint_auto + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp new file mode 100644 index 0000000..4cd34b9 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp @@ -0,0 +1,265 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +using ControllerReferenceMsg = mdog_simple_controller::MdogSimpleController::ControllerReferenceMsg; + +// called from RT control loop +void reset_controller_reference_msg( + std::shared_ptr & msg, const std::vector & joint_names) +{ + msg->joint_names = joint_names; + msg->displacements.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->velocities.resize(joint_names.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); +} + +} // namespace + +namespace mdog_simple_controller +{ +MdogSimpleController::MdogSimpleController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn MdogSimpleController::on_init() +{ + control_mode_.initRT(control_mode_type::FAST); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MdogSimpleController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + if (!params_.state_joints.empty()) + { + state_joints_ = params_.state_joints; + } + else + { + state_joints_ = params_.joints; + } + + if (params_.joints.size() != state_joints_.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", + params_.joints.size(), state_joints_.size()); + return CallbackReturn::FAILURE; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MdogSimpleController::reference_callback, this, std::placeholders::_1)); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg, params_.joints); + input_ref_.writeFromNonRT(msg); + + auto set_slow_mode_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + if (request->data) + { + control_mode_.writeFromNonRT(control_mode_type::SLOW); + } + else + { + control_mode_.writeFromNonRT(control_mode_type::FAST); + } + response->success = true; + }; + + set_slow_control_mode_service_ = get_node()->create_service( + "~/set_slow_control_mode", set_slow_mode_service_callback, + rmw_qos_profile_services_hist_keep_all); + + try + { + // State publisher + s_publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + // TODO(anyone): Reserve memory in state publisher depending on the message type + state_publisher_->lock(); + state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +void MdogSimpleController::reference_callback(const std::shared_ptr msg) +{ + if (msg->joint_names.size() == params_.joints.size()) + { + input_ref_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received %zu , but expected %zu joints in command. Ignoring message.", + msg->joint_names.size(), params_.joints.size()); + } +} + +controller_interface::InterfaceConfiguration MdogSimpleController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(params_.joints.size()); + for (const auto & joint : params_.joints) + { + command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration MdogSimpleController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_joints_.size()); + for (const auto & joint : state_joints_) + { + state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn MdogSimpleController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): if you have to manage multiple interfaces that need to be sorted check + // `on_activate` method in `JointTrajectoryController` for exemplary use of + // `controller_interface::get_ordered_interfaces` helper function + + // Set default value in command + reset_controller_reference_msg(*(input_ref_.readFromRT)(), params_.joints); + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MdogSimpleController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MdogSimpleController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + auto current_ref = input_ref_.readFromRT(); + + // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, + // instead of a loop + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isnan((*current_ref)->displacements[i])) + { + if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) + { + (*current_ref)->displacements[i] /= 2; + } + command_interfaces_[i].set_value((*current_ref)->displacements[i]); + + (*current_ref)->displacements[i] = std::numeric_limits::quiet_NaN(); + } + } + + if (state_publisher_ && state_publisher_->trylock()) + { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.set_point = command_interfaces_[CMD_MY_ITFS].get_value(); + state_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace mdog_simple_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + mdog_simple_controller::MdogSimpleController, controller_interface::ControllerInterface) diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml new file mode 100644 index 0000000..75843c1 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.yaml @@ -0,0 +1,50 @@ +# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# + +mdog_simple_controller: + joints: { + type: string_array, + default_value: [], + description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + state_joints: { + type: string_array, + default_value: [], + description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + validation: { + unique<>: null, + } + } + interface_name: { + type: string, + default_value: "", + description: "Name of the interface used by the controller on joints and command_joints.", + read_only: true, + validation: { + not_empty<>: null, + one_of<>: [["position", "velocity", "acceleration", "effort",]], + forbidden_interface_name_prefix: null + } + } diff --git a/src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml b/src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml new file mode 100644 index 0000000..a0d1615 --- /dev/null +++ b/src/controllers/mdog_simple_controller/test/mdog_simple_controller_params.yaml @@ -0,0 +1,26 @@ +# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# + +test_mdog_simple_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration diff --git a/src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml b/src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml new file mode 100644 index 0000000..06e227c --- /dev/null +++ b/src/controllers/mdog_simple_controller/test/mdog_simple_controller_preceeding_params.yaml @@ -0,0 +1,28 @@ +# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# + +test_mdog_simple_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state diff --git a/src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp new file mode 100644 index 0000000..2b61393 --- /dev/null +++ b/src/controllers/mdog_simple_controller/test/test_load_mdog_simple_controller.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadMdogSimpleController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_mdog_simple_controller", "mdog_simple_controller/MdogSimpleController")); + + rclcpp::shutdown(); +} diff --git a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp new file mode 100644 index 0000000..73d4c7d --- /dev/null +++ b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.cpp @@ -0,0 +1,277 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_mdog_simple_controller.hpp" + +using mdog_simple_controller::CMD_MY_ITFS; +using mdog_simple_controller::control_mode_type; +using mdog_simple_controller::STATE_MY_ITFS; + +class MdogSimpleControllerTest : public MdogSimpleControllerFixture +{ +}; + +TEST_F(MdogSimpleControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.joints.empty()); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); + ASSERT_EQ(controller_->params_.interface_name, interface_name_); +} + +TEST_F(MdogSimpleControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } +} + +TEST_F(MdogSimpleControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->displacements) + { + EXPECT_TRUE(std::isnan(cmd)); + } + EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); + for (const auto & cmd : (*msg)->velocities) + { + EXPECT_TRUE(std::isnan(cmd)); + } + + ASSERT_TRUE(std::isnan((*msg)->duration)); +} + +TEST_F(MdogSimpleControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MdogSimpleControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(MdogSimpleControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MdogSimpleControllerTest, test_setting_slow_mode_service) +{ + SetUpController(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + // initially set to false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // should stay false + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + + // set to true + ASSERT_NO_THROW(call_service(true, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + + // set back to false + ASSERT_NO_THROW(call_service(false, executor)); + ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +} + +TEST_F(MdogSimpleControllerTest, test_update_logic_fast) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +} + +TEST_F(MdogSimpleControllerTest, test_update_logic_slow) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // set command statically + static constexpr double TEST_DISPLACEMENT = 23.24; + std::shared_ptr msg = std::make_shared(); + // When slow mode is enabled command ends up being half of the value + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); + msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + msg->duration = std::numeric_limits::quiet_NaN(); + controller_->input_ref_.writeFromNonRT(msg); + controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + + EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); +} + +TEST_F(MdogSimpleControllerTest, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 101.101); +} + +TEST_F(MdogSimpleControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 101.101); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[CMD_MY_ITFS], 0.45); + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 0.45); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp new file mode 100644 index 0000000..274f3c2 --- /dev/null +++ b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller.hpp @@ -0,0 +1,274 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "mdog_simple_controller/mdog_simple_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = mdog_simple_controller::MdogSimpleController::ControllerStateMsg; +using ControllerReferenceMsg = mdog_simple_controller::MdogSimpleController::ControllerReferenceMsg; +using ControllerModeSrvType = mdog_simple_controller::MdogSimpleController::ControllerModeSrvType; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace + +// subclassing and friending so we can access member variables +class TestableMdogSimpleController : public mdog_simple_controller::MdogSimpleController +{ + FRIEND_TEST(MdogSimpleControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(MdogSimpleControllerTest, activate_success); + FRIEND_TEST(MdogSimpleControllerTest, reactivate_success); + FRIEND_TEST(MdogSimpleControllerTest, test_setting_slow_mode_service); + FRIEND_TEST(MdogSimpleControllerTest, test_update_logic_fast); + FRIEND_TEST(MdogSimpleControllerTest, test_update_logic_slow); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = mdog_simple_controller::MdogSimpleController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + + // TODO(anyone): add implementation of any methods of your controller is needed + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MdogSimpleControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_mdog_simple_controller/commands", rclcpp::SystemDefaultsQoS()); + + service_caller_node_ = std::make_shared("service_caller"); + slow_control_service_client_ = service_caller_node_->create_client( + "/test_mdog_simple_controller/set_slow_control_mode"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_mdog_simple_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (size_t i = 0; i < joint_command_values_.size(); ++i) + { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + // TODO(anyone): Add other command interfaces, if any + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + state_itfs_.emplace_back(hardware_interface::StateInterface( + joint_names_[i], interface_name_, &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + // TODO(anyone): Add other state interfaces, if any + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_mdog_simple_controller/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + // TODO(anyone): add/remove arguments as it suites your command message type + void publish_commands( + const std::vector & displacements = {0.45}, + const std::vector & velocities = {0.0}, const double duration = 1.25) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.joint_names = joint_names_; + msg.displacements = displacements; + msg.velocities = velocities; + msg.duration = duration; + + command_publisher_->publish(msg); + } + + std::shared_ptr call_service( + const bool slow_control, rclcpp::Executor & executor) + { + auto request = std::make_shared(); + request->data = slow_control; + + bool wait_for_service_ret = + slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto result = slow_control_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + std::vector joint_names_ = {"joint1"}; + std::vector state_joint_names_ = {"joint1state"}; + std::string interface_name_ = "acceleration"; + std::array joint_state_values_ = {1.1}; + std::array joint_command_values_ = {101.101}; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; + rclcpp::Client::SharedPtr slow_control_service_client_; +}; + +#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MDOG_SIMPLE_CONTROLLER_HPP_ diff --git a/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp new file mode 100644 index 0000000..39117ee --- /dev/null +++ b/src/controllers/mdog_simple_controller/test/test_mdog_simple_controller_preceeding.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_mdog_simple_controller.hpp" + +#include +#include +#include +#include +#include + +using mdog_simple_controller::CMD_MY_ITFS; +using mdog_simple_controller::control_mode_type; +using mdog_simple_controller::STATE_MY_ITFS; + +class MdogSimpleControllerTest : public MdogSimpleControllerFixture +{ +}; + +TEST_F(MdogSimpleControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.joints.empty()); + ASSERT_TRUE(controller_->params_.state_joints.empty()); + ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); + ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_EQ(controller_->params_.interface_name, interface_name_); +} + +TEST_F(MdogSimpleControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}