Merge pull request #27 from ros2/callable-compatible

Fix std::function disambiguation in Windows

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>
@@ -46,6 +47,23 @@ 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<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 ...)>
+{};
+
 /* ROS Node Interface.
  *
  * This is the single point of entry for creating publishers and subscribers.
@@ -109,19 +127,11 @@ public:
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
   /* Create and return a Service. */
-  template<typename ServiceT>
+  template<typename ServiceT, typename FunctorT>
   typename rclcpp::service::Service<ServiceT>::SharedPtr
   create_service(
     const std::string & service_name,
-    typename rclcpp::service::Service<ServiceT>::CallbackType callback,
+    FunctorT callback,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
-
-  /* Create and return a Service. */
-  template<typename ServiceT>
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
-  create_service(
-    const std::string & service_name,
-    typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
 private:
@@ -148,6 +158,56 @@ 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 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,
+    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 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>::type * = nullptr>
+  typename rclcpp::service::Service<ServiceT>::SharedPtr
+  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);
+  }
 };
 
 } /* namespace node */

rclcpp/include/rclcpp/node_impl.hpp

@@ -182,11 +182,11 @@ Node::create_client(
   return cli;
 }
 
-template<typename ServiceT>
+template<typename ServiceT, typename FunctorT>
-typename service::Service<ServiceT>::SharedPtr
+typename rclcpp::service::Service<ServiceT>::SharedPtr
 Node::create_service(
   const std::string & service_name,
-  typename rclcpp::service::Service<ServiceT>::CallbackType callback,
+  FunctorT callback,
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   using rosidl_generator_cpp::get_service_type_support_handle;
@@ -196,44 +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(
+  auto serv = create_service_internal<ServiceT>(
-    service_handle,
+    service_handle, service_name, callback);
-    service_name,
-    callback);
   auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
-  register_service(service_name, serv_base_ptr, group);
-  return serv;
-}
-
-template<typename ServiceT>
-typename service::Service<ServiceT>::SharedPtr
-Node::create_service(
-  const std::string & service_name,
-  typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group)
-{
-  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,
-  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
@@ -244,6 +209,7 @@ Node::register_service(
     default_callback_group_->add_service(serv_base_ptr);
   }
   number_of_services_++;
+  return serv;
 }
 
 #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */