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

Author: mergify[bot]

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

@@ -94,6 +94,7 @@ controller_manager
 * A latched topic ``~/activity`` has been added to the controller_manager to publish the activity of the controller_manager, where the change in states of the controllers and the hardware components are published. (`#2006 <https://github.com/ros-controls/ros2_control/pull/2006>`_).
 * The controller manager will use a monotonic clock for triggering read-update-write cycles, but when the ``use_sim_time`` parameter is set to true, it will use the ROS Clock for triggering. When monotonic clock is being used, all the hardware components will receive the monotonic time in their read and write method, instead the controllers will always receive the ROS time in their update method irrespective of the clock being used. (`#2046 <https://github.com/ros-controls/ros2_control/pull/2046>`_).
 * 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>`_).
 
 hardware_interface
 ******************