From 94f06fb9bd30bfd52a626477e9cee2a67edb2235 Mon Sep 17 00:00:00 2001
From: bpwilcox <bpwilcox@eng.ucsd.edu>
Date: Wed, 28 Aug 2019 14:35:34 -0700
Subject: [PATCH] configure and activate all nodes separately
---
.../lifecycle_manager.hpp | 2 ++
.../src/lifecycle_manager.cpp | 19 +++++++++++++------
2 files changed, 15 insertions(+), 6 deletions(-)
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 a4b61583..3142ae5e 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -76,6 +76,8 @@ protected:
// A map of all nodes to be controlled
std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
+ std::map<std::uint8_t, std::string> transition_label_map_;
+
// A map of the expected transitions to primary states
std::unordered_map<std::uint8_t, std::uint8_t> transition_state_map_;
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index dac20992..1f0ccaee 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -65,6 +65,13 @@ LifecycleManager::LifecycleManager()
transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
State::PRIMARY_STATE_FINALIZED;
+ transition_label_map_[Transition::TRANSITION_CONFIGURE] = std::string("Configuring ");
+ transition_label_map_[Transition::TRANSITION_CLEANUP] = std::string("Cleaning up ");
+ transition_label_map_[Transition::TRANSITION_ACTIVATE] = std::string("Activating ");
+ transition_label_map_[Transition::TRANSITION_DEACTIVATE] = std::string("Deactivating ");
+ transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
+ std::string("Shutting down ");;
+
if (autostart_) {
startup();
}
@@ -115,6 +122,7 @@ LifecycleManager::destroyLifecycleServiceClients()
bool
LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition)
{
+ message(transition_label_map_[transition] + node_name);
if (!node_map_[node_name]->change_state(transition) ||
!(node_map_[node_name]->get_state() == transition_state_map_[transition]))
{
@@ -173,12 +181,11 @@ LifecycleManager::startup()
{
message("Starting the system bringup...");
createLifecycleServiceClients();
- for (auto & node_name : node_names_) {
- if (!bringupNode(node_name)) {
- RCLCPP_ERROR(get_logger(), "Failed to bring up node: %s, aborting bringup",
- node_name.c_str());
- return;
- }
+ if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE) ||
+ !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))
+ {
+ RCLCPP_ERROR(get_logger(), "Failed to bring up nodes: aborting bringup");
+ return;
}
message("The system is active");
}
--
GitLab