From 2367549a50f72a36d48fb61a3e2338fac0379513 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Wed, 29 Apr 2015 14:29:56 -0700
Subject: [PATCH 1/9] Check callback arity and enable/disable
 compatible/incompatible versions of create_service. Workaround for
 http://www.open-std.org/jtc1/sc22/wg21/docs/lwg-defects.html#2132 in Windows

---
 rclcpp/include/rclcpp/node.hpp      | 38 ++++++++++++++++++++++++-----
 rclcpp/include/rclcpp/node_impl.hpp | 23 ++++++++++++-----
 2 files changed, 49 insertions(+), 12 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 7016098..3f110d2 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -46,6 +46,24 @@ class Executor;
 namespace node
 {
 
+// TODO: add support for functors, std::function, lambdas and object members
+template<typename FunctionT>
+struct function_traits;
+
+template<typename ReturnTypeT, typename ... Args>
+struct function_traits<ReturnTypeT(Args ...)>
+{
+  static constexpr std::size_t arity = sizeof ... (Args);
+};
+
+template<typename ReturnTypeT, typename ... Args>
+struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
+{};
+
+template<typename FunctorT, size_t arity, typename ReturnTypeT>
+struct function_arity : std::enable_if<function_traits<FunctorT>::arity == arity, ReturnTypeT>
+{};
+
 /* ROS Node Interface.
  *
  * This is the single point of entry for creating publishers and subscribers.
@@ -109,19 +127,27 @@ public:
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT>
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  template<typename ServiceT, typename FunctorT>
+  typename function_arity<
+    FunctorT,
+    2,
+    typename rclcpp::service::Service<ServiceT>::SharedPtr
+    >::type
   create_service(
     const std::string & service_name,
-    typename rclcpp::service::Service<ServiceT>::CallbackType callback,
+    FunctorT functor,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT>
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  template<typename ServiceT, typename FunctorT>
+  typename function_arity<
+    FunctorT,
+    3,
+    typename rclcpp::service::Service<ServiceT>::SharedPtr
+    >::type
   create_service(
     const std::string & service_name,
-    typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
+    FunctorT functor,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
 private:
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 363d5e7..0f9a207 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -182,13 +182,18 @@ Node::create_client(
   return cli;
 }
 
-template<typename ServiceT>
-typename service::Service<ServiceT>::SharedPtr
+template<typename ServiceT, typename FunctorT>
+typename function_arity<
+  FunctorT,
+  2,
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  >::type
 Node::create_service(
   const std::string & service_name,
-  typename rclcpp::service::Service<ServiceT>::CallbackType callback,
+  FunctorT functor,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
+  typename rclcpp::service::Service<ServiceT>::CallbackType callback = functor;
   using rosidl_generator_cpp::get_service_type_support_handle;
   auto service_type_support_handle =
     get_service_type_support_handle<ServiceT>();
@@ -205,13 +210,19 @@ Node::create_service(
   return serv;
 }
 
-template<typename ServiceT>
-typename service::Service<ServiceT>::SharedPtr
+template<typename ServiceT, typename FunctorT>
+typename function_arity<
+  FunctorT,
+  3,
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  >::type
 Node::create_service(
   const std::string & service_name,
-  typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
+  FunctorT functor,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
+  typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
+    functor;
   using rosidl_generator_cpp::get_service_type_support_handle;
   auto service_type_support_handle =
     get_service_type_support_handle<ServiceT>();

From db0238c7e2f5013b593b4d26c0368d5ad14f78f4 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Wed, 29 Apr 2015 16:40:00 -0700
Subject: [PATCH 2/9] Formatting

---
 rclcpp/include/rclcpp/node.hpp      | 6 ++----
 rclcpp/include/rclcpp/node_impl.hpp | 6 ++----
 2 files changed, 4 insertions(+), 8 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 3f110d2..0c9947b 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -131,8 +131,7 @@ public:
   typename function_arity<
     FunctorT,
     2,
-    typename rclcpp::service::Service<ServiceT>::SharedPtr
-    >::type
+    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
   create_service(
     const std::string & service_name,
     FunctorT functor,
@@ -143,8 +142,7 @@ public:
   typename function_arity<
     FunctorT,
     3,
-    typename rclcpp::service::Service<ServiceT>::SharedPtr
-    >::type
+    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
   create_service(
     const std::string & service_name,
     FunctorT functor,
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 0f9a207..6fc5b04 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -186,8 +186,7 @@ template<typename ServiceT, typename FunctorT>
 typename function_arity<
   FunctorT,
   2,
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
-  >::type
+  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
 Node::create_service(
   const std::string & service_name,
   FunctorT functor,
@@ -214,8 +213,7 @@ template<typename ServiceT, typename FunctorT>
 typename function_arity<
   FunctorT,
   3,
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
-  >::type
+  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
 Node::create_service(
   const std::string & service_name,
   FunctorT functor,

From ad3f5da3a55bdefd7200e6c65c07079469ef7083 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Wed, 29 Apr 2015 17:40:38 -0700
Subject: [PATCH 3/9] Let the compiler deduce the appropriate Service
 constructor to use

---
 rclcpp/include/rclcpp/node.hpp      | 20 +++------------
 rclcpp/include/rclcpp/node_impl.hpp | 38 +++--------------------------
 2 files changed, 6 insertions(+), 52 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 0c9947b..b58f0b4 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -127,25 +127,11 @@ public:
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT, typename FunctorT>
-  typename function_arity<
-    FunctorT,
-    2,
-    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+  template<typename ServiceT, typename F>
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
-    FunctorT functor,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
-
-  /* Create and return a Service. */
-  template<typename ServiceT, typename FunctorT>
-  typename function_arity<
-    FunctorT,
-    3,
-    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
-  create_service(
-    const std::string & service_name,
-    FunctorT functor,
+    F callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
 private:
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 6fc5b04..795dc0a 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -182,17 +182,13 @@ Node::create_client(
   return cli;
 }
 
-template<typename ServiceT, typename FunctorT>
-typename function_arity<
-  FunctorT,
-  2,
-  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+template<typename ServiceT, typename F>
+typename rclcpp::service::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
-  FunctorT functor,
+  F callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
-  typename rclcpp::service::Service<ServiceT>::CallbackType callback = functor;
   using rosidl_generator_cpp::get_service_type_support_handle;
   auto service_type_support_handle =
     get_service_type_support_handle<ServiceT>();
@@ -209,34 +205,6 @@ Node::create_service(
   return serv;
 }
 
-template<typename ServiceT, typename FunctorT>
-typename function_arity<
-  FunctorT,
-  3,
-  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
-Node::create_service(
-  const std::string & service_name,
-  FunctorT functor,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
-{
-  typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
-    functor;
-  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,

From 1181daf81e637a8352beec7095c0dd20e285becb Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 30 Apr 2015 08:48:42 -0700
Subject: [PATCH 4/9] Delegate service creation to SFINAE'd
 create_service_internal based on the arity of the callback function

---
 rclcpp/include/rclcpp/node.hpp      | 20 +++++++++++
 rclcpp/include/rclcpp/node_impl.hpp | 52 ++++++++++++++++++++---------
 2 files changed, 56 insertions(+), 16 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index b58f0b4..9231066 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -158,6 +158,26 @@ private:
     const std::string & service_name,
     std::shared_ptr<rclcpp::service::ServiceBase> serv_base_ptr,
     rclcpp::callback_group::CallbackGroup::SharedPtr group);
+
+  template<typename ServiceT, typename FunctorT>
+  typename function_arity<
+    FunctorT,
+    2,
+    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+  create_service_internal(
+    rmw_service_t * service_handle,
+    const std::string & service_name,
+    FunctorT callback);
+
+  template<typename ServiceT, typename FunctorT>
+  typename function_arity<
+    FunctorT,
+    3,
+    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+  create_service_internal(
+    rmw_service_t * service_handle,
+    const std::string & service_name,
+    FunctorT callback);
 };
 
 } /* namespace node */
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 795dc0a..1d87b83 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -182,11 +182,11 @@ Node::create_client(
   return cli;
 }
 
-template<typename ServiceT, typename F>
+template<typename ServiceT, typename FunctorT>
 typename rclcpp::service::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
-  F callback,
+  FunctorT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   using rosidl_generator_cpp::get_service_type_support_handle;
@@ -196,21 +196,9 @@ Node::create_service(
   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);
+  auto serv = create_service_internal<ServiceT>(service_handle, service_name,
+      callback);
   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
@@ -221,6 +209,38 @@ Node::register_service(
     default_callback_group_->add_service(serv_base_ptr);
   }
   number_of_services_++;
+  return serv;
+}
+
+template<typename ServiceT, typename FunctorT>
+typename function_arity<
+  FunctorT,
+  2,
+  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+Node::create_service_internal(
+  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(
+    service_handle, service_name, callback_without_header);
+}
+
+template<typename ServiceT, typename FunctorT>
+typename function_arity<
+  FunctorT,
+  3,
+  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+Node::create_service_internal(
+  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(
+    service_handle, service_name, callback_with_header);
 }
 
 #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

From 814f4101db0e3fd20a558fe3da1874b8afb1f45e Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 30 Apr 2015 12:08:59 -0700
Subject: [PATCH 5/9] Check callback argument types

---
 rclcpp/include/rclcpp/node.hpp      | 54 ++++++++++++++++++++++-------
 rclcpp/include/rclcpp/node_impl.hpp | 31 -----------------
 2 files changed, 42 insertions(+), 43 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 9231066..333edb6 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -18,6 +18,7 @@
 #include <list>
 #include <memory>
 #include <string>
+#include <tuple>
 
 #include <rclcpp/callback_group.hpp>
 #include <rclcpp/client.hpp>
@@ -54,16 +55,15 @@ template<typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT(Args ...)>
 {
   static constexpr std::size_t arity = sizeof ... (Args);
+
+  template<std::size_t N>
+  using argument_type = typename std::tuple_element<N, std::tuple<Args ...>>::type;
 };
 
 template<typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
 {};
 
-template<typename FunctorT, size_t arity, typename ReturnTypeT>
-struct function_arity : std::enable_if<function_traits<FunctorT>::arity == arity, ReturnTypeT>
-{};
-
 /* ROS Node Interface.
  *
  * This is the single point of entry for creating publishers and subscribers.
@@ -160,24 +160,54 @@ private:
     rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
   template<typename ServiceT, typename FunctorT>
-  typename function_arity<
-    FunctorT,
-    2,
+  typename std::enable_if<
+    function_traits<FunctorT>::arity == 2 &&
+    std::is_same<
+      typename function_traits<FunctorT>::template argument_type<0>,
+      typename std::shared_ptr<typename ServiceT::Request>
+      >::value &&
+    std::is_same<
+      typename function_traits<FunctorT>::template argument_type<1>,
+      typename std::shared_ptr<typename ServiceT::Response>
+      >::value,
     typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
   create_service_internal(
     rmw_service_t * service_handle,
     const std::string & service_name,
-    FunctorT callback);
+    FunctorT callback)
+  {
+    typename rclcpp::service::Service<ServiceT>::CallbackType callback_without_header =
+      callback;
+    return service::Service<ServiceT>::make_shared(
+      service_handle, service_name, callback_without_header);
+  }
 
   template<typename ServiceT, typename FunctorT>
-  typename function_arity<
-    FunctorT,
-    3,
+  typename std::enable_if<
+    function_traits<FunctorT>::arity == 3 &&
+    std::is_same<
+      typename function_traits<FunctorT>::template argument_type<0>,
+      std::shared_ptr<rmw_request_id_t>
+      >::value &&
+    std::is_same<
+      typename function_traits<FunctorT>::template argument_type<1>,
+      typename std::shared_ptr<typename ServiceT::Request>
+      >::value &&
+    std::is_same<
+      typename function_traits<FunctorT>::template argument_type<2>,
+      typename std::shared_ptr<typename ServiceT::Response>
+      >::value,
     typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
   create_service_internal(
     rmw_service_t * service_handle,
     const std::string & service_name,
-    FunctorT callback);
+    FunctorT callback)
+  {
+    typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
+      callback;
+    return service::Service<ServiceT>::make_shared(
+      service_handle, service_name, callback_with_header);
+  }
 };
 
 } /* namespace node */
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 1d87b83..19af801 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -212,35 +212,4 @@ Node::create_service(
   return serv;
 }
 
-template<typename ServiceT, typename FunctorT>
-typename function_arity<
-  FunctorT,
-  2,
-  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
-Node::create_service_internal(
-  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(
-    service_handle, service_name, callback_without_header);
-}
-
-template<typename ServiceT, typename FunctorT>
-typename function_arity<
-  FunctorT,
-  3,
-  typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
-Node::create_service_internal(
-  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(
-    service_handle, service_name, callback_with_header);
-}
-
 #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

From faba591f3d95cb41d47bf6a79221e3518876b040 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 30 Apr 2015 13:17:03 -0700
Subject: [PATCH 6/9] Moved std::enable_if to template parameters

---
 rclcpp/include/rclcpp/node.hpp | 60 ++++++++++++++++++----------------
 1 file changed, 32 insertions(+), 28 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 333edb6..5f222fb 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -159,18 +159,20 @@ private:
     std::shared_ptr<rclcpp::service::ServiceBase> serv_base_ptr,
     rclcpp::callback_group::CallbackGroup::SharedPtr group);
 
-  template<typename ServiceT, typename FunctorT>
-  typename std::enable_if<
-    function_traits<FunctorT>::arity == 2 &&
-    std::is_same<
-      typename function_traits<FunctorT>::template argument_type<0>,
-      typename std::shared_ptr<typename ServiceT::Request>
-      >::value &&
-    std::is_same<
-      typename function_traits<FunctorT>::template argument_type<1>,
-      typename std::shared_ptr<typename ServiceT::Response>
-      >::value,
-    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+  template<
+    typename ServiceT,
+    typename FunctorT,
+    typename std::enable_if<
+      function_traits<FunctorT>::arity == 2 &&
+      std::is_same<
+        typename function_traits<FunctorT>::template argument_type<0>,
+        typename std::shared_ptr<typename ServiceT::Request>
+        >::value &&
+      std::is_same<
+        typename function_traits<FunctorT>::template argument_type<1>,
+        typename std::shared_ptr<typename ServiceT::Response>
+        >::value>::type * = nullptr>
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service_internal(
     rmw_service_t * service_handle,
     const std::string & service_name,
@@ -182,22 +184,24 @@ private:
       service_handle, service_name, callback_without_header);
   }
 
-  template<typename ServiceT, typename FunctorT>
-  typename std::enable_if<
-    function_traits<FunctorT>::arity == 3 &&
-    std::is_same<
-      typename function_traits<FunctorT>::template argument_type<0>,
-      std::shared_ptr<rmw_request_id_t>
-      >::value &&
-    std::is_same<
-      typename function_traits<FunctorT>::template argument_type<1>,
-      typename std::shared_ptr<typename ServiceT::Request>
-      >::value &&
-    std::is_same<
-      typename function_traits<FunctorT>::template argument_type<2>,
-      typename std::shared_ptr<typename ServiceT::Response>
-      >::value,
-    typename rclcpp::service::Service<ServiceT>::SharedPtr>::type
+  template<
+    typename ServiceT,
+    typename FunctorT,
+    typename std::enable_if<
+      function_traits<FunctorT>::arity == 3 &&
+      std::is_same<
+        typename function_traits<FunctorT>::template argument_type<0>,
+        std::shared_ptr<rmw_request_id_t>
+        >::value &&
+      std::is_same<
+        typename function_traits<FunctorT>::template argument_type<1>,
+        typename std::shared_ptr<typename ServiceT::Request>
+        >::value &&
+      std::is_same<
+        typename function_traits<FunctorT>::template argument_type<2>,
+        typename std::shared_ptr<typename ServiceT::Response>
+        >::value>::type * = nullptr>
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service_internal(
     rmw_service_t * service_handle,
     const std::string & service_name,

From bbd7e39bf77e8556d25bd3947683383137231b54 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 30 Apr 2015 14:11:28 -0700
Subject: [PATCH 7/9] Use longer name

---
 rclcpp/include/rclcpp/node.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 5f222fb..a58b1ba 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -127,11 +127,11 @@ public:
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT, typename F>
+  template<typename ServiceT, typename FunctorT>
   typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
-    F callback,
+    FunctorT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
 private:

From 805d5cb00e874d4e4a18f0778e255cd3b8f79dc5 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 30 Apr 2015 14:11:42 -0700
Subject: [PATCH 8/9] Fix code style

---
 rclcpp/include/rclcpp/node_impl.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 19af801..cb9a414 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -196,8 +196,8 @@ Node::create_service(
   rmw_service_t * service_handle = rmw_create_service(
     this->node_handle_, service_type_support_handle, service_name.c_str());
 
-  auto serv = create_service_internal<ServiceT>(service_handle, service_name,
-      callback);
+  auto serv = create_service_internal<ServiceT>(
+    service_handle, service_name, callback);
   auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
   if (group) {
     if (!group_in_node(group)) {

From 02a41cd8ea3b7119f043129c962b47df3239fb97 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@osrfoundation.org>
Date: Thu, 30 Apr 2015 14:12:18 -0700
Subject: [PATCH 9/9] Removed check for third argument, fails with callbacks
 that don't take a request header

---
 rclcpp/include/rclcpp/node.hpp | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index a58b1ba..746ad5c 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -196,10 +196,6 @@ private:
       std::is_same<
         typename function_traits<FunctorT>::template argument_type<1>,
         typename std::shared_ptr<typename ServiceT::Request>
-        >::value &&
-      std::is_same<
-        typename function_traits<FunctorT>::template argument_type<2>,
-        typename std::shared_ptr<typename ServiceT::Response>
         >::value>::type * = nullptr>
   typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service_internal(