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 "rmw/error_handling.h"
34 #include "rmw/rmw.h"
35 
36 namespace rclcpp
37 {
38 namespace service
39 {
40 
42 {
43 public:
45 
48  std::shared_ptr<rcl_node_t> node_handle,
49  const std::string & service_name);
50 
52  explicit ServiceBase(
53  std::shared_ptr<rcl_node_t> node_handle);
54 
56  virtual ~ServiceBase();
57 
59  std::string
61 
63  const rcl_service_t *
65 
66  virtual std::shared_ptr<void> create_request() = 0;
67  virtual std::shared_ptr<rmw_request_id_t> create_request_header() = 0;
68  virtual void handle_request(
69  std::shared_ptr<rmw_request_id_t> request_header,
70  std::shared_ptr<void> request) = 0;
71 
72 protected:
73  RCLCPP_DISABLE_COPY(ServiceBase)
74 
76  rcl_node_t *
77  get_rcl_node_handle() const;
78 
79  std::shared_ptr<rcl_node_t> node_handle_;
80 
82  std::string service_name_;
83  bool owns_rcl_handle_ = true;
84 };
85 
87 
88 template<typename ServiceT>
89 class Service : public ServiceBase
90 {
91 public:
92  using CallbackType = std::function<
93  void(
94  const std::shared_ptr<typename ServiceT::Request>,
95  std::shared_ptr<typename ServiceT::Response>)>;
96 
97  using CallbackWithHeaderType = std::function<
98  void(
99  const std::shared_ptr<rmw_request_id_t>,
100  const std::shared_ptr<typename ServiceT::Request>,
101  std::shared_ptr<typename ServiceT::Response>)>;
103 
105  std::shared_ptr<rcl_node_t> node_handle,
106  const std::string & service_name,
107  AnyServiceCallback<ServiceT> any_callback,
108  rcl_service_options_t & service_options)
109  : ServiceBase(node_handle, service_name), any_callback_(any_callback)
110  {
111  using rosidl_typesupport_cpp::get_service_type_support_handle;
112  auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
113 
114  // rcl does the static memory allocation here
116  *service_handle_ = rcl_get_zero_initialized_service();
117 
119  service_handle_,
120  node_handle.get(),
121  service_type_support_handle,
122  service_name.c_str(),
123  &service_options);
124  if (ret != RCL_RET_OK) {
125  if (ret == RCL_RET_SERVICE_NAME_INVALID) {
126  auto rcl_node_handle = get_rcl_node_handle();
127  // this will throw on any validation problem
128  rcl_reset_error();
130  service_name,
131  rcl_node_get_name(rcl_node_handle),
132  rcl_node_get_namespace(rcl_node_handle),
133  true);
134  }
135 
136  rclcpp::exceptions::throw_from_rcl_error(ret, "could not create service");
137  }
138  }
139 
141  std::shared_ptr<rcl_node_t> node_handle,
142  rcl_service_t * service_handle,
143  AnyServiceCallback<ServiceT> any_callback)
144  : ServiceBase(node_handle),
145  any_callback_(any_callback)
146  {
147  // check if service handle was initialized
148  // TODO(karsten1987): Take this verification
149  // directly in rcl_*_t
150  // see: https://github.com/ros2/rcl/issues/81
151  if (!service_handle->impl) {
152  // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
153  throw std::runtime_error(
154  std::string("rcl_service_t in constructor argument must be initialized beforehand."));
155  // *INDENT-ON*
156  }
157  service_handle_ = service_handle;
158  service_name_ = std::string(rcl_service_get_service_name(service_handle));
159  owns_rcl_handle_ = false;
160  }
161 
162  Service() = delete;
163 
164  virtual ~Service()
165  {
166  // check if you have ownership of the handle
167  if (owns_rcl_handle_) {
169  std::stringstream ss;
170  ss << "Error in destruction of rcl service_handle_ handle: " <<
171  rcl_get_error_string_safe() << '\n';
172  (std::cerr << ss.str()).flush();
173  rcl_reset_error();
174  }
175  }
176  }
177 
178  std::shared_ptr<void> create_request()
179  {
180  return std::shared_ptr<void>(new typename ServiceT::Request());
181  }
182 
183  std::shared_ptr<rmw_request_id_t> create_request_header()
184  {
185  // TODO(wjwwood): This should probably use rmw_request_id's allocator.
186  // (since it is a C type)
187  return std::shared_ptr<rmw_request_id_t>(new rmw_request_id_t);
188  }
189 
190  void handle_request(std::shared_ptr<rmw_request_id_t> request_header,
191  std::shared_ptr<void> request)
192  {
193  auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
194  auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
195  any_callback_.dispatch(request_header, typed_request, response);
196  send_response(request_header, response);
197  }
198 
200  std::shared_ptr<rmw_request_id_t> req_id,
201  std::shared_ptr<typename ServiceT::Response> response)
202  {
203  rcl_ret_t status = rcl_send_response(get_service_handle(), req_id.get(), response.get());
204 
205  if (status != RCL_RET_OK) {
206  rclcpp::exceptions::throw_from_rcl_error(status, "failed to send response");
207  }
208  }
209 
210 private:
212 
213  AnyServiceCallback<ServiceT> any_callback_;
214 };
215 
216 } // namespace service
217 } // namespace rclcpp
218 
219 #endif // RCLCPP__SERVICE_HPP_
const char * rcl_service_get_service_name(const rcl_service_t *service)
virtual std::shared_ptr< void > create_request()=0
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)
std::function< void(const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< typename rcl_interfaces::srv::GetParameters ::Request >, std::shared_ptr< typename rcl_interfaces::srv::GetParameters ::Response >)> CallbackWithHeaderType
Definition: service.hpp:101
Definition: service.hpp:89
#define RCLCPP_DISABLE_COPY(...)
Definition: macros.hpp:26
#define rcl_reset_error
rmw_ret_t rcl_ret_t
Service(std::shared_ptr< rcl_node_t > node_handle, rcl_service_t *service_handle, AnyServiceCallback< ServiceT > any_callback)
Definition: service.hpp:140
struct rcl_service_impl_t * impl
Definition: allocator_common.hpp:24
Definition: parameter.hpp:235
bool owns_rcl_handle_
Definition: service.hpp:83
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
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.
Definition: service.hpp:41
std::shared_ptr< rcl_node_t > node_handle_
Definition: service.hpp:79
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
ServiceBase(std::shared_ptr< rcl_node_t > node_handle, const std::string &service_name)
rcl_node_t * get_rcl_node_handle() const
std::shared_ptr< void > create_request()
Definition: service.hpp:178
#define rcl_get_error_string_safe
rcl_ret_t rcl_service_fini(rcl_service_t *service, rcl_node_t *node)
#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...)
Definition: macros.hpp:51
virtual std::shared_ptr< rmw_request_id_t > create_request_header()=0
#define RCLCPP_PUBLIC
Definition: visibility_control.hpp:50
std::shared_ptr< rmw_request_id_t > create_request_header()
Definition: service.hpp:183
const rcl_service_t * get_service_handle()
#define RCL_RET_SERVICE_NAME_INVALID
const char * rcl_node_get_name(const rcl_node_t *node)
virtual ~Service()
Definition: service.hpp:164
const char * rcl_node_get_namespace(const rcl_node_t *node)
void handle_request(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > request)
Definition: service.hpp:190
rcl_service_t rcl_get_zero_initialized_service(void)
void send_response(std::shared_ptr< rmw_request_id_t > req_id, std::shared_ptr< typename ServiceT::Response > response)
Definition: service.hpp:199
rcl_service_t * service_handle_
Definition: service.hpp:81
Definition: any_service_callback.hpp:34
std::function< void(const std::shared_ptr< typename ServiceT::Request >, std::shared_ptr< typename ServiceT::Response >)> CallbackType
Definition: service.hpp:95
virtual void handle_request(std::shared_ptr< rmw_request_id_t > request_header, std::shared_ptr< void > request)=0
std::string service_name_
Definition: service.hpp:82