diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index bef2985c0b..e0f4b509f8 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -439,7 +439,13 @@ 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(), @@ -486,11 +492,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript } }; - if (cm_param_listener_->is_old(*params_)) - { - *params_ = cm_param_listener_->get_params(); - } - // unconfigured (loaded only) set_components_to_state( params_->hardware_components_initial_state.unconfigured, 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: [], diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index bdd48f15ae..3eb277a789 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -350,6 +350,57 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp })); } +class TestControllerManagerHWManagementSrvsNotLoaded : public TestControllerManagerHWManagementSrvs +{ +public: + void SetUp() override + { + executor_ = std::make_shared(); + 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}))); + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.inactive", + std::vector({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_NAME}))); + + 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); + } +}; + +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 (not existing) + 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 { diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 06b8f51952..627329e1b3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -91,10 +91,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 @@ -484,7 +486,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 skipped 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 a99ae3db3b..62d6ab1a22 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1058,7 +1058,8 @@ bool ResourceManager::shutdown_components() // 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; @@ -1078,6 +1079,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) != @@ -1121,7 +1132,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( @@ -1948,13 +1960,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);