From 01a6741ea73dc296d04968168c86eb1e2bf56cc5 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 15 Oct 2015 13:17:10 -0700
Subject: [PATCH] Removed create_service_internal and delegate SFINAE to
 AnyServiceCallback.

---
 .../include/rclcpp/any_service_callback.hpp   | 106 ++++++++++++++++++
 rclcpp/include/rclcpp/node.hpp                |  50 ---------
 rclcpp/include/rclcpp/node_impl.hpp           |   7 +-
 rclcpp/include/rclcpp/service.hpp             |  27 ++---
 4 files changed, 119 insertions(+), 71 deletions(-)
 create mode 100644 rclcpp/include/rclcpp/any_service_callback.hpp

diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp
new file mode 100644
index 0000000..5a99f64
--- /dev/null
+++ b/rclcpp/include/rclcpp/any_service_callback.hpp
@@ -0,0 +1,106 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
+#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
+
+#include <rclcpp/function_traits.hpp>
+
+#include <functional>
+#include <memory>
+#include <type_traits>
+
+#include <rmw/types.h>
+
+namespace rclcpp
+{
+
+namespace any_service_callback
+{
+
+template<typename ServiceT>
+class AnyServiceCallback
+{
+private:
+  using SharedPtrCallback = std::function<void(
+        const std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>
+      )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<void(
+        const std::shared_ptr<rmw_request_id_t>,
+        const std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>
+      )>;
+
+  SharedPtrCallback shared_ptr_callback_;
+  SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
+
+public:
+  AnyServiceCallback()
+  : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
+  {}
+
+  AnyServiceCallback(const AnyServiceCallback &) = default;
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::check_argument_types<
+        CallbackT,
+        std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    shared_ptr_callback_ = callback;
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::check_argument_types<
+        CallbackT,
+        std::shared_ptr<rmw_request_id_t>,
+        std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    shared_ptr_with_request_header_callback_ = callback;
+  }
+
+  void dispatch(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<typename ServiceT::Request> request,
+    std::shared_ptr<typename ServiceT::Response> response)
+  {
+    if (shared_ptr_callback_ != nullptr) {
+      (void)request_header;
+      shared_ptr_callback_(request, response);
+    } else if (shared_ptr_with_request_header_callback_ != nullptr) {
+      shared_ptr_with_request_header_callback_(request_header, request, response);
+    } else {
+      throw std::runtime_error("unexpected request without any callback set");
+    }
+  }
+};
+
+} /* namespace any_service_callback */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index f48ef80..bdac682 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -29,7 +29,6 @@
 #include <rclcpp/callback_group.hpp>
 #include <rclcpp/client.hpp>
 #include <rclcpp/context.hpp>
-#include <rclcpp/function_traits.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/message_memory_strategy.hpp>
 #include <rclcpp/parameter.hpp>
@@ -262,55 +261,6 @@ private:
   std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
 
   publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
-
-  template<
-    typename ServiceT,
-    typename FunctorT,
-    typename std::enable_if<
-      rclcpp::check_argument_types<
-        FunctorT,
-        typename std::shared_ptr<typename ServiceT::Request>,
-        typename std::shared_ptr<typename ServiceT::Response>
-      >::value
-    >::type * = nullptr
-  >
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
-  create_service_internal(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
-    const std::string & service_name,
-    FunctorT callback)
-  {
-    typename rclcpp::service::Service<ServiceT>::CallbackType callback_without_header =
-      callback;
-    return service::Service<ServiceT>::make_shared(
-      node_handle, service_handle, service_name, callback_without_header);
-  }
-
-  template<
-    typename ServiceT,
-    typename FunctorT,
-    typename std::enable_if<
-      rclcpp::check_argument_types<
-        FunctorT,
-        std::shared_ptr<rmw_request_id_t>,
-        typename std::shared_ptr<typename ServiceT::Request>,
-        typename std::shared_ptr<typename ServiceT::Response>
-      >::value
-    >::type * = nullptr
-  >
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
-  create_service_internal(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
-    const std::string & service_name,
-    FunctorT callback)
-  {
-    typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
-      callback;
-    return service::Service<ServiceT>::make_shared(
-      node_handle, service_handle, service_name, callback_with_header);
-  }
 };
 
 const rosidl_message_type_support_t * Node::ipm_ts_ =
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index b86cf8e..f66afad 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -414,6 +414,9 @@ Node::create_service(
   auto service_type_support_handle =
     get_service_type_support_handle<ServiceT>();
 
+  rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
+  any_service_callback.set(callback);
+
   rmw_service_t * service_handle = rmw_create_service(
     node_handle_.get(), service_type_support_handle, service_name.c_str());
   if (!service_handle) {
@@ -424,8 +427,8 @@ Node::create_service(
     // *INDENT-ON*
   }
 
-  auto serv = create_service_internal<ServiceT>(
-    node_handle_, service_handle, service_name, callback);
+  auto serv = service::Service<ServiceT>::make_shared(
+    node_handle_, service_handle, service_name, any_service_callback);
   auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
   if (group) {
     if (!group_in_node(group)) {
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index 1ef76f2..8848a85 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -25,7 +25,7 @@
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
-
+#include <rclcpp/any_service_callback.hpp>
 
 namespace rclcpp
 {
@@ -91,6 +91,8 @@ private:
 
 };
 
+using namespace any_service_callback;
+
 template<typename ServiceT>
 class Service : public ServiceBase
 {
@@ -111,19 +113,11 @@ public:
     std::shared_ptr<rmw_node_t> node_handle,
     rmw_service_t * service_handle,
     const std::string & service_name,
-    CallbackType callback)
-  : ServiceBase(node_handle, service_handle, service_name), callback_(callback),
-    callback_with_header_(nullptr)
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback)
   {}
 
-  Service(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
-    const std::string & service_name,
-    CallbackWithHeaderType callback_with_header)
-  : ServiceBase(node_handle, service_handle, service_name), callback_(nullptr),
-    callback_with_header_(callback_with_header)
-  {}
+  Service() = delete;
 
   std::shared_ptr<void> create_request()
   {
@@ -142,11 +136,7 @@ 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);
-    if (callback_with_header_ != nullptr) {
-      callback_with_header_(typed_request_header, typed_request, response);
-    } else {
-      callback_(typed_request, response);
-    }
+    any_callback_.dispatch(typed_request_header, typed_request, response);
     send_response(typed_request_header, response);
   }
 
@@ -166,8 +156,7 @@ public:
 private:
   RCLCPP_DISABLE_COPY(Service);
 
-  CallbackType callback_;
-  CallbackWithHeaderType callback_with_header_;
+  AnyServiceCallback<ServiceT> any_callback_;
 };
 
 } /* namespace service */