From 58d610f4f8d6852a59f8d4ab5b0508edb157daa3 Mon Sep 17 00:00:00 2001
From: bpwilcox <bpwilcox@eng.ucsd.edu>
Date: Wed, 12 Jun 2019 11:51:40 -0700
Subject: [PATCH] check for successful state transitions and some refactors

fix linters and uncrustify

white line

use unordered map for transitions
---
 .../lifecycle_manager.hpp                     |  8 +++-
 .../src/lifecycle_manager.cpp                 | 45 +++++++++++++------
 2 files changed, 39 insertions(+), 14 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 d07cced4..4daedb2e 100644
--- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
+++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
@@ -63,8 +63,11 @@ protected:
   void shutdownAllNodes();
   void destroyLifecycleServiceClients();
 
+  // For a node, transition to the new target state
+  bool changeStateForNode(const std::string & node_name, std::uint8_t transition);
+
   // For each node in the map, transition to the new target state
-  void changeStateForAllNodes(std::uint8_t transition);
+  bool changeStateForAllNodes(std::uint8_t transition);
 
   // Convenience function to highlight the output on the console
   void message(const std::string & msg);
@@ -72,6 +75,9 @@ protected:
   // A map of all nodes to be controlled
   std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
 
+  // A map of the expected transitions to primary states
+  std::unordered_map<std::uint8_t, std::uint8_t> transition_state_map_;
+
   // The names of the nodes to be managed, in the order of desired bring-up
   std::vector<std::string> node_names_;
 
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index 0b07c7f6..ff98b2a8 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -25,6 +25,7 @@ using namespace std::chrono_literals;
 using namespace std::placeholders;
 
 using lifecycle_msgs::msg::Transition;
+using lifecycle_msgs::msg::State;
 using nav2_util::LifecycleServiceClient;
 
 namespace nav2_lifecycle_manager
@@ -56,6 +57,14 @@ LifecycleManager::LifecycleManager()
   auto options = rclcpp::NodeOptions().arguments(
     {std::string("__node:=") + get_name() + "service_client"});
   service_client_node_ = std::make_shared<rclcpp::Node>("_", options);
+
+  transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
+  transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
+  transition_state_map_[Transition::TRANSITION_ACTIVATE] = State::PRIMARY_STATE_ACTIVE;
+  transition_state_map_[Transition::TRANSITION_DEACTIVATE] = State::PRIMARY_STATE_INACTIVE;
+  transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
+    State::PRIMARY_STATE_FINALIZED;
+
   if (autostart_) {
     startup();
   }
@@ -103,29 +112,38 @@ LifecycleManager::destroyLifecycleServiceClients()
   }
 }
 
-void
+bool
+LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition)
+{
+  if (!node_map_[node_name]->change_state(transition) ||
+    !(node_map_[node_name]->get_state() == transition_state_map_[transition]))
+  {
+    RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", node_name.c_str());
+    return false;
+  }
+
+  return true;
+}
+
+bool
 LifecycleManager::changeStateForAllNodes(std::uint8_t transition)
 {
   for (const auto & kv : node_map_) {
-    if (!kv.second->change_state(transition)) {
-      RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", kv.first.c_str());
-      return;
+    if (!changeStateForNode(kv.first, transition)) {
+      return false;
     }
   }
+
+  return true;
 }
 
 bool
 LifecycleManager::bringupNode(const std::string & node_name)
 {
   message(std::string("Configuring and activating ") + node_name);
-  if (!node_map_[node_name]->change_state(Transition::TRANSITION_CONFIGURE)) {
-    RCLCPP_ERROR(get_logger(), "Failed to configure node: %s", node_name.c_str());
-    return false;
-  }
-
-  auto rc = node_map_[node_name]->change_state(Transition::TRANSITION_ACTIVATE);
-  if (!rc) {
-    RCLCPP_ERROR(get_logger(), "Failed to activate node: %s", node_name.c_str());
+  if (!changeStateForNode(node_name, Transition::TRANSITION_CONFIGURE) ||
+    !changeStateForNode(node_name, Transition::TRANSITION_ACTIVATE))
+  {
     return false;
   }
 
@@ -148,7 +166,8 @@ LifecycleManager::startup()
   createLifecycleServiceClients();
   for (auto & node_name : node_names_) {
     if (!bringupNode(node_name)) {
-      RCLCPP_ERROR(get_logger(), "Failed to bring up node: %s, aboring bringup", node_name.c_str());
+      RCLCPP_ERROR(get_logger(), "Failed to bring up node: %s, aborting bringup",
+        node_name.c_str());
       return;
     }
   }
-- 
GitLab