[CM] Add option to avoid shutting down on hardware initial state failure (#2230)

---------

Co-authored-by: Sai Kishor Kothakota <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>

Author: 3 people

controller_manager/src/controller_manager.cpp

@@ -657,10 +657,19 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
           resource_manager_->set_component_state(component, state) ==
           hardware_interface::return_type::ERROR)
         {
-          throw std::runtime_error(
-            fmt::format(
-              FMT_COMPILE("Failed to set the initial state of the component : {} to {}"),
-              component.c_str(), state.label()));
+          if (params_->hardware_components_initial_state.shutdown_on_initial_state_failure)
+          {
+            throw std::runtime_error(
+              fmt::format(
+                FMT_COMPILE("Failed to set the initial state of the component : {} to {}"),
+                component.c_str(), state.label()));
+          }
+          else
+          {
+            RCLCPP_ERROR(
+              get_logger(), "Failed to set the initial state of the component : '%s' to '%s'",
+              component.c_str(), state.label().c_str());
+          }
         }
         components_to_activate.erase(component);
       }
@@ -693,10 +702,19 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       resource_manager_->set_component_state(component, active_state) ==
       hardware_interface::return_type::ERROR)
     {
-      throw std::runtime_error(
-        fmt::format(
-          FMT_COMPILE("Failed to set the initial state of the component : {} to {}"),
-          component.c_str(), active_state.label()));
+      if (params_->hardware_components_initial_state.shutdown_on_initial_state_failure)
+      {
+        throw std::runtime_error(
+          fmt::format(
+            FMT_COMPILE("Failed to set the initial state of the component : {} to {}"),
+            component.c_str(), active_state.label()));
+      }
+      else
+      {
+        RCLCPP_ERROR(
+          get_logger(), "Failed to set the initial state of the component : '%s' to '%s'",
+          component.c_str(), active_state.label().c_str());
+      }
     }
   }
   robot_description_notification_timer_->cancel();

controller_manager/src/controller_manager_parameters.yaml

@@ -25,6 +25,12 @@ controller_manager:
       }
     }
 
+    shutdown_on_initial_state_failure: {
+      type: bool,
+      default_value: false,
+      description: "Specifies whether the controller manager should shut down if setting the desired initial state fails during startup.",
+    }
+
   defaults:
     switch_controller:
       strictness: {

doc/release_notes.rst

@@ -8,6 +8,7 @@ This list summarizes the changes between Jazzy (previous) and Kilted (current) r
 controller_manager
 ******************
 * The default strictness of the ``swtich_controllers`` can now we be chosen using ROS 2 parameters. The default behaviour is still left to ``BEST_EFFORT`` (`#2168 <https://github.com/ros-controls/ros2_control/pull/2168>`_).
+* Parameter ``shutdown_on_initial_state_failure`` was added to avoid shutting down on hardware initial state failure  (`#2330 <https://github.com/ros-controls/ros2_control/pull/2330>`_).
 
 ros2controlcli
 **************