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(callback), rmw_qos_profile_parameter_events); } + RCLCPP_PUBLIC + bool + service_is_ready() const; + + template + bool + wait_for_service( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_service_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + +protected: + RCLCPP_PUBLIC + bool + wait_for_service_nanoseconds(std::chrono::nanoseconds timeout); + private: const rclcpp::node::Node::SharedPtr node_; rclcpp::client::Client::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> 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::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)