From 40b09b5b14c6df9fe9eaf6a6a907c65c5b70747e Mon Sep 17 00:00:00 2001
From: Dirk Thomas <dirk-thomas@users.noreply.github.com>
Date: Mon, 10 Jul 2017 10:22:08 -0700
Subject: [PATCH] added wait method to AsyncParametersClient (#342)

* added wait and ready methods to AsyncParametersClient

* style only

* style only

* remove RCLCPP_PUBLIC from template methods

* style
---
 rclcpp/include/rclcpp/parameter_client.hpp | 19 +++++++++++
 rclcpp/src/rclcpp/parameter_client.cpp     | 37 ++++++++++++++++++++++
 2 files changed, 56 insertions(+)

diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp
index 180d4aa..6b7e640 100644
--- a/rclcpp/include/rclcpp/parameter_client.hpp
+++ b/rclcpp/include/rclcpp/parameter_client.hpp
@@ -101,6 +101,25 @@ public:
       "parameter_events", std::forward<CallbackT>(callback), rmw_qos_profile_parameter_events);
   }
 
+  RCLCPP_PUBLIC
+  bool
+  service_is_ready() const;
+
+  template<typename RatioT = std::milli>
+  bool
+  wait_for_service(
+    std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
+  {
+    return wait_for_service_nanoseconds(
+      std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
+    );
+  }
+
+protected:
+  RCLCPP_PUBLIC
+  bool
+  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
+
 private:
   const rclcpp::node::Node::SharedPtr node_;
   rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;
diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp
index 03523ea..63a7289 100644
--- a/rclcpp/src/rclcpp/parameter_client.cpp
+++ b/rclcpp/src/rclcpp/parameter_client.cpp
@@ -229,6 +229,43 @@ AsyncParametersClient::list_parameters(
   return future_result;
 }
 
+bool
+AsyncParametersClient::service_is_ready() const
+{
+  return
+    get_parameters_client_->service_is_ready() &&
+    get_parameter_types_client_->service_is_ready() &&
+    set_parameters_client_->service_is_ready() &&
+    list_parameters_client_->service_is_ready() &&
+    describe_parameters_client_->service_is_ready();
+}
+
+bool
+AsyncParametersClient::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
+{
+  const std::vector<std::shared_ptr<rclcpp::client::ClientBase>> clients = {
+    get_parameters_client_,
+    get_parameter_types_client_,
+    set_parameters_client_,
+    list_parameters_client_,
+    describe_parameters_client_
+  };
+  for (auto & client : clients) {
+    auto stamp = std::chrono::steady_clock::now();
+    if (!client->wait_for_service(timeout)) {
+      return false;
+    }
+    if (timeout > std::chrono::nanoseconds::zero()) {
+      timeout -= std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::steady_clock::now() - stamp);
+      if (timeout < std::chrono::nanoseconds::zero()) {
+        timeout = std::chrono::nanoseconds::zero();
+      }
+    }
+  }
+  return true;
+}
+
 SyncParametersClient::SyncParametersClient(
   rclcpp::node::Node::SharedPtr node,
   const rmw_qos_profile_t & qos_profile)