From ac757ddbf74e3c73f9eb9262b22bb34df30566c9 Mon Sep 17 00:00:00 2001 From: Brice Date: Thu, 29 Apr 2021 11:51:10 +0200 Subject: [PATCH 1/2] open option to use parent node --- .../lifecycle_manager_client.hpp | 10 +++-- .../src/lifecycle_manager_client.cpp | 37 ++++++++++--------- 2 files changed, 27 insertions(+), 20 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 af410407400..f053f1f7639 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 @@ -26,6 +26,7 @@ #include "std_srvs/srv/empty.hpp" #include "nav2_msgs/srv/manage_lifecycle_nodes.hpp" #include "std_srvs/srv/trigger.hpp" +#include "nav2_util/service_client.hpp" namespace nav2_lifecycle_manager { @@ -45,7 +46,10 @@ class LifecycleManagerClient /** * @brief A constructor for LifeCycleMangerClient */ - explicit LifecycleManagerClient(const std::string & name, const std::string & ns = ""); + explicit LifecycleManagerClient( + const std::string & name, + const std::string & ns = "", + std::shared_ptr parent_node = nullptr); // Client-side interface to the Nav2 lifecycle manager /** @@ -110,8 +114,8 @@ class LifecycleManagerClient // The node to use for the service call rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr manager_client_; - rclcpp::Client::SharedPtr is_active_client_; + std::shared_ptr> manager_client_; + std::shared_ptr> is_active_client_; std::string manage_service_name_; std::string active_service_name_; }; diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 5bda93576f7..38e4825919b 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -28,17 +28,23 @@ using nav2_util::geometry_utils::orientationAroundZAxis; LifecycleManagerClient::LifecycleManagerClient( const std::string & name, - const std::string & ns) + const std::string & ns, std::shared_ptr parent_node) { manage_service_name_ = name + std::string("/manage_nodes"); active_service_name_ = name + std::string("/is_active"); // Create the node to use for all of the service clients - node_ = std::make_shared(name + "_service_client", ns); + if (parent_node == nullptr) { + node_ = std::make_shared(name + "_service_client", ns); + } else { + node_ = parent_node; + } // Create the service clients - manager_client_ = node_->create_client(manage_service_name_); - is_active_client_ = node_->create_client(active_service_name_); + manager_client_ = std::make_shared>( + manage_service_name_, node_); + is_active_client_ = std::make_shared>( + active_service_name_, node_); } bool @@ -75,6 +81,7 @@ SystemStatus LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) { auto request = std::make_shared(); + auto response = std::make_shared(); RCLCPP_DEBUG( node_->get_logger(), "Waiting for the %s service...", @@ -87,15 +94,14 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) RCLCPP_DEBUG( node_->get_logger(), "Sending %s request", active_service_name_.c_str()); - auto future_result = is_active_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { + try { + response = is_active_client_->invoke(request, timeout); + } catch (std::runtime_error &) { return SystemStatus::TIMEOUT; } - if (future_result.get()->success) { + if (response.get()->success) { return SystemStatus::ACTIVE; } else { return SystemStatus::INACTIVE; @@ -112,7 +118,7 @@ LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseco node_->get_logger(), "Waiting for the %s service...", manage_service_name_.c_str()); - while (!manager_client_->wait_for_service(std::chrono::seconds(1))) { + while (!manager_client_->wait_for_service(timeout)) { if (!rclcpp::ok()) { RCLCPP_ERROR(node_->get_logger(), "Client interrupted while waiting for service to appear"); return false; @@ -123,15 +129,12 @@ LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseco RCLCPP_DEBUG( node_->get_logger(), "Sending %s request", manage_service_name_.c_str()); - auto future_result = manager_client_->async_send_request(request); - - if (rclcpp::spin_until_future_complete(node_, future_result, timeout) != - rclcpp::FutureReturnCode::SUCCESS) - { + try { + auto future_result = manager_client_->invoke(request, timeout); + return future_result.get()->success; + } catch (std::runtime_error &) { return false; } - - return future_result.get()->success; } } // namespace nav2_lifecycle_manager From 9ddf72a5e132d5353a5410220c3e9b4d0b25c171 Mon Sep 17 00:00:00 2001 From: Brice Date: Fri, 30 Apr 2021 12:28:00 +0200 Subject: [PATCH 2/2] add constructor and clean --- .../lifecycle_manager_client.hpp | 14 +++++++-- .../src/lifecycle_manager_client.cpp | 31 +++++++++++++------ 2 files changed, 34 insertions(+), 11 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 f053f1f7639..00e38ee82cd 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 @@ -45,11 +45,21 @@ class LifecycleManagerClient public: /** * @brief A constructor for LifeCycleMangerClient + * @param name Managed node name + * @param ns Service node namespace */ explicit LifecycleManagerClient( const std::string & name, - const std::string & ns = "", - std::shared_ptr parent_node = nullptr); + const std::string & ns = ""); + + /** + * @brief A constructor for LifeCycleMangerClient + * @param name Managed node name + * @param parent_node Node that execute the service calls + */ + explicit LifecycleManagerClient( + const std::string & name, + std::shared_ptr parent_node); // Client-side interface to the Nav2 lifecycle manager /** diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 38e4825919b..ae75963a537 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -28,17 +28,30 @@ using nav2_util::geometry_utils::orientationAroundZAxis; LifecycleManagerClient::LifecycleManagerClient( const std::string & name, - const std::string & ns, std::shared_ptr parent_node) + const std::string & ns) { manage_service_name_ = name + std::string("/manage_nodes"); active_service_name_ = name + std::string("/is_active"); // Create the node to use for all of the service clients - if (parent_node == nullptr) { - node_ = std::make_shared(name + "_service_client", ns); - } else { - node_ = parent_node; - } + node_ = std::make_shared(name + "_service_client", ns); + + // Create the service clients + manager_client_ = std::make_shared>( + manage_service_name_, node_); + is_active_client_ = std::make_shared>( + active_service_name_, node_); +} + +LifecycleManagerClient::LifecycleManagerClient( + const std::string & name, + std::shared_ptr parent_node) +{ + manage_service_name_ = name + std::string("/manage_nodes"); + active_service_name_ = name + std::string("/is_active"); + + // Use parent node for service call and logging + node_ = parent_node; // Create the service clients manager_client_ = std::make_shared>( @@ -87,7 +100,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) node_->get_logger(), "Waiting for the %s service...", active_service_name_.c_str()); - if (!is_active_client_->wait_for_service(timeout)) { + if (!is_active_client_->wait_for_service(std::chrono::seconds(1))) { return SystemStatus::TIMEOUT; } @@ -101,7 +114,7 @@ LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) return SystemStatus::TIMEOUT; } - if (response.get()->success) { + if (response->success) { return SystemStatus::ACTIVE; } else { return SystemStatus::INACTIVE; @@ -131,7 +144,7 @@ LifecycleManagerClient::callService(uint8_t command, const std::chrono::nanoseco manage_service_name_.c_str()); try { auto future_result = manager_client_->invoke(request, timeout); - return future_result.get()->success; + return future_result->success; } catch (std::runtime_error &) { return false; }