Enable non-loaded hardware components after start of CM.

controller_manager/src/controller_manager.cpp

@@ -369,7 +369,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(),
@@ -416,11 +422,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,

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: [],

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<rclcpp::executors::SingleThreadedExecutor>();
+    cm_ = std::make_shared<controller_manager::ControllerManager>(executor_, TEST_CM_NAME);
+    run_updater_ = false;
+
+    SetUpSrvsCMExecutor();
+    cm_->set_parameter(rclcpp::Parameter(
+      "hardware_components_initial_state.not_loaded",
+      std::vector<std::string>({TEST_SYSTEM_HARDWARE_NAME})));
+    cm_->set_parameter(rclcpp::Parameter(
+      "hardware_components_initial_state.inactive",
+      std::vector<std::string>({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<uint8_t>(
+      {LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
+       LFC_STATE::PRIMARY_STATE_UNKNOWN}),
+    std::vector<std::string>({INACTIVE, INACTIVE, UNKNOWN}),
+    std::vector<std::vector<std::vector<bool>>>({
+      // is available
+      {{true, true}, {true, true, true}},  // actuator
+      {{}, {true}},                        // sensor
+      {{false, false, false, false}, {false, false, false, false, false, false, false}},  // system
+    }),
+    std::vector<std::vector<std::vector<bool>>>({
+      // 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
 {

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<std::string> & dont_load_components = std::vector<std::string>());
 
   /**
    * @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_interface::HardwareInfo> & 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_interface::HardwareInfo> & hardware_info,
+    const std::vector<std::string> & dont_load_components) const;
 
   void release_command_interface(const std::string & key);
 

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<std::string> & dont_load_components)
 {
   components_are_loaded_and_initialized_ = true;
 
@@ -1062,6 +1063,16 @@ bool ResourceManager::load_and_initialize_components(
   std::lock_guard<std::recursive_mutex> 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<std::recursive_mutex> 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_interface::HardwareInfo> & hardware_info) const
+  const std::vector<hardware_interface::HardwareInfo> & hardware_info,
+  const std::vector<std::string> & dont_load_components) const
 {
   std::vector<std::string> missing_state_keys = {};
   std::vector<std::string> 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)

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<std::string> 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);