1#ifndef MRS_LIB_SERVICE_CLIENT_IMPL_HPP_
2#define MRS_LIB_SERVICE_CLIENT_IMPL_HPP_
4#include "mrs_lib/service_client.hpp"
13#include <rclcpp/client.hpp>
14#include <rclcpp/create_client.hpp>
16#include "mrs_lib/coro/cancellation.hpp"
17#include "mrs_lib/coro/event.hpp"
18#include "mrs_lib/logger.hpp"
25 template <
class ServiceType>
29 Impl(
const ServiceClientOptions& options, std::string_view service_name)
30 : node_interfaces_(options.node_interfaces),
31 logger_(node_interfaces_),
32 unresolved_service_name_(service_name),
33 service_client_(rclcpp::create_client<ServiceType>(node_interfaces_.get_node_base_interface(), node_interfaces_.get_node_graph_interface(),
34 node_interfaces_.get_node_services_interface(), std::string(service_name), options.qos,
35 options.callback_group))
41 Impl(
const Impl&) =
delete;
42 Impl& operator=(
const Impl&) =
delete;
43 Impl(Impl&&) =
delete;
44 Impl& operator=(Impl&&) =
delete;
46 Task<std::expected<std::shared_ptr<Response>, std::string>>
call(
const std::shared_ptr<Request>& request)
48 using Response = ServiceType::Response;
49 using Client = rclcpp::Client<ServiceType>;
50 using SharedFutureAndRequestId = Client::SharedFutureAndRequestId;
51 using StopTokenBehavior = coro::internal::LowLevelEventAwaitable::StopTokenBehavior;
56 bool cancelled =
false;
58 std::optional<SharedFutureAndRequestId> future_and_id{};
61 if (!service_client_->service_is_ready())
63 co_return std::unexpected(
"Service not ready.");
66 auto [event, awaitable] = coro::make_event();
68 std::shared_ptr<StateData> state_data = std::make_shared<StateData>();
70 auto register_waker = [&event, client = service_client_, request, state_data]() {
71 std::lock_guard lock(state_data->mutex);
72 if (state_data->cancelled)
77 auto shared_event = std::make_shared<coro::Event>(std::move(event));
78 state_data->future_and_id =
79 client->async_send_request(request, [shared_event](std::shared_future<std::shared_ptr<Response>>) { shared_event->try_trigger(); });
82 auto low_level_awaitable = coro::internal::get_low_level_event_awaitable(std::move(awaitable));
89 std::stop_callback remove_request_if_canceled(
co_await coro::get_task_stop_token(), [state_data, client_weak = std::weak_ptr(service_client_)]() {
90 std::lock_guard lock(state_data->mutex);
91 state_data->cancelled =
true;
92 std::shared_ptr<Client> client = client_weak.lock();
93 if (client ==
nullptr || !state_data->future_and_id.has_value())
97 client->remove_pending_request(state_data->future_and_id.value());
100 co_await std::move(low_level_awaitable).get_awaitable(StopTokenBehavior::ignore, register_waker);
104 std::lock_guard lock(state_data->mutex);
105 if (!state_data->future_and_id.has_value())
107 co_return std::unexpected(
"No future stored after resume.");
110 auto future = state_data->future_and_id.value().future;
111 if (future.wait_for(std::chrono::nanoseconds(0)) == std::future_status::timeout)
113 co_return std::unexpected(
"Future not ready after resume.");
116 co_return std::shared_ptr<Response>(future.get());
120 std::expected<std::future<std::shared_ptr<Response>>, std::string>
call_async(
const std::shared_ptr<Request>& request)
122 if (!service_client_->service_is_ready())
124 return std::unexpected(
"Service not ready.");
127 auto future = service_client_->async_send_request(request).future;
131 return std::unexpected(
"Future not valid.");
139 return service_client_->get_service_name();
144 return unresolved_service_name_;
147 template <
typename RepT =
int64_t,
typename RatioT = std::milli>
148 [[nodiscard]]
bool wait_for_service(std::chrono::duration<RepT, RatioT> timeout)
150 return service_client_->wait_for_service(timeout);
155 return service_client_->service_is_ready();
160 return service_client_->prune_pending_requests();
165 mrs_lib::Logger logger_;
167 std::string unresolved_service_name_;
169 std::shared_ptr<rclcpp::Client<ServiceType>> service_client_;
173 template <
class ServiceType>
175 : impl_(std::in_place_type_t<Impl>{}, options, service_name)
179 template <
class ServiceType>
182 co_return co_await impl_->call(request);
185 template <
class ServiceType>
188 return impl_->call_async(request);
191 template <
class ServiceType>
194 return impl_->get_service_name();
197 template <
class ServiceType>
200 return impl_->get_unresolved_service_name();
203 template <
class ServiceType>
204 template <
typename RepT,
typename RatioT>
207 return impl_->wait_for_service(timeout);
210 template <
class ServiceType>
213 return impl_->is_service_ready();
216 template <
class ServiceType>
219 return impl_->prune_pending_requests();
rclcpp::Client wrapper with coroutines support.
Definition service_client.hpp:58
size_t prune_pending_requests()
Remove all pending requests.
Definition service_client.impl.hpp:217
bool wait_for_service(std::chrono::duration< RepT, RatioT > timeout)
Wait for the service to become available.
Definition service_client.impl.hpp:205
std::string get_unresolved_service_name() const
Returns the unresolved name of the service handled by this service client.
Definition service_client.impl.hpp:198
std::string get_service_name() const
Returns the resolved (full) name of the service handled by this service client.
Definition service_client.impl.hpp:192
ServiceClient(const ServiceClientOptions &options, std::string_view service_name)
Construct the client with the specified options.
Definition service_client.impl.hpp:174
std::expected< std::future< std::shared_ptr< Response > >, std::string > call_async(const std::shared_ptr< Request > &request)
Async call of the service.
Definition service_client.impl.hpp:186
bool is_service_ready() const
Check if the service is available.
Definition service_client.impl.hpp:211
Task< std::expected< std::shared_ptr< Response >, std::string > > call(const std::shared_ptr< Request > &request)
Coroutine call of the service.
Definition service_client.impl.hpp:180
Task type for creating coroutines.
Definition task.hpp:230
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
rclcpp::node_interfaces::NodeInterfaces< rclcpp::node_interfaces::NodeServicesInterface, rclcpp::node_interfaces::NodeGraphInterface, rclcpp::node_interfaces::NodeBaseInterface, rclcpp::node_interfaces::NodeLoggingInterface, rclcpp::node_interfaces::NodeClockInterface > ServiceClientNodeInterfaces
ROS node interfaces required by the ServiceClient.
Definition service_client.hpp:28
Options for configuring the ServiceClient.
Definition service_client.hpp:37