From 57dd8d4f51e8431dcad419d5da843ba9b0ed67c2 Mon Sep 17 00:00:00 2001 From: Plaqueoff <44152820+Plaqueoff@users.noreply.github.com> Date: Wed, 4 Oct 2023 23:28:21 +0300 Subject: [PATCH] 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 --- .../lifecycle_manager.hpp | 22 ++++++- .../src/lifecycle_manager.cpp | 60 ++++++++++++++----- 2 files changed, 64 insertions(+), 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 index 51c771323e..8d302907c4 100644 --- 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 index ed34d49e38..cea3a18260 100644 --- 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(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/*request_header*/, const std::shared_ptr/*request*/, std::shared_ptr 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,10 +310,11 @@ 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; } @@ -294,7 +322,6 @@ LifecycleManager::startup() 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;