[CM] Throw an exception when the components initially fail to be in t…

…he required state (#1729)

(cherry picked from commit 1c7a5d1)

# Conflicts:
#	controller_manager/src/controller_manager.cpp

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);
         }
       }
@@ -446,6 +453,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
       "'hardware_components_initial_state' parameter structure.");
     rclcpp_lifecycle::State active_state(
       State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+<<<<<<< HEAD
     for (const auto & component : activate_components_on_start)
     {
       resource_manager_->set_component_state(component, active_state);
@@ -460,6 +468,15 @@ 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());
+>>>>>>> 1c7a5d1 ([CM] Throw an exception when the components initially fail to be in the required state (#1729))
     }
   }
 }