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 51c771323e..06363db603 100644 --- 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 index 076e848902..80e9305e9e 100644 --- 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 index 52cbeff310..0347a25505 100644 --- 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 index 632eb1aa8e..123c69b58e 100644 --- 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 index 96114f98a5..c2c0cb75f0 100644 --- 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 index 8f37cbf819..bb84f3e231 100644 --- 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 ---