From 5bf0da5b58930cba57a314d598c2cd8c66e930eb Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Wed, 9 Oct 2024 17:28:52 +0100
Subject: [PATCH] [CM] Throw an exception when the components initially fail to
 be in the required state (backport #1729) (#1777)

---
 controller_manager/src/controller_manager.cpp | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 85aee476a8..ab89c8760c 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -395,7 +395,14 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
           RCLCPP_INFO(
             get_logger(), "Setting component '%s' to '%s' state.", component.c_str(),
             state.label().c_str());
-          resource_manager_->set_component_state(component, state);
+          if (
+            resource_manager_->set_component_state(component, state) ==
+            hardware_interface::return_type::ERROR)
+          {
+            throw std::runtime_error(
+              "Failed to set the initial state of the component : " + component + " to " +
+              state.label());
+          }
           components_to_activate.erase(component);
         }
       }
@@ -459,7 +466,14 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
     {
       rclcpp_lifecycle::State active_state(
         State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
-      resource_manager_->set_component_state(component, active_state);
+      if (
+        resource_manager_->set_component_state(component, active_state) ==
+        hardware_interface::return_type::ERROR)
+      {
+        throw std::runtime_error(
+          "Failed to set the initial state of the component : " + component + " to " +
+          active_state.label());
+      }
     }
   }
 }