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] 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_ */