diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index b56e08238c8..07a1e256adc 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -259,6 +259,7 @@ class LifecycleManager : public rclcpp::Node // Whether to automatically start up the system bool autostart_; bool attempt_respawn_reconnection_; + bool parallel_state_transitions_; NodeState managed_nodes_state_{NodeState::UNCONFIGURED}; diagnostic_updater::Updater diagnostics_updater_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 333bda694cc..faaa1825213 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -16,6 +16,7 @@ #include "nav2_lifecycle_manager/lifecycle_manager.hpp" #include +#include #include #include #include @@ -45,6 +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)); registerRclPreshutdownCallback(); @@ -65,6 +67,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) bond_respawn_max_duration_ = rclcpp::Duration::from_seconds(respawn_timeout_s); get_parameter("attempt_respawn_reconnection", attempt_respawn_reconnection_); + get_parameter("parallel_state_transitions", parallel_state_transitions_); callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -281,34 +284,104 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t bool LifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool hard_change) { - // Hard change will continue even if a node fails - if (transition == Transition::TRANSITION_CONFIGURE || - transition == Transition::TRANSITION_ACTIVATE) - { - for (auto & node_name : node_names_) { - try { - if (!changeStateForNode(node_name, transition) && !hard_change) { - return false; + /* 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 + std::vector> futures; + std::vector 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 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( + 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); + } + } + + // 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(&state), "Inactive")) { + inactive_nodes += node_name + delimiter; + } else { + unconfigured_nodes += node_name + delimiter; } - } 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; + } else if (success) { + ++active_nodes_count; } } + if (active_nodes_count != node_names_.size()) { + return false; + } + } else { - std::vector::reverse_iterator rit; - for (rit = node_names_.rbegin(); rit != node_names_.rend(); ++rit) { - try { - if (!changeStateForNode(*rit, transition) && !hard_change) { + // 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) { + 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()); return false; } - } catch (const std::runtime_error & e) { - RCLCPP_ERROR( + } + } else { + std::vector::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()); - return false; + return false; + } } } }