diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 211716f301..6049b7cb8f 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 @@ -313,12 +314,32 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC void wait_for_trigger_update_to_finish(); + /// Get the remapping of state interfaces defined in the controller namespace + /** + * Get the remapping of state interfaces defined in the controller namespace + * + * \returns map of state interfaces remapping + */ + CONTROLLER_INTERFACE_PUBLIC + const std::map & get_state_interfaces_remap() const; + + /// Get the remapping of command interfaces defined in the controller namespace + /** + * Get the remapping of command interfaces defined in the controller namespace + * + * \returns map of command interfaces remapping + */ + CONTROLLER_INTERFACE_PUBLIC + const std::map & get_command_interfaces_remap() const; + protected: std::vector command_interfaces_; std::vector state_interfaces_; private: std::shared_ptr node_; + std::map state_interfaces_remap_; + std::map command_interfaces_remap_; std::unique_ptr> async_handler_; unsigned int update_rate_ = 0; bool is_async_ = false; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a6e0da988f..e56057cf42 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -32,11 +32,48 @@ return_type ControllerInterfaceBase::init( controller_name, node_namespace, node_options, false); // disable LifecycleNode service interfaces + auto retrieve_remap_interface = [this]( + const std::string & remap_namespace, + std::map & remappings) -> bool + { + if (!node_->has_parameter(remap_namespace)) + { + node_->declare_parameters(remap_namespace, remappings); + } + return node_->get_parameters(remap_namespace, remappings); + }; + try { auto_declare("update_rate", update_rate_); auto_declare("is_async", false); auto_declare("thread_priority", 50); + if ( + retrieve_remap_interface("remap.state_interfaces", state_interfaces_remap_) && + !state_interfaces_remap_.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "The controller : %s will be able to remap the following state interfaces:", + controller_name.c_str()); + for (const auto & [key, value] : state_interfaces_remap_) + { + RCLCPP_INFO(node_->get_logger(), "\t'%s' to '%s'", key.c_str(), value.c_str()); + } + } + if ( + retrieve_remap_interface("remap.command_interfaces", command_interfaces_remap_) && + !command_interfaces_remap_.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "The controller : %s will be able to remap the following command interfaces:", + controller_name.c_str()); + for (const auto & [key, value] : command_interfaces_remap_) + { + RCLCPP_INFO(node_->get_logger(), "\t'%s' to '%s'", key.c_str(), value.c_str()); + } + } } catch (const std::exception & e) { @@ -129,7 +166,70 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } trigger_stats_.reset(); - return get_node()->configure(); + const auto & state = get_node()->configure(); + + auto retrieve_remap_interface = [this]( + const std::string & remap_namespace, + std::map & remappings) -> bool + { + 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( @@ -205,4 +305,16 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish() async_handler_->wait_for_trigger_cycle_to_finish(); } } + +const std::map & +controller_interface::ControllerInterfaceBase::get_state_interfaces_remap() const +{ + return state_interfaces_remap_; +} + +const std::map & +controller_interface::ControllerInterfaceBase::get_command_interfaces_remap() const +{ + return command_interfaces_remap_; +} } // namespace controller_interface 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 a8668f7f1b..64852d5113 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -767,6 +767,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) { @@ -850,8 +862,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; @@ -1294,7 +1306,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) { @@ -1329,7 +1341,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(); @@ -1342,7 +1354,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(); @@ -1439,7 +1451,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(); @@ -1674,7 +1686,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) @@ -1722,7 +1734,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) @@ -1842,7 +1854,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(); @@ -1854,7 +1866,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(); @@ -2633,8 +2645,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()); @@ -2671,8 +2683,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 087994bd23..2aef15fbfc 100644 --- a/controller_manager/test/test_controller_spawner_with_type.yaml +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -40,3 +40,28 @@ ctrl_with_parameters_and_no_type: /**/wildcard_ctrl_with_parameters_and_no_type: ros__parameters: joint_names: ["joint2"] + +controller_joint1: + ros__parameters: + type: "controller_manager/test_controller_with_interfaces" + joint_name: "joint1" + +controller_joint2: + ros__parameters: + type: "controller_manager/test_controller_with_interfaces" + joint_name: "joint2" + remap: + state_interfaces: + "joint2/effort": "joint2/torque" + command_interfaces: + "joint2/effort": "joint2/torque" + +controller_joint3: + ros__parameters: + type: "controller_manager/test_controller_with_interfaces" + joint_name: "joint3" + remap: + state_interfaces: + "joint3/effort": "joint3/force" + command_interfaces: + "joint3/effort": "joint3/force" diff --git a/controller_manager/test/test_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 611e8d8176..770dd6d24d 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -21,7 +21,20 @@ For details see the controller_manager section. * Pass controller manager update rate on the init of the controller interface (`#1141 `_) * A method to get node options to setup the controller node #api-breaking (`#1169 `_) * Export state interfaces from the chainable controller #api-breaking (`#1021 `_) -* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces. +* All chainable controllers must implement the method ``export_state_interfaces`` to export the state interfaces, similar to ``export_reference_interfaces`` method that is exporting the reference interfaces.* Controllers can now be able to remap the interfaces they are going to claim from the controller_manager. For instance, this can be used when a controller is going to claim the ``effort`` interface but the hardware exposes a different interface such as ``torque`` or ``force`` or ``current`` etc. This can be easily defined in the controller configuration file as shown below (`#1667 `_) + + .. code-block:: yaml + + : + ros__parameters: + type: + remap: + state_interfaces: + "/effort": "/torque" + command_interfaces: + "/effort": "/torque" + + In the above example, the controller is going to claim the ``effort`` interface but the hardware exposes the ``torque`` interface. By defining the above remapping definition, The controller will claim the ``torque`` interface instead of the ``effort`` interface. * The controllers will now set ``use_global_arguments`` from `NodeOptions `__ to false, to avoid getting influenced by global arguments (Issue : `#1684 `_) (`#1694 `_). From now on, in order to set the parameters to the controller, the ``--param-file`` option from spawner should be used. * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * The ``assign_interfaces`` and ``release_interfaces`` methods are now virtual, so that the user can override them to store the interfaces into custom variable types, so that the user can have the flexibility to take the ownership of the loaned interfaces to the controller (`#1743 `_) 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 e72a4a8214..5982370940 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -156,16 +156,18 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) EXPECT_EQ(1u, rm.system_components_size()); auto state_interface_keys = rm.state_interface_keys(); - ASSERT_THAT(state_interface_keys, SizeIs(11)); + ASSERT_THAT(state_interface_keys, SizeIs(12)); EXPECT_TRUE(rm.state_interface_exists("joint1/position")); EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")); + EXPECT_TRUE(rm.state_interface_exists("joint1/effort")); EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")); EXPECT_TRUE(rm.state_interface_exists("joint2/position")); EXPECT_TRUE(rm.state_interface_exists("joint3/position")); auto command_interface_keys = rm.command_interface_keys(); - ASSERT_THAT(command_interface_keys, SizeIs(6)); + ASSERT_THAT(command_interface_keys, SizeIs(7)); EXPECT_TRUE(rm.command_interface_exists("joint1/position")); + EXPECT_TRUE(rm.command_interface_exists("joint1/effort")); EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")); EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } @@ -397,8 +399,8 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) EXPECT_EQ(1u, rm.sensor_components_size()); EXPECT_EQ(1u, rm.system_components_size()); - ASSERT_THAT(rm.state_interface_keys(), SizeIs(11)); - ASSERT_THAT(rm.command_interface_keys(), SizeIs(6)); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); hardware_interface::HardwareInfo external_component_hw_info; external_component_hw_info.name = "ExternalComponent"; @@ -407,9 +409,9 @@ TEST_F(ResourceManagerTest, post_initialization_add_components) rm.import_component(std::make_unique(), external_component_hw_info); EXPECT_EQ(2u, rm.actuator_components_size()); - ASSERT_THAT(rm.state_interface_keys(), SizeIs(12)); + ASSERT_THAT(rm.state_interface_keys(), SizeIs(13)); EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")); - ASSERT_THAT(rm.command_interface_keys(), SizeIs(7)); + ASSERT_THAT(rm.command_interface_keys(), SizeIs(8)); EXPECT_TRUE(rm.command_interface_exists("external_joint/external_command_interface")); auto status_map = rm.get_components_status(); 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";