From 339dc73e19d419b85a0ed4db53938047a723d759 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 4 Jun 2023 14:59:13 +0200 Subject: [PATCH 01/13] Can compile. --- controller_manager/src/controller_manager.cpp | 95 +++++++++++++------ .../hardware_interface/resource_manager.hpp | 15 +-- hardware_interface/src/resource_manager.cpp | 21 ---- 3 files changed, 67 insertions(+), 64 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6b6efb0a5c..8f26c6c0e1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -194,46 +194,79 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... resource_manager_->load_urdf(robot_description); + // Get all components and if they are not defined in parameters activate them automatically + auto components_to_activate = resource_manager_->get_components_status(); + using lifecycle_msgs::msg::State; - std::vector configure_components_on_start = std::vector({}); - if (get_parameter("configure_components_on_start", configure_components_on_start)) + auto set_components_to_state = + [&](const std::string & parameter_name, rclcpp_lifecycle::State state) { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use " - "hardware_spawner instead."); - rclcpp_lifecycle::State inactive_state( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE); - for (const auto & component : configure_components_on_start) + std::vector components_to_set = std::vector({}); + if (get_parameter(parameter_name, components_to_set)) { - resource_manager_->set_component_state(component, inactive_state); + for (const auto & component : components_to_set) + { + if (components_to_activate.find(component) == components_to_activate.end()) + { + if (state.id() == State::PRIMARY_STATE_UNKNOWN) + { + RCLCPP_WARN( + get_logger(), + "Hardware component '%s' is unknown, but defined to not be initial loaded. " + "Please check the parameters of Controller Manager.", + component.c_str()); + } + else + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } + } + else + { + // if state is not set then component should not be loaded + if (state.id() == State::PRIMARY_STATE_UNKNOWN) + { + RCLCPP_INFO( + get_logger(), "Known component '%s' will not be loaded.", component.c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + resource_manager_->set_component_state(component, state); + } + components_to_activate.erase(component); + } + } } - } + }; + + // not loaded + set_components_to_state( + "hardware_components_initial_state.not_loaded", rclcpp_lifecycle::State()); + + // unconfigured (loaded only) + set_components_to_state( + "hardware_components_initial_state.unconfigured", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); - std::vector activate_components_on_start = std::vector({}); - if (get_parameter("activate_components_on_start", activate_components_on_start)) + // inactive (configured) + set_components_to_state( + "hardware_components_initial_state.inactive", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); + + // activate all other components + for (const auto & [component, state] : components_to_activate) { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use " - "hardware_spawner instead."); rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - for (const auto & component : activate_components_on_start) - { - resource_manager_->set_component_state(component, active_state); - } - } - // if both parameter are empty or non-existing preserve behavior where all components are - // activated per default - if (configure_components_on_start.empty() && activate_components_on_start.empty()) - { - RCLCPP_WARN_STREAM( - get_logger(), - "[Deprecated]: Automatic activation of all hardware components will not be supported in the " - "future anymore. Use hardware_spawner instead."); - resource_manager_->activate_all_components(); + resource_manager_->set_component_state(component, active_state); } } diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 4d2f995756..c60eaaad34 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -66,8 +66,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] validate_interfaces boolean argument indicating whether the exported * interfaces ought to be validated. Defaults to true. * \param[in] activate_all boolean argument indicating if all resources should be immediately - * activated. Currently used only in tests. In typical applications use parameters - * "autostart_components" and "autoconfigure_components" instead. + * activated. Currently used only in tests. */ explicit ResourceManager( const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, @@ -364,7 +363,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Reads from all active hardware components. * * Part of the real-time critical update loop. - * It is realtime-safe if used hadware interfaces are implemented adequately. + * It is realtime-safe if used hardware interfaces are implemented adequately. */ HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); @@ -373,18 +372,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Writes to all active hardware components. * * Part of the real-time critical update loop. - * It is realtime-safe if used hadware interfaces are implemented adequately. + * It is realtime-safe if used hardware interfaces are implemented adequately. */ HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period); - /// Activates all available hardware components in the system. - /** - * All available hardware components int the ros2_control framework are activated. - * This is used to preserve default behavior from previous versions where all hardware components - * are activated per default. - */ - void activate_all_components(); - /// Checks whether a command interface is registered under the given key. /** * \param[in] key string identifying the interface to check. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e23c34d94..9f646bbe58 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1353,25 +1353,4 @@ void ResourceManager::validate_storage( // END: private methods -// Temporary method to keep old interface and reduce framework changes in the PRs -void ResourceManager::activate_all_components() -{ - using lifecycle_msgs::msg::State; - rclcpp_lifecycle::State active_state( - State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - - for (auto & component : resource_storage_->actuators_) - { - set_component_state(component.get_name(), active_state); - } - for (auto & component : resource_storage_->sensors_) - { - set_component_state(component.get_name(), active_state); - } - for (auto & component : resource_storage_->systems_) - { - set_component_state(component.get_name(), active_state); - } -} - } // namespace hardware_interface From 70d8c33e76989ef2188898cd291f689a2e27c448 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 5 Jun 2023 09:31:41 +0200 Subject: [PATCH 02/13] Prepare everything for unloaded components. --- controller_manager/doc/userdoc.rst | 33 ++-- controller_manager/src/controller_manager.cpp | 57 +++++- .../test/test_hardware_management_srvs.cpp | 186 +++++++++++++++--- 3 files changed, 226 insertions(+), 50 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index de42850b6b..16d065f7c0 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -39,21 +39,32 @@ The limits will be applied after you log out and in again. Parameters ----------- -activate_components_on_start (optional; list; default: empty) - Define which hardware components should be activated when controller manager is started. +hardware_components_initial_state + Map of parameters for controlled lifecycle management of hardware components. The names of the components are defined as attribute of ````-tag in ``robot_description``. - All other components will stay ``UNCONFIGURED``. - If this and ``configure_components_on_start`` are empty, all available components will be activated. - If this or ``configure_components_on_start`` are not empty, any component not in either list will be in unconfigured state. + Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated. + Detailed explanation of each parameter is given below. + The full structure of the map is given in the following example: +.. code-block:: yaml -configure_components_on_start (optional; list; default: empty) - Define which hardware components should be configured when controller manager is started. - The names of the components are defined as attribute of ````-tag in ``robot_description``. - All other components will stay ``UNCONFIGURED``. - If this and ``activate_components_on_start`` are empty, all available components will be activated. - If this or ``activate_components_on_start`` are not empty, any component not in either list will be in unconfigured state. + hardware_components_initial_state: + not_loaded: + - "gripper1" + unconfigured: + - "arm1" + - "arm2" + inactive: + - "base3" + +hardware_components_initial_state.not_loaded (optional; list; default: empty) + Defines which hardware components (pluings) should not be loaded activated when controller manager is started. + +hardware_components_initial_state.unconfigured (optional; list; default: empty) + Defines which hardware components will be only loaded immediately when controller manager is started. +hardware_components_initial_state.inactive (optional; list; default: empty) + Defines which hardware components will be configured immediately when controller manager is started. robot_description (mandatory; string) String with the URDF string as robot description. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 8f26c6c0e1..dcecc3d8dd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -256,17 +256,60 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED)); // inactive (configured) - set_components_to_state( - "hardware_components_initial_state.inactive", - rclcpp_lifecycle::State( - State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); + // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) + std::vector configure_components_on_start = std::vector({}); + get_parameter("configure_components_on_start", configure_components_on_start); + if (!configure_components_on_start.empty()) + { + RCLCPP_WARN( + get_logger(), + "Parameter 'configure_components_on_start' is deprecated. " + "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial " + "state to 'inactive'. Don't use this parameters in combination with the new " + "'hardware_interface_state_after_start' parameter structure."); + set_components_to_state( + "configure_components_on_start", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); + } + // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023) + else + { + set_components_to_state( + "hardware_components_initial_state.inactive", + rclcpp_lifecycle::State( + State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); + } - // activate all other components - for (const auto & [component, state] : components_to_activate) + // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023) + std::vector activate_components_on_start = std::vector({}); + get_parameter("activate_components_on_start", activate_components_on_start); + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + if (!activate_components_on_start.empty()) { + RCLCPP_WARN( + get_logger(), + "Parameter 'activate_components_on_start' is deprecated. " + "Components are activated per default. Don't use this parameters in combination with the new " + "'hardware_interface_state_after_start' parameter structure."); rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); - resource_manager_->set_component_state(component, active_state); + for (const auto & component : activate_components_on_start) + { + resource_manager_->set_component_state(component, active_state); + } + } + // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023) + else + { + // activate all other components + for (const auto & [component, state] : components_to_activate) + { + rclcpp_lifecycle::State active_state( + State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); + resource_manager_->set_component_state(component, active_state); + } } } diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 273e09d6a2..c3ea0219c6 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -35,6 +35,7 @@ using hardware_interface::lifecycle_state_names::ACTIVE; using hardware_interface::lifecycle_state_names::FINALIZED; using hardware_interface::lifecycle_state_names::INACTIVE; using hardware_interface::lifecycle_state_names::UNCONFIGURED; +using hardware_interface::lifecycle_state_names::UNKNOWN; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES; using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME; @@ -69,9 +70,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_->set_parameter( rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( - "activate_components_on_start", std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + "hardware_components_initial_state.unconfigured", + std::vector({TEST_SYSTEM_HARDWARE_NAME}))); cm_->set_parameter(rclcpp::Parameter( - "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + "hardware_components_initial_state.inactive", + std::vector({TEST_SENSOR_HARDWARE_NAME}))); std::string robot_description = ""; cm_->get_parameter("robot_description", robot_description); @@ -199,36 +202,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs } }; -class TestControllerManagerHWManagementSrvsWithoutParams -: public TestControllerManagerHWManagementSrvs -{ -public: - void SetUp() override - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - run_updater_ = false; - - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - - cm_->init_resource_manager(robot_description); - - SetUpSrvsCMExecutor(); - } -}; - TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components) { // Default status after start: @@ -386,6 +359,96 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp })); } +class TestControllerManagerHWManagementSrvsNotLoaded : public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + run_updater_ = false; + + cm_->set_parameter( + rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.not_loaded", + std::vector({TEST_SYSTEM_HARDWARE_NAME}))); + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.inactive", + std::vector({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_NAME}))); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + cm_->init_resource_manager(robot_description); + + SetUpSrvsCMExecutor(); + } +}; + +TEST_F(TestControllerManagerHWManagementSrvsNotLoaded, test_component_not_loaded) +{ + // Default status after start: + // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read + + // there is not system loaded therefore test should not break having only two members for checking + // results + list_hardware_components_and_check( + // actuator, sensor, system + std::vector( + {LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, + LFC_STATE::PRIMARY_STATE_UNKNOWN}), + std::vector({INACTIVE, INACTIVE, UNKNOWN}), + std::vector>>({ + // is available + {{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, false, false, false, false}}, // system + })); +} + +class TestControllerManagerHWManagementSrvsWithoutParams +: public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + run_updater_ = false; + + // TODO(destogl): separate this to init_tests method where parameter can be set for each test + // separately + cm_->set_parameter( + rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + cm_->init_resource_manager(robot_description); + + SetUpSrvsCMExecutor(); + } +}; + TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware) { // "configure_components_on_start" and "activate_components_on_start" are not set (empty) @@ -409,3 +472,62 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } + +// BEGIN: Remove at the end of 2023 +class TestControllerManagerHWManagementSrvsOldParameters +: public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + run_updater_ = false; + + cm_->set_parameter( + rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); + cm_->set_parameter(rclcpp::Parameter( + "activate_components_on_start", std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); + cm_->set_parameter(rclcpp::Parameter( + "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + cm_->init_resource_manager(robot_description); + + SetUpSrvsCMExecutor(); + } +}; + +TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_components) +{ + // Default status after start: + // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read + + list_hardware_components_and_check( + // actuator, sensor, system + std::vector( + {LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, + LFC_STATE::PRIMARY_STATE_UNCONFIGURED}), + std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), + std::vector>>({ + // is available + {{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, false, false, false, false}}, // system + })); +} +// END: Remove at the end of 2023 From 9322373577978fab7a157b314eeaf2310f8dd191 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 5 Jun 2023 09:46:20 +0200 Subject: [PATCH 03/13] Remove not-loaded functionality that needs more changes. --- controller_manager/doc/userdoc.rst | 5 -- controller_manager/src/controller_manager.cpp | 38 +++--------- .../test/test_hardware_management_srvs.cpp | 60 ------------------- 3 files changed, 7 insertions(+), 96 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 16d065f7c0..a05f6a3afc 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -49,17 +49,12 @@ hardware_components_initial_state .. code-block:: yaml hardware_components_initial_state: - not_loaded: - - "gripper1" unconfigured: - "arm1" - "arm2" inactive: - "base3" -hardware_components_initial_state.not_loaded (optional; list; default: empty) - Defines which hardware components (pluings) should not be loaded activated when controller manager is started. - hardware_components_initial_state.unconfigured (optional; list; default: empty) Defines which hardware components will be only loaded immediately when controller manager is started. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index dcecc3d8dd..6b9d9fe3ec 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -209,46 +209,22 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { if (components_to_activate.find(component) == components_to_activate.end()) { - if (state.id() == State::PRIMARY_STATE_UNKNOWN) - { - RCLCPP_WARN( - get_logger(), - "Hardware component '%s' is unknown, but defined to not be initial loaded. " - "Please check the parameters of Controller Manager.", - component.c_str()); - } - else - { - RCLCPP_WARN( - get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", - component.c_str(), state.label().c_str()); - } + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); } else { - // if state is not set then component should not be loaded - if (state.id() == State::PRIMARY_STATE_UNKNOWN) - { - RCLCPP_INFO( - get_logger(), "Known component '%s' will not be loaded.", component.c_str()); - } - else - { - RCLCPP_INFO( - get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), - state.label().c_str()); - resource_manager_->set_component_state(component, state); - } + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + resource_manager_->set_component_state(component, state); components_to_activate.erase(component); } } } }; - // not loaded - set_components_to_state( - "hardware_components_initial_state.not_loaded", rclcpp_lifecycle::State()); - // unconfigured (loaded only) set_components_to_state( "hardware_components_initial_state.unconfigured", diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index c3ea0219c6..0fc7a2f27e 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -359,66 +359,6 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp })); } -class TestControllerManagerHWManagementSrvsNotLoaded : public TestControllerManagerHWManagementSrvs -{ -public: - void SetUp() override - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - run_updater_ = false; - - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - cm_->set_parameter(rclcpp::Parameter( - "hardware_components_initial_state.not_loaded", - std::vector({TEST_SYSTEM_HARDWARE_NAME}))); - cm_->set_parameter(rclcpp::Parameter( - "hardware_components_initial_state.inactive", - std::vector({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_NAME}))); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - - cm_->init_resource_manager(robot_description); - - SetUpSrvsCMExecutor(); - } -}; - -TEST_F(TestControllerManagerHWManagementSrvsNotLoaded, test_component_not_loaded) -{ - // Default status after start: - // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read - - // there is not system loaded therefore test should not break having only two members for checking - // results - list_hardware_components_and_check( - // actuator, sensor, system - std::vector( - {LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, - LFC_STATE::PRIMARY_STATE_UNKNOWN}), - std::vector({INACTIVE, INACTIVE, UNKNOWN}), - std::vector>>({ - // is available - {{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, false, false, false, false}}, // system - })); -} - class TestControllerManagerHWManagementSrvsWithoutParams : public TestControllerManagerHWManagementSrvs { From 57ae147fef2d8d0e235d279f7f8587270f015509 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 5 Jun 2023 10:06:16 +0200 Subject: [PATCH 04/13] Revert "Remove not-loaded functionality that needs more changes." This reverts commit 9322373577978fab7a157b314eeaf2310f8dd191. --- controller_manager/doc/userdoc.rst | 5 ++ controller_manager/src/controller_manager.cpp | 38 +++++++++--- .../test/test_hardware_management_srvs.cpp | 60 +++++++++++++++++++ 3 files changed, 96 insertions(+), 7 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index a05f6a3afc..16d065f7c0 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -49,12 +49,17 @@ hardware_components_initial_state .. code-block:: yaml hardware_components_initial_state: + not_loaded: + - "gripper1" unconfigured: - "arm1" - "arm2" inactive: - "base3" +hardware_components_initial_state.not_loaded (optional; list; default: empty) + Defines which hardware components (pluings) should not be loaded activated when controller manager is started. + hardware_components_initial_state.unconfigured (optional; list; default: empty) Defines which hardware components will be only loaded immediately when controller manager is started. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6b9d9fe3ec..dcecc3d8dd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -209,22 +209,46 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { if (components_to_activate.find(component) == components_to_activate.end()) { - RCLCPP_WARN( - get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", - component.c_str(), state.label().c_str()); + if (state.id() == State::PRIMARY_STATE_UNKNOWN) + { + RCLCPP_WARN( + get_logger(), + "Hardware component '%s' is unknown, but defined to not be initial loaded. " + "Please check the parameters of Controller Manager.", + component.c_str()); + } + else + { + RCLCPP_WARN( + get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.", + component.c_str(), state.label().c_str()); + } } else { - RCLCPP_INFO( - get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), - state.label().c_str()); - resource_manager_->set_component_state(component, state); + // if state is not set then component should not be loaded + if (state.id() == State::PRIMARY_STATE_UNKNOWN) + { + RCLCPP_INFO( + get_logger(), "Known component '%s' will not be loaded.", component.c_str()); + } + else + { + RCLCPP_INFO( + get_logger(), "Setting component '%s' to '%s' state.", component.c_str(), + state.label().c_str()); + resource_manager_->set_component_state(component, state); + } components_to_activate.erase(component); } } } }; + // not loaded + set_components_to_state( + "hardware_components_initial_state.not_loaded", rclcpp_lifecycle::State()); + // unconfigured (loaded only) set_components_to_state( "hardware_components_initial_state.unconfigured", diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 0fc7a2f27e..c3ea0219c6 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -359,6 +359,66 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp })); } +class TestControllerManagerHWManagementSrvsNotLoaded : public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + run_updater_ = false; + + cm_->set_parameter( + rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.not_loaded", + std::vector({TEST_SYSTEM_HARDWARE_NAME}))); + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.inactive", + std::vector({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_NAME}))); + + std::string robot_description = ""; + cm_->get_parameter("robot_description", robot_description); + if (robot_description.empty()) + { + throw std::runtime_error( + "Unable to initialize resource manager, no robot description found."); + } + + cm_->init_resource_manager(robot_description); + + SetUpSrvsCMExecutor(); + } +}; + +TEST_F(TestControllerManagerHWManagementSrvsNotLoaded, test_component_not_loaded) +{ + // Default status after start: + // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read + + // there is not system loaded therefore test should not break having only two members for checking + // results + list_hardware_components_and_check( + // actuator, sensor, system + std::vector( + {LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, + LFC_STATE::PRIMARY_STATE_UNKNOWN}), + std::vector({INACTIVE, INACTIVE, UNKNOWN}), + std::vector>>({ + // is available + {{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, false, false, false, false}}, // system + })); +} + class TestControllerManagerHWManagementSrvsWithoutParams : public TestControllerManagerHWManagementSrvs { From 6f96ea7830ae7944fd2be3d5bb96c59d3efd19c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Mon, 5 Jun 2023 15:05:31 +0200 Subject: [PATCH 05/13] Prepare for enabling not loaded components. --- .../hardware_interface/resource_manager.hpp | 3 ++ hardware_interface/src/resource_manager.cpp | 44 +++++++++++++++++-- 2 files changed, 43 insertions(+), 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index c60eaaad34..6272993ff4 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -404,6 +404,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + + std::vector hardware_infos_; + bool validate_interfaces_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9f646bbe58..c076eb92ac 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -97,9 +97,9 @@ class ResourceStorage RCUTILS_LOG_INFO_NAMED( "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str()); // hardware_plugin_name has to match class name in plugin xml description - auto interface = std::unique_ptr( + auto component = std::unique_ptr( loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); - HardwareT hardware(std::move(interface)); + HardwareT hardware(std::move(component)); container.emplace_back(std::move(hardware)); // initialize static data about hardware component to reduce later calls HardwareComponentInfo component_info; @@ -702,13 +702,49 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) +{ + hardware_infos_ = hardware_interface::parse_control_resources_from_urdf(urdf); + validate_interfaces_ = validate_interfaces; + + const std::string system_type = "system"; + const std::string sensor_type = "sensor"; + const std::string actuator_type = "actuator"; + + for (const auto & individual_hardware_info : hardware_infos_) + { + if (individual_hardware_info.type == actuator_type) + { + std::lock_guard guard(resource_interfaces_lock_); + std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + resource_storage_->load_and_initialize_actuator(individual_hardware_info); + } + if (individual_hardware_info.type == sensor_type) + { + std::lock_guard guard(resource_interfaces_lock_); + resource_storage_->load_and_initialize_sensor(individual_hardware_info); + } + if (individual_hardware_info.type == system_type) + { + std::lock_guard guard(resource_interfaces_lock_); + std::lock_guard guard_claimed(claimed_command_interfaces_lock_); + resource_storage_->load_and_initialize_system(individual_hardware_info); + } + } + + // throw on missing state and command interfaces, not specified keys are being ignored + if (validate_interfaces) + { + validate_storage(hardware_info); + } +} + +void ResourceManager::load_hardware() { const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; - const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - for (const auto & individual_hardware_info : hardware_info) + for (const auto & individual_hardware_info : hardware_infos_) { if (individual_hardware_info.type == actuator_type) { From 61265e68b70527f7d2ed1056400c0dcbd457ded4 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 2 Jan 2025 12:06:10 +0100 Subject: [PATCH 06/13] Added not-loaded parameter for HW plugins. --- .../src/controller_manager_parameters.yaml | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/controller_manager/src/controller_manager_parameters.yaml b/controller_manager/src/controller_manager_parameters.yaml index 1bb9b152b1..3370694cf6 100644 --- a/controller_manager/src/controller_manager_parameters.yaml +++ b/controller_manager/src/controller_manager_parameters.yaml @@ -7,6 +7,15 @@ controller_manager: } hardware_components_initial_state: + not_loaded: { + type: string_array, + default_value: [], + description: "Defines which hardware components should not be loaded activated when controller manager is started. If later used, these hardware components will need to be loaded, configured and activated manually or via a hardware spawner.", + validation: { + unique<>: null, + } + } + unconfigured: { type: string_array, default_value: [], From e6cadf6ff86b87b33266051f0793c19afe4e9dca Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Jan 2025 14:29:47 +0100 Subject: [PATCH 07/13] Update controller_manager/src/controller_manager.cpp --- controller_manager/src/controller_manager.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index e54712734b..c62f1dccb3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -450,8 +450,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE)); // activate all other components - for (const auto & [component, state] : - ) + for (const auto & [component, state] : components_to_activate) { rclcpp_lifecycle::State active_state( State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); From 74d376e0bf3da8f673c43b1d05384e84a5936976 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Jan 2025 16:44:17 +0100 Subject: [PATCH 08/13] Update controller_manager/test/test_hardware_management_srvs.cpp --- .../test/test_hardware_management_srvs.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 9e16a70e41..167e062c24 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -356,10 +356,10 @@ class TestControllerManagerHWManagementSrvsNotLoaded : public TestControllerMana void SetUp() override { executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + cm_ = std::make_shared(executor_, TEST_CM_NAME); run_updater_ = false; + SetUpSrvsCMExecutor(); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.not_loaded", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -367,14 +367,10 @@ class TestControllerManagerHWManagementSrvsNotLoaded : public TestControllerMana "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_NAME}))); - cm_ = std::make_shared(executor_, TEST_CM_NAME); - run_updater_ = false; - + std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - - SetUpSrvsCMExecutor(); } }; From 8f890c7fb9314bcfb5a0e3e936c58233bfd52bba Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Jan 2025 16:45:06 +0100 Subject: [PATCH 09/13] Update controller_manager/test/test_hardware_management_srvs.cpp --- .../test/test_hardware_management_srvs.cpp | 59 ------------------- 1 file changed, 59 deletions(-) diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 167e062c24..931567ed39 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -443,62 +443,3 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system })); } - -// BEGIN: Remove at the end of 2023 -class TestControllerManagerHWManagementSrvsOldParameters -: public TestControllerManagerHWManagementSrvs -{ -public: - void SetUp() override - { - executor_ = std::make_shared(); - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - run_updater_ = false; - - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - cm_->set_parameter(rclcpp::Parameter( - "activate_components_on_start", std::vector({TEST_ACTUATOR_HARDWARE_NAME}))); - cm_->set_parameter(rclcpp::Parameter( - "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - - cm_->init_resource_manager(robot_description); - - SetUpSrvsCMExecutor(); - } -}; - -TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_components) -{ - // Default status after start: - // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read - - list_hardware_components_and_check( - // actuator, sensor, system - std::vector( - {LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, - LFC_STATE::PRIMARY_STATE_UNCONFIGURED}), - std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), - std::vector>>({ - // is available - {{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, false, false, false, false}}, // system - })); -} -// END: Remove at the end of 2023 From b21954ec8cdd9f6cac4abe5abec9b9b6a85bb75c Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Jan 2025 16:49:13 +0100 Subject: [PATCH 10/13] Apply suggestions from code review --- controller_manager/src/controller_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c62f1dccb3..08b9ed1215 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -419,14 +419,14 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { *params_ = cm_param_listener_->get_params(); } - // not-loaded for (const auto & component : params_->hardware_components_initial_state.not_loaded) { if (components_to_activate.find(component) == components_to_activate.end()) { RCLCPP_WARN( - get_logger(), "Hardware component '%s' is unknown, therefore cannot be exclueded from loading.", + get_logger(), + "Hardware component '%s' is unknown, therefore cannot be exclueded from loading.", component.c_str()); } else From 6a3870c5a085b6d2e08c66e5546652364871ffe1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 9 Jan 2025 16:49:37 +0100 Subject: [PATCH 11/13] Update controller_manager/src/controller_manager.cpp --- controller_manager/src/controller_manager.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 08b9ed1215..5bf2f05579 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -431,8 +431,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } else { - RCLCPP_INFO( - get_logger(), "Excluding component '%s' from loading.", component.c_str()); + RCLCPP_INFO(get_logger(), "Excluding component '%s' from loading.", component.c_str()); components_to_activate.erase(component); } } From 3c61830428f8d698a89c4dca47027accd0ca1c8f Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 10 Jan 2025 12:18:46 +0100 Subject: [PATCH 12/13] Extend RM to support not-loading of compoentns and fix tests. --- controller_manager/src/controller_manager.cpp | 28 ++++--------------- .../test/test_hardware_management_srvs.cpp | 2 +- .../hardware_interface/resource_manager.hpp | 15 ++++++++-- hardware_interface/src/resource_manager.cpp | 26 +++++++++++++++-- .../test/test_resource_manager.cpp | 18 ++++++++++++ 5 files changed, 61 insertions(+), 28 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6a3e5cf79b..7e882c5604 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -369,7 +369,12 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & void ControllerManager::init_resource_manager(const std::string & robot_description) { - if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_)) + if (cm_param_listener_->is_old(*params_)) + { + *params_ = cm_param_listener_->get_params(); + } + + if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_, params_->hardware_components_initial_state.not_loaded)) { RCLCPP_WARN( get_logger(), @@ -416,27 +421,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } }; - if (cm_param_listener_->is_old(*params_)) - { - *params_ = cm_param_listener_->get_params(); - } - // not-loaded - for (const auto & component : params_->hardware_components_initial_state.not_loaded) - { - if (components_to_activate.find(component) == components_to_activate.end()) - { - RCLCPP_WARN( - get_logger(), - "Hardware component '%s' is unknown, therefore cannot be exclueded from loading.", - component.c_str()); - } - else - { - RCLCPP_INFO(get_logger(), "Excluding component '%s' from loading.", component.c_str()); - components_to_activate.erase(component); - } - } - // unconfigured (loaded only) set_components_to_state( params_->hardware_components_initial_state.unconfigured, diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 931567ed39..3eb277a789 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -382,7 +382,7 @@ TEST_F(TestControllerManagerHWManagementSrvsNotLoaded, test_component_not_loaded // there is not system loaded therefore test should not break having only two members for checking // results list_hardware_components_and_check( - // actuator, sensor, system + // actuator, sensor, system (not existing) std::vector( {LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_UNKNOWN}), diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b67aba088d..28d2ffea48 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -84,10 +84,12 @@ class ResourceManager * * \param[in] urdf string containing the URDF. * \param[in] update_rate update rate of the main control loop, i.e., of the controller manager. + * \param[in] dont_load_components list of component names that should be excluded from loading * \returns false if URDF validation has failed. */ virtual bool load_and_initialize_components( - const std::string & urdf, const unsigned int update_rate = 100); + const std::string & urdf, const unsigned int update_rate = 100, + const std::vector & dont_load_components = std::vector()); /** * @brief if the resource manager load_and_initialize_components(...) function has been called @@ -477,7 +479,16 @@ class ResourceManager mutable std::recursive_mutex resources_lock_; private: - bool validate_storage(const std::vector & hardware_info) const; + /// Validates if all interfaces are exported as defined in URDF. + /** + * \param[in] hardware_info vector of hardware parsed from URDF that storage is validated against. + * \param[in] dont_load_components list of component names that should not be loaded and therefore + * should be skiped during validation. + * \return true if storage is consistent with URDF, false otherwise. + */ + bool validate_storage( + const std::vector & hardware_info, + const std::vector & dont_load_components) const; void release_command_interface(const std::string & key); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7a616d890d..32c197cfa3 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1042,7 +1042,8 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread bool ResourceManager::load_and_initialize_components( - const std::string & urdf, const unsigned int update_rate) + const std::string & urdf, const unsigned int update_rate, + const std::vector & dont_load_components) { components_are_loaded_and_initialized_ = true; @@ -1062,6 +1063,16 @@ bool ResourceManager::load_and_initialize_components( std::lock_guard resource_guard(resources_lock_); for (const auto & individual_hardware_info : hardware_info) { + const auto find_if = std::find( + dont_load_components.begin(), dont_load_components.end(), individual_hardware_info.name); + if (find_if != dont_load_components.end()) + { + RCUTILS_LOG_INFO_NAMED( + "resource_manager", "Skipping loading for hardware with name %s.", + individual_hardware_info.name.c_str()); + continue; + } + // Check for identical names if ( resource_storage_->hardware_info_map_.find(individual_hardware_info.name) != @@ -1105,7 +1116,8 @@ bool ResourceManager::load_and_initialize_components( } } - if (components_are_loaded_and_initialized_ && validate_storage(hardware_info)) + if ( + components_are_loaded_and_initialized_ && validate_storage(hardware_info, dont_load_components)) { std::lock_guard guard(resources_lock_); read_write_status.failed_hardware_names.reserve( @@ -1932,13 +1944,21 @@ rclcpp::Clock::SharedPtr ResourceManager::get_clock() const // BEGIN: private methods bool ResourceManager::validate_storage( - const std::vector & hardware_info) const + const std::vector & hardware_info, + const std::vector & dont_load_components) const { std::vector missing_state_keys = {}; std::vector missing_command_keys = {}; for (const auto & hardware : hardware_info) { + const auto find_if = + std::find(dont_load_components.begin(), dont_load_components.end(), hardware.name); + if (find_if != dont_load_components.end()) + { + continue; + } + for (const auto & joint : hardware.joints) { for (const auto & state_interface : joint.state_interfaces) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index be9e672b3b..90581d4f68 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -273,6 +273,24 @@ TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) ASSERT_TRUE(rm.are_components_initialized()); } +TEST_F( + ResourceManagerTest, + if_component_excluded_from_loading_expect_that_component_not_loaded_and_initialized) +{ + const auto component_to_not_load = "TestSystemHardware"; + std::vector dont_load_components; + dont_load_components.push_back(component_to_not_load); + + TestableResourceManager rm(node_); + rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_urdf, 100, dont_load_components); + ASSERT_TRUE(rm.are_components_initialized()); + + const auto components = rm.get_components_status(); + EXPECT_EQ(components.size(), 2); // Sensor and Actuator are loaded + EXPECT_TRUE(components.find(component_to_not_load) == components.end()); // not found +} + TEST_F(ResourceManagerTest, resource_claiming) { TestableResourceManager rm(node_, ros2_control_test_assets::minimal_robot_urdf); From 416b4df7361c73e614ec4bd281a0edee72c94a43 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 10 Jan 2025 12:55:33 +0100 Subject: [PATCH 13/13] Fixed formatting and typo. --- controller_manager/src/controller_manager.cpp | 3 ++- .../include/hardware_interface/resource_manager.hpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 7e882c5604..229a35c7aa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -374,7 +374,8 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript *params_ = cm_param_listener_->get_params(); } - if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_, params_->hardware_components_initial_state.not_loaded)) + if (!resource_manager_->load_and_initialize_components( + robot_description, update_rate_, params_->hardware_components_initial_state.not_loaded)) { RCLCPP_WARN( get_logger(), diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 28d2ffea48..b052684c52 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -483,7 +483,7 @@ class ResourceManager /** * \param[in] hardware_info vector of hardware parsed from URDF that storage is validated against. * \param[in] dont_load_components list of component names that should not be loaded and therefore - * should be skiped during validation. + * should be skipped during validation. * \return true if storage is consistent with URDF, false otherwise. */ bool validate_storage(