Activate hardware components by group sequentially (#2984) (#3169)

Author: mergify[bot]

controller_manager/src/controller_manager.cpp

@@ -878,30 +878,159 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
     rclcpp_lifecycle::State(
       State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
 
-  // activate all other components
-  for (const auto & [component, state] : components_to_activate)
+  // Group components by their group name for coordinated lifecycle transitions
+  std::unordered_map<std::string, std::vector<std::string>> components_by_group;
+  std::vector<std::string> ungrouped_components;
+
+  for (const auto & [component_name, component_info] : components_to_activate)
+  {
+    if (component_info.group.empty())
+    {
+      ungrouped_components.push_back(component_name);
+    }
+    else
+    {
+      components_by_group[component_info.group].push_back(component_name);
+    }
+  }
+
+  // Helper lambda to set component state with error handling
+  auto set_component_state_with_error_handling =
+    [&](const std::string & component_name, rclcpp_lifecycle::State target_state) -> bool
   {
-    rclcpp_lifecycle::State active_state(
-      State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
     if (
-      resource_manager_->set_component_state(component, active_state) ==
+      resource_manager_->set_component_state(component_name, target_state) ==
       hardware_interface::return_type::ERROR)
     {
       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()));
+            component_name.c_str(), target_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());
+          component_name.c_str(), target_state.label().c_str());
+        return false;
+      }
+    }
+    return true;
+  };
+
+  // Define lifecycle states
+  rclcpp_lifecycle::State inactive_state(
+    State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE);
+  rclcpp_lifecycle::State active_state(
+    State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+
+  // Process grouped components: first configure all in group, then activate all
+  // If any component fails, rollback all components in the group to a safe state
+  for (const auto & [group_name, group_components] : components_by_group)
+  {
+    RCLCPP_INFO(
+      get_logger(), "Processing hardware component group '%s' with %zu components.",
+      group_name.c_str(), group_components.size());
+
+    // First, configure all components in the group (transition to inactive state)
+    std::vector<std::string> successfully_configured;
+    bool configuration_failed = false;
+    for (const auto & component_name : group_components)
+    {
+      RCLCPP_INFO(
+        get_logger(), "Configuring component '%s' in group '%s'.", component_name.c_str(),
+        group_name.c_str());
+      if (set_component_state_with_error_handling(component_name, inactive_state))
+      {
+        successfully_configured.push_back(component_name);
+      }
+      else
+      {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Component '%s' in group '%s' failed to configure. Configuring of the remaining "
+          "components in the group will be skipped....",
+          component_name.c_str(), group_name.c_str());
+        configuration_failed = true;
+        break;
+      }
+    }
+
+    // If configuration failed, skip activation
+    if (configuration_failed)
+    {
+      RCLCPP_ERROR(
+        get_logger(),
+        "Group '%s' failed during configuration phase. All components in the group will remain "
+        "in their current state.",
+        group_name.c_str());
+      continue;  // Skip to next group
+    }
+
+    // Then, activate all successfully configured components in the group
+    std::vector<std::string> successfully_activated;
+    bool activation_failed = false;
+    for (const auto & component_name : successfully_configured)
+    {
+      RCLCPP_INFO(
+        get_logger(), "Activating component '%s' in group '%s'.", component_name.c_str(),
+        group_name.c_str());
+      if (set_component_state_with_error_handling(component_name, active_state))
+      {
+        successfully_activated.push_back(component_name);
+      }
+      else
+      {
+        RCLCPP_ERROR(
+          get_logger(),
+          "Component '%s' in group '%s' failed to activate. Rolling back all activated components "
+          "in the group to inactive state.",
+          component_name.c_str(), group_name.c_str());
+        activation_failed = true;
+        break;
+      }
+    }
+
+    // If activation failed, deactivate all successfully activated components back to inactive
+    if (activation_failed)
+    {
+      for (const auto & activated_component : successfully_activated)
+      {
+        RCLCPP_WARN(
+          get_logger(),
+          "Deactivating component '%s' in group '%s' due to group activation failure.",
+          activated_component.c_str(), group_name.c_str());
+        if (
+          resource_manager_->set_component_state(activated_component, inactive_state) ==
+          hardware_interface::return_type::ERROR)
+        {
+          RCLCPP_ERROR(
+            get_logger(),
+            "Failed to deactivate component '%s' during rollback. Component may be in an "
+            "inconsistent state.",
+            activated_component.c_str());
+        }
       }
+      RCLCPP_ERROR(
+        get_logger(),
+        "Group '%s' failed during activation phase. All components in the group have been "
+        "deactivated.",
+        group_name.c_str());
+    }
+  }
+
+  // Process ungrouped components individually (configure and activate each one)
+  for (const auto & component_name : ungrouped_components)
+  {
+    RCLCPP_INFO(get_logger(), "Activating component '%s'.", component_name.c_str());
+    if (set_component_state_with_error_handling(component_name, active_state))
+    {
+      RCLCPP_DEBUG(get_logger(), "Successfully activated component '%s'.", component_name.c_str());
     }
   }
+
   robot_description_notification_timer_->cancel();
 
   auto hw_components_info = resource_manager_->get_components_status();