mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
service_client.impl.hpp
1#ifndef MRS_LIB_SERVICE_CLIENT_IMPL_HPP_
2#define MRS_LIB_SERVICE_CLIENT_IMPL_HPP_
3
4#include "mrs_lib/service_client.hpp"
5
6#include <expected>
7#include <future>
8#include <memory>
9#include <string>
10#include <string_view>
11#include <utility>
12
13#include <rclcpp/client.hpp>
14#include <rclcpp/create_client.hpp>
15
16#include "mrs_lib/coro/cancellation.hpp"
17#include "mrs_lib/coro/event.hpp"
18#include "mrs_lib/logger.hpp"
20
21namespace mrs_lib
22{
23
25 template <class ServiceType>
26 class ServiceClient<ServiceType>::Impl
27 {
28 public:
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))
36 {
37 logger_.info("Created client of service '{}' -> '{}'", get_unresolved_service_name(), get_service_name());
38 }
39
40 ~Impl() = default;
41 Impl(const Impl&) = delete;
42 Impl& operator=(const Impl&) = delete;
43 Impl(Impl&&) = delete;
44 Impl& operator=(Impl&&) = delete;
45
46 Task<std::expected<std::shared_ptr<Response>, std::string>> call(const std::shared_ptr<Request>& request)
47 {
48 using Response = ServiceType::Response;
49 using Client = rclcpp::Client<ServiceType>;
50 using SharedFutureAndRequestId = Client::SharedFutureAndRequestId;
51 using StopTokenBehavior = coro::internal::LowLevelEventAwaitable::StopTokenBehavior;
52
53 struct StateData
54 {
55 std::mutex mutex{};
56 bool cancelled = false;
57
58 std::optional<SharedFutureAndRequestId> future_and_id{};
59 };
60
61 if (!service_client_->service_is_ready())
62 {
63 co_return std::unexpected("Service not ready.");
64 }
65
66 auto [event, awaitable] = coro::make_event();
67
68 std::shared_ptr<StateData> state_data = std::make_shared<StateData>();
69
70 auto register_waker = [&event, client = service_client_, request, state_data]() {
71 std::lock_guard lock(state_data->mutex);
72 if (state_data->cancelled)
73 {
74 event.try_cancel();
75 return;
76 }
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(); });
80 };
81
82 auto low_level_awaitable = coro::internal::get_low_level_event_awaitable(std::move(awaitable));
83
84 {
85 // If cancellation is requested via stop token, this callback sets
86 // cancelled flag on the state and removes a the pending request.
87 // The flag is set to stop sending the request in case the stop token
88 // is triggered before the service is called.
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())
94 {
95 return;
96 }
97 client->remove_pending_request(state_data->future_and_id.value());
98 });
99
100 co_await std::move(low_level_awaitable).get_awaitable(StopTokenBehavior::ignore, register_waker);
101 }
102
103 {
104 std::lock_guard lock(state_data->mutex);
105 if (!state_data->future_and_id.has_value())
106 {
107 co_return std::unexpected("No future stored after resume.");
108 }
109
110 auto future = state_data->future_and_id.value().future;
111 if (future.wait_for(std::chrono::nanoseconds(0)) == std::future_status::timeout)
112 {
113 co_return std::unexpected("Future not ready after resume.");
114 }
115
116 co_return std::shared_ptr<Response>(future.get());
117 }
118 }
119
120 std::expected<std::future<std::shared_ptr<Response>>, std::string> call_async(const std::shared_ptr<Request>& request)
121 {
122 if (!service_client_->service_is_ready())
123 {
124 return std::unexpected("Service not ready.");
125 }
126
127 auto future = service_client_->async_send_request(request).future;
128
129 if (!future.valid())
130 {
131 return std::unexpected("Future not valid.");
132 }
133
134 return future;
135 }
136
137 [[nodiscard]] std::string get_service_name() const
138 {
139 return service_client_->get_service_name();
140 }
141
142 [[nodiscard]] std::string get_unresolved_service_name() const
143 {
144 return unresolved_service_name_;
145 }
146
147 template <typename RepT = int64_t, typename RatioT = std::milli>
148 [[nodiscard]] bool wait_for_service(std::chrono::duration<RepT, RatioT> timeout)
149 {
150 return service_client_->wait_for_service(timeout);
151 }
152
153 [[nodiscard]] bool is_service_ready() const
154 {
155 return service_client_->service_is_ready();
156 }
157
159 {
160 return service_client_->prune_pending_requests();
161 }
162
163 private:
164 ServiceClientNodeInterfaces node_interfaces_;
165 mrs_lib::Logger logger_;
166
167 std::string unresolved_service_name_;
168
169 std::shared_ptr<rclcpp::Client<ServiceType>> service_client_;
170 };
171
172
173 template <class ServiceType>
174 ServiceClient<ServiceType>::ServiceClient(const ServiceClientOptions& options, std::string_view service_name)
175 : impl_(std::in_place_type_t<Impl>{}, options, service_name)
176 {
177 }
178
179 template <class ServiceType>
180 auto ServiceClient<ServiceType>::call(const std::shared_ptr<Request>& request) -> Task<std::expected<std::shared_ptr<Response>, std::string>>
181 {
182 co_return co_await impl_->call(request);
183 }
184
185 template <class ServiceType>
186 auto ServiceClient<ServiceType>::call_async(const std::shared_ptr<Request>& request) -> std::expected<std::future<std::shared_ptr<Response>>, std::string>
187 {
188 return impl_->call_async(request);
189 }
190
191 template <class ServiceType>
193 {
194 return impl_->get_service_name();
195 }
196
197 template <class ServiceType>
199 {
200 return impl_->get_unresolved_service_name();
201 }
202
203 template <class ServiceType>
204 template <typename RepT, typename RatioT>
205 bool ServiceClient<ServiceType>::wait_for_service(std::chrono::duration<RepT, RatioT> timeout)
206 {
207 return impl_->wait_for_service(timeout);
208 }
209
210 template <class ServiceType>
212 {
213 return impl_->is_service_ready();
214 }
215
216 template <class ServiceType>
218 {
219 return impl_->prune_pending_requests();
220 }
221
222} // namespace mrs_lib
223
224
225#endif // MRS_LIB_SERVICE_CLIENT_IMPL_HPP_
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