Add parameter to allow controllers with inactive hardware components

controller_manager/src/controller_manager.cpp

@@ -393,21 +393,8 @@ rclcpp::NodeOptions get_cm_node_options()
 ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   const std::string & node_namespace, const rclcpp::NodeOptions & options)
-: rclcpp::Node(manager_node_name, node_namespace, options),
-  diagnostics_updater_(this),
-  executor_(executor),
-  loader_(
-    std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
-      kControllerInterfaceNamespace, kControllerInterfaceClassName)),
-  chainable_loader_(
-    std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
-  cm_node_options_(options)
+: ControllerManager(executor, "", false, manager_node_name, node_namespace, options)
 {
-  initialize_parameters();
-  resource_manager_ =
-    std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, this->get_logger());
-  init_controller_manager();
 }
 
 ControllerManager::ControllerManager(
@@ -428,13 +415,18 @@ ControllerManager::ControllerManager(
 {
   initialize_parameters();
   hardware_interface::ResourceManagerParams params;
-  params.robot_description = urdf;
+  params.robot_description = robot_description_;
   params.clock = trigger_clock_;
   params.logger = this->get_logger();
   params.activate_all = activate_all_hw_components;
   params.update_rate = static_cast<unsigned int>(params_->update_rate);
   params.executor = executor_;
-  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, true);
+  params.allow_controller_activation_with_inactive_hardware =
+    params_->defaults.allow_controller_activation_with_inactive_hardware;
+  params.return_failed_hardware_names_on_return_deactivate_write_cycle_ =
+    params_->defaults.deactivate_controllers_on_hardware_return_deactivate;
+  resource_manager_ =
+    std::make_unique<hardware_interface::ResourceManager>(params, !robot_description_.empty());
   init_controller_manager();
 }
 

controller_manager/src/controller_manager_parameters.yaml

@@ -54,6 +54,23 @@ controller_manager:
         }
       }
 
+    allow_controller_activation_with_inactive_hardware: {
+      type: bool,
+      default_value: false,
+      read_only: true,
+      description: "If true, controllers are allowed to claim resources from inactive hardware components. If false, controllers can only claim resources from active hardware components. \
+      However, It is not recommended to set this parameter to true for the safety reasons with the hardware, this is purely added for backward compatibility reasons.",
+    }
+
+    deactivate_controllers_on_hardware_return_deactivate: {
+      type: bool,
+      default_value: true,
+      read_only: true,
+      description: "If true, when a hardware component returns DEACTIVATE on the write cycle, the controllers using those interfaces will be deactivated. If false, the controllers using \
+      those interfaces will continue to run. It is not recommended to set this parameter to false for the safety reasons with the hardware. This will be the default behavior of the controller \
+      manager and this parameter will be removed in the future releases. Please use with caution.",
+    }
+
   diagnostics:
     threshold:
       controller_manager:

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -631,6 +631,8 @@ class ResourceManager
   rclcpp::Clock::SharedPtr get_clock() const;
 
   bool components_are_loaded_and_initialized_ = false;
+  bool allow_controller_activation_with_inactive_hardware_ = false;
+  bool return_failed_hardware_names_on_return_deactivate_write_cycle_ = true;
 
   mutable std::recursive_mutex resource_interfaces_lock_;
   mutable std::recursive_mutex claimed_command_interfaces_lock_;

hardware_interface/include/hardware_interface/types/resource_manager_params.hpp

@@ -61,6 +61,29 @@ struct ResourceManagerParams
    */
   bool activate_all = false;
 
+  /**
+   * @brief If true, controllers are allowed to claim resources from inactive hardware components.
+   * If false, controllers can only claim resources from active hardware components.
+   * Moreover, when the hardware component returns DEACTIVATE on read/write cycle: If set to true,
+   * the controllers using those interfaces will continue to run. If set to false, the controllers
+   * using those interfaces will be deactivated.
+   * @warning Allowing control with inactive hardware is not recommended for safety reasons.
+   * Use with caution only if you really know what you are doing.
+   * @note This parameter might be deprecated or removed in the future releases. Please use with
+   * caution.
+   */
+  bool allow_controller_activation_with_inactive_hardware = false;
+
+  /**
+   * @brief If true, when a hardware component returns DEACTIVATE on the write cycle,
+   * its name will be included in the returned HardwareReadWriteStatus.failed_hardware_names list.
+   * If false, the names of such hardware components will not be included in that list.
+   * This can be useful when controllers are allowed to operate with inactive hardware components.
+   * @note This parameter might be deprecated or removed in future releases. Please use with
+   * caution.
+   */
+  bool return_failed_hardware_names_on_return_deactivate_write_cycle_ = true;
+
   /**
    * @brief The update rate (in Hz) of the ControllerManager.
    * This can be used by ResourceManager to configure asynchronous hardware components

hardware_interface/src/resource_manager.cpp

@@ -1397,6 +1397,16 @@ ResourceManager::ResourceManager(
   const hardware_interface::ResourceManagerParams & params, bool load)
 : resource_storage_(std::make_unique<ResourceStorage>(params.clock, params.logger))
 {
+  RCLCPP_WARN_EXPRESSION(
+    params.logger, params.allow_controller_activation_with_inactive_hardware,
+    "The parameter 'allow_controller_activation_with_inactive_hardware' is set to true. It is "
+    "recommended to use "
+    "the settings to false in order to avoid controllers to use inactive hardware components and "
+    "to avoid any unexpected behavior. This feature might be removed in future releases.");
+  allow_controller_activation_with_inactive_hardware_ =
+    params.allow_controller_activation_with_inactive_hardware;
+  return_failed_hardware_names_on_return_deactivate_write_cycle_ =
+    params.return_failed_hardware_names_on_return_deactivate_write_cycle_;
   if (load)
   {
     load_and_initialize_components(params);
@@ -2006,7 +2016,9 @@ bool ResourceManager::prepare_command_mode_switch(
 
   const auto & hardware_info_map = resource_storage_->hardware_info_map_;
   auto call_prepare_mode_switch =
-    [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger()](
+    [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger(),
+     allow_controller_activation_with_inactive_hardware =
+       allow_controller_activation_with_inactive_hardware_](
       auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer)
   {
     bool ret = true;
@@ -2024,7 +2036,9 @@ bool ResourceManager::prepare_command_mode_switch(
       }
       if (
         !start_interfaces_buffer.empty() &&
-        component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+        component.get_lifecycle_state().id() ==
+          lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE &&
+        !allow_controller_activation_with_inactive_hardware)
       {
         RCLCPP_WARN(
           logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s",
@@ -2106,7 +2120,9 @@ bool ResourceManager::perform_command_mode_switch(
 
   const auto & hardware_info_map = resource_storage_->hardware_info_map_;
   auto call_perform_mode_switch =
-    [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger()](
+    [&start_interfaces, &stop_interfaces, &hardware_info_map, logger = get_logger(),
+     allow_controller_activation_with_inactive_hardware =
+       allow_controller_activation_with_inactive_hardware_](
       auto & components, auto & start_interfaces_buffer, auto & stop_interfaces_buffer)
   {
     bool ret = true;
@@ -2124,7 +2140,9 @@ bool ResourceManager::perform_command_mode_switch(
       }
       if (
         !start_interfaces_buffer.empty() &&
-        component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+        component.get_lifecycle_state().id() ==
+          lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE &&
+        !allow_controller_activation_with_inactive_hardware)
       {
         RCLCPP_WARN(
           logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s",
@@ -2507,7 +2525,10 @@ HardwareReadWriteStatus ResourceManager::write(
           lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE);
         set_component_state(component_name, inactive_state);
         read_write_status.result = ret_val;
-        read_write_status.failed_hardware_names.push_back(component_name);
+        if (!return_failed_hardware_names_on_return_deactivate_write_cycle_)
+        {
+          read_write_status.failed_hardware_names.push_back(component_name);
+        }
       }
     }
   };