From 5aa6b51bd504afc8551d86845fb514c831c079c6 Mon Sep 17 00:00:00 2001
From: bpwilcox <bpwilcox@eng.ucsd.edu>
Date: Thu, 29 Aug 2019 16:25:16 -0700
Subject: [PATCH] add pause, resume, reset to manager client
---
.../lifecycle_manager_client.hpp | 9 ++++--
.../src/lifecycle_manager.cpp | 19 ++----------
.../src/lifecycle_manager_client.cpp | 31 +++++++++++++++----
3 files changed, 33 insertions(+), 26 deletions(-)
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 08812873..9865339b 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
@@ -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_;
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index 5f1a7673..5b331e26 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/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");
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
index d35321bc..4bdc4aac 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp
+++ b/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
--
GitLab