From 72421483379c1deaf1d8ff899d2a04a6e6029ad2 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 9eaee9f15b..d916201654 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 @@ -260,6 +261,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; + std::map state_interfaces_remap_; + std::map command_interfaces_remap_; unsigned int update_rate_ = 0; bool is_async_ = false; std::string urdf_ = ""; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index db9fb99d81..4bec65a94a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -32,10 +32,47 @@ 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", cm_update_rate); auto_declare("is_async", false); + 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 0b09028815894dd9b02358bc8c076564266640e6 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 d916201654..e241daf40b 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -255,6 +255,24 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; + /// 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 4bec65a94a..95d55419ac 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -172,4 +172,16 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } +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 85aa876d968a0a74af981887dd9e87cb6c8885ee 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 5da888976d829add2bf4cdb48098ca7c307ab690 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 87414ce4ec..fa974e8f61 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -769,8 +769,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; @@ -1198,7 +1198,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) { @@ -1233,7 +1233,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(); @@ -1246,7 +1246,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(); @@ -1323,7 +1323,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(); @@ -1558,7 +1558,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) @@ -1603,7 +1603,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) @@ -1725,7 +1725,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(); @@ -1737,7 +1737,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(); @@ -2407,8 +2407,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()); @@ -2445,8 +2447,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 2113614975dd80a4e994070a2b97d959317d96b0 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 af04cc5406..c04519aa82 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")); cm_->switch_controller( {}, {test_controller::TEST_CONTROLLER_NAME}, @@ -180,13 +181,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 1f83254c5d1eb133e1ef741fecae7aab5ee05a46 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 81752bb5bf10e053057b10cffb968461296f40eb 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 | 99 ++++++++++++------- 1 file changed, 61 insertions(+), 38 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 95d55419ac..e4214cd2be 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -32,47 +32,10 @@ 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", cm_update_rate); auto_declare("is_async", false); - 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) { @@ -126,7 +89,67 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() is_async_ = get_node()->get_parameter("is_async").as_bool(); } - 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 ceb3e7780e0f3f7f932ab5cc8bd9c18499d8b5d6 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 81adce4d9869d8240649218bff09261b11f10883 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 892427bab7..f5b4143cb0 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -25,3 +25,28 @@ ctrl_with_parameters_and_no_type: /foo_namespace/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 c0305324b6..95f7e3737e 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_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 c37a08bea162ae17ae8af8a1b1c3c3d7718ce33f 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 e4214cd2be..2fefd68470 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -95,9 +95,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 fa974e8f61..f523042c58 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -691,6 +691,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 f8007689c186968f34aea319007b1c9adead1953 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 3971510b2c2319f2a23658f6fb676d85cf7e7764 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 f523042c58..cd6150e7d3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -777,8 +777,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; @@ -1206,7 +1206,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) { @@ -1241,7 +1241,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(); @@ -1254,7 +1254,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(); @@ -1331,7 +1331,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(); @@ -1566,7 +1566,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) @@ -1611,7 +1611,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) @@ -1733,7 +1733,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(); @@ -1745,7 +1745,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(); @@ -2415,10 +2415,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()); @@ -2455,10 +2453,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 e6b8d9eaf3a056864508a5f0ee8c60eb1d94145b 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 95f7e3737e..2b7bd42307 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 cd62c79c0fab0fd4372ce84daf1caa2776318c18 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 | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index caa28ca9d0..3309b667c1 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,6 +22,20 @@ For details see the controller_manager section. * 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. +* 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. controller_manager ****************** From b33f656237bdc3c954c31f05367c5ed9b4688b6d 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 51d81a90ab..78c075da47 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 4c0b6017eb3389c1d36ad754c582fdd6a9bccad3 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 c46dcf463b..25252ef8fc 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 f0f539b1b1936a8f69f871e68276a11fee894a6f 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 07d0fbe929..b83b89cf5e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -692,13 +692,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) {