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 b56e08238c8..04cac45302f 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -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" @@ -78,6 +80,10 @@ class LifecycleManager : public rclcpp::Node // The services provided by this node nav2::ServiceServer::SharedPtr manager_srv_; nav2::ServiceServer::SharedPtr is_active_srv_; + + // Latched publisher for is_active status + nav2::Publisher::SharedPtr is_active_pub_; + /** * @brief Lifecycle node manager callback function * @param request_header Header of the service request @@ -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 @@ -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 /** @@ -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_; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index 333bda694cc..a7a77685f44 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -86,6 +86,7 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options) 0s, [this]() -> void { init_timer_->cancel(); + createLifecyclePublishers(); createLifecycleServiceClients(); createLifecycleServiceServers(); if (autostart_) { @@ -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(); + message->data = isActive(); + is_active_pub_->publish(std::move(message)); + } +} + void LifecycleManager::isActiveCallback( const std::shared_ptr/*request_header*/, @@ -217,6 +235,21 @@ LifecycleManager::createLifecycleServiceServers() callback_group_); } +void +LifecycleManager::createLifecyclePublishers() +{ + message("Creating and initializing lifecycle publishers"); + + is_active_pub_ = nav2::interfaces::create_publisher( + 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() { @@ -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) { @@ -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); @@ -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; } @@ -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; } @@ -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; } @@ -378,6 +421,7 @@ LifecycleManager::shutdown() message("Shutting down managed nodes..."); shutdownAllNodes(); destroyLifecycleServiceClients(); + destroyLifecyclePublishers(); message("Managed nodes have been shut down"); return true; } @@ -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; } @@ -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; } @@ -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; }