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] 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>();