From 4b290d66e8708a6e0453786d334ec4b674677c91 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 8 Jan 2015 09:53:24 -0800
Subject: [PATCH] Refactored to split request messages

---
 rclcpp/include/rclcpp/executor.hpp  |  6 ++++--
 rclcpp/include/rclcpp/node.hpp      |  5 ++++-
 rclcpp/include/rclcpp/node_impl.hpp |  3 ++-
 rclcpp/include/rclcpp/service.hpp   | 28 +++++++++++++++++++---------
 4 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index 8de14cf..df38ffc 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -171,12 +171,14 @@ protected:
     rclcpp::service::ServiceBase::SharedPtr &service)
   {
     std::shared_ptr<void> request = service->create_request();
+    std::shared_ptr<void> request_header = service->create_request_header();
     bool taken = ros_middleware_interface::take_request(
       service->service_handle_,
-      request.get());
+      request.get(),
+      request_header.get());
     if (taken)
     {
-      service->handle_request(request);
+      service->handle_request(request, request_header);
     }
     else
     {
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 63ff7e5..53bc208 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -29,6 +29,8 @@
 #include <rclcpp/subscription.hpp>
 #include <rclcpp/timer.hpp>
 
+#include <userland_msgs/RequestId.h>
+
 namespace rclcpp
 {
 
@@ -101,7 +103,8 @@ public:
   typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service(
     std::string service_name,
-    std::function<void(const std::shared_ptr<typename ServiceT::RequestWithHeader> &,
+    std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
+                       const std::shared_ptr<userland_msgs::RequestId> &,
                        std::shared_ptr<typename ServiceT::Response>&)> callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
 
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 7fb6841..5f9c3b2 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -187,7 +187,8 @@ template <typename ServiceT>
 typename service::Service<ServiceT>::SharedPtr
 Node::create_service(
   std::string service_name,
-  std::function<void(const std::shared_ptr<typename ServiceT::RequestWithHeader> &,
+  std::function<void(const std::shared_ptr<typename ServiceT::Request> &,
+                     const std::shared_ptr<userland_msgs::RequestId> &,
                      std::shared_ptr<typename ServiceT::Response>&)> callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index bd5b6ef..2e85fe3 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -23,6 +23,8 @@
 
 #include <rclcpp/macros.hpp>
 
+#include <userland_msgs/RequestId.h>
+
 namespace rclcpp
 {
 
@@ -55,7 +57,8 @@ public:
   }
 
   virtual std::shared_ptr<void> create_request() = 0;
-  virtual void handle_request(std::shared_ptr<void> &request) = 0;
+  virtual std::shared_ptr<void> create_request_header() = 0;
+  virtual void handle_request(std::shared_ptr<void> &request, std::shared_ptr<void> &req_id) = 0;
 
 private:
   RCLCPP_DISABLE_COPY(ServiceBase);
@@ -70,7 +73,8 @@ class Service : public ServiceBase
 {
 public:
   typedef std::function<
-    void(const std::shared_ptr<typename ServiceT::RequestWithHeader> &,
+    void(const std::shared_ptr<typename ServiceT::Request> &,
+         const std::shared_ptr<userland_msgs::RequestId> &,
          std::shared_ptr<typename ServiceT::Response>&)> CallbackType;
   RCLCPP_MAKE_SHARED_DEFINITIONS(Service);
 
@@ -83,22 +87,28 @@ public:
 
   std::shared_ptr<void> create_request()
   {
-    return std::shared_ptr<void>(new typename ServiceT::RequestWithHeader());
+    return std::shared_ptr<void>(new typename ServiceT::Request());
   }
 
-  void handle_request(std::shared_ptr<void> &request)
+  std::shared_ptr<void> create_request_header()
   {
-    auto typed_request = std::static_pointer_cast<typename ServiceT::RequestWithHeader>(request);
+    return std::shared_ptr<void>(new userland_msgs::RequestId());
+  }
+
+  void handle_request(std::shared_ptr<void> &request, std::shared_ptr<void> &req_id)
+  {
+    auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
+    auto typed_req_id = std::static_pointer_cast<userland_msgs::RequestId>(req_id);
     auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
-    callback_(typed_request, response);
-    send_response(typed_request, response);
+    callback_(typed_request, typed_req_id, response);
+    send_response(typed_req_id, response);
   }
 
   void send_response(
-    std::shared_ptr<typename ServiceT::RequestWithHeader> &request,
+    std::shared_ptr<userland_msgs::RequestId> &req_id,
     std::shared_ptr<typename ServiceT::Response> &response)
   {
-    ::ros_middleware_interface::send_response(get_service_handle(), request.get(), response.get());
+    ::ros_middleware_interface::send_response(get_service_handle(), req_id.get(), response.get());
   }
 
 private: