add pause, resume, reset to manager client

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -34,8 +34,11 @@ public:
   LifecycleManagerClient();
 
   // Client-side interface to the Nav2 lifecycle manager
-  void startup();
-  void shutdown();
+  bool startup();
+  bool shutdown();
+  bool pause();
+  bool resume();
+  bool reset();
 
   // A couple convenience methods to facilitate scripting tests
   void set_initial_pose(double x, double y, double theta);
@@ -45,7 +48,7 @@ protected:
   using ManageNodes = nav2_msgs::srv::ManageNodes;
 
   // A generic method used to call startup, shutdown, etc.
-  void callService(uint8_t command);
+  bool callService(uint8_t command);
 
   // The node to use for the service call
   rclcpp::Node::SharedPtr node_;

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -70,6 +70,8 @@ LifecycleManager::LifecycleManager()
   transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] = 
     std::string("Shutting down ");;
 
+  createLifecycleServiceClients();
+
   if (autostart_) {
     startup();
   }
@@ -160,19 +162,6 @@ LifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool reverse_o
   return true;
 }
 
-bool
-LifecycleManager::bringupNode(const std::string & node_name)
-{
-  message(std::string("Configuring and activating ") + node_name);
-  if (!changeStateForNode(node_name, Transition::TRANSITION_CONFIGURE) ||
-    !changeStateForNode(node_name, Transition::TRANSITION_ACTIVATE))
-  {
-    return false;
-  }
-
-  return true;
-}
-
 void
 LifecycleManager::shutdownAllNodes()
 {
@@ -186,7 +175,6 @@ bool
 LifecycleManager::startup()
 {
   message("Starting the system bringup...");
-  createLifecycleServiceClients();
   if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE) ||
     !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))
   {
@@ -211,7 +199,6 @@ bool
 LifecycleManager::reset()
 {
   message("Resetting the system...");
-  createLifecycleServiceClients();
   if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE) ||
     !changeStateForAllNodes(Transition::TRANSITION_CLEANUP))
   {
@@ -226,7 +213,6 @@ bool
 LifecycleManager::pause()
 {
   message("Pausing the system...");
-  createLifecycleServiceClients();
   if (!changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE))
   {
     RCLCPP_ERROR(get_logger(), "Failed to pause nodes: aborting pause");
@@ -240,7 +226,6 @@ bool
 LifecycleManager::resume()
 {
   message("Resuming the system...");
-  createLifecycleServiceClients();
   if (!changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))
   {
     RCLCPP_ERROR(get_logger(), "Failed to resume nodes: aborting resume");

nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

@@ -41,16 +41,34 @@ LifecycleManagerClient::LifecycleManagerClient()
     "initialpose", rclcpp::SystemDefaultsQoS());
 }
 
-void
+bool
 LifecycleManagerClient::startup()
 {
-  callService(Command::STARTUP);
+  return callService(Command::STARTUP);
 }
 
-void
+bool
 LifecycleManagerClient::shutdown()
 {
-  callService(Command::SHUTDOWN);
+  return callService(Command::SHUTDOWN);
+}
+
+bool
+LifecycleManagerClient::pause()
+{
+  return callService(Command::PAUSE);
+}
+
+bool
+LifecycleManagerClient::resume()
+{
+  return callService(Command::RESUME);
+}
+
+bool
+LifecycleManagerClient::reset()
+{
+  return callService(Command::RESET);
 }
 
 void
@@ -122,7 +140,7 @@ LifecycleManagerClient::navigate_to_pose(double x, double y, double theta)
   return wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED;
 }
 
-void
+bool
 LifecycleManagerClient::callService(uint8_t command)
 {
   std::string service_name = "lifecycle_manager/manage_nodes";
@@ -135,7 +153,7 @@ LifecycleManagerClient::callService(uint8_t command)
   while (!manager_client_->wait_for_service(std::chrono::seconds(1))) {
     if (!rclcpp::ok()) {
       RCLCPP_ERROR(node_->get_logger(), "Client interrupted while waiting for service to appear");
-      return;
+      return false;
     }
     RCLCPP_INFO(node_->get_logger(), "Waiting for service to appear...");
   }
@@ -144,6 +162,7 @@ LifecycleManagerClient::callService(uint8_t command)
     service_name);
   auto future_result = manager_client_->async_send_request(request);
   rclcpp::spin_until_future_complete(node_, future_result);
+  return future_result.get()->success;
 }
 
 }  // namespace nav2_lifecycle_manager