Stop error diagnostics when pausing nav (#3830)

* Added nodestate enum and a variable to keep track of current state of managed nodes. * Updating state_of_managed_nodes_ when switching states and using it to determine an accurate diagnostics message. * Fixing bugs. * Updated/added docstrings. * Publishing OK status when nodes are unconfigured. Changed if-else chain to switch case. * Renamed NodeState PAUSED to INACTIVE, state_of_managed_nodes_ to managed_nodes_state_ and replaced system_active_ with an inline method. * Bugfix. --------- Co-authored-by: Pekka Myller <[email protected]>
ros-navigation · Oct 4, 2023 · 57dd8d4 · 57dd8d4
1 parent 4ce7702
commit 57dd8d4
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 18 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
@@ -38,6 +38,17 @@ namespace nav2_lifecycle_manager
 using namespace std::chrono_literals;  // NOLINT
 
 using nav2_msgs::srv::ManageLifecycleNodes;
+
+/// @brief Enum to for keeping track of the state of managed nodes
+enum NodeState
+{
+  UNCONFIGURED,
+  ACTIVE,
+  INACTIVE,
+  FINALIZED,
+  UNKNOWN,
+};
+
 /**
  * @class nav2_lifecycle_manager::LifecycleManager
  * @brief Implements service interface to transition the lifecycle nodes of
@@ -189,9 +200,9 @@ class LifecycleManager : public rclcpp::Node
 
   // Diagnostics functions
   /**
-   * @brief function to check if the Nav2 system is active
+   * @brief function to check the state of Nav2 nodes
    */
-  void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void CreateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   /**
    * Register our preshutdown callback for this Node's rcl Context.
@@ -201,6 +212,11 @@ class LifecycleManager : public rclcpp::Node
    */
   void registerRclPreshutdownCallback();
 
+  /**
+   * @brief function to check if managed nodes are active
+   */
+  bool isActive();
+
   // Timer thread to look at bond connections
   rclcpp::TimerBase::SharedPtr init_timer_;
   rclcpp::TimerBase::SharedPtr bond_timer_;
@@ -225,7 +241,7 @@ class LifecycleManager : public rclcpp::Node
   bool autostart_;
   bool attempt_respawn_reconnection_;
 
-  bool system_active_{false};
+  NodeState managed_nodes_state_{NodeState::UNCONFIGURED};
   diagnostic_updater::Updater diagnostics_updater_;
 
   rclcpp::Time bond_respawn_start_time_{0};

diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -106,7 +106,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
       service_thread_ = std::make_unique<nav2_util::NodeThread>(executor);
     });
   diagnostics_updater_.setHardwareID("Nav2");
-  diagnostics_updater_.add("Nav2 Health", this, &LifecycleManager::CreateActiveDiagnostic);
+  diagnostics_updater_.add("Nav2 Health", this, &LifecycleManager::CreateDiagnostic);
 }
 
 LifecycleManager::~LifecycleManager()
@@ -140,23 +140,49 @@ LifecycleManager::managerCallback(
   }
 }
 
+inline bool
+LifecycleManager::isActive()
+{
+  return managed_nodes_state_ == NodeState::ACTIVE;
+}
+
 void
 LifecycleManager::isActiveCallback(
   const std::shared_ptr<rmw_request_id_t>/*request_header*/,
   const std::shared_ptr<std_srvs::srv::Trigger::Request>/*request*/,
   std::shared_ptr<std_srvs::srv::Trigger::Response> response)
 {
-  response->success = system_active_;
+  response->success = isActive();
 }
 
 void
-LifecycleManager::CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat)
+LifecycleManager::CreateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
-  if (system_active_) {
-    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 is active");
-  } else {
-    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 is inactive");
+  unsigned char error_level;
+  std::string message;
+  switch (managed_nodes_state_) {
+    case NodeState::ACTIVE:
+      error_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+      message = "Managed nodes are active";
+      break;
+    case NodeState::INACTIVE:
+      error_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+      message = "Managed nodes are inactive";
+      break;
+    case NodeState::UNCONFIGURED:
+      error_level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+      message = "Managed nodes are unconfigured";
+      break;
+    case NodeState::FINALIZED:
+      error_level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+      message = "Managed nodes have been shut down";
+      break;
+    case NodeState::UNKNOWN:
+      error_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+      message = "An error has occurred during a node state transition";
+      break;
   }
+  stat.summary(error_level, message);
 }
 
 void
@@ -270,6 +296,7 @@ void
 LifecycleManager::shutdownAllNodes()
 {
   message("Deactivate, cleanup, and shutdown nodes");
+  managed_nodes_state_ = NodeState::FINALIZED;
   changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE);
   changeStateForAllNodes(Transition::TRANSITION_CLEANUP);
   changeStateForAllNodes(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
@@ -283,18 +310,18 @@ LifecycleManager::startup()
     !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))
   {
     RCLCPP_ERROR(get_logger(), "Failed to bring up all requested nodes. Aborting bringup.");
+    managed_nodes_state_ = NodeState::UNKNOWN;
     return false;
   }
   message("Managed nodes are active");
-  system_active_ = true;
+  managed_nodes_state_ = NodeState::ACTIVE;
   createBondTimer();
   return true;
 }
 
 bool
 LifecycleManager::shutdown()
 {
-  system_active_ = false;
   destroyBondTimer();
 
   message("Shutting down managed nodes...");
@@ -307,7 +334,6 @@ LifecycleManager::shutdown()
 bool
 LifecycleManager::reset(bool hard_reset)
 {
-  system_active_ = false;
   destroyBondTimer();
 
   message("Resetting managed nodes...");
@@ -317,27 +343,30 @@ LifecycleManager::reset(bool hard_reset)
   {
     if (!hard_reset) {
       RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset");
+      managed_nodes_state_ = NodeState::UNKNOWN;
       return false;
     }
   }
 
   message("Managed nodes have been reset");
+  managed_nodes_state_ = NodeState::UNCONFIGURED;
   return true;
 }
 
 bool
 LifecycleManager::pause()
 {
-  system_active_ = false;
   destroyBondTimer();
 
   message("Pausing managed nodes...");
   if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) {
     RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause");
+    managed_nodes_state_ = NodeState::UNKNOWN;
     return false;
   }
 
   message("Managed nodes have been paused");
+  managed_nodes_state_ = NodeState::INACTIVE;
   return true;
 }
 
@@ -347,11 +376,12 @@ LifecycleManager::resume()
   message("Resuming managed nodes...");
   if (!changeStateForAllNodes(Transition::TRANSITION_ACTIVATE)) {
     RCLCPP_ERROR(get_logger(), "Failed to resume nodes: aborting resume");
+    managed_nodes_state_ = NodeState::UNKNOWN;
     return false;
   }
 
   message("Managed nodes are active");
-  system_active_ = true;
+  managed_nodes_state_ = NodeState::ACTIVE;
   createBondTimer();
   return true;
 }
@@ -412,7 +442,7 @@ LifecycleManager::registerRclPreshutdownCallback()
 void
 LifecycleManager::checkBondConnections()
 {
-  if (!system_active_ || !rclcpp::ok() || bond_map_.empty()) {
+  if (!isActive() || !rclcpp::ok() || bond_map_.empty()) {
     return;
   }
 
@@ -457,9 +487,9 @@ LifecycleManager::checkBondRespawnConnection()
     bond_respawn_start_time_ = now();
   }
 
-  // Note: system_active_ is inverted since this should be in a failure
+  // Note: isActive() is inverted since this should be in a failure
   // condition. If another outside user actives the system again, this should not process.
-  if (system_active_ || !rclcpp::ok() || node_names_.empty()) {
+  if (isActive() || !rclcpp::ok() || node_names_.empty()) {
     bond_respawn_start_time_ = rclcpp::Time(0);
     bond_respawn_timer_.reset();
     return;