From b8914846125f09fcd0b6a9021e5feba8d0e73a1a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 21:49:04 +0200 Subject: [PATCH 01/17] add interface mapping to the ControllerInterfaceBase class --- .../controller_interface_base.hpp | 3 ++ .../src/controller_interface_base.cpp | 37 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 211716f301..6ecd7702c1 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_ #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_ +#include #include #include #include @@ -319,6 +320,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; + std::map state_interfaces_remap_; + std::map command_interfaces_remap_; std::unique_ptr> async_handler_; unsigned int update_rate_ = 0; bool is_async_ = false; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a6e0da988f..050135ecf0 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -32,11 +32,48 @@ return_type ControllerInterfaceBase::init( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces + auto retrieve_remap_interface = [this]( + const std::string & remap_namespace, + std::map & remappings) -> bool + { + if (!node_->has_parameter(remap_namespace)) + { + node_->declare_parameters(remap_namespace, remappings); + } + return node_->get_parameters(remap_namespace, remappings); + }; + try { auto_declare("update_rate", update_rate_); auto_declare("is_async", false); auto_declare("thread_priority", 50); + if ( + retrieve_remap_interface("remap.state_interfaces", state_interfaces_remap_) && + !state_interfaces_remap_.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "The controller : %s will be able to remap the following state interfaces:", + controller_name.c_str()); + for (const auto & [key, value] : state_interfaces_remap_) + { + RCLCPP_INFO(node_->get_logger(), "\t'%s' to '%s'", key.c_str(), value.c_str()); + } + } + if ( + retrieve_remap_interface("remap.command_interfaces", command_interfaces_remap_) && + !command_interfaces_remap_.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "The controller : %s will be able to remap the following command interfaces:", + controller_name.c_str()); + for (const auto & [key, value] : command_interfaces_remap_) + { + RCLCPP_INFO(node_->get_logger(), "\t'%s' to '%s'", key.c_str(), value.c_str()); + } + } } catch (const std::exception & e) { From 8db28a5105287100cd5390d3d0a27418aea3679c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 22:13:07 +0200 Subject: [PATCH 02/17] add methods to access the state and command remapping interface map --- .../controller_interface_base.hpp | 18 ++++++++++++++++++ .../src/controller_interface_base.cpp | 12 ++++++++++++ 2 files changed, 30 insertions(+) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 6ecd7702c1..6049b7cb8f 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -314,6 +314,24 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC void wait_for_trigger_update_to_finish(); + /// Get the remapping of state interfaces defined in the controller namespace + /** + * Get the remapping of state interfaces defined in the controller namespace + * + * \returns map of state interfaces remapping + */ + CONTROLLER_INTERFACE_PUBLIC + const std::map & get_state_interfaces_remap() const; + + /// Get the remapping of command interfaces defined in the controller namespace + /** + * Get the remapping of command interfaces defined in the controller namespace + * + * \returns map of command interfaces remapping + */ + CONTROLLER_INTERFACE_PUBLIC + const std::map & get_command_interfaces_remap() const; + protected: std::vector command_interfaces_; std::vector state_interfaces_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 050135ecf0..a7a7aa357f 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -242,4 +242,16 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish() async_handler_->wait_for_trigger_cycle_to_finish(); } } + +const std::map & +controller_interface::ControllerInterfaceBase::get_state_interfaces_remap() const +{ + return state_interfaces_remap_; +} + +const std::map & +controller_interface::ControllerInterfaceBase::get_command_interfaces_remap() const +{ + return command_interfaces_remap_; +} } // namespace controller_interface From 958ab19c8d64da7d535356286d53cc826023ae59 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 22:13:34 +0200 Subject: [PATCH 03/17] add a remapped integration configuration method to ControllerSpec --- .../controller_manager/controller_spec.hpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9ce1aab8b6..81e3e64f58 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -38,6 +38,50 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr next_update_cycle_time; + + controller_interface::InterfaceConfiguration get_remapped_command_interface_configuration() const + { + return get_remapped_interface_configuration( + c->command_interface_configuration(), c->get_command_interfaces_remap()); + } + + controller_interface::InterfaceConfiguration get_remapped_state_interface_configuration() const + { + return get_remapped_interface_configuration( + c->state_interface_configuration(), c->get_state_interfaces_remap()); + } + +private: + controller_interface::InterfaceConfiguration get_remapped_interface_configuration( + const controller_interface::InterfaceConfiguration & interface_cfg, + const std::map & remap) const + { + if (interface_cfg.type != controller_interface::interface_configuration_type::INDIVIDUAL) + { + return interface_cfg; + } + else + { + if (c->get_command_interfaces_remap().empty()) + { + return interface_cfg; + } + else + { + controller_interface::InterfaceConfiguration remapped_cmd_itf_cfg = interface_cfg; + for (auto & [key, value] : remap) + { + auto it = + std::find(remapped_cmd_itf_cfg.names.begin(), remapped_cmd_itf_cfg.names.end(), key); + if (it != remapped_cmd_itf_cfg.names.end()) + { + *it = value; + } + } + return remapped_cmd_itf_cfg; + } + } + } }; struct ControllerChainSpec From bc45b7c8fe71bcf4ddccdf53915d4fb74c468c92 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Jul 2024 22:21:05 +0200 Subject: [PATCH 04/17] utilize the remapped interface configuration in the CM --- .../controller_manager/controller_spec.hpp | 1 + controller_manager/src/controller_manager.cpp | 32 +++++++++++-------- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 81e3e64f58..359b553270 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -19,6 +19,7 @@ #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_ +#include #include #include #include diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a8668f7f1b..61aafda251 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -850,8 +850,8 @@ controller_interface::return_type ControllerManager::configure_controller( } // let's update the list of following and preceding controllers - const auto cmd_itfs = controller->command_interface_configuration().names; - const auto state_itfs = controller->state_interface_configuration().names; + const auto cmd_itfs = found_it->get_remapped_command_interface_configuration().names; + const auto state_itfs = found_it->get_remapped_state_interface_configuration().names; for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; @@ -1294,7 +1294,7 @@ controller_interface::return_type ControllerManager::switch_controller( const auto extract_interfaces_for_controller = [this](const ControllerSpec ctrl, std::vector & request_interface_list) { - auto command_interface_config = ctrl.c->command_interface_configuration(); + auto command_interface_config = ctrl.get_remapped_command_interface_configuration(); std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { @@ -1329,7 +1329,7 @@ controller_interface::return_type ControllerManager::switch_controller( { std::vector interface_names = {}; - auto command_interface_config = controller.c->command_interface_configuration(); + auto command_interface_config = controller.get_remapped_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { interface_names = resource_manager_->available_command_interfaces(); @@ -1342,7 +1342,7 @@ controller_interface::return_type ControllerManager::switch_controller( } std::vector interfaces = {}; - auto state_interface_config = controller.c->state_interface_configuration(); + auto state_interface_config = controller.get_remapped_state_interface_configuration(); if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { interfaces = resource_manager_->available_state_interfaces(); @@ -1439,7 +1439,7 @@ controller_interface::return_type ControllerManager::switch_controller( { if (is_controller_active(controller.c)) { - auto command_interface_config = controller.c->command_interface_configuration(); + auto command_interface_config = controller.get_remapped_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller.info.claimed_interfaces = resource_manager_->available_command_interfaces(); @@ -1674,7 +1674,7 @@ void ControllerManager::activate_controllers( bool assignment_successful = true; // assign command interfaces to the controller - auto command_interface_config = controller->command_interface_configuration(); + auto command_interface_config = found_it->get_remapped_command_interface_configuration(); // default to controller_interface::configuration_type::NONE std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) @@ -1722,7 +1722,7 @@ void ControllerManager::activate_controllers( } // assign state interfaces to the controller - auto state_interface_config = controller->state_interface_configuration(); + auto state_interface_config = found_it->get_remapped_state_interface_configuration(); // default to controller_interface::configuration_type::NONE std::vector state_interface_names = {}; if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) @@ -1842,7 +1842,7 @@ void ControllerManager::list_controllers_srv_cb( // Get information about interfaces if controller are in 'inactive' or 'active' state if (is_controller_active(controllers[i].c) || is_controller_inactive(controllers[i].c)) { - auto command_interface_config = controllers[i].c->command_interface_configuration(); + auto command_interface_config = controllers[i].get_remapped_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller_state.required_command_interfaces = resource_manager_->command_interface_keys(); @@ -1854,7 +1854,7 @@ void ControllerManager::list_controllers_srv_cb( controller_state.required_command_interfaces = command_interface_config.names; } - auto state_interface_config = controllers[i].c->state_interface_configuration(); + auto state_interface_config = controllers[i].get_remapped_state_interface_configuration(); if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller_state.required_state_interfaces = resource_manager_->state_interface_keys(); @@ -2633,8 +2633,10 @@ void ControllerManager::propagate_deactivation_of_chained_mode( break; } - const auto ctrl_cmd_itf_names = controller.c->command_interface_configuration().names; - const auto ctrl_state_itf_names = controller.c->state_interface_configuration().names; + const auto ctrl_cmd_itf_names = + controller.get_remapped_command_interface_configuration().names; + const auto ctrl_state_itf_names = + controller.get_remapped_state_interface_configuration().names; auto ctrl_itf_names = ctrl_cmd_itf_names; ctrl_itf_names.insert( ctrl_itf_names.end(), ctrl_state_itf_names.begin(), ctrl_state_itf_names.end()); @@ -2671,8 +2673,10 @@ controller_interface::return_type ControllerManager::check_following_controllers get_logger(), "Checking following controllers of preceding controller with name '%s'.", controller_it->info.name.c_str()); - const auto controller_cmd_interfaces = controller_it->c->command_interface_configuration().names; - const auto controller_state_interfaces = controller_it->c->state_interface_configuration().names; + const auto controller_cmd_interfaces = + controller_it->get_remapped_command_interface_configuration().names; + const auto controller_state_interfaces = + controller_it->get_remapped_state_interface_configuration().names; // get all interfaces of the controller auto controller_interfaces = controller_cmd_interfaces; controller_interfaces.insert( From ab750265609c501886a5da5dd31507bd1d744481 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 10 Aug 2024 11:55:04 +0200 Subject: [PATCH 05/17] Add support for effort interface inside the test_actuator component --- .../test/test_controller_manager_srvs.cpp | 12 +++-- .../test_controller_with_interfaces.hpp | 2 +- .../test/test_hardware_management_srvs.cpp | 54 +++++++++---------- .../test/test_components/test_actuator.cpp | 15 ++++++ .../ros2_control_test_assets/descriptions.hpp | 6 ++- 5 files changed, 54 insertions(+), 35 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 26a5299a07..61ba991abc 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -155,18 +155,19 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) result->controller[0].claimed_interfaces, UnorderedElementsAre( "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk", - "joint1/position", "joint1/max_velocity")); + "joint1/position", "joint1/max_velocity", "joint1/effort")); ASSERT_THAT( result->controller[0].required_command_interfaces, UnorderedElementsAre( "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", - "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity", "joint1/effort")); ASSERT_THAT( result->controller[0].required_state_interfaces, UnorderedElementsAre( "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", - "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity", + "joint1/effort")); // Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch ASSERT_EQ( @@ -209,13 +210,14 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) result->controller[0].required_command_interfaces, UnorderedElementsAre( "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", - "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity", "joint1/effort")); ASSERT_THAT( result->controller[0].required_state_interfaces, UnorderedElementsAre( "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", - "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity", + "joint1/effort")); ASSERT_EQ( controller_interface::return_type::OK, diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index ed083c17cb..a7f5da1ef3 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -24,7 +24,7 @@ namespace test_controller_with_interfaces constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] = "controller_manager/test_controller_with_interfaces"; // Corresponds to the command interface to claim -constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity"; +constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint1/effort"; class TestControllerWithInterfaces : public controller_interface::ControllerInterface { public: diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c7b67e0cfe..d846c65fdb 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -206,14 +206,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator - {{}, {true}}, // sensor + {{true, true, true}, {true, true, true, true}}, // actuator + {{}, {true}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } @@ -232,14 +232,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator - {{}, {true}}, // sensor + {{true, true, true}, {true, true, true, true}}, // actuator + {{}, {true}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -253,14 +253,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp std::vector({ACTIVE, ACTIVE, UNCONFIGURED}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator - {{}, {true}}, // sensor + {{true, true, true}, {true, true, true, true}}, // actuator + {{}, {true}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -274,14 +274,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp std::vector({ACTIVE, ACTIVE, ACTIVE}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator + {{true, true, true}, {true, true, true, true}}, // actuator {{}, {true}}, // sensor {{true, true, true, true}, {true, true, true, true, true, true, true}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -295,14 +295,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp std::vector({INACTIVE, ACTIVE, ACTIVE}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator + {{true, true, true}, {true, true, true, true}}, // actuator {{}, {true}}, // sensor {{true, true, true, true}, {true, true, true, true, true, true, true}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -316,14 +316,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp std::vector({INACTIVE, ACTIVE, ACTIVE}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator + {{true, true, true}, {true, true, true, true}}, // actuator {{}, {true}}, // sensor {{true, true, true, true}, {true, true, true, true, true, true, true}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); @@ -338,14 +338,14 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp std::vector({INACTIVE, UNCONFIGURED, ACTIVE}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator + {{true, true, true}, {true, true, true, true}}, // actuator {{}, {false}}, // sensor {{true, true, true, true}, {true, true, true, true, true, true, true}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } @@ -381,14 +381,14 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati std::vector({ACTIVE, ACTIVE, ACTIVE}), std::vector>>({ // is available - {{true, true}, {true, true, true}}, // actuator + {{true, true, true}, {true, true, true, true}}, // actuator {{}, {true}}, // sensor {{true, true, true, true}, {true, true, true, true, true, true, true}}, // system }), std::vector>>({ // is claimed - {{false, false}, {false, false, false}}, // actuator - {{}, {false}}, // sensor + {{false, false, false}, {false, false, false, false}}, // actuator + {{}, {false}}, // sensor {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 86debc1f4d..1a73b7ed59 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -61,6 +61,12 @@ class TestActuator : public ActuatorInterface state_interfaces.emplace_back(hardware_interface::StateInterface( get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[1].name, &velocity_state_)); + if (get_hardware_info().joints[0].state_interfaces.size() > 2) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + get_hardware_info().joints[0].name, get_hardware_info().joints[0].state_interfaces[2].name, + &effort_state_)); + } state_interfaces.emplace_back(hardware_interface::StateInterface( get_hardware_info().joints[0].name, "some_unlisted_interface", nullptr)); @@ -80,6 +86,12 @@ class TestActuator : public ActuatorInterface get_hardware_info().joints[0].name, get_hardware_info().joints[0].command_interfaces[1].name, &max_velocity_command_)); } + if (get_hardware_info().joints[0].command_interfaces.size() > 2) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + get_hardware_info().joints[0].name, + get_hardware_info().joints[0].command_interfaces[2].name, &effort_command_)); + } return command_interfaces; } @@ -115,6 +127,7 @@ class TestActuator : public ActuatorInterface { return return_type::DEACTIVATE; } + effort_state_ = effort_command_ / 2; // The next line is for the testing purposes. We need value to be changed to // be sure that the feedback from hardware to controllers in the chain is // working as it should. This makes value checks clearer and confirms there @@ -147,6 +160,8 @@ class TestActuator : public ActuatorInterface double velocity_state_ = 0.0; double velocity_command_ = 0.0; double max_velocity_command_ = 0.0; + double effort_state_ = 0.0; + double effort_command_ = 0.0; }; class TestUninitializableActuator : public TestActuator diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index cc2b1798d4..6d027384fe 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -621,7 +621,9 @@ const auto hardware_resources = + + @@ -1758,9 +1760,9 @@ const auto minimal_robot_missing_command_keys_urdf = [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_TYPE = "actuator"; [[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_PLUGIN_NAME = "test_actuator"; [[maybe_unused]] const std::vector TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES = { - "joint1/position", "joint1/max_velocity"}; + "joint1/position", "joint1/max_velocity", "joint1/effort"}; [[maybe_unused]] const std::vector TEST_ACTUATOR_HARDWARE_STATE_INTERFACES = { - "joint1/position", "joint1/velocity", "joint1/some_unlisted_interface"}; + "joint1/position", "joint1/velocity", "joint1/effort", "joint1/some_unlisted_interface"}; [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_NAME = "TestSensorHardware"; [[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_TYPE = "sensor"; From 896af92e6c93c8b4010cdfa0377f469236b12de3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 10 Aug 2024 13:16:54 +0200 Subject: [PATCH 06/17] parameterize the joint name in the controller with interfaces --- .../test_controller_with_interfaces.cpp | 2 ++ .../test_controller_with_interfaces.hpp | 9 ++++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp index d5695e41e5..16ca7f898a 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.cpp @@ -24,6 +24,7 @@ TestControllerWithInterfaces::TestControllerWithInterfaces() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn TestControllerWithInterfaces::on_init() { + auto_declare("joint_name", "joint1"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -36,6 +37,7 @@ controller_interface::return_type TestControllerWithInterfaces::update( rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn TestControllerWithInterfaces::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/) { + get_node()->get_parameter("joint_name", joint_name_); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index a7f5da1ef3..418ab699e9 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -15,16 +15,16 @@ #ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ #define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +#include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" +#include "hardware_interface/types/hardware_interface_type_values.hpp" namespace test_controller_with_interfaces { // Corresponds to the name listed within the pluginglib xml constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] = "controller_manager/test_controller_with_interfaces"; -// Corresponds to the command interface to claim -constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint1/effort"; class TestControllerWithInterfaces : public controller_interface::ControllerInterface { public: @@ -38,7 +38,7 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte { return controller_interface::InterfaceConfiguration{ controller_interface::interface_configuration_type::INDIVIDUAL, - {TEST_CONTROLLER_COMMAND_INTERFACE}}; + {joint_name_ + "/" + hardware_interface::HW_IF_EFFORT}}; } controller_interface::InterfaceConfiguration state_interface_configuration() const override @@ -61,6 +61,9 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte CONTROLLER_MANAGER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::string joint_name_; }; } // namespace test_controller_with_interfaces From d76ff92b7d3258df3dd02f435104d21287729fbc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Aug 2024 22:55:11 +0200 Subject: [PATCH 07/17] Move the remapping logic to the configure method of controller interface base --- .../src/controller_interface_base.cpp | 62 ++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a7a7aa357f..5bf539d2c0 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -166,7 +166,67 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } trigger_stats_.reset(); - return get_node()->configure(); + const auto & state = get_node()->configure(); + + auto retrieve_remap_interface = [this]( + const std::string & remap_namespace, + std::map & remappings) -> bool + { + if (!node_->has_parameter(remap_namespace)) + { + node_->declare_parameters(remap_namespace, remappings); + } + return node_->get_parameters(remap_namespace, remappings); + }; + + // set the map keys and values by iteratinf over the names + const auto & state_itf = state_interface_configuration(); + const auto & cmd_itfs = command_interface_configuration(); + for (const auto & interface : state_itf.names) + { + state_interfaces_remap_[interface] = interface; + } + for (const auto & interface : cmd_itfs.names) + { + command_interfaces_remap_[interface] = interface; + } + + if (retrieve_remap_interface("remap.state_interfaces", state_interfaces_remap_)) + { + if (std::any_of( + state_interfaces_remap_.begin(), state_interfaces_remap_.end(), + [](const auto & pair) { return pair.first != pair.second; })) + { + RCLCPP_WARN( + node_->get_logger(), + "The controller : %s will remap the following state interfaces:", get_node()->get_name()); + for (const auto & [key, value] : state_interfaces_remap_) + { + RCLCPP_WARN_EXPRESSION( + node_->get_logger(), key != value, "\t'%s' to '%s'", key.c_str(), value.c_str()); + } + } + } + if ( + retrieve_remap_interface("remap.command_interfaces", command_interfaces_remap_) && + !command_interfaces_remap_.empty()) + { + if (std::any_of( + command_interfaces_remap_.begin(), command_interfaces_remap_.end(), + [](const auto & pair) { return pair.first != pair.second; })) + { + RCLCPP_WARN( + node_->get_logger(), + "The controller : %s will remap the following command interfaces:", get_node()->get_name()); + for (const auto & [key, value] : command_interfaces_remap_) + { + RCLCPP_WARN_EXPRESSION( + node_->get_logger(), key != value, "\t'%s' to '%s'", key.c_str(), value.c_str()); + } + } + } + + return state; } void ControllerInterfaceBase::assign_interfaces( From 8c609f3896f8475a755b2ca53a898d7ac8e32b4d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Aug 2024 22:55:56 +0200 Subject: [PATCH 08/17] Request also state interfaces from TestControllerWithInterfaces class --- .../test_controller_with_interfaces.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp index 418ab699e9..8fbe951b4c 100644 --- a/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp +++ b/controller_manager/test/test_controller_with_interfaces/test_controller_with_interfaces.hpp @@ -44,7 +44,8 @@ class TestControllerWithInterfaces : public controller_interface::ControllerInte controller_interface::InterfaceConfiguration state_interface_configuration() const override { return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::NONE}; + controller_interface::interface_configuration_type::INDIVIDUAL, + {joint_name_ + "/" + hardware_interface::HW_IF_EFFORT}}; } CONTROLLER_MANAGER_PUBLIC From 0cf83d3f6e2c895f622c5d2d04cc10fa86e3d5e2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Aug 2024 23:14:51 +0200 Subject: [PATCH 09/17] Added test for the remapping controllers --- .../test_controller_spawner_with_type.yaml | 25 ++++ .../test/test_release_interfaces.cpp | 119 ++++++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 43 +++++++ 3 files changed, 187 insertions(+) diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml index 087994bd23..2aef15fbfc 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -40,3 +40,28 @@ ctrl_with_parameters_and_no_type: /**/wildcard_ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] + +controller_joint1: + ros__parameters: + type: "controller_manager/test_controller_with_interfaces" + joint_name: "joint1" + +controller_joint2: + ros__parameters: + type: "controller_manager/test_controller_with_interfaces" + joint_name: "joint2" + remap: + state_interfaces: + "joint2/effort": "joint2/torque" + command_interfaces: + "joint2/effort": "joint2/torque" + +controller_joint3: + ros__parameters: + type: "controller_manager/test_controller_with_interfaces" + joint_name: "joint3" + remap: + state_interfaces: + "joint3/effort": "joint3/force" + command_interfaces: + "joint3/effort": "joint3/force" diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 9caef761ab..346650bbf2 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -197,3 +197,122 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) abstract_test_controller2.c->get_lifecycle_state().id()); } } + +class TestControllerInterfacesRemapping; + +class TestableControllerManager : public controller_manager::ControllerManager +{ + friend TestControllerInterfacesRemapping; + + FRIEND_TEST(TestControllerInterfacesRemapping, check_resource_manager_resources); + +public: + TestableControllerManager( + std::unique_ptr resource_manager, + std::shared_ptr executor, + const std::string & manager_node_name = "controller_manager", + const std::string & node_namespace = "") + : controller_manager::ControllerManager( + std::move(resource_manager), executor, manager_node_name, node_namespace) + { + } +}; + +class TestControllerInterfacesRemapping : public ControllerManagerFixture +{ +public: + TestControllerInterfacesRemapping() + : ControllerManagerFixture( + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::hardware_resources_with_types_of_effort) + + std::string(ros2_control_test_assets::urdf_tail)) + { + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + using namespace std::chrono_literals; + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestControllerInterfacesRemapping, check_resource_manager_resources) +{ + ASSERT_TRUE(cm_->is_resource_manager_initialized()); + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); + std::vector expected_command_interfaces( + {"joint1/position", "joint1/max_velocity", "joint1/effort", "joint2/position", + "joint2/max_velocity", "joint2/torque", "joint3/position", "joint3/max_velocity", + "joint3/force"}); + std::vector expected_state_interfaces( + {"joint1/position", "joint1/velocity", "joint1/effort", "joint2/position", "joint2/velocity", + "joint2/torque", "joint3/position", "joint3/velocity", "joint3/force"}); + + for (const auto & itf : expected_command_interfaces) + { + ASSERT_TRUE(cm_->resource_manager_->command_interface_is_available(itf)) + << "Couldn't find command interface: " << itf; + ASSERT_FALSE(cm_->resource_manager_->command_interface_is_claimed(itf)) + << "The command interface is not supposed to be claimed by any controller: " << itf; + } + for (const auto & itf : expected_state_interfaces) + { + ASSERT_TRUE(cm_->resource_manager_->state_interface_is_available(itf)) + << "Couldn't find state interface: " << itf; + } + + // There is no effort interface for joint2 and joint3 + ASSERT_FALSE(cm_->resource_manager_->command_interface_is_available("joint2/effort")); + ASSERT_FALSE(cm_->resource_manager_->command_interface_is_available("joint3/effort")); + + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_type.yaml"; + + // Provide controller type via the parsed file + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + std::system(std::string( + "ros2 run controller_manager spawner controller_joint1 controller_joint2 " + "controller_joint3 -c test_controller_manager -p " + + test_file_path) + .c_str()), + 0); + + // once the controllers are successfully started, check the command interfaces are remapped as + // expected + for (const auto & itf : {"joint1/effort", "joint2/torque", "joint3/force"}) + { + ASSERT_TRUE(cm_->resource_manager_->command_interface_is_available(itf)) + << "The command interface are not supposed to be available: " << itf; + ASSERT_TRUE(cm_->resource_manager_->command_interface_is_claimed(itf)) + << "The command interface is supposed to be claimed by the controller : " << itf; + } +} diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 6d027384fe..6c1e39fff6 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -1165,6 +1165,49 @@ const auto hardware_resources_missing_command_keys = )"; +const auto hardware_resources_with_types_of_effort = + R"( + + + test_actuator + + + + + + + + + + + + + test_actuator + + + + + + + + + + + + + test_actuator + + + + + + + + + + +)"; + const auto diffbot_urdf = R"( From 58446cfa29f83a45c2fa776475a4c580fd85a677 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 11 Aug 2024 23:33:04 +0200 Subject: [PATCH 10/17] predeclare all the available state interfaces for the controllers --- controller_interface/src/controller_interface_base.cpp | 7 +++++-- controller_manager/src/controller_manager.cpp | 8 ++++++++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 5bf539d2c0..e56057cf42 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -172,9 +172,12 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() const std::string & remap_namespace, std::map & remappings) -> bool { - if (!node_->has_parameter(remap_namespace)) + for (auto & [key, value] : remappings) { - node_->declare_parameters(remap_namespace, remappings); + if (!node_->has_parameter(remap_namespace + "." + key)) + { + node_->declare_parameter(remap_namespace + "." + key, value); + } } return node_->get_parameters(remap_namespace, remappings); }; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 61aafda251..5f1097bba2 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -767,6 +767,14 @@ controller_interface::return_type ControllerManager::configure_controller( try { + const auto avilable_state_interfaces = resource_manager_->available_state_interfaces(); + std::map default_state_interfaces_remap; + std::for_each( + avilable_state_interfaces.begin(), avilable_state_interfaces.end(), + [&default_state_interfaces_remap](const auto & state_interface) + { default_state_interfaces_remap[state_interface] = state_interface; }); + controller->get_node()->declare_parameters( + "remap.state_interfaces", default_state_interfaces_remap); new_state = controller->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { From 2180130e5297844011990d2583736764d78d56da Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 20:01:54 +0200 Subject: [PATCH 11/17] simplify the remapping logic in the ControllerSpec class --- .../controller_manager/controller_spec.hpp | 21 +++++++------------ 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 359b553270..1f2a8efbbc 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -63,24 +63,17 @@ struct ControllerSpec } else { - if (c->get_command_interfaces_remap().empty()) + controller_interface::InterfaceConfiguration remapped_cmd_itf_cfg = interface_cfg; + for (auto & [key, value] : remap) { - return interface_cfg; - } - else - { - controller_interface::InterfaceConfiguration remapped_cmd_itf_cfg = interface_cfg; - for (auto & [key, value] : remap) + auto it = + std::find(remapped_cmd_itf_cfg.names.begin(), remapped_cmd_itf_cfg.names.end(), key); + if (it != remapped_cmd_itf_cfg.names.end()) { - auto it = - std::find(remapped_cmd_itf_cfg.names.begin(), remapped_cmd_itf_cfg.names.end(), key); - if (it != remapped_cmd_itf_cfg.names.end()) - { - *it = value; - } + *it = value; } - return remapped_cmd_itf_cfg; } + return remapped_cmd_itf_cfg; } } }; From 00be2ae15dcb16140f2b92570842b08719d2f6e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 20:06:24 +0200 Subject: [PATCH 12/17] simplify the method names --- .../controller_manager/controller_spec.hpp | 4 +-- controller_manager/src/controller_manager.cpp | 32 ++++++++----------- 2 files changed, 16 insertions(+), 20 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 1f2a8efbbc..c332f587ff 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -40,13 +40,13 @@ struct ControllerSpec controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr next_update_cycle_time; - controller_interface::InterfaceConfiguration get_remapped_command_interface_configuration() const + controller_interface::InterfaceConfiguration get_command_interface_configuration() const { return get_remapped_interface_configuration( c->command_interface_configuration(), c->get_command_interfaces_remap()); } - controller_interface::InterfaceConfiguration get_remapped_state_interface_configuration() const + controller_interface::InterfaceConfiguration get_state_interface_configuration() const { return get_remapped_interface_configuration( c->state_interface_configuration(), c->get_state_interfaces_remap()); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 5f1097bba2..f2dcdb30ca 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -858,8 +858,8 @@ controller_interface::return_type ControllerManager::configure_controller( } // let's update the list of following and preceding controllers - const auto cmd_itfs = found_it->get_remapped_command_interface_configuration().names; - const auto state_itfs = found_it->get_remapped_state_interface_configuration().names; + const auto cmd_itfs = found_it->get_command_interface_configuration().names; + const auto state_itfs = found_it->get_state_interface_configuration().names; for (const auto & cmd_itf : cmd_itfs) { controller_manager::ControllersListIterator ctrl_it; @@ -1302,7 +1302,7 @@ controller_interface::return_type ControllerManager::switch_controller( const auto extract_interfaces_for_controller = [this](const ControllerSpec ctrl, std::vector & request_interface_list) { - auto command_interface_config = ctrl.get_remapped_command_interface_configuration(); + auto command_interface_config = ctrl.get_command_interface_configuration(); std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { @@ -1337,7 +1337,7 @@ controller_interface::return_type ControllerManager::switch_controller( { std::vector interface_names = {}; - auto command_interface_config = controller.get_remapped_command_interface_configuration(); + auto command_interface_config = controller.get_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { interface_names = resource_manager_->available_command_interfaces(); @@ -1350,7 +1350,7 @@ controller_interface::return_type ControllerManager::switch_controller( } std::vector interfaces = {}; - auto state_interface_config = controller.get_remapped_state_interface_configuration(); + auto state_interface_config = controller.get_state_interface_configuration(); if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { interfaces = resource_manager_->available_state_interfaces(); @@ -1447,7 +1447,7 @@ controller_interface::return_type ControllerManager::switch_controller( { if (is_controller_active(controller.c)) { - auto command_interface_config = controller.get_remapped_command_interface_configuration(); + auto command_interface_config = controller.get_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller.info.claimed_interfaces = resource_manager_->available_command_interfaces(); @@ -1682,7 +1682,7 @@ void ControllerManager::activate_controllers( bool assignment_successful = true; // assign command interfaces to the controller - auto command_interface_config = found_it->get_remapped_command_interface_configuration(); + auto command_interface_config = found_it->get_command_interface_configuration(); // default to controller_interface::configuration_type::NONE std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) @@ -1730,7 +1730,7 @@ void ControllerManager::activate_controllers( } // assign state interfaces to the controller - auto state_interface_config = found_it->get_remapped_state_interface_configuration(); + auto state_interface_config = found_it->get_state_interface_configuration(); // default to controller_interface::configuration_type::NONE std::vector state_interface_names = {}; if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) @@ -1850,7 +1850,7 @@ void ControllerManager::list_controllers_srv_cb( // Get information about interfaces if controller are in 'inactive' or 'active' state if (is_controller_active(controllers[i].c) || is_controller_inactive(controllers[i].c)) { - auto command_interface_config = controllers[i].get_remapped_command_interface_configuration(); + auto command_interface_config = controllers[i].get_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller_state.required_command_interfaces = resource_manager_->command_interface_keys(); @@ -1862,7 +1862,7 @@ void ControllerManager::list_controllers_srv_cb( controller_state.required_command_interfaces = command_interface_config.names; } - auto state_interface_config = controllers[i].get_remapped_state_interface_configuration(); + auto state_interface_config = controllers[i].get_state_interface_configuration(); if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller_state.required_state_interfaces = resource_manager_->state_interface_keys(); @@ -2641,10 +2641,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode( break; } - const auto ctrl_cmd_itf_names = - controller.get_remapped_command_interface_configuration().names; - const auto ctrl_state_itf_names = - controller.get_remapped_state_interface_configuration().names; + const auto ctrl_cmd_itf_names = controller.get_command_interface_configuration().names; + const auto ctrl_state_itf_names = controller.get_state_interface_configuration().names; auto ctrl_itf_names = ctrl_cmd_itf_names; ctrl_itf_names.insert( ctrl_itf_names.end(), ctrl_state_itf_names.begin(), ctrl_state_itf_names.end()); @@ -2681,10 +2679,8 @@ controller_interface::return_type ControllerManager::check_following_controllers get_logger(), "Checking following controllers of preceding controller with name '%s'.", controller_it->info.name.c_str()); - const auto controller_cmd_interfaces = - controller_it->get_remapped_command_interface_configuration().names; - const auto controller_state_interfaces = - controller_it->get_remapped_state_interface_configuration().names; + const auto controller_cmd_interfaces = controller_it->get_command_interface_configuration().names; + const auto controller_state_interfaces = controller_it->get_state_interface_configuration().names; // get all interfaces of the controller auto controller_interfaces = controller_cmd_interfaces; controller_interfaces.insert( From df1a32cda655afa149720ed972569c479bef0ba6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 20:10:29 +0200 Subject: [PATCH 13/17] update test to unspawn the controllers and recheck the release of interfaces --- .../test/test_release_interfaces.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 346650bbf2..3ecf5ae27f 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -315,4 +315,30 @@ TEST_F(TestControllerInterfacesRemapping, check_resource_manager_resources) ASSERT_TRUE(cm_->resource_manager_->command_interface_is_claimed(itf)) << "The command interface is supposed to be claimed by the controller : " << itf; } + + EXPECT_EQ( + std::system( + std::string( + "ros2 run controller_manager unspawner controller_joint1 -c test_controller_manager") + .c_str()), + 0); + ASSERT_FALSE(cm_->resource_manager_->command_interface_is_claimed("joint1/effort")); + + // Now unspawn the controller_joint2 and controller_joint3 and see if the respective interfaces + // are released + EXPECT_EQ( + std::system( + std::string( + "ros2 run controller_manager unspawner controller_joint2 -c test_controller_manager") + .c_str()), + 0); + ASSERT_FALSE(cm_->resource_manager_->command_interface_is_claimed("joint2/torque")); + + EXPECT_EQ( + std::system( + std::string( + "ros2 run controller_manager unspawner controller_joint3 -c test_controller_manager") + .c_str()), + 0); + ASSERT_FALSE(cm_->resource_manager_->command_interface_is_claimed("joint3/force")); } From d77cde147fd74af26cbcd838af9fafbb1661b0b3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 20:36:40 +0200 Subject: [PATCH 14/17] Add release notes --- doc/release_notes.rst | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 611e8d8176..770dd6d24d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -21,7 +21,20 @@ For details see the controller_manager section. * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) -* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces.* Controllers can now be able to remap the interfaces they are going to claim from the controller_manager. For instance, this can be used when a controller is going to claim the ``effort`` interface but the hardware exposes a different interface such as ``torque`` or ``force`` or ``current`` etc. This can be easily defined in the controller configuration file as shown below (`#1667 `_) + + .. code-block:: yaml + + : + ros__parameters: + type: + remap: + state_interfaces: + "/effort": "/torque" + command_interfaces: + "/effort": "/torque" + + In the above example, the controller is going to claim the ``effort`` interface but the hardware exposes the ``torque`` interface. By defining the above remapping definition, The controller will claim the ``torque`` interface instead of the ``effort`` interface. * The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) From 3895be2263a20c541a23c8b4fce69d55ce7b54e0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 21:20:02 +0200 Subject: [PATCH 15/17] fix hardware_interface_testing package tests --- .../test/test_resource_manager.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index e72a4a8214..5982370940 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -156,16 +156,18 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) EXPECT_EQ(1u, rm.system_components_size()); auto state_interface_keys = rm.state_interface_keys(); - ASSERT_THAT(state_interface_keys, SizeIs(11)); + ASSERT_THAT(state_interface_keys, SizeIs(12)); EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/effort")); EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); EXPECT_TRUE(rm.state_interface_exists("joint2/position")); EXPECT_TRUE(rm.state_interface_exists("joint3/position")); auto command_interface_keys = rm.command_interface_keys(); - ASSERT_THAT(command_interface_keys, SizeIs(6)); + ASSERT_THAT(command_interface_keys, SizeIs(7)); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/effort")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } @@ -397,8 +399,8 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) EXPECT_EQ(1u, rm.sensor_components_size()); EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_THAT(rm.state_interface_keys(), SizeIs(11)); - ASSERT_THAT(rm.command_interface_keys(), SizeIs(6)); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; @@ -407,9 +409,9 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); - ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(13)); EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); - ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(8)); EXPECT_TRUE(rm.command_interface_exists("external_joint/external_command_interface")); auto status_map = rm.get_components_status(); From 5665b3a18735d157eb19fd9a0719ff0e77e94606 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 22:47:09 +0200 Subject: [PATCH 16/17] Push controller manager service tests pending fix --- controller_manager/test/test_controller_manager_srvs.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 61ba991abc..2b2f90040a 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -184,18 +184,19 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv) result->controller[0].claimed_interfaces, UnorderedElementsAre( "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk", - "joint1/position", "joint1/max_velocity")); + "joint1/position", "joint1/max_velocity", "joint1/effort")); ASSERT_THAT( result->controller[0].required_command_interfaces, UnorderedElementsAre( "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position", - "joint2/max_acceleration", "joint2/velocity", "joint3/velocity")); + "joint2/max_acceleration", "joint2/velocity", "joint3/velocity", "joint1/effort")); ASSERT_THAT( result->controller[0].required_state_interfaces, UnorderedElementsAre( "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface", "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity", - "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity")); + "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity", + "joint1/effort")); // Try again with higher timeout cm_->switch_controller( From 836de9acb5f40e575f87078cf1c8a28ecd45fa37 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 12 Aug 2024 22:47:29 +0200 Subject: [PATCH 17/17] Refine the logic of the global state interfaces declare parameters --- controller_manager/src/controller_manager.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f2dcdb30ca..64852d5113 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -768,13 +768,17 @@ controller_interface::return_type ControllerManager::configure_controller( try { const auto avilable_state_interfaces = resource_manager_->available_state_interfaces(); - std::map default_state_interfaces_remap; std::for_each( avilable_state_interfaces.begin(), avilable_state_interfaces.end(), - [&default_state_interfaces_remap](const auto & state_interface) - { default_state_interfaces_remap[state_interface] = state_interface; }); - controller->get_node()->declare_parameters( - "remap.state_interfaces", default_state_interfaces_remap); + [&controller](const auto & state_interface) + { + const auto ctrl_node = controller->get_node(); + if (!ctrl_node->has_parameter("remap.state_interfaces." + state_interface)) + { + ctrl_node->declare_parameter( + "remap.state_interfaces." + state_interface, state_interface); + } + }); new_state = controller->configure(); if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {