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

jonipol · SteveMacenski · commit 06ec9585ec58 · 2024-08-23T23:04:57.000Z
Signed-off-by: Joni Pöllänen &lt;joni.pollanen@karelics.fi&gt;
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/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
diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/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
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/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()
 {
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/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)
 {
diff --git a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp b/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());
 }
 
diff --git a/nav2_msgs/srv/ManageLifecycleNodes.srv b/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
 ---

Original file line number	Diff line number	Diff line change
`@@ -100,6 +100,8 @@ TEST(LifecycleClientTest, BasicTest)`
`100`	`100`	`client.is_active(std::chrono::nanoseconds(1000000000)));`
`101`	`101`	`EXPECT_TRUE(client.resume());`
`102`	`102`	`EXPECT_TRUE(client.reset());`
	`103`	`+ EXPECT_TRUE(client.configure());`
	`104`	`+ EXPECT_TRUE(client.cleanup());`
`103`	`105`	`EXPECT_TRUE(client.shutdown());`
`104`	`106`	`}`
`105`	`107`