Addded support for passing MessageT explicitly

rclcpp/include/rclcpp/function_traits.hpp

@@ -36,13 +36,17 @@ struct plain_message<std::shared_ptr<MessageT>> : plain_message<MessageT>
 {};
 
 template<typename MessageT>
-struct plain_message<const std::shared_ptr<MessageT>> : plain_message<MessageT>
+struct plain_message<std::shared_ptr<MessageT const>> : plain_message<MessageT>
 {};
 
 template<typename MessageT, typename Deleter>
 struct plain_message<std::unique_ptr<MessageT, Deleter>> : plain_message<MessageT>
 {};
 
+template<typename MessageT, typename Deleter>
+struct plain_message<std::unique_ptr<MessageT const, Deleter>> : plain_message<MessageT>
+{};
+
 /* NOTE(esteve):
  * We support service callbacks that can optionally take the request id,
  * which should be possible with two overloaded create_service methods,
@@ -164,6 +168,46 @@ struct same_arguments : std::is_same<
   >
 {};
 
+// Taken from http://stackoverflow.com/a/25859000
+// Original author http://stackoverflow.com/users/847987/charphacy
+template<typename T>
+using remove_ref_t = typename std::remove_reference<T>::type;
+
+template<typename T>
+using remove_refptr_t = typename std::remove_pointer<remove_ref_t<T>>::type;
+
+template<typename T>
+using is_function_t = typename std::is_function<remove_refptr_t<T>>::type;
+
+template<bool isObject, typename T>
+struct is_callable_impl : public is_function_t<T>{};
+
+template<typename T>
+struct is_callable_impl<true, T>
+{
+private:
+  struct Fallback { void operator()(); };
+  struct Derived : T, Fallback {};
+
+  template<typename U, U>
+  struct Check;
+
+  template<typename>
+  static std::true_type test(...);
+
+  template<typename C>
+  static std::false_type test(Check<void (Fallback::*)(), & C::operator()> *);
+
+public:
+  using type = decltype(test<Derived>(nullptr));
+};
+
+template<typename T>
+using is_callable_t =
+    typename is_callable_impl<std::is_class<remove_ref_t<T>>::value,
+    remove_ref_t<T>>::type;
+
+
 }  // namespace function_traits
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node.hpp

@@ -171,13 +171,39 @@ class Node : public std::enable_shared_from_this<Node>
      Windows build breaks when static member function passed as default
      argument to msg_mem_strat, nullptr is a workaround.
    */
+  template<
+    typename MessageT,
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>,
+    typename DisableIfMessageTCallable = std::enable_if<
+      !rclcpp::function_traits::is_callable_t<MessageT>::value
+    >,
+    typename EnableIfCallbackTCallable = std::enable_if<
+      rclcpp::function_traits::is_callable_t<CallbackT>::value
+    >
+  >
+  std::shared_ptr<SubscriptionT>
+  create_subscription(
+    const std::string & topic_name,
+    CallbackT && callback,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
+    bool ignore_local_publications = false,
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    msg_mem_strat = nullptr,
+    std::shared_ptr<Alloc> allocator = nullptr);
+
   template<
     typename CallbackT,
     typename MessageT = typename rclcpp::function_traits::plain_message<
       typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>
     >::type,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>>
+    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>,
+    typename EnableIfCallbackTCallable = std::enable_if<
+      rclcpp::function_traits::is_callable_t<CallbackT>::value
+    >
   >
   std::shared_ptr<SubscriptionT>
   create_subscription(
@@ -205,13 +231,39 @@ class Node : public std::enable_shared_from_this<Node>
      Windows build breaks when static member function passed as default
      argument to msg_mem_strat, nullptr is a workaround.
    */
+  template<
+    typename MessageT,
+    typename CallbackT,
+    typename Alloc = std::allocator<void>,
+    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>,
+    typename DisableIfMessageTCallable = std::enable_if<
+      !rclcpp::function_traits::is_callable_t<MessageT>::value
+    >,
+    typename EnableIfCallbackTCallable = std::enable_if<
+      rclcpp::function_traits::is_callable_t<CallbackT>::value
+    >
+  >
+  std::shared_ptr<SubscriptionT>
+  create_subscription(
+    const std::string & topic_name,
+    size_t qos_history_depth,
+    CallbackT && callback,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
+    bool ignore_local_publications = false,
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    msg_mem_strat = nullptr,
+    std::shared_ptr<Alloc> allocator = nullptr);
+
   template<
     typename CallbackT,
     typename MessageT = typename rclcpp::function_traits::plain_message<
       typename rclcpp::function_traits::function_traits<CallbackT>::template argument_type<0>
     >::type,
     typename Alloc = std::allocator<void>,
-    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>>
+    typename SubscriptionT = rclcpp::Subscription<MessageT, Alloc>,
+    typename EnableIfCallbackTCallable = std::enable_if<
+      rclcpp::function_traits::is_callable_t<CallbackT>::value
+    >
   >
   std::shared_ptr<SubscriptionT>
   create_subscription(

rclcpp/include/rclcpp/node_impl.hpp

@@ -81,7 +81,14 @@ Node::create_publisher(
     allocator);
 }
 
-template<typename CallbackT, typename MessageT, typename Alloc, typename SubscriptionT>
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT,
+  typename DisableIfMessageTCallable,
+  typename EnableIfCallbackTCallable
+>
 std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
@@ -114,7 +121,42 @@ Node::create_subscription(
     allocator);
 }
 
-template<typename CallbackT, typename MessageT, typename Alloc, typename SubscriptionT>
+template<
+  typename CallbackT,
+  typename MessageT,
+  typename Alloc,
+  typename SubscriptionT,
+  typename EnableIfCallbackTCallable
+>
+std::shared_ptr<SubscriptionT>
+Node::create_subscription(
+  const std::string & topic_name,
+  CallbackT && callback,
+  const rmw_qos_profile_t & qos_profile,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  bool ignore_local_publications,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  msg_mem_strat,
+  std::shared_ptr<Alloc> allocator)
+{
+  return this->create_subscription(
+    topic_name,
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group,
+    ignore_local_publications,
+    msg_mem_strat,
+    allocator);
+}
+
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename Alloc,
+  typename SubscriptionT,
+  typename DisableIfMessageTCallable,
+  typename EnableIfCallbackTCallable
+>
 std::shared_ptr<SubscriptionT>
 Node::create_subscription(
   const std::string & topic_name,
@@ -128,7 +170,9 @@ Node::create_subscription(
 {
   rmw_qos_profile_t qos = rmw_qos_profile_default;
   qos.depth = qos_history_depth;
-  return this->create_subscription<CallbackT, MessageT, Alloc, SubscriptionT>(
+  return this->create_subscription<
+    MessageT, CallbackT, Alloc, SubscriptionT, DisableIfMessageTCallable, EnableIfCallbackTCallable
+  >(
     topic_name,
     std::forward<CallbackT>(callback),
     qos,
@@ -138,6 +182,34 @@ Node::create_subscription(
     allocator);
 }
 
+template<
+  typename CallbackT,
+  typename MessageT,
+  typename Alloc,
+  typename SubscriptionT,
+  typename EnableIfCallbackTCallable
+>
+std::shared_ptr<SubscriptionT>
+Node::create_subscription(
+  const std::string & topic_name,
+  size_t qos_history_depth,
+  CallbackT && callback,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group,
+  bool ignore_local_publications,
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  msg_mem_strat,
+  std::shared_ptr<Alloc> allocator)
+{
+  return this->create_subscription(
+    topic_name,
+    qos_history_depth,
+    std::forward<CallbackT>(callback),
+    group,
+    ignore_local_publications,
+    msg_mem_strat,
+    allocator);
+}
+
 template<typename DurationT, typename CallbackT>
 typename rclcpp::WallTimer<CallbackT>::SharedPtr
 Node::create_wall_timer(

rclcpp/test/test_function_traits.cpp

@@ -95,6 +95,8 @@ struct ObjectMember
   }
 };
 
+struct NotCallable {};
+
 template<
   typename FunctorT,
   std::size_t Arity = 0,
@@ -706,3 +708,93 @@ TEST(TestFunctionTraits, sfinae_match) {
 
   EXPECT_EQ("foo", func_accept_callback_return_type(lambda_no_args_string));
 }
+
+/*
+   Tests that funcion_traits detects that a functor is callabale.
+ */
+TEST(TestFunctionTraits, is_callable) {
+  // Test regular functions
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(func_no_args)>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(func_one_int)>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(func_two_ints)>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(func_one_int_one_char)>::value,
+    "Functor must be callable");
+
+  // Test lambdas
+  auto lambda_no_args = []() {
+      return 0;
+    };
+
+  auto lambda_one_int = [](int one) {
+      (void)one;
+      return 1;
+    };
+
+  auto lambda_two_ints = [](int one, int two) {
+      (void)one;
+      (void)two;
+      return 2;
+    };
+
+  auto lambda_one_int_one_char = [](int one, char two) {
+      (void)one;
+      (void)two;
+      return 3;
+    };
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(lambda_no_args)>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(lambda_one_int)>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(lambda_two_ints)>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<decltype(lambda_one_int_one_char)>::value,
+    "Functor must be callable");
+
+  // Test objects that have a call operator
+  static_assert(
+    rclcpp::function_traits::is_callable_t<FunctionObjectNoArgs>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<FunctionObjectOneInt>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<FunctionObjectTwoInts>::value,
+    "Functor must be callable");
+
+  static_assert(
+    rclcpp::function_traits::is_callable_t<FunctionObjectOneIntOneChar>::value,
+    "Functor must be callable");
+
+  // Test that types that don't have a call operator return false
+  static_assert(
+    !rclcpp::function_traits::is_callable_t<int>::value,
+    "Type is not callable");
+
+  static_assert(
+    !rclcpp::function_traits::is_callable_t<std::string>::value,
+    "Type is not callable");
+
+  static_assert(
+    !rclcpp::function_traits::is_callable_t<NotCallable>::value,
+    "Type is not callable");
+}