Skip to content

Commit

Permalink
Stop error diagnostics when pausing nav (ros-navigation#3830)
Browse files Browse the repository at this point in the history 
* 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]>
  • Loading branch information
@Plaqueoff
Plaqueoff and Pekka Myller authored Oct 4, 2023
1 parent 4ce7702 commit 57dd8d4
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 18 deletions.
22 changes: 19 additions & 3 deletions nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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.
Expand All @@ -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_;
Expand All @@ -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};
Expand Down
60 changes: 45 additions & 15 deletions nav2_lifecycle_manager/src/lifecycle_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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...");
Expand All @@ -307,7 +334,6 @@ LifecycleManager::shutdown()
bool
LifecycleManager::reset(bool hard_reset)
{
system_active_ = false;
destroyBondTimer();

message("Resetting managed nodes...");
Expand All @@ -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;
}

Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 57dd8d4

Please sign in to comment.