rclcpp  master
C++ ROS Client Library API
client.hpp
Go to the documentation of this file.
1 // Copyright 2014 Open Source Robotics Foundation, Inc.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef RCLCPP__CLIENT_HPP_
16 #define RCLCPP__CLIENT_HPP_
17 
18 #include <future>
19 #include <map>
20 #include <memory>
21 #include <sstream>
22 #include <string>
23 #include <tuple>
24 #include <utility>
25 
26 #include "rcl/client.h"
27 #include "rcl/error_handling.h"
28 #include "rcl/wait.h"
29 
30 #include "rclcpp/exceptions.hpp"
32 #include "rclcpp/macros.hpp"
35 #include "rclcpp/utilities.hpp"
38 
39 #include "rmw/error_handling.h"
40 #include "rmw/rmw.h"
41 
42 namespace rclcpp
43 {
44 
45 namespace node_interfaces
46 {
47 class NodeBaseInterface;
48 } // namespace node_interfaces
49 
50 namespace client
51 {
52 
54 {
55 public:
57 
59  ClientBase(
61  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
62  const std::string & service_name);
63 
65  virtual ~ClientBase();
66 
68  const std::string &
69  get_service_name() const;
70 
72  const rcl_client_t *
73  get_client_handle() const;
74 
76  bool
77  service_is_ready() const;
78 
79  template<typename RatioT = std::milli>
80  bool
82  std::chrono::duration<int64_t, RatioT> timeout = std::chrono::duration<int64_t, RatioT>(-1))
83  {
84  return wait_for_service_nanoseconds(
85  std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
86  );
87  }
88 
89  virtual std::shared_ptr<void> create_response() = 0;
90  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
91  virtual void handle_response(
92  std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
93 
94 protected:
95  RCLCPP_DISABLE_COPY(ClientBase)
96 
98  bool
99  wait_for_service_nanoseconds(std::chrono::nanoseconds timeout);
100 
102  rcl_node_t *
103  get_rcl_node_handle() const;
104 
105  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
106  std::shared_ptr<rcl_node_t> node_handle_;
107 
109  std::string service_name_;
110 };
111 
112 template<typename ServiceT>
113 class Client : public ClientBase
114 {
115 public:
116  using SharedRequest = typename ServiceT::Request::SharedPtr;
117  using SharedResponse = typename ServiceT::Response::SharedPtr;
118 
119  using Promise = std::promise<SharedResponse>;
120  using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;
121 
122  using SharedPromise = std::shared_ptr<Promise>;
123  using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
124 
125  using SharedFuture = std::shared_future<SharedResponse>;
126  using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
127 
128  using CallbackType = std::function<void(SharedFuture)>;
129  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
130 
132 
134  rclcpp::node_interfaces::NodeBaseInterface * node_base,
135  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
136  const std::string & service_name,
137  rcl_client_options_t & client_options)
138  : ClientBase(node_base, node_graph, service_name)
139  {
140  using rosidl_typesupport_cpp::get_service_type_support_handle;
141  auto service_type_support_handle =
142  get_service_type_support_handle<ServiceT>();
144  &client_handle_,
145  this->get_rcl_node_handle(),
146  service_type_support_handle,
147  service_name.c_str(),
148  &client_options);
149  if (ret != RCL_RET_OK) {
150  if (ret == RCL_RET_SERVICE_NAME_INVALID) {
151  auto rcl_node_handle = this->get_rcl_node_handle();
152  // this will throw on any validation problem
153  rcl_reset_error();
155  service_name,
156  rcl_node_get_name(rcl_node_handle),
157  rcl_node_get_namespace(rcl_node_handle),
158  true);
159  }
160  rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
161  }
162  }
163 
164  virtual ~Client()
165  {
166  if (rcl_client_fini(&client_handle_, this->get_rcl_node_handle()) != RCL_RET_OK) {
167  fprintf(stderr,
168  "Error in destruction of rcl client handle: %s\n", rcl_get_error_string_safe());
169  rcl_reset_error();
170  }
171  }
172 
173  std::shared_ptr<void>
175  {
176  return std::shared_ptr<void>(new typename ServiceT::Response());
177  }
178 
179  std::shared_ptr<rmw_request_id_t>
181  {
182  // TODO(wjwwood): This should probably use rmw_request_id's allocator.
183  // (since it is a C type)
184  return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
185  }
186 
187  void
188  handle_response(std::shared_ptr<rmw_request_id_t> request_header,
189  std::shared_ptr<void> response)
190  {
191  std::lock_guard<std::mutex> lock(pending_requests_mutex_);
192  auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
193  int64_t sequence_number = request_header->sequence_number;
194  // TODO(esteve) this should throw instead since it is not expected to happen in the first place
195  if (this->pending_requests_.count(sequence_number) == 0) {
196  fprintf(stderr, "Received invalid sequence number. Ignoring...\n");
197  return;
198  }
199  auto tuple = this->pending_requests_[sequence_number];
200  auto call_promise = std::get<0>(tuple);
201  auto callback = std::get<1>(tuple);
202  auto future = std::get<2>(tuple);
203  this->pending_requests_.erase(sequence_number);
204  call_promise->set_value(typed_response);
205  callback(future);
206  }
207 
210  {
211  return async_send_request(request, [](SharedFuture) {});
212  }
213 
214  template<
215  typename CallbackT,
216  typename std::enable_if<
218  CallbackT,
220  >::value
221  >::type * = nullptr
222  >
224  async_send_request(SharedRequest request, CallbackT && cb)
225  {
226  std::lock_guard<std::mutex> lock(pending_requests_mutex_);
227  int64_t sequence_number;
228  rcl_ret_t ret = rcl_send_request(get_client_handle(), request.get(), &sequence_number);
229  if (RCL_RET_OK != ret) {
230  rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
231  }
232 
233  SharedPromise call_promise = std::make_shared<Promise>();
234  SharedFuture f(call_promise->get_future());
235  pending_requests_[sequence_number] =
236  std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
237  return f;
238  }
239 
240  template<
241  typename CallbackT,
242  typename std::enable_if<
244  CallbackT,
246  >::value
247  >::type * = nullptr
248  >
250  async_send_request(SharedRequest request, CallbackT && cb)
251  {
252  SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
253  SharedFutureWithRequest future_with_request(promise->get_future());
254 
255  auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
256  auto response = future.get();
257  promise->set_value(std::make_pair(request, response));
258  cb(future_with_request);
259  };
260 
261  async_send_request(request, wrapping_cb);
262 
263  return future_with_request;
264  }
265 
266 private:
268 
269  std::map<int64_t, std::tuple<SharedPromise, CallbackType, SharedFuture>> pending_requests_;
270  std::mutex pending_requests_mutex_;
271 };
272 
273 } // namespace client
274 } // namespace rclcpp
275 
276 #endif // RCLCPP__CLIENT_HPP_
std::shared_ptr< rmw_request_id_t > create_request_header()
Definition: client.hpp:180
SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT &&cb)
Definition: client.hpp:250
std::shared_ptr< rcl_node_t > node_handle_
Definition: client.hpp:106
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
#define rcl_reset_error
rmw_ret_t rcl_ret_t
Definition: allocator_common.hpp:24
Definition: parameter.hpp:235
rcl_ret_t rcl_send_request(const rcl_client_t *client, const void *ros_request, int64_t *sequence_number)
std::string expand_topic_or_service_name(const std::string &name, const std::string &node_name, const std::string &namespace_, bool is_service=false)
Expand a topic or service name and throw if it is not valid.
#define RCL_RET_OK
std::shared_ptr< Promise > SharedPromise
Definition: client.hpp:122
virtual ~Client()
Definition: client.hpp:164
std::shared_future< std::pair< SharedRequest, SharedResponse > > SharedFutureWithRequest
Definition: client.hpp:126
std::promise< SharedResponse > Promise
Definition: client.hpp:119
void throw_from_rcl_error(rcl_ret_t ret, const std::string &prefix="", const rcl_error_state_t *error_state=nullptr, void(*reset_error)()=rcl_reset_error)
Throw a C++ std::exception which was created based on an rcl error.
rcl_ret_t rcl_client_init(rcl_client_t *client, const rcl_node_t *node, const rosidl_service_type_support_t *type_support, const char *service_name, const rcl_client_options_t *options)
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
SharedFuture async_send_request(SharedRequest request, CallbackT &&cb)
Definition: client.hpp:224
Definition: function_traits.hpp:141
Pure virtual interface class for the NodeBase part of the Node API.
Definition: node_base_interface.hpp:36
std::promise< std::pair< SharedRequest, SharedResponse > > PromiseWithRequest
Definition: client.hpp:120
typename rcl_interfaces::srv::GetParameters ::Response::SharedPtr SharedResponse
Definition: client.hpp:117
#define rcl_get_error_string_safe
std::string service_name_
Definition: client.hpp:109
typename rcl_interfaces::srv::GetParameters ::Request::SharedPtr SharedRequest
Definition: client.hpp:116
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
std::function< void(SharedFuture)> CallbackType
Definition: client.hpp:128
#define RCLCPP_PUBLIC
Definition: visibility_control.hpp:50
Definition: client.hpp:53
std::shared_ptr< void > create_response()
Definition: client.hpp:174
#define RCL_RET_SERVICE_NAME_INVALID
const char * rcl_node_get_name(const rcl_node_t *node)
SharedFuture async_send_request(SharedRequest request)
Definition: client.hpp:209
const char * rcl_node_get_namespace(const rcl_node_t *node)
std::function< void(SharedFutureWithRequest)> CallbackWithRequestType
Definition: client.hpp:129
void handle_response(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > response)
Definition: client.hpp:188
rcl_client_t rcl_get_zero_initialized_client(void)
rcl_ret_t rcl_client_fini(rcl_client_t *client, rcl_node_t *node)
Definition: client.hpp:113
rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_
Definition: client.hpp:105
std::shared_future< SharedResponse > SharedFuture
Definition: client.hpp:125
bool wait_for_service(std::chrono::duration< int64_t, RatioT > timeout=std::chrono::duration< int64_t, RatioT >(-1))
Definition: client.hpp:81
std::shared_ptr< PromiseWithRequest > SharedPromiseWithRequest
Definition: client.hpp:123