From e95d3f249d3da20b47574be5bdd8d7e46bcdaa5b Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Wed, 18 Nov 2015 11:43:00 -0800
Subject: [PATCH] Added async_send_request_return_request to return the
 originating request

---
 rclcpp/include/rclcpp/client.hpp          | 55 ++++++++++++++++++++---
 rclcpp/include/rclcpp/function_traits.hpp | 12 ++++-
 2 files changed, 58 insertions(+), 9 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index 839c8f8..589719f 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -23,6 +23,7 @@
 #include <tuple>
 #include <utility>
 
+#include "rclcpp/function_traits.hpp"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/utilities.hpp"
 #include "rclcpp/visibility_control.hpp"
@@ -74,11 +75,20 @@ template<typename ServiceT>
 class Client : public ClientBase
 {
 public:
-  using Promise = std::promise<typename ServiceT::Response::SharedPtr>;
+  using SharedRequest = typename ServiceT::Request::SharedPtr;
+  using SharedResponse = typename ServiceT::Response::SharedPtr;
+
+  using Promise = std::promise<SharedResponse>;
+  using PromiseWithRequest = std::promise<std::pair<SharedRequest, SharedResponse>>;
+
   using SharedPromise = std::shared_ptr<Promise>;
-  using SharedFuture = std::shared_future<typename ServiceT::Response::SharedPtr>;
+  using SharedPromiseWithRequest = std::shared_ptr<PromiseWithRequest>;
+
+  using SharedFuture = std::shared_future<SharedResponse>;
+  using SharedFutureWithRequest = std::shared_future<std::pair<SharedRequest, SharedResponse>>;
 
   using CallbackType = std::function<void(SharedFuture)>;
+  using CallbackWithRequestType = std::function<void(SharedFutureWithRequest)>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Client);
 
@@ -117,15 +127,21 @@ public:
     callback(future);
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request)
+  SharedFuture async_send_request(SharedRequest request)
   {
     return async_send_request(request, [](SharedFuture) {});
   }
 
-  SharedFuture async_send_request(
-    typename ServiceT::Request::SharedPtr request,
-    CallbackType && cb)
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        CallbackType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFuture async_send_request(SharedRequest request, CallbackT && cb)
   {
     int64_t sequence_number;
     if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
@@ -142,6 +158,31 @@ public:
     return f;
   }
 
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        CallbackWithRequestType
+      >::value
+    >::type * = nullptr
+  >
+  SharedFutureWithRequest async_send_request(SharedRequest request, CallbackT && cb)
+  {
+    SharedPromiseWithRequest promise = std::make_shared<PromiseWithRequest>();
+    SharedFutureWithRequest future_with_request(promise->get_future());
+
+    auto wrapping_cb = [future_with_request, promise, request, &cb](SharedFuture future) {
+        auto response = future.get();
+        promise->set_value(std::make_pair(request, response));
+        cb(future_with_request);
+      };
+
+    async_send_request(request, wrapping_cb);
+
+    return future_with_request;
+  }
+
 private:
   RCLCPP_DISABLE_COPY(Client);
 
diff --git a/rclcpp/include/rclcpp/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp
index d491202..5c1ce9a 100644
--- a/rclcpp/include/rclcpp/function_traits.hpp
+++ b/rclcpp/include/rclcpp/function_traits.hpp
@@ -67,13 +67,21 @@ struct function_traits<ReturnTypeT(Args ...)>
 
 // Function pointers
 template<typename ReturnTypeT, typename ... Args>
-struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
+struct function_traits<ReturnTypeT (*)(Args ...)>: function_traits<ReturnTypeT(Args ...)>
 {};
 
 // Lambdas
 template<typename ClassT, typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
-  : public function_traits<ReturnTypeT(ClassT &, Args ...)>
+  : function_traits<ReturnTypeT(ClassT &, Args ...)>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &>: function_traits<FunctionT>
+{};
+
+template<typename FunctionT>
+struct function_traits<FunctionT &&>: function_traits<FunctionT>
 {};
 
 /* NOTE(esteve):