Merge pull request #22 from ros2/request-header

Pass request header to callbacks

rclcpp/include/rclcpp/node.hpp

@@ -105,17 +105,23 @@ public:
   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,
+    const std::string & service_name,
-    std::function<void(
+    typename rclcpp::service::Service<ServiceT>::CallbackType callback,
-      const std::shared_ptr<typename ServiceT::Request> &,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
-      std::shared_ptr<typename ServiceT::Response> &)> callback,
+
+  /* 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 @@ private:
   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,
+  const std::string & service_name,
-  std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
+  typename rclcpp::service::Service<ServiceT>::CallbackType callback,
-  std::shared_ptr<typename ServiceT::Response> &)> 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::Service<ServiceT>::make_shared(
-
-  auto serv = 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> &,
+      void (
-      std::shared_ptr<typename ServiceT::Response> &)> CallbackType;
+        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 @@ public:
     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 @@ private:
   RCLCPP_DISABLE_COPY(Service);
 
   CallbackType callback_;
+  CallbackWithHeaderType callback_with_header_;
 };
 
 } /* namespace service */