cleanup

Author: jplapp

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -46,7 +46,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   declare_parameter("service_timeout", 5.0);
   declare_parameter("bond_respawn_max_duration", 10.0);
   declare_parameter("attempt_respawn_reconnection", true);
-  declare_parameter("parallel_state_transitions", rclcpp::ParameterValue(true));
+  declare_parameter("parallel_state_transitions", rclcpp::ParameterValue(false));
 
   registerRclPreshutdownCallback();
 
@@ -285,101 +285,80 @@ bool
 LifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool hard_change)
 {
   /* Function partially created using claude */
-  size_t active_nodes_count = 0;
-  std::string nodes_in_error_state = "";
-  std::string unconfigured_nodes = "";
-  std::string inactive_nodes = "";
-  std::string delimiter(", ");
-
-  if (parallel_state_transitions_) {
-    // Parallel execution
+  // Hard change will continue even if a node fails
+  if (transition == Transition::TRANSITION_CONFIGURE ||
+    transition == Transition::TRANSITION_ACTIVATE)
+  {
     std::vector<std::future<bool>> futures;
-    std::vector<std::string> processing_nodes;
-
-    // Hard change will continue even if a node fails
-    if (transition == Transition::TRANSITION_CONFIGURE ||
-      transition == Transition::TRANSITION_ACTIVATE)
-    {
-      // Launch all state changes in parallel
-      for (auto & node_name : node_names_) {
-        futures.emplace_back(std::async(std::launch::async, [this, node_name, transition]() {
-            try {
-              return changeStateForNode(node_name, transition);
-            } catch (const std::runtime_error & e) {
-              RCLCPP_ERROR(
-              get_logger(),
-              "Failed to change state for node: %s. Exception: %s.", node_name.c_str(), e.what());
-              return false;
-            }
-        }));
-        processing_nodes.push_back(node_name);
-      }
-    } else {
-      // For deactivation/cleanup, process in reverse order but still in parallel
-      std::vector<std::string> reverse_nodes(node_names_.rbegin(), node_names_.rend());
-      for (auto & node_name : reverse_nodes) {
-        futures.emplace_back(std::async(std::launch::async, [this, node_name, transition]() {
-            try {
-              return changeStateForNode(node_name, transition);
-            } catch (const std::runtime_error & e) {
-              RCLCPP_ERROR(
+    futures.reserve(node_names_.size());
+
+    for (auto & node_name : node_names_) {
+      auto future = std::async(
+        std::launch::async,
+        [this, node_name, transition]() {
+          try {
+            return changeStateForNode(node_name, transition);
+          } catch (const std::runtime_error & e) {
+            RCLCPP_ERROR(
               get_logger(),
               "Failed to change state for node: %s. Exception: %s.", node_name.c_str(), e.what());
-              return false;
-            }
-        }));
-        processing_nodes.push_back(node_name);
+            return false;
+          }
+        });
+
+      if (!parallel_state_transitions_) {
+        // Sequential execution: wait for each future immediately
+        if (!future.get() && !hard_change) {
+          return false;
+        }
+      } else {
+        // Parallel execution: collect futures to wait later
+        futures.push_back(std::move(future));
       }
     }
 
-    // Wait for all futures and collect results
-    for (size_t i = 0; i < futures.size(); ++i) {
-      bool success = futures[i].get();
-      const std::string & node_name = processing_nodes[i];
-
-      if (!success && !hard_change) {
-        uint8_t state = node_map_[node_name]->get_state();
-        if (!strcmp(reinterpret_cast<char *>(&state), "Inactive")) {
-          inactive_nodes += node_name + delimiter;
-        } else {
-          unconfigured_nodes += node_name + delimiter;
+    // If parallel execution, wait for all futures now
+    if (parallel_state_transitions_) {
+      for (auto & future : futures) {
+        if (!future.get() && !hard_change) {
+          return false;
         }
-      } else if (success) {
-        ++active_nodes_count;
       }
     }
-    if (active_nodes_count != node_names_.size()) {
-      return false;
-    }
-
   } else {
-    // Sequential execution (original behavior)
-    if (transition == Transition::TRANSITION_CONFIGURE ||
-      transition == Transition::TRANSITION_ACTIVATE)
-    {
-      for (auto & node_name : node_names_) {
-        try {
-          if (!changeStateForNode(node_name, transition) && !hard_change) {
+    std::vector<std::future<bool>> futures;
+    futures.reserve(node_names_.size());
+
+    std::vector<std::string>::reverse_iterator rit;
+    for (rit = node_names_.rbegin(); rit != node_names_.rend(); ++rit) {
+      auto future = std::async(
+        std::launch::async,
+        [this, rit, transition]() {
+          try {
+            return changeStateForNode(*rit, transition);
+          } catch (const std::runtime_error & e) {
+            RCLCPP_ERROR(
+              get_logger(),
+              "Failed to change state for node: %s. Exception: %s.", (*rit).c_str(), e.what());
             return false;
           }
-        } catch (const std::runtime_error & e) {
-          RCLCPP_ERROR(
-          get_logger(),
-          "Failed to change state for node: %s. Exception: %s.", node_name.c_str(), e.what());
+        });
+
+      if (!parallel_state_transitions_) {
+        // Sequential execution: wait for each future immediately
+        if (!future.get() && !hard_change) {
           return false;
         }
+      } else {
+        // Parallel execution: collect futures to wait later
+        futures.push_back(std::move(future));
       }
-    } else {
-      std::vector<std::string>::reverse_iterator rit;
-      for (rit = node_names_.rbegin(); rit != node_names_.rend(); ++rit) {
-        try {
-          if (!changeStateForNode(*rit, transition) && !hard_change) {
-            return false;
-          }
-        } catch (const std::runtime_error & e) {
-          RCLCPP_ERROR(
-          get_logger(),
-          "Failed to change state for node: %s. Exception: %s.", (*rit).c_str(), e.what());
+    }
+
+    // If parallel execution, wait for all futures now
+    if (parallel_state_transitions_) {
+      for (auto & future : futures) {
+        if (!future.get() && !hard_change) {
           return false;
         }
       }