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 af410407400eabfde84aacf78ed801fe4039ced9..00e38ee82cd5f35907868d6f6b00a88914dc7bca 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 { @@ -44,8 +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 = ""); + explicit LifecycleManagerClient( + const std::string & name, + 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<rclcpp::Node> parent_node); // Client-side interface to the Nav2 lifecycle manager /** @@ -110,8 +124,8 @@ protected: // The node to use for the service call rclcpp::Node::SharedPtr node_; - rclcpp::Client<ManageLifecycleNodes>::SharedPtr manager_client_; - rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr is_active_client_; + std::shared_ptr<nav2_util::ServiceClient<ManageLifecycleNodes>> manager_client_; + std::shared_ptr<nav2_util::ServiceClient<std_srvs::srv::Trigger>> 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 5bda93576f702b3326ae0179117d56a233e3c06a..ae75963a537ab6af7c742e9b7ee21dc7eb52acb9 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -37,8 +37,27 @@ LifecycleManagerClient::LifecycleManagerClient( node_ = std::make_shared<rclcpp::Node>(name + "_service_client", ns); // Create the service clients - manager_client_ = node_->create_client<ManageLifecycleNodes>(manage_service_name_); - is_active_client_ = node_->create_client<std_srvs::srv::Trigger>(active_service_name_); + manager_client_ = std::make_shared<nav2_util::ServiceClient<ManageLifecycleNodes>>( + manage_service_name_, node_); + is_active_client_ = std::make_shared<nav2_util::ServiceClient<std_srvs::srv::Trigger>>( + active_service_name_, node_); +} + +LifecycleManagerClient::LifecycleManagerClient( + const std::string & name, + std::shared_ptr<rclcpp::Node> 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<nav2_util::ServiceClient<ManageLifecycleNodes>>( + manage_service_name_, node_); + is_active_client_ = std::make_shared<nav2_util::ServiceClient<std_srvs::srv::Trigger>>( + active_service_name_, node_); } bool @@ -75,27 +94,27 @@ SystemStatus LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) { auto request = std::make_shared<std_srvs::srv::Trigger::Request>(); + auto response = std::make_shared<std_srvs::srv::Trigger::Response>(); RCLCPP_DEBUG( 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; } 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->success) { return SystemStatus::ACTIVE; } else { return SystemStatus::INACTIVE; @@ -112,7 +131,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 +142,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->success; + } catch (std::runtime_error &) { return false; } - - return future_result.get()->success; } } // namespace nav2_lifecycle_manager