Skip to content

Publish is_active status from LifecycleManager #5

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Sushant-Chavan wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Publish is_active status from LifecycleManager #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2871d95
Publish `is_active` status from LifecycleManager
Sushant-Chavan Oct 20, 2025
fe2d199
Update nav2_lifecycle_manager/src/lifecycle_manager.cpp
SteveMacenski Oct 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 26 additions & 0 deletions nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,10 @@

#include "nav2_util/lifecycle_service_client.hpp"
#include "nav2_ros_common/node_thread.hpp"
#include "nav2_ros_common/publisher.hpp"
#include "nav2_ros_common/service_server.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_srvs/srv/empty.hpp"
#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
#include "std_srvs/srv/trigger.hpp"
Expand Down Expand Up @@ -78,6 +80,10 @@ class LifecycleManager : public rclcpp::Node
// The services provided by this node
nav2::ServiceServer<ManageLifecycleNodes>::SharedPtr manager_srv_;
nav2::ServiceServer<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;

// Latched publisher for is_active status
nav2::Publisher<std_msgs::msg::Bool>::SharedPtr is_active_pub_;

/**
* @brief Lifecycle node manager callback function
* @param request_header Header of the service request
Expand Down Expand Up @@ -156,6 +162,12 @@ class LifecycleManager : public rclcpp::Node
*/
void createLifecycleServiceServers();

// Support function for creating publishers
/**
* @brief Support function for creating publishers
*/
void createLifecyclePublishers();

// Support functions for shutdown
/**
* @brief Support function for shutdown
Expand All @@ -165,6 +177,10 @@ class LifecycleManager : public rclcpp::Node
* @brief Destroy all the lifecycle service clients.
*/
void destroyLifecycleServiceClients();
/**
* @brief Destroy all the lifecycle publishers.
*/
void destroyLifecyclePublishers();

// Support function for creating bond timer
/**
Expand Down Expand Up @@ -230,11 +246,21 @@ class LifecycleManager : public rclcpp::Node
*/
void registerRclPreshutdownCallback();

/**
* @brief Set the state of managed nodes
*/
void setState(const NodeState & state);

/**
* @brief function to check if managed nodes are active
*/
bool isActive();

/**
* @brief Publish the is_active state
*/
void publishIsActiveState();

// Timer thread to look at bond connections
rclcpp::TimerBase::SharedPtr init_timer_;
rclcpp::TimerBase::SharedPtr bond_timer_;
Expand Down
70 changes: 57 additions & 13 deletions nav2_lifecycle_manager/src/lifecycle_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
0s,
[this]() -> void {
init_timer_->cancel();
createLifecyclePublishers();
createLifecycleServiceClients();
createLifecycleServiceServers();
if (autostart_) {
Expand Down Expand Up @@ -142,12 +143,29 @@ LifecycleManager::managerCallback(
}
}

void
LifecycleManager::setState(const NodeState & state)
{
managed_nodes_state_ = state;
publishIsActiveState();
}

inline bool
LifecycleManager::isActive()
{
return managed_nodes_state_ == NodeState::ACTIVE;
}

void
LifecycleManager::publishIsActiveState()
{
if (is_active_pub_ && is_active_pub_->is_activated()) {
auto message = std::make_unique<std_msgs::msg::Bool>();
message->data = isActive();
is_active_pub_->publish(std::move(message));
}
}

void
LifecycleManager::isActiveCallback(
const std::shared_ptr<rmw_request_id_t>/*request_header*/,
Expand Down Expand Up @@ -217,6 +235,21 @@ LifecycleManager::createLifecycleServiceServers()
callback_group_);
}

void
LifecycleManager::createLifecyclePublishers()
{
message("Creating and initializing lifecycle publishers");

is_active_pub_ = nav2::interfaces::create_publisher<std_msgs::msg::Bool>(
shared_from_this(),
get_name() + std::string("/is_active"),
nav2::qos::LatchedSubscriptionQoS(),
callback_group_);
is_active_pub_->on_activate();
// Publish the initial state once at startup
publishIsActiveState();
}

void
LifecycleManager::destroyLifecycleServiceClients()
{
Expand All @@ -226,6 +259,16 @@ LifecycleManager::destroyLifecycleServiceClients()
}
}

void
LifecycleManager::destroyLifecyclePublishers()
{
message("Destroying lifecycle publishers");
if (is_active_pub_) {
is_active_pub_->on_deactivate();
is_active_pub_.reset();
}
}

bool
LifecycleManager::createBondConnection(const std::string & node_name)
{
Expand Down Expand Up @@ -319,7 +362,7 @@ void
LifecycleManager::shutdownAllNodes()
{
message("Deactivate, cleanup, and shutdown nodes");
managed_nodes_state_ = NodeState::FINALIZED;
setState(NodeState::FINALIZED);
changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE);
changeStateForAllNodes(Transition::TRANSITION_CLEANUP);
changeStateForAllNodes(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
Expand All @@ -333,11 +376,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;
setState(NodeState::UNKNOWN);
return false;
}
message("Managed nodes are active");
managed_nodes_state_ = NodeState::ACTIVE;
setState(NodeState::ACTIVE);
createBondTimer();
return true;
}
Expand All @@ -348,11 +391,11 @@ LifecycleManager::configure()
message("Configuring managed nodes...");
if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE)) {
RCLCPP_ERROR(get_logger(), "Failed to configure all requested nodes. Aborting bringup.");
managed_nodes_state_ = NodeState::UNKNOWN;
setState(NodeState::UNKNOWN);
return false;
}
message("Managed nodes are now configured");
managed_nodes_state_ = NodeState::INACTIVE;
setState(NodeState::INACTIVE);
return true;
}

Expand All @@ -362,11 +405,11 @@ LifecycleManager::cleanup()
message("Cleaning up managed nodes...");
if (!changeStateForAllNodes(Transition::TRANSITION_CLEANUP)) {
RCLCPP_ERROR(get_logger(), "Failed to cleanup all requested nodes. Aborting cleanup.");
managed_nodes_state_ = NodeState::UNKNOWN;
setState(NodeState::UNKNOWN);
return false;
}
message("Managed nodes have been cleaned up");
managed_nodes_state_ = NodeState::UNCONFIGURED;
setState(NodeState::UNCONFIGURED);
return true;
}

Expand All @@ -378,6 +421,7 @@ LifecycleManager::shutdown()
message("Shutting down managed nodes...");
shutdownAllNodes();
destroyLifecycleServiceClients();
destroyLifecyclePublishers();
message("Managed nodes have been shut down");
return true;
}
Expand All @@ -394,13 +438,13 @@ LifecycleManager::reset(bool hard_reset)
{
if (!hard_reset) {
RCLCPP_ERROR(get_logger(), "Failed to reset nodes: aborting reset");
managed_nodes_state_ = NodeState::UNKNOWN;
setState(NodeState::UNKNOWN);
return false;
}
}

message("Managed nodes have been reset");
managed_nodes_state_ = NodeState::UNCONFIGURED;
setState(NodeState::UNCONFIGURED);
return true;
}

Expand All @@ -412,12 +456,12 @@ LifecycleManager::pause()
message("Pausing managed nodes...");
if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) {
RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause");
managed_nodes_state_ = NodeState::UNKNOWN;
setState(NodeState::UNKNOWN);
return false;
}

message("Managed nodes have been paused");
managed_nodes_state_ = NodeState::INACTIVE;
setState(NodeState::INACTIVE);
return true;
}

Expand All @@ -427,12 +471,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;
setState(NodeState::UNKNOWN);
return false;
}

message("Managed nodes are active");
managed_nodes_state_ = NodeState::ACTIVE;
setState(NodeState::ACTIVE);
createBondTimer();
return true;
}
Expand Down
Loading