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 4daedb2eaf8145616630ba49eaefeef5962fce46..cf39a48a18999533da6c6d5025a771001f53d6ed 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -67,7 +67,7 @@ protected: bool changeStateForNode(const std::string & node_name, std::uint8_t transition); // For each node in the map, transition to the new target state - bool changeStateForAllNodes(std::uint8_t transition); + bool changeStateForAllNodes(std::uint8_t transition, bool reverse_order = false); // Convenience function to highlight the output on the console void message(const std::string & msg); diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index ff98b2a8c03c416c0e60b872b806c0ac3ea6369a..d99dbd323fc259b430d0bad5671c119062eeaa1d 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -126,11 +126,20 @@ LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t } bool -LifecycleManager::changeStateForAllNodes(std::uint8_t transition) +LifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool reverse_order) { - for (const auto & kv : node_map_) { - if (!changeStateForNode(kv.first, transition)) { - return false; + if (!reverse_order) { + for (auto & node_name : node_names_) { + if (!changeStateForNode(node_name, transition)) { + return false; + } + } + } else { + std::vector<std::string>::reverse_iterator rit; + for (rit = node_names_.rbegin(); rit != node_names_.rend(); ++rit) { + if (!changeStateForNode(*rit, transition)) { + return false; + } } } @@ -154,9 +163,9 @@ void LifecycleManager::shutdownAllNodes() { message("Deactivate, cleanup, and shutdown nodes"); - changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE); - changeStateForAllNodes(Transition::TRANSITION_CLEANUP); - changeStateForAllNodes(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE, true); + changeStateForAllNodes(Transition::TRANSITION_CLEANUP, true); + changeStateForAllNodes(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN, true); } void