diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index fbb2111cc9..07ac427b4e 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 @@ -254,12 +255,32 @@ 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_; 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 61d2c780b8..6671567207 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -89,7 +89,70 @@ 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 + { + for (auto & [key, value] : remappings) + { + if (!node_->has_parameter(remap_namespace + "." + key)) + { + node_->declare_parameter(remap_namespace + "." + key, value); + } + } + 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( @@ -135,4 +198,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 diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9ce1aab8b6..c332f587ff 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 @@ -38,6 +39,43 @@ struct ControllerSpec hardware_interface::ControllerInfo info; controller_interface::ControllerInterfaceBaseSharedPtr c; std::shared_ptr next_update_cycle_time; + + 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_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 + { + 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 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 275f12f542..01a10944a5 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -700,6 +700,18 @@ controller_interface::return_type ControllerManager::configure_controller( try { + const auto avilable_state_interfaces = resource_manager_->available_state_interfaces(); + std::for_each( + avilable_state_interfaces.begin(), avilable_state_interfaces.end(), + [&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) { @@ -778,8 +790,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_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; @@ -1217,7 +1229,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_command_interface_configuration(); std::vector command_interface_names = {}; if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { @@ -1252,7 +1264,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_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { interface_names = resource_manager_->available_command_interfaces(); @@ -1265,7 +1277,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_state_interface_configuration(); if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { interfaces = resource_manager_->available_state_interfaces(); @@ -1350,7 +1362,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_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller.info.claimed_interfaces = resource_manager_->available_command_interfaces(); @@ -1585,7 +1597,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_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) @@ -1630,7 +1642,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_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) @@ -1752,7 +1764,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_command_interface_configuration(); if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller_state.required_command_interfaces = resource_manager_->command_interface_keys(); @@ -1764,7 +1776,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_state_interface_configuration(); if (state_interface_config.type == controller_interface::interface_configuration_type::ALL) { controller_state.required_state_interfaces = resource_manager_->state_interface_keys(); @@ -2480,8 +2492,8 @@ 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_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()); @@ -2518,8 +2530,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->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_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( diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 26a5299a07..2b2f90040a 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( @@ -183,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( @@ -209,13 +211,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_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_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 ed083c17cb..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 @@ -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[] = "joint2/velocity"; class TestControllerWithInterfaces : public controller_interface::ControllerInterface { public: @@ -38,13 +38,14 @@ 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 { 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 @@ -61,6 +62,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 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/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 9caef761ab..3ecf5ae27f 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -197,3 +197,148 @@ 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; + } + + 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")); +} diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 50583b3bf4..800c008b76 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -22,6 +22,22 @@ 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. +* 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. +* 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. 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/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(); 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..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 @@ -621,7 +621,9 @@ const auto hardware_resources = + + @@ -1163,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"( @@ -1758,9 +1803,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";