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 "rcutils/logging_macros.h"
40 
41 #include "rmw/error_handling.h"
42 #include "rmw/rmw.h"
43 
44 namespace rclcpp
45 {
46 
47 namespace node_interfaces
48 {
49 class NodeBaseInterface;
50 } // namespace node_interfaces
51 
53 {
54 public:
56 
58  ClientBase(
60  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph);
61 
63  virtual ~ClientBase();
64 
66  const char *
67  get_service_name() const;
68 
72 
75  get_client_handle() const;
76 
78  bool
79  service_is_ready() const;
80 
81  template<typename RatioT = std::milli>
82  bool
85  {
87  std::chrono::duration_cast<std::chrono::nanoseconds>(timeout)
88  );
89  }
90 
93  virtual void handle_response(
94  std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<void> response) = 0;
95 
96 protected:
98 
100  bool
102 
104  rcl_node_t *
106 
108  const rcl_node_t *
109  get_rcl_node_handle() const;
110 
111  rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_;
113 
115 };
116 
117 template<typename ServiceT>
118 class Client : public ClientBase
119 {
120 public:
121  using SharedRequest = typename ServiceT::Request::SharedPtr;
122  using SharedResponse = typename ServiceT::Response::SharedPtr;
123 
126 
129 
132 
135 
137 
139  rclcpp::node_interfaces::NodeBaseInterface * node_base,
140  rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph,
141  const std::string & service_name,
142  rcl_client_options_t & client_options)
143  : ClientBase(node_base, node_graph)
144  {
145  using rosidl_typesupport_cpp::get_service_type_support_handle;
146  auto service_type_support_handle =
147  get_service_type_support_handle<ServiceT>();
149  this->get_client_handle().get(),
150  this->get_rcl_node_handle(),
151  service_type_support_handle,
152  service_name.c_str(),
153  &client_options);
154  if (ret != RCL_RET_OK) {
155  if (ret == RCL_RET_SERVICE_NAME_INVALID) {
156  auto rcl_node_handle = this->get_rcl_node_handle();
157  // this will throw on any validation problem
158  rcl_reset_error();
160  service_name,
161  rcl_node_get_name(rcl_node_handle),
162  rcl_node_get_namespace(rcl_node_handle),
163  true);
164  }
165  rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client");
166  }
167  }
168 
169  virtual ~Client()
170  {
171  }
172 
175  {
176  return std::shared_ptr<void>(new typename ServiceT::Response());
177  }
178 
181  {
182  // TODO(wjwwood): This should probably use rmw_request_id's allocator.
183  // (since it is a C type)
185  }
186 
187  void
189  std::shared_ptr<rmw_request_id_t> request_header,
190  std::shared_ptr<void> response)
191  {
192  std::unique_lock<std::mutex> lock(pending_requests_mutex_);
193  auto typed_response = std::static_pointer_cast<typename ServiceT::Response>(response);
194  int64_t sequence_number = request_header->sequence_number;
195  // TODO(esteve) this should throw instead since it is not expected to happen in the first place
196  if (this->pending_requests_.count(sequence_number) == 0) {
198  "rclcpp",
199  "Received invalid sequence number. Ignoring...");
200  return;
201  }
202  auto tuple = this->pending_requests_[sequence_number];
203  auto call_promise = std::get<0>(tuple);
204  auto callback = std::get<1>(tuple);
205  auto future = std::get<2>(tuple);
206  this->pending_requests_.erase(sequence_number);
207  // Unlock here to allow the service to be called recursively from one of its callbacks.
208  lock.unlock();
209 
210  call_promise->set_value(typed_response);
211  callback(future);
212  }
213 
216  {
217  return async_send_request(request, [](SharedFuture) {});
218  }
219 
220  template<
221  typename CallbackT,
222  typename std::enable_if<
224  CallbackT,
226  >::value
227  >::type * = nullptr
228  >
230  async_send_request(SharedRequest request, CallbackT && cb)
231  {
232  std::lock_guard<std::mutex> lock(pending_requests_mutex_);
233  int64_t sequence_number;
234  rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number);
235  if (RCL_RET_OK != ret) {
236  rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request");
237  }
238 
239  SharedPromise call_promise = std::make_shared<Promise>();
240  SharedFuture f(call_promise->get_future());
241  pending_requests_[sequence_number] =
242  std::make_tuple(call_promise, std::forward<CallbackType>(cb), f);
243  return f;
244  }
245 
246  template<
247  typename CallbackT,
248  typename std::enable_if<
250  CallbackT,
252  >::value
253  >::type * = nullptr
254  >
256  async_send_request(SharedRequest request, CallbackT && cb)
257  {
258  SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
259  SharedFutureWithRequest future_with_request(promise->get_future());
260 
261  auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
262  auto response = future.get();
263  promise->set_value(std::make_pair(request, response));
264  cb(future_with_request);
265  };
266 
267  async_send_request(request, wrapping_cb);
268 
269  return future_with_request;
270  }
271 
272 private:
274 
276  std::mutex pending_requests_mutex_;
277 };
278 
279 } // namespace rclcpp
280 
281 #endif // RCLCPP__CLIENT_HPP_
SharedFuture async_send_request(SharedRequest request)
Definition: client.hpp:215
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
typename rcl_interfaces::srv::GetParameters ::Request::SharedPtr SharedRequest
Definition: client.hpp:121
#define rcl_reset_error
void handle_response(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > response)
Definition: client.hpp:188
rmw_ret_t rcl_ret_t
std::shared_ptr< rcl_client_t > client_handle_
Definition: client.hpp:114
virtual ~ClientBase()
Definition: client.hpp:118
virtual ~Client()
Definition: client.hpp:169
Definition: allocator_common.hpp:24
std::shared_ptr< rcl_client_t > get_client_handle()
T make_tuple(T... args)
const char * get_service_name() const
virtual std::shared_ptr< rmw_request_id_t > create_request_header()=0
rcl_node_t * get_rcl_node_handle()
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.
bool wait_for_service_nanoseconds(std::chrono::nanoseconds timeout)
rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_
Definition: client.hpp:111
#define RCL_RET_OK
SharedFuture async_send_request(SharedRequest request, CallbackT &&cb)
Definition: client.hpp:230
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.
std::shared_ptr< rmw_request_id_t > create_request_header()
Definition: client.hpp:180
std::shared_ptr< void > create_response()
Definition: client.hpp:174
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
std::shared_ptr< rcl_node_t > node_handle_
Definition: client.hpp:112
typename rcl_interfaces::srv::GetParameters ::Response::SharedPtr SharedResponse
Definition: client.hpp:122
std::shared_future< std::pair< SharedRequest, SharedResponse > > SharedFutureWithRequest
Definition: client.hpp:131
ClientBase(rclcpp::node_interfaces::NodeBaseInterface *node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph)
Definition: function_traits.hpp:143
T make_pair(T... args)
T static_pointer_cast(T... args)
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
std::function< void(SharedFutureWithRequest)> CallbackWithRequestType
Definition: client.hpp:134
#define RCUTILS_LOG_ERROR_NAMED(name,...)
T get(T... args)
std::function< void(SharedFuture)> CallbackType
Definition: client.hpp:133
#define RCLCPP_PUBLIC
Definition: visibility_control.hpp:50
Pure virtual interface class for the NodeBase part of the Node API.
Definition: node_base_interface.hpp:36
#define RCL_RET_SERVICE_NAME_INVALID
const char * rcl_node_get_name(const rcl_node_t *node)
virtual void handle_response(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > response)=0
const char * rcl_node_get_namespace(const rcl_node_t *node)
std::shared_future< SharedResponse > SharedFuture
Definition: client.hpp:130
SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT &&cb)
Definition: client.hpp:256
bool service_is_ready() const
Definition: client.hpp:52
bool wait_for_service(std::chrono::duration< int64_t, RatioT > timeout=std::chrono::duration< int64_t, RatioT >(-1))
Definition: client.hpp:83
virtual std::shared_ptr< void > create_response()=0