Open option to use parent node in lifecycle Manger Client (#2328)

* open option to use parent node

* add constructor and clean

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_;
 };

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