Add configure and cleanup transitions to lifecycle manager and client (ros-navigation#4371)

Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>

Author: jonipol

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -93,6 +93,16 @@ class LifecycleManager : public rclcpp::Node
    * @return true or false
    */
   bool startup();
+  /**
+   * @brief Configures the managed nodes.
+   * @return true or false
+   */
+  bool configure();
+  /**
+   * @brief Cleanups the managed nodes
+   * @return true or false
+   */
+  bool cleanup();
   /**
    * @brief Deactivate, clean up and shut down all the managed nodes.
    * @return true or false

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -78,6 +78,16 @@ class LifecycleManagerClient
    * @return true or false
    */
   bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Make configure service call
+   * @return true or false
+   */
+  bool configure(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+  /**
+   * @brief Make cleanup service call
+   * @return true or false
+   */
+  bool cleanup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
   /**
    * @brief Check if lifecycle node manager server is active
    * @return ACTIVE or INACTIVE or TIMEOUT

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -125,6 +125,12 @@ LifecycleManager::managerCallback(
     case ManageLifecycleNodes::Request::STARTUP:
       response->success = startup();
       break;
+    case ManageLifecycleNodes::Request::CONFIGURE:
+      response->success = configure();
+      break;
+    case ManageLifecycleNodes::Request::CLEANUP:
+      response->success = cleanup();
+      break;
     case ManageLifecycleNodes::Request::RESET:
       response->success = reset();
       break;
@@ -291,6 +297,34 @@ LifecycleManager::startup()
   return true;
 }
 
+bool
+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;
+    return false;
+  }
+  message("Managed nodes are now configured");
+  managed_nodes_state_ = NodeState::INACTIVE;
+  return true;
+}
+
+bool
+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;
+    return false;
+  }
+  message("Managed nodes have been cleaned up");
+  managed_nodes_state_ = NodeState::UNCONFIGURED;
+  return true;
+}
+
 bool
 LifecycleManager::shutdown()
 {

nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

@@ -73,6 +73,18 @@ LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout)
   return callService(ManageLifecycleNodes::Request::RESET, timeout);
 }
 
+bool
+LifecycleManagerClient::configure(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::CONFIGURE, timeout);
+}
+
+bool
+LifecycleManagerClient::cleanup(const std::chrono::nanoseconds timeout)
+{
+  return callService(ManageLifecycleNodes::Request::CLEANUP, timeout);
+}
+
 SystemStatus
 LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout)
 {

nav2_lifecycle_manager/test/test_lifecycle_manager.cpp

@@ -100,6 +100,8 @@ TEST(LifecycleClientTest, BasicTest)
     client.is_active(std::chrono::nanoseconds(1000000000)));
   EXPECT_TRUE(client.resume());
   EXPECT_TRUE(client.reset());
+  EXPECT_TRUE(client.configure());
+  EXPECT_TRUE(client.cleanup());
   EXPECT_TRUE(client.shutdown());
 }
 

nav2_msgs/srv/ManageLifecycleNodes.srv

@@ -3,6 +3,8 @@ uint8 PAUSE = 1
 uint8 RESUME = 2
 uint8 RESET = 3
 uint8 SHUTDOWN = 4
+uint8 CONFIGURE = 5
+uint8 CLEANUP = 6
 
 uint8 command
 ---