rclcpp  master
C++ ROS Client Library API
service.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__SERVICE_HPP_
16 #define RCLCPP__SERVICE_HPP_
17 
18 #include <functional>
19 #include <iostream>
20 #include <memory>
21 #include <sstream>
22 #include <string>
23 
24 #include "rcl/error_handling.h"
25 #include "rcl/service.h"
26 
28 #include "rclcpp/exceptions.hpp"
29 #include "rclcpp/macros.hpp"
33 #include "rclcpp/logging.hpp"
34 #include "rmw/error_handling.h"
35 #include "rmw/rmw.h"
36 
37 namespace rclcpp
38 {
39 
41 {
42 public:
44 
46  explicit ServiceBase(
47  std::shared_ptr<rcl_node_t> node_handle);
48 
50  virtual ~ServiceBase();
51 
53  const char *
55 
59 
62  get_service_handle() const;
63 
66  virtual void handle_request(
67  std::shared_ptr<rmw_request_id_t> request_header,
68  std::shared_ptr<void> request) = 0;
69 
70 protected:
71  RCLCPP_DISABLE_COPY(ServiceBase)
72 
74  rcl_node_t *
76 
78  const rcl_node_t *
79  get_rcl_node_handle() const;
80 
82 
84  bool owns_rcl_handle_ = true;
85 };
86 
87 template<typename ServiceT>
88 class Service : public ServiceBase
89 {
90 public:
92  void (
95 
97  void (
102 
104  std::shared_ptr<rcl_node_t> node_handle,
105  const std::string & service_name,
106  AnyServiceCallback<ServiceT> any_callback,
107  rcl_service_options_t & service_options)
108  : ServiceBase(node_handle), any_callback_(any_callback)
109  {
110  using rosidl_typesupport_cpp::get_service_type_support_handle;
111  auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
112 
113  std::weak_ptr<rcl_node_t> weak_node_handle(node_handle_);
114  // rcl does the static memory allocation here
116  new rcl_service_t, [weak_node_handle](rcl_service_t * service)
117  {
118  auto handle = weak_node_handle.lock();
119  if (handle) {
120  if (rcl_service_fini(service, handle.get()) != RCL_RET_OK) {
121  RCLCPP_ERROR(
122  rclcpp::get_node_logger(handle.get()).get_child("rclcpp"),
123  "Error in destruction of rcl service handle: %s",
124  rcl_get_error_string().str);
125  rcl_reset_error();
126  }
127  } else {
128  RCLCPP_ERROR(
129  rclcpp::get_logger("rclcpp"),
130  "Error in destruction of rcl service handle: "
131  "the Node Handle was destructed too early. You will leak memory");
132  }
133  delete service;
134  });
136 
139  node_handle.get(),
140  service_type_support_handle,
141  service_name.c_str(),
142  &service_options);
143  if (ret != RCL_RET_OK) {
144  if (ret == RCL_RET_SERVICE_NAME_INVALID) {
145  auto rcl_node_handle = get_rcl_node_handle();
146  // this will throw on any validation problem
147  rcl_reset_error();
149  service_name,
150  rcl_node_get_name(rcl_node_handle),
151  rcl_node_get_namespace(rcl_node_handle),
152  true);
153  }
154 
155  rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
156  }
157  }
158 
160  std::shared_ptr<rcl_node_t> node_handle,
161  std::shared_ptr<rcl_service_t> service_handle,
162  AnyServiceCallback<ServiceT> any_callback)
163  : ServiceBase(node_handle),
164  any_callback_(any_callback)
165  {
166  // check if service handle was initialized
167  if (!rcl_service_is_valid(service_handle.get())) {
168  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
169  throw std::runtime_error(
170  std::string("rcl_service_t in constructor argument must be initialized beforehand."));
171  // *INDENT-ON*
172  }
173 
174  service_handle_ = service_handle;
175  }
176 
178  std::shared_ptr<rcl_node_t> node_handle,
179  rcl_service_t * service_handle,
180  AnyServiceCallback<ServiceT> any_callback)
181  : ServiceBase(node_handle),
182  any_callback_(any_callback)
183  {
184  // check if service handle was initialized
185  if (!rcl_service_is_valid(service_handle)) {
186  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
187  throw std::runtime_error(
188  std::string("rcl_service_t in constructor argument must be initialized beforehand."));
189  // *INDENT-ON*
190  }
191 
192  // In this case, rcl owns the service handle memory
194  service_handle_->impl = service_handle->impl;
195  }
196 
197  Service() = delete;
198 
199  virtual ~Service()
200  {
201  }
202 
204  {
205  return std::shared_ptr<void>(new typename ServiceT::Request());
206  }
207 
209  {
210  // TODO(wjwwood): This should probably use rmw_request_id's allocator.
211  // (since it is a C type)
213  }
214 
216  std::shared_ptr<rmw_request_id_t> request_header,
217  std::shared_ptr<void> request)
218  {
219  auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
220  auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
221  any_callback_.dispatch(request_header, typed_request, response);
222  send_response(request_header, response);
223  }
224 
228  {
229  rcl_ret_t status = rcl_send_response(get_service_handle().get(), req_id.get(), response.get());
230 
231  if (status != RCL_RET_OK) {
232  rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
233  }
234  }
235 
236 private:
238 
239  AnyServiceCallback<ServiceT> any_callback_;
240 };
241 
242 } // namespace rclcpp
243 
244 #endif // RCLCPP__SERVICE_HPP_
bool rcl_service_is_valid(const rcl_service_t *service)
Service(std::shared_ptr< rcl_node_t > node_handle, std::shared_ptr< rcl_service_t > service_handle, AnyServiceCallback< ServiceT > any_callback)
Definition: service.hpp:159
#define rcl_get_error_string
void send_response(std::shared_ptr< rmw_request_id_t > req_id, std::shared_ptr< typename ServiceT::Response > response)
Definition: service.hpp:225
rcl_ret_t rcl_service_init(rcl_service_t *service, const rcl_node_t *node, const rosidl_service_type_support_t *type_support, const char *service_name, const rcl_service_options_t *options)
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
#define rcl_reset_error
rmw_ret_t rcl_ret_t
Logger get_logger(const std::string &name)
Return a named logger.
struct rcl_service_impl_t * impl
This header provides the get_node_topics_interface() template function.
Definition: allocator_common.hpp:24
std::shared_ptr< rcl_service_t > get_service_handle()
virtual ~ServiceBase()
bool owns_rcl_handle_
Definition: service.hpp:84
Definition: service.hpp:88
virtual void handle_request(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > request)=0
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.
ServiceBase(std::shared_ptr< rcl_node_t > node_handle)
#define RCL_RET_OK
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_send_response(const rcl_service_t *service, rmw_request_id_t *response_header, void *ros_response)
#define RCLCPP_SMART_PTR_DEFINITIONS(...)
Definition: macros.hpp:36
#define RCLCPP_ERROR(logger,...)
Definition: logging.hpp:488
T static_pointer_cast(T... args)
const char * get_service_name()
std::shared_ptr< rcl_node_t > node_handle_
Definition: service.hpp:81
rcl_ret_t rcl_service_fini(rcl_service_t *service, rcl_node_t *node)
std::shared_ptr< void > create_request()
Definition: service.hpp:203
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
T get(T... args)
virtual ~Service()
Definition: service.hpp:199
Logger get_node_logger(const rcl_node_t *node)
Return a named logger using an rcl_node_t.
#define RCLCPP_PUBLIC
Definition: visibility_control.hpp:50
virtual std::shared_ptr< rmw_request_id_t > create_request_header()=0
#define RCL_RET_SERVICE_NAME_INVALID
Definition: service.hpp:40
const char * rcl_node_get_name(const rcl_node_t *node)
const char * rcl_node_get_namespace(const rcl_node_t *node)
Service(std::shared_ptr< rcl_node_t > node_handle, rcl_service_t *service_handle, AnyServiceCallback< ServiceT > any_callback)
Definition: service.hpp:177
rcl_service_t rcl_get_zero_initialized_service(void)
rcl_node_t * get_rcl_node_handle()
void handle_request(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > request)
Definition: service.hpp:215
virtual std::shared_ptr< void > create_request()=0
std::shared_ptr< rmw_request_id_t > create_request_header()
Definition: service.hpp:208
std::shared_ptr< rcl_service_t > service_handle_
Definition: service.hpp:83
Definition: any_service_callback.hpp:31