From 44736ad23a854f784dae9823b8dda7b1bdf399c2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 30 Mar 2023 10:18:32 -0500 Subject: [PATCH 1/4] Add an outline for the "passthrough controller" --- passthrough_controller/CMakeLists.txt | 73 +++++++++++++++++ .../passthrough_controller.hpp | 78 +++++++++++++++++++ .../visibility_control.h | 49 ++++++++++++ passthrough_controller/package.xml | 29 +++++++ .../passthrough_controller.xml | 9 +++ .../src/passthrough_controller.cpp | 25 ++++++ .../test/test_load_controller.cpp | 40 ++++++++++ 7 files changed, 303 insertions(+) create mode 100644 passthrough_controller/CMakeLists.txt create mode 100644 passthrough_controller/include/passthrough_controller/passthrough_controller.hpp create mode 100644 passthrough_controller/include/passthrough_controller/visibility_control.h create mode 100644 passthrough_controller/package.xml create mode 100644 passthrough_controller/passthrough_controller.xml create mode 100644 passthrough_controller/src/passthrough_controller.cpp create mode 100644 passthrough_controller/test/test_load_controller.cpp diff --git a/passthrough_controller/CMakeLists.txt b/passthrough_controller/CMakeLists.txt new file mode 100644 index 0000000000..cb5b4daee2 --- /dev/null +++ b/passthrough_controller/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.16) +project(passthrough_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(passthrough_controller SHARED + src/passthrough_controller.cpp +) +target_compile_features(passthrough_controller PUBLIC cxx_std_17) +target_include_directories(passthrough_controller PUBLIC + $ + $ +) +# target_link_libraries(passthrough_controller PUBLIC +# passthrough_controller_parameters +# ) +ament_target_dependencies(passthrough_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(passthrough_controller PRIVATE "passthrough_controller_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface passthrough_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock(test_load_controller test/test_load_controller.cpp) + target_include_directories(test_load_controller PRIVATE include) + ament_target_dependencies( + test_load_controller + controller_manager + ros2_control_test_assets + ) +endif() + +install( + DIRECTORY include/ + DESTINATION include/passthrough_controller +) + +install(TARGETS + passthrough_controller + EXPORT export_passthrough_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_passthrough_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp b/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp new file mode 100644 index 0000000000..d55a31b574 --- /dev/null +++ b/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp @@ -0,0 +1,78 @@ +// Copyright (c) 2023, PAL Robotics +// +// 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. + +#ifndef PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ +#define PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ + +#include + +#include "passthrough_controller/visibility_control.h" + +namespace passthrough_controller +{ +class PassthroughController : public controller_interface::ChainableControllerInterface +{ +public: + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + PassthroughController(){} + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override + { + return controller_interface::CallbackReturn::SUCCESS; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + controller_interface::InterfaceConfiguration command_interfaces_config; + return command_interfaces_config; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + controller_interface::InterfaceConfiguration state_interfaces_config; + return state_interfaces_config; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override + { + return controller_interface::return_type::OK; + } + +protected: + std::vector on_export_reference_interfaces() override + { + return std::vector(); + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + bool on_set_chained_mode(bool /* chained_mode */) override + { + return true; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update_and_write_commands( + const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override + { + return controller_interface::return_type::OK; + } +}; + +} // namespace passthrough_controller + +#endif // PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ diff --git a/passthrough_controller/include/passthrough_controller/visibility_control.h b/passthrough_controller/include/passthrough_controller/visibility_control.h new file mode 100644 index 0000000000..ce8c772e9c --- /dev/null +++ b/passthrough_controller/include/passthrough_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright (c) 2023, PAL Robotics +// +// 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. + +#ifndef PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_ +#define PASSTHROUGH_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 PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef PASSTHROUGH_CONTROLLER__VISIBILITY_BUILDING_DLL +#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT +#else +#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT +#endif +#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC_TYPE PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC +#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL +#else +#define PASSTHROUGH_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define PASSTHROUGH_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC +#define PASSTHROUGH_CONTROLLER__VISIBILITY_LOCAL +#endif +#define PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // PASSTHROUGH_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/passthrough_controller/package.xml b/passthrough_controller/package.xml new file mode 100644 index 0000000000..203b752c51 --- /dev/null +++ b/passthrough_controller/package.xml @@ -0,0 +1,29 @@ + + + + passthrough_controller + 0.0.0 + A simple demo of chainable controllers. It passes commands through without change. + Sai Kishor Kothakota + Andy Zelenak + Apache-2.0 + + ament_cmake + + control_msgs + controller_interface + hardware_interface + parameter_traits + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + + ament_cmake_gmock + controller_manager + ros2_control_test_assets + + + ament_cmake + + \ No newline at end of file diff --git a/passthrough_controller/passthrough_controller.xml b/passthrough_controller/passthrough_controller.xml new file mode 100644 index 0000000000..3fc85562c5 --- /dev/null +++ b/passthrough_controller/passthrough_controller.xml @@ -0,0 +1,9 @@ + + + + A simple demo of chainable controllers. It passes commands through without change. + + + \ No newline at end of file diff --git a/passthrough_controller/src/passthrough_controller.cpp b/passthrough_controller/src/passthrough_controller.cpp new file mode 100644 index 0000000000..ef1b923c6c --- /dev/null +++ b/passthrough_controller/src/passthrough_controller.cpp @@ -0,0 +1,25 @@ +// Copyright (c) 2023, PAL Robotics +// +// 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. + +#include "passthrough_controller/passthrough_controller.hpp" + +namespace passthrough_controller +{ + +} // namespace passthrough_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + passthrough_controller::PassthroughController, controller_interface::ChainableControllerInterface) diff --git a/passthrough_controller/test/test_load_controller.cpp b/passthrough_controller/test/test_load_controller.cpp new file mode 100644 index 0000000000..0c672c7308 --- /dev/null +++ b/passthrough_controller/test/test_load_controller.cpp @@ -0,0 +1,40 @@ +// Copyright (c) 2023, PAL Robotics +// +// 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. + +#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(TestLoadPidController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller("test_passthrough_controller", "passthrough_controller/PassthroughController")); + + rclcpp::shutdown(); +} From 32849ae2156d3d99247202b5b179003c88b5d4b5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 4 Apr 2023 22:02:50 +0200 Subject: [PATCH 2/4] added passthrough controller to chain a list of interfaces --- passthrough_controller/CMakeLists.txt | 13 +- .../passthrough_controller.hpp | 156 ++++++++++++++++-- passthrough_controller/package.xml | 8 +- .../passthrough_controller_parameters.yaml | 7 + 4 files changed, 168 insertions(+), 16 deletions(-) create mode 100644 passthrough_controller/src/passthrough_controller_parameters.yaml diff --git a/passthrough_controller/CMakeLists.txt b/passthrough_controller/CMakeLists.txt index cb5b4daee2..0f3473f35c 100644 --- a/passthrough_controller/CMakeLists.txt +++ b/passthrough_controller/CMakeLists.txt @@ -7,6 +7,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + generate_parameter_library controller_interface hardware_interface parameter_traits @@ -14,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + std_msgs ) find_package(ament_cmake REQUIRED) @@ -22,6 +24,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(passthrough_controller_parameters + src/passthrough_controller_parameters.yaml +) + add_library(passthrough_controller SHARED src/passthrough_controller.cpp ) @@ -30,9 +36,9 @@ target_include_directories(passthrough_controller PUBLIC $ $ ) -# target_link_libraries(passthrough_controller PUBLIC -# passthrough_controller_parameters -# ) + target_link_libraries(passthrough_controller PUBLIC + passthrough_controller_parameters + ) ament_target_dependencies(passthrough_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Causes the visibility macros to use dllexport rather than dllimport, @@ -62,6 +68,7 @@ install( install(TARGETS passthrough_controller + passthrough_controller_parameters EXPORT export_passthrough_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp b/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp index d55a31b574..e3c0e7ce1a 100644 --- a/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp +++ b/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp @@ -16,20 +16,122 @@ #define PASSTHROUGH_CONTROLLER__PASSTHROUGH_CONTROLLER_HPP_ #include +#include "controller_interface/helpers.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "std_msgs/msg/float64_multi_array.hpp" +// auto-generated by generate_parameter_library +#include "passthrough_controller_parameters.hpp" #include "passthrough_controller/visibility_control.h" namespace passthrough_controller { +using DataType = std_msgs::msg::Float64MultiArray; class PassthroughController : public controller_interface::ChainableControllerInterface { public: PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC - PassthroughController(){} + PassthroughController() {} PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override { + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + params_ = param_listener_->get_params(); + command_interface_names_ = params_.interfaces; + + if (command_interface_names_.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "'interfaces' parameter list was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + joints_cmd_sub_ = this->get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), + [this](const DataType::SharedPtr msg) + { + // check if message is correct size, if not ignore + if (msg->data.size() == command_interface_names_.size()) + { + rt_buffer_ptr_.writeFromNonRT(msg); + } + else + { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Invalid command received of %zu size, expected %zu size", msg->data.size(), + command_interface_names_.size()); + } + }); + + // pre-reserve command interfaces + command_interfaces_.reserve(command_interface_names_.size()); + + RCLCPP_INFO(this->get_node()->get_logger(), "configure successful"); + + // The names should be in the same order as for command interfaces for easier matching + reference_interface_names_ = command_interface_names_; + // for any case make reference interfaces size of command interfaces + reference_interfaces_.resize( + reference_interface_names_.size(), std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + // check if we have all resources defined in the "points" parameter + // also verify that we *only* have the resources defined in the "points" parameter + // ATTENTION(destogl): Shouldn't we use ordered interface all the time? + std::vector> + ordered_interfaces; + if ( + !controller_interface::get_ordered_interfaces( + command_interfaces_, command_interface_names_, std::string(""), ordered_interfaces) || + command_interface_names_.size() != ordered_interfaces.size()) + { + RCLCPP_ERROR( + this->get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + command_interface_names_.size(), ordered_interfaces.size()); + return controller_interface::CallbackReturn::ERROR; + } + + // reset command buffer if a command came through callback when controller was inactive + rt_buffer_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); + + RCLCPP_INFO(this->get_node()->get_logger(), "activate successful"); + + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); + + return controller_interface::CallbackReturn::SUCCESS; + } + + PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override + { + // reset command buffer + rt_buffer_ptr_ = realtime_tools::RealtimeBuffer>(nullptr); return controller_interface::CallbackReturn::SUCCESS; } @@ -37,40 +139,74 @@ class PassthroughController : public controller_interface::ChainableControllerIn controller_interface::InterfaceConfiguration command_interface_configuration() const override { controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names = command_interface_names_; + return command_interfaces_config; } PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override { - controller_interface::InterfaceConfiguration state_interfaces_config; - return state_interfaces_config; + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; } PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override + controller_interface::return_type update_reference_from_subscribers( + const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override { + auto joint_commands = rt_buffer_ptr_.readFromRT(); + // message is valid + if (!(!joint_commands || !(*joint_commands))) + { + reference_interfaces_ = (*joint_commands)->data; + } + return controller_interface::return_type::OK; } protected: std::vector on_export_reference_interfaces() override { - return std::vector(); + std::vector reference_interfaces; + + for (size_t i = 0; i < reference_interface_names_.size(); ++i) + { + reference_interfaces.push_back(hardware_interface::CommandInterface( + get_node()->get_name(), reference_interface_names_[i], &reference_interfaces_[i])); + } + + return reference_interfaces; } PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC - bool on_set_chained_mode(bool /* chained_mode */) override - { - return true; - } + bool on_set_chained_mode(bool /* chained_mode */) override { return true; } PASSTHROUGH_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands( - const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override + const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) override { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isnan(reference_interfaces_[i])) + { + command_interfaces_[i].set_value(reference_interfaces_[i]); + } + } + return controller_interface::return_type::OK; } + + std::shared_ptr param_listener_; + Params params_; + + realtime_tools::RealtimeBuffer> rt_buffer_ptr_; + rclcpp::Subscription::SharedPtr joints_cmd_sub_; + + std::vector reference_interface_names_; + + std::vector command_interface_names_; }; } // namespace passthrough_controller diff --git a/passthrough_controller/package.xml b/passthrough_controller/package.xml index 203b752c51..29545390d7 100644 --- a/passthrough_controller/package.xml +++ b/passthrough_controller/package.xml @@ -2,15 +2,16 @@ passthrough_controller - 0.0.0 + 3.3.0 A simple demo of chainable controllers. It passes commands through without change. - Sai Kishor Kothakota + Sai Kishor Kothakota Andy Zelenak Apache-2.0 ament_cmake control_msgs + generate_parameter_library controller_interface hardware_interface parameter_traits @@ -18,6 +19,7 @@ rclcpp rclcpp_lifecycle realtime_tools + std_msgs ament_cmake_gmock controller_manager @@ -26,4 +28,4 @@ ament_cmake - \ No newline at end of file + diff --git a/passthrough_controller/src/passthrough_controller_parameters.yaml b/passthrough_controller/src/passthrough_controller_parameters.yaml new file mode 100644 index 0000000000..493cd6e819 --- /dev/null +++ b/passthrough_controller/src/passthrough_controller_parameters.yaml @@ -0,0 +1,7 @@ +passthrough_controller: + interfaces: { + type: string_array, + default_value: [], + description: "Names of the interfaces to be forwarded", + } + From d42ccc1628c6b8d517ec9764767995e0177f2caf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Apr 2023 23:13:27 +0200 Subject: [PATCH 3/4] added a fix to not update the reference interface if size doesn't match --- .../passthrough_controller/passthrough_controller.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp b/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp index e3c0e7ce1a..12390300ca 100644 --- a/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp +++ b/passthrough_controller/include/passthrough_controller/passthrough_controller.hpp @@ -160,6 +160,14 @@ class PassthroughController : public controller_interface::ChainableControllerIn // message is valid if (!(!joint_commands || !(*joint_commands))) { + if (reference_interfaces_.size() != (*joint_commands)->data.size()) + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *(get_node()->get_clock()), 1000, + "command size (%zu) does not match number of reference interfaces (%zu)", + (*joint_commands)->data.size(), reference_interfaces_.size()); + return controller_interface::return_type::ERROR; + } reference_interfaces_ = (*joint_commands)->data; } From a5ed335c2f2e6ced0ebaa95e23537312c9e4986c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 5 Apr 2023 23:13:56 +0200 Subject: [PATCH 4/4] added tests to the passthrough controller --- passthrough_controller/CMakeLists.txt | 7 + .../passthrough_controller.xml | 2 +- .../src/passthrough_controller.cpp | 2 +- .../passthrough_controller_parameters.yaml | 1 - .../test/test_load_controller.cpp | 3 +- .../test/test_passthrough_controller.cpp | 357 ++++++++++++++++++ .../test/test_passthrough_controller.hpp | 70 ++++ 7 files changed, 438 insertions(+), 4 deletions(-) create mode 100644 passthrough_controller/test/test_passthrough_controller.cpp create mode 100644 passthrough_controller/test/test_passthrough_controller.hpp diff --git a/passthrough_controller/CMakeLists.txt b/passthrough_controller/CMakeLists.txt index 0f3473f35c..08ab40ed98 100644 --- a/passthrough_controller/CMakeLists.txt +++ b/passthrough_controller/CMakeLists.txt @@ -59,6 +59,13 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + ament_add_gmock(test_passthrough_controller + test/test_passthrough_controller.cpp + ) + target_link_libraries(test_passthrough_controller + passthrough_controller + ) endif() install( diff --git a/passthrough_controller/passthrough_controller.xml b/passthrough_controller/passthrough_controller.xml index 3fc85562c5..7a679a575b 100644 --- a/passthrough_controller/passthrough_controller.xml +++ b/passthrough_controller/passthrough_controller.xml @@ -6,4 +6,4 @@ A simple demo of chainable controllers. It passes commands through without change. - \ No newline at end of file + diff --git a/passthrough_controller/src/passthrough_controller.cpp b/passthrough_controller/src/passthrough_controller.cpp index ef1b923c6c..ba31a95586 100644 --- a/passthrough_controller/src/passthrough_controller.cpp +++ b/passthrough_controller/src/passthrough_controller.cpp @@ -17,7 +17,7 @@ namespace passthrough_controller { -} // namespace passthrough_controller +} // namespace passthrough_controller #include "pluginlib/class_list_macros.hpp" diff --git a/passthrough_controller/src/passthrough_controller_parameters.yaml b/passthrough_controller/src/passthrough_controller_parameters.yaml index 493cd6e819..5fd4b24901 100644 --- a/passthrough_controller/src/passthrough_controller_parameters.yaml +++ b/passthrough_controller/src/passthrough_controller_parameters.yaml @@ -4,4 +4,3 @@ passthrough_controller: default_value: [], description: "Names of the interfaces to be forwarded", } - diff --git a/passthrough_controller/test/test_load_controller.cpp b/passthrough_controller/test/test_load_controller.cpp index 0c672c7308..f57e78cb6e 100644 --- a/passthrough_controller/test/test_load_controller.cpp +++ b/passthrough_controller/test/test_load_controller.cpp @@ -34,7 +34,8 @@ TEST(TestLoadPidController, load_controller) ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); - ASSERT_NO_THROW(cm.load_controller("test_passthrough_controller", "passthrough_controller/PassthroughController")); + ASSERT_NO_THROW(cm.load_controller( + "test_passthrough_controller", "passthrough_controller/PassthroughController")); rclcpp::shutdown(); } diff --git a/passthrough_controller/test/test_passthrough_controller.cpp b/passthrough_controller/test/test_passthrough_controller.cpp new file mode 100644 index 0000000000..4e792f9444 --- /dev/null +++ b/passthrough_controller/test/test_passthrough_controller.cpp @@ -0,0 +1,357 @@ +// Copyright (c) 2023, PAL Robotics +// +// 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. +/// \author Sai Kishor Kothakota + +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/loaned_command_interface.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "test_passthrough_controller.hpp" + +using hardware_interface::LoanedCommandInterface; + +namespace +{ +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} +} // namespace + +void PassthroughControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } + +void PassthroughControllerTest::TearDownTestCase() { rclcpp::shutdown(); } + +void PassthroughControllerTest::SetUp() +{ + // initialize controller + controller_ = std::make_unique(); +} + +void PassthroughControllerTest::TearDown() { controller_.reset(nullptr); } + +void PassthroughControllerTest::SetUpController() +{ + const auto result = controller_->init("passthrough_controller"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(joint_2_pos_cmd_); + command_ifs.emplace_back(joint_3_pos_cmd_); + controller_->assign_interfaces(std::move(command_ifs), {}); +} + +TEST_F(PassthroughControllerTest, InterfaceParameterNotSet) +{ + SetUpController(); + // configure failed, 'interfaces' parameter not set + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(PassthroughControllerTest, InterfaceParameterEmpty) +{ + SetUpController(); + controller_->get_node()->set_parameter({"interfaces", std::vector()}); + + // configure failed, 'interfaces' is empty + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(PassthroughControllerTest, ConfigureParamsSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter( + {"interfaces", std::vector{"joint1/interface", "joint2/interface"}}); + + // configure successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(PassthroughControllerTest, ActivateWithWrongInterfaceNameFails) +{ + SetUpController(); + + controller_->get_node()->set_parameter( + {"interfaces", std::vector{"joint1/input", "joint2/input"}}); + + // activate failed, 'input' is not a registered interface for `joint1` + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::ERROR); +} + +TEST_F(PassthroughControllerTest, ActivateSuccess) +{ + SetUpController(); + + controller_->get_node()->set_parameter( + {"interfaces", std::vector{"joint1/interface", "joint2/interface"}}); + + // activate successful + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + controller_->on_activate(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + +TEST_F(PassthroughControllerTest, CommandSuccessTest) +{ + SetUpController(); + + // configure controller + controller_->get_node()->set_parameter( + {"interfaces", + std::vector{"joint1/interface", "joint2/interface", "joint3/interface"}}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // update successful though no command has been send yet + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check joint commands are still the default ones + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + + // send command + auto command_ptr = std::make_shared(); + command_ptr->data = {10.0, 20.0, 30.0}; + controller_->rt_buffer_ptr_.writeFromNonRT(command_ptr); + + // update successful, command received + ASSERT_EQ( + controller_->update(rclcpp::Time(0.1), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check joint commands have been modified + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); +} + +TEST_F(PassthroughControllerTest, WrongCommandCheckTest) +{ + SetUpController(); + + // configure controller + controller_->get_node()->set_parameter( + {"interfaces", std::vector{"joint1/interface", "joint2/interface"}}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // send command with wrong number of joints + auto command_ptr = std::make_shared(); + command_ptr->data = {10.0}; + controller_->rt_buffer_ptr_.writeFromNonRT(command_ptr); + + // update failed, command size does not match number of joints + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::ERROR); + + // check joint commands are still the default ones + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); +} + +TEST_F(PassthroughControllerTest, NoCommandCheckTest) +{ + SetUpController(); + + // configure controller + controller_->get_node()->set_parameter( + {"interfaces", std::vector{"joint1/interface", "joint2/interface"}}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + // update successful, no command received yet + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check joint commands are still the default ones + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); +} + +TEST_F(PassthroughControllerTest, CommandCallbackTest) +{ + SetUpController(); + + // configure controller + controller_->get_node()->set_parameter( + {"interfaces", + std::vector{"joint1/interface", "joint2/interface", "joint3/interface"}}); + + // default values + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + + auto node_state = controller_->get_node()->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // send a new command + rclcpp::Node test_node("test_node"); + auto command_pub = test_node.create_publisher( + std::string(controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS()); + std_msgs::msg::Float64MultiArray command_msg; + command_msg.data = {10.0, 20.0, 30.0}; + command_pub->publish(command_msg); + + // wait for command message to be passed + ASSERT_EQ(wait_for(controller_->joints_cmd_sub_), rclcpp::WaitResultKind::Ready); + + // process callbacks + rclcpp::spin_some(controller_->get_node()->get_node_base_interface()); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0); +} + +TEST_F(PassthroughControllerTest, ActivateDeactivateCommandsResetSuccess) +{ + SetUpController(); + + // configure controller + controller_->get_node()->set_parameter( + {"interfaces", + std::vector{"joint1/interface", "joint2/interface", "joint3/interface"}}); + + // default values + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1); + + auto node_state = controller_->configure(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + auto command_msg = std::make_shared(); + command_msg->data = {10.0, 20.0, 30.0}; + + controller_->rt_buffer_ptr_.writeFromNonRT(command_msg); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + + node_state = controller_->get_node()->deactivate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // command ptr should be reset (nullptr) after deactivation - same check as in `update` + ASSERT_FALSE( + controller_->rt_buffer_ptr_.readFromNonRT() && *(controller_->rt_buffer_ptr_.readFromNonRT())); + ASSERT_FALSE( + controller_->rt_buffer_ptr_.readFromRT() && *(controller_->rt_buffer_ptr_.readFromRT())); + + // Controller is inactive but let's put some data into buffer (simulate callback when inactive) + command_msg = std::make_shared(); + command_msg->data = {5.5, 6.6, 7.7}; + + controller_->rt_buffer_ptr_.writeFromNonRT(command_msg); + + // command ptr should be available and message should be there - same check as in `update` + ASSERT_TRUE( + controller_->rt_buffer_ptr_.readFromNonRT() && *(controller_->rt_buffer_ptr_.readFromNonRT())); + ASSERT_TRUE( + controller_->rt_buffer_ptr_.readFromRT() && *(controller_->rt_buffer_ptr_.readFromRT())); + + // Now activate again + node_state = controller_->get_node()->activate(); + ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // command ptr should be reset (nullptr) after activation - same check as in `update` + ASSERT_FALSE( + controller_->rt_buffer_ptr_.readFromNonRT() && *(controller_->rt_buffer_ptr_.readFromNonRT())); + ASSERT_FALSE( + controller_->rt_buffer_ptr_.readFromRT() && *(controller_->rt_buffer_ptr_.readFromRT())); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // values should not change + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30); + + // set commands again + controller_->rt_buffer_ptr_.writeFromNonRT(command_msg); + + // update successful + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // check command in handle was set + ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5); + ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6); + ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7); +} diff --git a/passthrough_controller/test/test_passthrough_controller.hpp b/passthrough_controller/test/test_passthrough_controller.hpp new file mode 100644 index 0000000000..76f3582112 --- /dev/null +++ b/passthrough_controller/test/test_passthrough_controller.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2023, PAL Robotics +// +// 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. +/// \author Sai Kishor Kothakota + +#ifndef TEST_PASSTHROUGH_CONTROLLER_HPP +#define TEST_PASSTHROUGH_CONTROLLER_HPP + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "hardware_interface/handle.hpp" +#include "passthrough_controller/passthrough_controller.hpp" + +using hardware_interface::CommandInterface; + +// subclassing and friending so we can access member variables +class FriendPassthroughController : public passthrough_controller::PassthroughController +{ + FRIEND_TEST(PassthroughControllerTest, InterfaceParameterNotSet); + FRIEND_TEST(PassthroughControllerTest, InterfaceParameterEmpty); + FRIEND_TEST(PassthroughControllerTest, ConfigureParamsSuccess); + + FRIEND_TEST(PassthroughControllerTest, ActivateWithWrongInterfaceNameFails); + FRIEND_TEST(PassthroughControllerTest, ActivateSuccess); + FRIEND_TEST(PassthroughControllerTest, CommandSuccessTest); + FRIEND_TEST(PassthroughControllerTest, WrongCommandCheckTest); + FRIEND_TEST(PassthroughControllerTest, NoCommandCheckTest); + FRIEND_TEST(PassthroughControllerTest, CommandCallbackTest); + FRIEND_TEST(PassthroughControllerTest, ActivateDeactivateCommandsResetSuccess); +}; + +class PassthroughControllerTest : public ::testing::Test +{ +public: + static void SetUpTestCase(); + static void TearDownTestCase(); + + void SetUp(); + void TearDown(); + + void SetUpController(); + void SetUpHandles(); + +protected: + std::unique_ptr controller_; + + // dummy joint state values used for tests + const std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + std::vector joint_commands_ = {1.1, 2.1, 3.1}; + + CommandInterface joint_1_pos_cmd_{joint_names_[0], "interface", &joint_commands_[0]}; + CommandInterface joint_2_pos_cmd_{joint_names_[1], "interface", &joint_commands_[1]}; + CommandInterface joint_3_pos_cmd_{joint_names_[2], "interface", &joint_commands_[2]}; +}; + +#endif // TEST_PASSTHROUGH_CONTROLLER_HPP