Publish is_active status from LifecycleManager (#5627)

* Publish `is_active` status from LifecycleManager

Signed-off-by: Sushant Chavan <[email protected]>

* Update nav2_lifecycle_manager/src/lifecycle_manager.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Sushant Chavan <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: Sushant-Chavan

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<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
@@ -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_;

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<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*/,
@@ -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()
 {
@@ -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;
 }