From 00daba2726015961a3770a44bfb05867766ceade Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Fri, 9 Oct 2015 11:21:00 -0700
Subject: [PATCH] add any subscription callback types for const, remove
 references from unique_ptrs

---
 .../rclcpp/any_subscription_callback.hpp      | 54 +++++++++++++++++--
 rclcpp/include/rclcpp/subscription.hpp        | 21 +++++---
 2 files changed, 65 insertions(+), 10 deletions(-)

diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
index 3165608..507d3ac 100644
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -32,20 +32,26 @@ namespace any_subscription_callback
 template<typename MessageT>
 struct AnySubscriptionCallback
 {
-  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT> &)>;
+  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT>)>;
   using SharedPtrWithInfoCallback =
-      std::function<void(const std::shared_ptr<MessageT> &, const rmw_message_info_t &)>;
-  using UniquePtrCallback = std::function<void(std::unique_ptr<MessageT> &)>;
+      std::function<void(const std::shared_ptr<MessageT>, const rmw_message_info_t &)>;
+  using ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
+  using ConstSharedPtrWithInfoCallback =
+      std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
+  using UniquePtrCallback = std::function<void(std::unique_ptr<MessageT>)>;
   using UniquePtrWithInfoCallback =
-      std::function<void(std::unique_ptr<MessageT> &, const rmw_message_info_t &)>;
+      std::function<void(std::unique_ptr<MessageT>, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback;
+  ConstSharedPtrCallback const_shared_ptr_callback;
+  ConstSharedPtrWithInfoCallback const_shared_ptr_with_info_callback;
   UniquePtrCallback unique_ptr_callback;
   UniquePtrWithInfoCallback unique_ptr_with_info_callback;
 
   AnySubscriptionCallback()
   : shared_ptr_callback(nullptr), shared_ptr_with_info_callback(nullptr),
+    const_shared_ptr_callback(nullptr), const_shared_ptr_with_info_callback(nullptr),
     unique_ptr_callback(nullptr), unique_ptr_with_info_callback(nullptr)
   {}
 
@@ -80,8 +86,48 @@ struct AnySubscriptionCallback
   >
   void set(CallbackT callback)
   {
+    static_assert(std::is_same<
+        typename function_traits<CallbackT>::template argument_type<1>,
+        const rmw_message_info_t &>::value,
+      "Passed incorrect argument type to callback, should be rmw_message_info_t");
     shared_ptr_with_info_callback = callback;
   }
+
+  template<typename CallbackT,
+  typename std::enable_if<
+    function_traits<CallbackT>::arity == 1
+  >::type * = nullptr,
+  typename std::enable_if<
+    std::is_same<
+      typename function_traits<CallbackT>::template argument_type<0>,
+      typename std::shared_ptr<const MessageT>
+    >::value
+  >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    const_shared_ptr_callback = callback;
+  }
+
+  template<typename CallbackT,
+  typename std::enable_if<
+    function_traits<CallbackT>::arity == 2
+  >::type * = nullptr,
+  typename std::enable_if<
+    std::is_same<
+      typename function_traits<CallbackT>::template argument_type<0>,
+      typename std::shared_ptr<const MessageT>
+    >::value
+  >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    static_assert(std::is_same<
+        typename function_traits<CallbackT>::template argument_type<1>,
+        const rmw_message_info_t &>::value,
+      "Passed incorrect argument type to callback, should be rmw_message_info_t");
+    const_shared_ptr_with_info_callback = callback;
+  }
 /*
   template<typename CallbackT,
     typename std::enable_if<
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index d2dd094..d5be8ea 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -207,12 +207,15 @@ public:
       any_callback_.shared_ptr_callback(typed_message);
     } else if (any_callback_.shared_ptr_with_info_callback) {
       any_callback_.shared_ptr_with_info_callback(typed_message, message_info);
+    } else if (any_callback_.const_shared_ptr_callback) {
+      any_callback_.const_shared_ptr_callback(typed_message);
+    } else if (any_callback_.const_shared_ptr_with_info_callback) {
+      any_callback_.const_shared_ptr_with_info_callback(typed_message, message_info);
     } else if (any_callback_.unique_ptr_callback) {
-      std::unique_ptr<MessageT> unique_msg(new MessageT(*typed_message));
-      any_callback_.unique_ptr_callback(unique_msg);
+      any_callback_.unique_ptr_callback(std::unique_ptr<MessageT>(new MessageT(*typed_message)));
     } else if (any_callback_.unique_ptr_with_info_callback) {
-      std::unique_ptr<MessageT> unique_msg(new MessageT(*typed_message));
-      any_callback_.unique_ptr_with_info_callback(unique_msg, message_info);
+      any_callback_.unique_ptr_with_info_callback(std::unique_ptr<MessageT>(new MessageT(*
+        typed_message)), message_info);
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
@@ -253,10 +256,16 @@ public:
     } else if (any_callback_.shared_ptr_with_info_callback) {
       typename MessageT::SharedPtr shared_msg = std::move(msg);
       any_callback_.shared_ptr_with_info_callback(shared_msg, message_info);
+    } else if (any_callback_.const_shared_ptr_callback) {
+      typename MessageT::ConstSharedPtr const_shared_msg = std::move(msg);
+      any_callback_.const_shared_ptr_callback(const_shared_msg);
+    } else if (any_callback_.const_shared_ptr_with_info_callback) {
+      typename MessageT::ConstSharedPtr const_shared_msg = std::move(msg);
+      any_callback_.const_shared_ptr_with_info_callback(const_shared_msg, message_info);
     } else if (any_callback_.unique_ptr_callback) {
-      any_callback_.unique_ptr_callback(msg);
+      any_callback_.unique_ptr_callback(std::move(msg));
     } else if (any_callback_.unique_ptr_with_info_callback) {
-      any_callback_.unique_ptr_with_info_callback(msg, message_info);
+      any_callback_.unique_ptr_with_info_callback(std::move(msg), message_info);
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }