Merge pull request #22 from ros2/request-header

Pass request header to callbacks

rclcpp/include/rclcpp/node.hpp

@@ -105,17 +105,23 @@ class Node
  template<typename ServiceT>
  typename rclcpp::client::Client<ServiceT>::SharedPtr
  create_client(
- std::string service_name,
+ const std::string & service_name,
  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
  /* Create and return a Service. */
  template<typename ServiceT>
  typename rclcpp::service::Service<ServiceT>::SharedPtr
  create_service(
- std::string service_name,
- std::function<void(
- const std::shared_ptr<typename ServiceT::Request> &,
- std::shared_ptr<typename ServiceT::Response> &)> callback,
+ const std::string & service_name,
+ typename rclcpp::service::Service<ServiceT>::CallbackType callback,
+ rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+
+ /* Create and return a Service. */
+ template<typename ServiceT>
+ typename rclcpp::service::Service<ServiceT>::SharedPtr
+ create_service(
+ const std::string & service_name,
+ typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
 private:
@@ -138,6 +144,10 @@ class Node
  size_t number_of_services_;
  size_t number_of_clients_;
 
+ void register_service(
+ const std::string & service_name,
+ std::shared_ptr<rclcpp::service::ServiceBase> serv_base_ptr,
+ rclcpp::callback_group::CallbackGroup::SharedPtr group);
 };
 
 } /* namespace node */

rclcpp/include/rclcpp/node_impl.hpp

@@ -151,7 +151,7 @@ Node::create_wall_timer(
 template<typename ServiceT>
 typename client::Client<ServiceT>::SharedPtr
 Node::create_client(
- std::string service_name,
+ const std::string & service_name,
  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
  using rosidl_generator_cpp::get_service_type_support_handle;
@@ -185,9 +185,8 @@ Node::create_client(
 template<typename ServiceT>
 typename service::Service<ServiceT>::SharedPtr
 Node::create_service(
- std::string service_name,
- std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
- std::shared_ptr<typename ServiceT::Response> &)> callback,
+ const std::string & service_name,
+ typename rclcpp::service::Service<ServiceT>::CallbackType callback,
  rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
  using rosidl_generator_cpp::get_service_type_support_handle;
@@ -197,14 +196,44 @@ Node::create_service(
  rmw_service_t * service_handle = rmw_create_service(
  this->node_handle_, service_type_support_handle, service_name.c_str());
 
- using namespace rclcpp::service;
-
- auto serv = Service<ServiceT>::make_shared(
+ auto serv = service::Service<ServiceT>::make_shared(
  service_handle,
  service_name,
  callback);
- auto serv_base_ptr = std::dynamic_pointer_cast<ServiceBase>(serv);
+ auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
+ register_service(service_name, serv_base_ptr, group);
+ return serv;
+}
+
+template<typename ServiceT>
+typename service::Service<ServiceT>::SharedPtr
+Node::create_service(
+ const std::string & service_name,
+ typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
+ rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+ using rosidl_generator_cpp::get_service_type_support_handle;
+ auto service_type_support_handle =
+ get_service_type_support_handle<ServiceT>();
 
+ rmw_service_t * service_handle = rmw_create_service(
+ this->node_handle_, service_type_support_handle, service_name.c_str());
+
+ auto serv = service::Service<ServiceT>::make_shared(
+ service_handle,
+ service_name,
+ callback_with_header);
+ auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
+ register_service(service_name, serv_base_ptr, group);
+ return serv;
+}
+
+void
+Node::register_service(
+ const std::string & service_name,
+ std::shared_ptr<rclcpp::service::ServiceBase> serv_base_ptr,
+ rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
  if (group) {
  if (!group_in_node(group)) {
  // TODO: use custom exception
@@ -215,8 +244,6 @@ Node::create_service(
  default_callback_group_->add_service(serv_base_ptr);
  }
  number_of_services_++;
-
- return serv;
 }
 
 #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

rclcpp/include/rclcpp/service.hpp

@@ -86,15 +86,30 @@ class Service : public ServiceBase
 {
 public:
  typedef std::function<
- void (const std::shared_ptr<typename ServiceT::Request> &,
- std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
+ void (
+ const std::shared_ptr<typename ServiceT::Request> &,
+ std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
+
+ typedef std::function<
+ void (
+ const std::shared_ptr<rmw_request_id_t> &,
+ const std::shared_ptr<typename ServiceT::Request> &,
+ std::shared_ptr<typename ServiceT::Response> &)> CallbackWithHeaderType;
  RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
 
  Service(
  rmw_service_t * service_handle,
  const std::string & service_name,
  CallbackType callback)
- : ServiceBase(service_handle, service_name), callback_(callback)
+ : ServiceBase(service_handle, service_name), callback_(callback), callback_with_header_(nullptr)
+ {}
+
+ Service(
+ rmw_service_t * service_handle,
+ const std::string & service_name,
+ CallbackWithHeaderType callback_with_header)
+ : ServiceBase(service_handle, service_name), callback_(nullptr),
+ callback_with_header_(callback_with_header)
  {}
 
  std::shared_ptr<void> create_request()
@@ -114,7 +129,11 @@ class Service : public ServiceBase
  auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
  auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
  auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
- callback_(typed_request, response);
+ if (callback_with_header_ != nullptr) {
+ callback_with_header_(typed_request_header, typed_request, response);
+ } else {
+ callback_(typed_request, response);
+ }
  send_response(typed_request_header, response);
  }
 
@@ -129,6 +148,7 @@ class Service : public ServiceBase
  RCLCPP_DISABLE_COPY(Service);
 
  CallbackType callback_;
+ CallbackWithHeaderType callback_with_header_;
 };
 
 } /* namespace service */