Merge pull request #144 from ros2/fix_sleep_for_windows

account for spurious wake ups from std::condition_variable::wait_for

rclcpp/CMakeLists.txt

@@ -22,6 +22,7 @@ if(AMENT_ENABLE_TESTING)
 
   include_directories(include)
 
+  ament_add_gtest(test_function_traits test/test_function_traits.cpp)
   ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp)
   ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp)
   if(TARGET test_intra_process_manager)

rclcpp/include/rclcpp/allocator/allocator_common.hpp

@@ -0,0 +1,33 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
+#define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
+
+#include <memory>
+
+#include "rclcpp/allocator/allocator_deleter.hpp"
+
+namespace rclcpp
+{
+namespace allocator
+{
+
+template<typename T, typename Alloc>
+using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
+
+}  // namespace allocator
+}  // namespace rclcpp
+
+#endif  // RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_

rclcpp/include/rclcpp/allocator/allocator_deleter.hpp

@@ -0,0 +1,105 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
+#define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
+
+#include <memory>
+
+namespace rclcpp
+{
+
+namespace allocator
+{
+
+template<typename Allocator>
+class AllocatorDeleter
+{
+  template<typename T>
+  using AllocRebind = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
+
+public:
+  AllocatorDeleter()
+  : allocator_(nullptr)
+  {
+  }
+
+  explicit AllocatorDeleter(Allocator * a)
+  : allocator_(a)
+  {
+  }
+
+  template<typename T>
+  AllocatorDeleter(const AllocatorDeleter<T> & a)
+  {
+    allocator_ = a.get_allocator();
+  }
+
+  template<typename T>
+  void operator()(T * ptr)
+  {
+    std::allocator_traits<AllocRebind<T>>::destroy(*allocator_, ptr);
+    std::allocator_traits<AllocRebind<T>>::deallocate(*allocator_, ptr, 1);
+    ptr = nullptr;
+  }
+
+  Allocator * get_allocator() const
+  {
+    return allocator_;
+  }
+
+  void set_allocator(Allocator * alloc)
+  {
+    allocator_ = alloc;
+  }
+
+private:
+  Allocator * allocator_;
+};
+
+template<typename Alloc, typename T, typename D>
+void set_allocator_for_deleter(D * deleter, Alloc * alloc)
+{
+  (void) alloc;
+  (void) deleter;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T, typename U>
+void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<U> * alloc)
+{
+  (void) deleter;
+  (void) alloc;
+}
+
+template<typename Alloc, typename T>
+void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
+{
+  if (!deleter || !alloc) {
+    throw std::invalid_argument("Argument was NULL to set_allocator_for_deleter");
+  }
+  deleter->set_allocator(alloc);
+}
+
+template<typename Alloc, typename T>
+using Deleter = typename std::conditional<
+    std::is_same<typename std::allocator_traits<Alloc>::template rebind_alloc<T>,
+    typename std::allocator<void>::template rebind<T>::other>::value,
+    std::default_delete<T>,
+    AllocatorDeleter<Alloc>
+    >::type;
+}  // namespace allocator
+}  // namespace rclcpp
+
+#endif  // RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_

rclcpp/include/rclcpp/any_service_callback.hpp

@@ -0,0 +1,103 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
+#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_
+
+#include <rclcpp/function_traits.hpp>
+
+#include <functional>
+#include <memory>
+#include <type_traits>
+
+#include <rmw/types.h>
+
+namespace rclcpp
+{
+
+namespace any_service_callback
+{
+
+template<typename ServiceT>
+class AnyServiceCallback
+{
+private:
+  using SharedPtrCallback = std::function<void(
+        const std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>
+      )>;
+  using SharedPtrWithRequestHeaderCallback = std::function<void(
+        const std::shared_ptr<rmw_request_id_t>,
+        const std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>
+      )>;
+
+  SharedPtrCallback shared_ptr_callback_;
+  SharedPtrWithRequestHeaderCallback shared_ptr_with_request_header_callback_;
+
+public:
+  AnyServiceCallback()
+  : shared_ptr_callback_(nullptr), shared_ptr_with_request_header_callback_(nullptr)
+  {}
+
+  AnyServiceCallback(const AnyServiceCallback &) = default;
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        SharedPtrCallback
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    shared_ptr_callback_ = callback;
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        SharedPtrWithRequestHeaderCallback
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    shared_ptr_with_request_header_callback_ = callback;
+  }
+
+  void dispatch(
+    std::shared_ptr<rmw_request_id_t> request_header,
+    std::shared_ptr<typename ServiceT::Request> request,
+    std::shared_ptr<typename ServiceT::Response> response)
+  {
+    if (shared_ptr_callback_ != nullptr) {
+      (void)request_header;
+      shared_ptr_callback_(request, response);
+    } else if (shared_ptr_with_request_header_callback_ != nullptr) {
+      shared_ptr_with_request_header_callback_(request_header, request, response);
+    } else {
+      throw std::runtime_error("unexpected request without any callback set");
+    }
+  }
+};
+
+} /* namespace any_service_callback */
+} /* namespace rclcpp */
+
+#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */

rclcpp/include/rclcpp/any_subscription_callback.hpp

@@ -12,16 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_
-#define RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_
+#ifndef RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_
+#define RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_
 
-#include <rclcpp/function_traits.hpp>
+#include <rmw/types.h>
 
 #include <functional>
 #include <memory>
 #include <type_traits>
 
-#include <rmw/types.h>
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/function_traits.hpp"
 
 namespace rclcpp
 {
@@ -29,99 +30,183 @@ namespace rclcpp
 namespace any_subscription_callback
 {
 
-template<typename MessageT>
-struct AnySubscriptionCallback
+template<typename MessageT, typename Alloc>
+class AnySubscriptionCallback
 {
-  using SharedPtrCallback = std::function<void(const std::shared_ptr<MessageT> &)>;
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+
+  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(MessageUniquePtr)>;
   using UniquePtrWithInfoCallback =
-      std::function<void(std::unique_ptr<MessageT> &, const rmw_message_info_t &)>;
+      std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
 
-  SharedPtrCallback shared_ptr_callback;
-  SharedPtrWithInfoCallback shared_ptr_with_info_callback;
-  UniquePtrCallback unique_ptr_callback;
-  UniquePtrWithInfoCallback unique_ptr_with_info_callback;
+  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),
-    unique_ptr_callback(nullptr), unique_ptr_with_info_callback(nullptr)
-  {}
+public:
+  explicit AnySubscriptionCallback(std::shared_ptr<Alloc> allocator)
+  : 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)
+  {
+    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+  }
 
   AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
 
-  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<MessageT>
-    >::value
-  >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    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<MessageT>
-    >::value
-  >::type * = nullptr
-  >
-  void set(CallbackT callback)
-  {
-    shared_ptr_with_info_callback = callback;
-  }
-/*
-  template<typename CallbackT,
+  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::unique_ptr<MessageT>
+      rclcpp::same_arguments<
+        CallbackT,
+        SharedPtrCallback
       >::value
     >::type * = nullptr
   >
   void set(CallbackT callback)
   {
-    static_assert(std::is_same<
-        typename function_traits<CallbackT>::template argument_type<0>,
-        typename std::unique_ptr<MessageT>
-      >::value, "Not a unique pointer");
-    unique_ptr_callback = callback;
+    shared_ptr_callback_ = callback;
   }
 
-  template<typename CallbackT,
+  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::unique_ptr<MessageT>
+      rclcpp::same_arguments<
+        CallbackT,
+        SharedPtrWithInfoCallback
       >::value
     >::type * = nullptr
   >
   void set(CallbackT callback)
   {
-    unique_ptr_with_info_callback = callback;
+    shared_ptr_with_info_callback_ = callback;
   }
-  */
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        ConstSharedPtrCallback
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    const_shared_ptr_callback_ = callback;
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        ConstSharedPtrWithInfoCallback
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    const_shared_ptr_with_info_callback_ = callback;
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        UniquePtrCallback
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    unique_ptr_callback_ = callback;
+  }
+
+  template<
+    typename CallbackT,
+    typename std::enable_if<
+      rclcpp::same_arguments<
+        CallbackT,
+        UniquePtrWithInfoCallback
+      >::value
+    >::type * = nullptr
+  >
+  void set(CallbackT callback)
+  {
+    unique_ptr_with_info_callback_ = callback;
+  }
+
+  void dispatch(
+    std::shared_ptr<MessageT> message, const rmw_message_info_t & message_info)
+  {
+    (void)message_info;
+    if (shared_ptr_callback_) {
+      shared_ptr_callback_(message);
+    } else if (shared_ptr_with_info_callback_) {
+      shared_ptr_with_info_callback_(message, message_info);
+    } else if (const_shared_ptr_callback_) {
+      const_shared_ptr_callback_(message);
+    } else if (const_shared_ptr_with_info_callback_) {
+      const_shared_ptr_with_info_callback_(message, message_info);
+    } else if (unique_ptr_callback_) {
+      auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+      MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message);
+      unique_ptr_callback_(MessageUniquePtr(ptr, message_deleter_));
+    } else if (unique_ptr_with_info_callback_) {
+      auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+      MessageAllocTraits::construct(*message_allocator_.get(), ptr, *message);
+      unique_ptr_with_info_callback_(MessageUniquePtr(ptr, message_deleter_), message_info);
+    } else {
+      throw std::runtime_error("unexpected message without any callback set");
+    }
+  }
+
+  void dispatch_intra_process(
+    MessageUniquePtr & message, const rmw_message_info_t & message_info)
+  {
+    (void)message_info;
+    if (shared_ptr_callback_) {
+      typename MessageT::SharedPtr shared_message = std::move(message);
+      shared_ptr_callback_(shared_message);
+    } else if (shared_ptr_with_info_callback_) {
+      typename MessageT::SharedPtr shared_message = std::move(message);
+      shared_ptr_with_info_callback_(shared_message, message_info);
+    } else if (const_shared_ptr_callback_) {
+      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
+      const_shared_ptr_callback_(const_shared_message);
+    } else if (const_shared_ptr_with_info_callback_) {
+      typename MessageT::ConstSharedPtr const_shared_message = std::move(message);
+      const_shared_ptr_with_info_callback_(const_shared_message, message_info);
+    } else if (unique_ptr_callback_) {
+      unique_ptr_callback_(std::move(message));
+    } else if (unique_ptr_with_info_callback_) {
+      unique_ptr_with_info_callback_(std::move(message), message_info);
+    } else {
+      throw std::runtime_error("unexpected message without any callback set");
+    }
+  }
+
+private:
+  std::shared_ptr<MessageAlloc> message_allocator_;
+  MessageDeleter message_deleter_;
 };
 
-} /* namespace any_subscription_callback */
-} /* namespace rclcpp */
+}  // namespace any_subscription_callback
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_ */
+#endif  // RCLCPP__ANY_SUBSCRIPTION_CALLBACK_HPP_

rclcpp/include/rclcpp/callback_group.hpp

@@ -32,10 +32,6 @@ namespace node
 {
 class Node;
 } // namespace node
-namespace executor
-{
-class Executor;
-} // namespace executor
 
 namespace callback_group
 {
@@ -49,7 +45,6 @@ enum class CallbackGroupType
 class CallbackGroup
 {
   friend class rclcpp::node::Node;
-  friend class rclcpp::executor::Executor;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup);
@@ -58,6 +53,40 @@ public:
   : type_(group_type), can_be_taken_from_(true)
   {}
 
+  const std::vector<subscription::SubscriptionBase::WeakPtr> &
+  get_subscription_ptrs() const
+  {
+    return subscription_ptrs_;
+  }
+
+  const std::vector<timer::TimerBase::WeakPtr> &
+  get_timer_ptrs() const
+  {
+    return timer_ptrs_;
+  }
+
+  const std::vector<service::ServiceBase::SharedPtr> &
+  get_service_ptrs() const
+  {
+    return service_ptrs_;
+  }
+
+  const std::vector<client::ClientBase::SharedPtr> &
+  get_client_ptrs() const
+  {
+    return client_ptrs_;
+  }
+
+  std::atomic_bool & can_be_taken_from()
+  {
+    return can_be_taken_from_;
+  }
+
+  const CallbackGroupType & type() const
+  {
+    return type_;
+  }
+
 private:
   RCLCPP_DISABLE_COPY(CallbackGroup);
 

rclcpp/include/rclcpp/client.hpp

@@ -31,18 +31,11 @@
 namespace rclcpp
 {
 
-// Forward declaration for friend statement
-namespace executor
-{
-class Executor;
-} // namespace executor
-
 namespace client
 {
 
 class ClientBase
 {
-  friend class rclcpp::executor::Executor;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase);

rclcpp/include/rclcpp/executor.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_EXECUTOR_HPP_
-#define RCLCPP_RCLCPP_EXECUTOR_HPP_
+#ifndef RCLCPP__EXECUTOR_HPP_
+#define RCLCPP__EXECUTOR_HPP_
 
 #include <algorithm>
 #include <cassert>
@@ -23,13 +23,14 @@
 #include <memory>
 #include <vector>
 
-#include <rcl_interfaces/msg/intra_process_message.hpp>
+#include "rcl_interfaces/msg/intra_process_message.hpp"
 
-#include <rclcpp/any_executable.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/memory_strategy.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/any_executable.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
 
 namespace rclcpp
 {
@@ -48,15 +49,13 @@ namespace executor
  */
 class Executor
 {
-  friend class memory_strategy::MemoryStrategy;
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
 
   /// Default constructor.
   // \param[in] ms The memory strategy to be used with this executor.
   explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategy::create_default_strategy())
+    memory_strategies::create_default_strategy())
   : interrupt_guard_condition_(rmw_create_guard_condition()),
     memory_strategy_(ms)
   {
@@ -94,7 +93,7 @@ public:
     for (auto & weak_node : weak_nodes_) {
       auto node = weak_node.lock();
       if (node == node_ptr) {
-        // TODO: Use a different error here?
+        // TODO(jacquelinekay): Use a different error here?
         throw std::runtime_error("Cannot add node to executor, node already added.");
       }
     }
@@ -228,7 +227,7 @@ protected:
       execute_client(any_exec->client);
     }
     // Reset the callback_group, regardless of type
-    any_exec->callback_group->can_be_taken_from_.store(true);
+    any_exec->callback_group->can_be_taken_from().store(true);
     // Wake the wait, because it may need to be recalculated or work that
     // was previously blocked is now available.
     rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_);
@@ -245,7 +244,8 @@ protected:
     bool taken = false;
     rmw_message_info_t message_info;
     auto ret =
-      rmw_take_with_info(subscription->subscription_handle_, message.get(), &taken, &message_info);
+      rmw_take_with_info(subscription->get_subscription_handle(),
+        message.get(), &taken, &message_info);
     if (ret == RMW_RET_OK) {
       if (taken) {
         message_info.from_intra_process = false;
@@ -267,7 +267,7 @@ protected:
     bool taken = false;
     rmw_message_info_t message_info;
     rmw_ret_t status = rmw_take_with_info(
-      subscription->intra_process_subscription_handle_,
+      subscription->get_intra_process_subscription_handle(),
       &ipm,
       &taken,
       &message_info);
@@ -287,7 +287,7 @@ protected:
   execute_timer(
     rclcpp::timer::TimerBase::SharedPtr timer)
   {
-    timer->callback_();
+    timer->execute_callback();
   }
 
   static void
@@ -298,7 +298,7 @@ protected:
     std::shared_ptr<void> request = service->create_request();
     bool taken = false;
     rmw_ret_t status = rmw_take_request(
-      service->service_handle_,
+      service->get_service_handle(),
       request_header.get(),
       request.get(),
       &taken);
@@ -321,7 +321,7 @@ protected:
     std::shared_ptr<void> response = client->create_response();
     bool taken = false;
     rmw_ret_t status = rmw_take_response(
-      client->client_handle_,
+      client->get_client_handle(),
       request_header.get(),
       response.get(),
       &taken);
@@ -342,36 +342,11 @@ protected:
   void
   wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
+    memory_strategy_->clear_active_entities();
+
     // Collect the subscriptions and timers to be waited on
-    bool has_invalid_weak_nodes = false;
-    std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr> subs;
-    std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
-    std::vector<rclcpp::client::ClientBase::SharedPtr> clients;
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        has_invalid_weak_nodes = false;
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from_.load()) {
-          continue;
-        }
-        for (auto & weak_subscription : group->subscription_ptrs_) {
-          auto subscription = weak_subscription.lock();
-          if (subscription) {
-            subs.push_back(subscription);
-          }
-        }
-        for (auto & service : group->service_ptrs_) {
-          services.push_back(service);
-        }
-        for (auto & client : group->client_ptrs_) {
-          clients.push_back(client);
-        }
-      }
-    }
+    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+
     // Clean up any invalid nodes, if they were detected
     if (has_invalid_weak_nodes) {
       weak_nodes_.erase(
@@ -381,74 +356,26 @@ protected:
             return i.expired();
           }));
     }
+
     // Use the number of subscriptions to allocate memory in the handles
-    size_t max_number_of_subscriptions = subs.size() * 2;  // Times two for intra-process.
     rmw_subscriptions_t subscriber_handles;
-    subscriber_handles.subscriber_count = 0;
-    // TODO(wjwwood): Avoid redundant malloc's
-    subscriber_handles.subscribers = memory_strategy_->borrow_handles(
-      HandleType::subscription_handle, max_number_of_subscriptions);
-    if (subscriber_handles.subscribers == NULL &&
-      max_number_of_subscriptions > 0)
-    {
-      // TODO(wjwwood): Use a different error here? maybe std::bad_alloc?
-      throw std::runtime_error("Could not malloc for subscriber pointers.");
-    }
-    // Then fill the SubscriberHandles with ready subscriptions
-    for (auto & subscription : subs) {
-      subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
-        subscription->subscription_handle_->data;
-      if (subscription->intra_process_subscription_handle_) {
-        subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
-          subscription->intra_process_subscription_handle_->data;
-      }
-    }
+    subscriber_handles.subscriber_count =
+      memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers);
 
-    // Use the number of services to allocate memory in the handles
-    size_t number_of_services = services.size();
     rmw_services_t service_handles;
-    service_handles.service_count = number_of_services;
-    service_handles.services =
-      memory_strategy_->borrow_handles(HandleType::service_handle, number_of_services);
-    if (service_handles.services == NULL &&
-      number_of_services > 0)
-    {
-      // TODO(esteve): Use a different error here? maybe std::bad_alloc?
-      throw std::runtime_error("Could not malloc for service pointers.");
-    }
-    // Then fill the ServiceHandles with ready services
-    size_t service_handle_index = 0;
-    for (auto & service : services) {
-      service_handles.services[service_handle_index] = \
-        service->service_handle_->data;
-      service_handle_index += 1;
-    }
+    service_handles.service_count =
+      memory_strategy_->fill_service_handles(service_handles.services);
 
-    // Use the number of clients to allocate memory in the handles
-    size_t number_of_clients = clients.size();
     rmw_clients_t client_handles;
-    client_handles.client_count = number_of_clients;
-    client_handles.clients =
-      memory_strategy_->borrow_handles(HandleType::client_handle, number_of_clients);
-    if (client_handles.clients == NULL && number_of_clients > 0) {
-      // TODO: Use a different error here? maybe std::bad_alloc?
-      throw std::runtime_error("Could not malloc for client pointers.");
-    }
-    // Then fill the ServiceHandles with ready clients
-    size_t client_handle_index = 0;
-    for (auto & client : clients) {
-      client_handles.clients[client_handle_index] = \
-        client->client_handle_->data;
-      client_handle_index += 1;
-    }
+    client_handles.client_count =
+      memory_strategy_->fill_client_handles(client_handles.clients);
 
     // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond,
     // and one for the executor's guard cond (interrupt_guard_condition_)
     size_t number_of_guard_conds = 2;
     rmw_guard_conditions_t guard_condition_handles;
     guard_condition_handles.guard_condition_count = number_of_guard_conds;
-    guard_condition_handles.guard_conditions =
-      memory_strategy_->borrow_handles(HandleType::guard_condition_handle, number_of_guard_conds);
+    guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
     if (guard_condition_handles.guard_conditions == NULL &&
       number_of_guard_conds > 0)
     {
@@ -499,129 +426,14 @@ protected:
     // If ctrl-c guard condition, return directly
     if (guard_condition_handles.guard_conditions[0] != 0) {
       // Make sure to free or clean memory
-      memory_strategy_->return_handles(HandleType::subscription_handle,
-        subscriber_handles.subscribers);
-      memory_strategy_->return_handles(HandleType::service_handle,
-        service_handles.services);
-      memory_strategy_->return_handles(HandleType::client_handle,
-        client_handles.clients);
-      memory_strategy_->return_handles(HandleType::guard_condition_handle,
-        guard_condition_handles.guard_conditions);
+      memory_strategy_->clear_handles();
       return;
     }
-    // Add the new work to the class's list of things waiting to be executed
-    // Starting with the subscribers
-    for (size_t i = 0; i < subscriber_handles.subscriber_count; ++i) {
-      void * handle = subscriber_handles.subscribers[i];
-      if (handle) {
-        subscriber_handles_.push_back(handle);
-      }
-    }
-    // Then the services
-    for (size_t i = 0; i < number_of_services; ++i) {
-      void * handle = service_handles.services[i];
-      if (handle) {
-        service_handles_.push_back(handle);
-      }
-    }
-    // Then the clients
-    for (size_t i = 0; i < number_of_clients; ++i) {
-      void * handle = client_handles.clients[i];
-      if (handle) {
-        client_handles_.push_back(handle);
-      }
-    }
-
-    memory_strategy_->return_handles(HandleType::subscription_handle,
-      subscriber_handles.subscribers);
-    memory_strategy_->return_handles(HandleType::service_handle,
-      service_handles.services);
-    memory_strategy_->return_handles(HandleType::client_handle,
-      client_handles.clients);
-    memory_strategy_->return_handles(HandleType::guard_condition_handle,
-      guard_condition_handles.guard_conditions);
 
+    memory_strategy_->revalidate_handles();
   }
 
 /******************************/
-
-  rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_subscription : group->subscription_ptrs_) {
-          auto subscription = weak_subscription.lock();
-          if (subscription) {
-            if (subscription->subscription_handle_->data == subscriber_handle) {
-              return subscription;
-            }
-            if (subscription->intra_process_subscription_handle_ &&
-              subscription->intra_process_subscription_handle_->data == subscriber_handle)
-            {
-              return subscription;
-            }
-          }
-        }
-      }
-    }
-    return nullptr;
-  }
-
-  rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & service : group->service_ptrs_) {
-          if (service->service_handle_->data == service_handle) {
-            return service;
-          }
-        }
-      }
-    }
-    return rclcpp::service::ServiceBase::SharedPtr();
-  }
-
-  rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & client : group->client_ptrs_) {
-          if (client->client_handle_->data == client_handle) {
-            return client;
-          }
-        }
-      }
-    }
-    return rclcpp::client::ClientBase::SharedPtr();
-  }
-
   rclcpp::node::Node::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
   {
@@ -633,7 +445,7 @@ protected:
       if (!node) {
         continue;
       }
-      for (auto & weak_group : node->callback_groups_) {
+      for (auto & weak_group : node->get_callback_groups()) {
         auto callback_group = weak_group.lock();
         if (callback_group == group) {
           return node;
@@ -652,12 +464,12 @@ protected:
       if (!node) {
         continue;
       }
-      for (auto & weak_group : node->callback_groups_) {
+      for (auto & weak_group : node->get_callback_groups()) {
         auto group = weak_group.lock();
         if (!group) {
           continue;
         }
-        for (auto & weak_timer : group->timer_ptrs_) {
+        for (auto & weak_timer : group->get_timer_ptrs()) {
           auto t = weak_timer.lock();
           if (t == timer) {
             return group;
@@ -676,12 +488,12 @@ protected:
       if (!node) {
         continue;
       }
-      for (auto & weak_group : node->callback_groups_) {
+      for (auto & weak_group : node->get_callback_groups()) {
         auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from_.load()) {
+        if (!group || !group->can_be_taken_from().load()) {
           continue;
         }
-        for (auto & timer_ref : group->timer_ptrs_) {
+        for (auto & timer_ref : group->get_timer_ptrs()) {
           auto timer = timer_ref.lock();
           if (timer && timer->check_and_trigger()) {
             any_exec->timer = timer;
@@ -704,12 +516,12 @@ protected:
       if (!node) {
         continue;
       }
-      for (auto & weak_group : node->callback_groups_) {
+      for (auto & weak_group : node->get_callback_groups()) {
         auto group = weak_group.lock();
-        if (!group || !group->can_be_taken_from_.load()) {
+        if (!group || !group->can_be_taken_from().load()) {
           continue;
         }
-        for (auto & timer_ref : group->timer_ptrs_) {
+        for (auto & timer_ref : group->get_timer_ptrs()) {
           timers_empty = false;
           // Check the expected trigger time
           auto timer = timer_ref.lock();
@@ -725,213 +537,32 @@ protected:
     return latest;
   }
 
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & weak_sub : group->subscription_ptrs_) {
-          auto sub = weak_sub.lock();
-          if (sub == subscription) {
-            return group;
-          }
-        }
-      }
-    }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
-
-  void
-  get_next_subscription(AnyExecutable::SharedPtr any_exec)
-  {
-    auto it = subscriber_handles_.begin();
-    while (it != subscriber_handles_.end()) {
-      auto subscription = get_subscription_by_handle(*it);
-      if (subscription) {
-        // Figure out if this is for intra-process or not.
-        bool is_intra_process = false;
-        if (subscription->intra_process_subscription_handle_) {
-          is_intra_process = subscription->intra_process_subscription_handle_->data == *it;
-        }
-        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_subscription(subscription);
-        if (!group) {
-          // Group was not found, meaning the subscription is not valid...
-          // Remove it from the ready list and continue looking
-          subscriber_handles_.erase(it++);
-          continue;
-        }
-        if (!group->can_be_taken_from_.load()) {
-          // Group is mutually exclusive and is being used, so skip it for now
-          // Leave it to be checked next time, but continue searching
-          ++it;
-          continue;
-        }
-        // Otherwise it is safe to set and return the any_exec
-        if (is_intra_process) {
-          any_exec->subscription_intra_process = subscription;
-        } else {
-          any_exec->subscription = subscription;
-        }
-        any_exec->callback_group = group;
-        any_exec->node = get_node_by_group(group);
-        subscriber_handles_.erase(it++);
-        return;
-      }
-      // Else, the subscription is no longer valid, remove it and continue
-      subscriber_handles_.erase(it++);
-    }
-  }
-
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_service(
-    rclcpp::service::ServiceBase::SharedPtr service)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & serv : group->service_ptrs_) {
-          if (serv == service) {
-            return group;
-          }
-        }
-      }
-    }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
-
-  void
-  get_next_service(AnyExecutable::SharedPtr any_exec)
-  {
-    auto it = service_handles_.begin();
-    while (it != service_handles_.end()) {
-      auto service = get_service_by_handle(*it);
-      if (service) {
-        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_service(service);
-        if (!group) {
-          // Group was not found, meaning the service is not valid...
-          // Remove it from the ready list and continue looking
-          service_handles_.erase(it++);
-          continue;
-        }
-        if (!group->can_be_taken_from_.load()) {
-          // Group is mutually exclusive and is being used, so skip it for now
-          // Leave it to be checked next time, but continue searching
-          ++it;
-          continue;
-        }
-        // Otherwise it is safe to set and return the any_exec
-        any_exec->service = service;
-        any_exec->callback_group = group;
-        any_exec->node = get_node_by_group(group);
-        service_handles_.erase(it++);
-        return;
-      }
-      // Else, the service is no longer valid, remove it and continue
-      service_handles_.erase(it++);
-    }
-  }
-
-  rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_client(
-    rclcpp::client::ClientBase::SharedPtr client)
-  {
-    for (auto & weak_node : weak_nodes_) {
-      auto node = weak_node.lock();
-      if (!node) {
-        continue;
-      }
-      for (auto & weak_group : node->callback_groups_) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & cli : group->client_ptrs_) {
-          if (cli == client) {
-            return group;
-          }
-        }
-      }
-    }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
-
-  void
-  get_next_client(AnyExecutable::SharedPtr any_exec)
-  {
-    auto it = client_handles_.begin();
-    while (it != client_handles_.end()) {
-      auto client = get_client_by_handle(*it);
-      if (client) {
-        // Find the group for this handle and see if it can be serviced
-        auto group = get_group_by_client(client);
-        if (!group) {
-          // Group was not found, meaning the service is not valid...
-          // Remove it from the ready list and continue looking
-          client_handles_.erase(it++);
-          continue;
-        }
-        if (!group->can_be_taken_from_.load()) {
-          // Group is mutually exclusive and is being used, so skip it for now
-          // Leave it to be checked next time, but continue searching
-          ++it;
-          continue;
-        }
-        // Otherwise it is safe to set and return the any_exec
-        any_exec->client = client;
-        any_exec->callback_group = group;
-        any_exec->node = get_node_by_group(group);
-        client_handles_.erase(it++);
-        return;
-      }
-      // Else, the service is no longer valid, remove it and continue
-      client_handles_.erase(it++);
-    }
-  }
-
   AnyExecutable::SharedPtr
   get_next_ready_executable()
   {
-    auto any_exec = this->memory_strategy_->instantiate_next_executable();
+    auto any_exec = memory_strategy_->instantiate_next_executable();
     // Check the timers to see if there are any that are ready, if so return
     get_next_timer(any_exec);
     if (any_exec->timer) {
       return any_exec;
     }
     // Check the subscriptions to see if there are any that are ready
-    get_next_subscription(any_exec);
+    memory_strategy_->get_next_subscription(any_exec, weak_nodes_);
     if (any_exec->subscription || any_exec->subscription_intra_process) {
       return any_exec;
     }
     // Check the services to see if there are any that are ready
-    get_next_service(any_exec);
+    memory_strategy_->get_next_service(any_exec, weak_nodes_);
     if (any_exec->service) {
       return any_exec;
     }
     // Check the clients to see if there are any that are ready
-    get_next_client(any_exec);
+    memory_strategy_->get_next_client(any_exec, weak_nodes_);
     if (any_exec->client) {
       return any_exec;
     }
     // If there is no ready executable, return a null ptr
-    return std::shared_ptr<AnyExecutable>();
+    return nullptr;
   }
 
   template<typename T = std::milli>
@@ -954,13 +585,13 @@ protected:
     if (any_exec) {
       // If it is valid, check to see if the group is mutually exclusive or
       // not, then mark it accordingly
-      if (any_exec->callback_group && any_exec->callback_group->type_ == \
+      if (any_exec->callback_group && any_exec->callback_group->type() == \
         callback_group::CallbackGroupType::MutuallyExclusive)
       {
         // It should not have been taken otherwise
-        assert(any_exec->callback_group->can_be_taken_from_.load());
+        assert(any_exec->callback_group->can_be_taken_from().load());
         // Set to false to indicate something is being run from this group
-        any_exec->callback_group->can_be_taken_from_.store(false);
+        any_exec->callback_group->can_be_taken_from().store(false);
       }
     }
     return any_exec;
@@ -976,16 +607,10 @@ private:
   RCLCPP_DISABLE_COPY(Executor);
 
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
-  using SubscriberHandles = std::list<void *>;
-  SubscriberHandles subscriber_handles_;
-  using ServiceHandles = std::list<void *>;
-  ServiceHandles service_handles_;
-  using ClientHandles = std::list<void *>;
-  ClientHandles client_handles_;
-
+  std::array<void *, 2> guard_cond_handles_;
 };
 
-} /* executor */
-} /* rclcpp */
+}  // namespace executor
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */
+#endif  // RCLCPP__EXECUTOR_HPP_

rclcpp/include/rclcpp/executors.hpp

@@ -29,20 +29,56 @@ namespace executors
 using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
 using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
 
-template<typename FutureT>
-std::shared_future<FutureT> &
+/// Return codes to be used with spin_until_future_complete.
+/**
+ * SUCCESS: The future is complete and can be accessed with "get" without blocking.
+ * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error.
+ * TIMEOUT: Spinning timed out.
+ */
+enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT};
+
+/// Spin (blocking) until the future is complete, until the function times out (if applicable),
+/// or until rclcpp is interrupted.
+/**
+ * \param[in] executor The executor which will spin the node.
+ * \param[in] node_ptr The node to spin.
+ * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function
+ * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once.
+   -1 is block forever, 0 is non-blocking.
+   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code.
+ * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT.
+ */
+template<typename ResponseT, typename TimeT = std::milli>
+FutureReturnCode
 spin_node_until_future_complete(
   rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr,
-  std::shared_future<FutureT> & future)
+  std::shared_future<ResponseT> & future,
+  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
 {
-  std::future_status status;
   // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete
   // inside a callback executed by an executor.
-  do {
-    executor.spin_node_once(node_ptr);
+
+  // Check the future before entering the while loop.
+  // If the future is already complete, don't try to spin.
+  std::future_status status = future.wait_for(std::chrono::seconds(0));
+
+  auto start_time = std::chrono::system_clock::now();
+
+  while (status != std::future_status::ready && rclcpp::utilities::ok()) {
+    executor.spin_node_once(node_ptr, timeout);
+    if (timeout.count() >= 0) {
+      if (start_time + timeout < std::chrono::system_clock::now()) {
+        return TIMEOUT;
+      }
+    }
     status = future.wait_for(std::chrono::seconds(0));
-  } while (status != std::future_status::ready && rclcpp::utilities::ok());
-  return future;
+  }
+
+  // If the future completed, and we weren't interrupted by ctrl-C, return the response
+  if (status == std::future_status::ready) {
+    return FutureReturnCode::SUCCESS;
+  }
+  return FutureReturnCode::INTERRUPTED;
 }
 
 } // namespace executors

rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp

@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
-#define RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_
+#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
+#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_
+
+#include <rmw/rmw.h>
 
 #include <cassert>
 #include <cstdlib>
@@ -21,12 +23,10 @@
 #include <mutex>
 #include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executor.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/utilities.hpp>
+#include "rclcpp/executor.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
 
 namespace rclcpp
 {
@@ -41,7 +41,7 @@ public:
   RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor);
 
   MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategy::create_default_strategy())
+    memory_strategies::create_default_strategy())
   : executor::Executor(ms)
   {
     number_of_threads_ = std::thread::hardware_concurrency();
@@ -97,11 +97,10 @@ private:
 
   std::mutex wait_mutex_;
   size_t number_of_threads_;
-
 };
 
-} /* namespace multi_threaded_executor */
-} /* namespace executors */
-} /* namespace rclcpp */
+}  // namespace multi_threaded_executor
+}  // namespace executors
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ */
+#endif  // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_

rclcpp/include/rclcpp/executors/single_threaded_executor.hpp

@@ -12,22 +12,22 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
-#define RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_
+#ifndef RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
+#define RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
+
+#include <rmw/rmw.h>
 
 #include <cassert>
 #include <cstdlib>
 #include <memory>
 #include <vector>
 
-#include <rmw/rmw.h>
-
-#include <rclcpp/executor.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/memory_strategies.hpp>
-#include <rclcpp/node.hpp>
-#include <rclcpp/utilities.hpp>
-#include <rclcpp/rate.hpp>
+#include "rclcpp/executor.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/rate.hpp"
 
 namespace rclcpp
 {
@@ -45,7 +45,7 @@ public:
 
   /// Default constructor. See the default constructor for Executor.
   SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms =
-    memory_strategy::create_default_strategy())
+    memory_strategies::create_default_strategy())
   : executor::Executor(ms) {}
 
   /// Default destrcutor.
@@ -64,11 +64,10 @@ public:
 
 private:
   RCLCPP_DISABLE_COPY(SingleThreadedExecutor);
-
 };
 
-} /* namespace single_threaded_executor */
-} /* namespace executors */
-} /* namespace rclcpp */
+}  // namespace single_threaded_executor
+}  // namespace executors
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ */
+#endif  // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_

rclcpp/include/rclcpp/function_traits.hpp

@@ -25,39 +25,76 @@ namespace rclcpp
  * but unfortunately std::function's constructor on VS2015 is too greedy,
  * so we need a mechanism for checking the arity and the type of each argument
  * in a callback function.
+ * See http://blogs.msdn.com/b/vcblog/archive/2015/06/19/c-11-14-17-features-in-vs-2015-rtm.aspx
  */
 
+// Remove the first item in a tuple
+template<typename T>
+struct tuple_tail;
 
+template<typename Head, typename ... Tail>
+struct tuple_tail<std::tuple<Head, Tail ...>>
+{
+  using type = std::tuple<Tail ...>;
+};
+
+// std::function
 template<typename FunctionT>
 struct function_traits
 {
-  static constexpr std::size_t arity =
-    function_traits<decltype( & FunctionT::operator())>::arity - 1;
+  using arguments = typename tuple_tail<
+      typename function_traits<decltype( & FunctionT::operator())>::arguments>::type;
 
+  static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
   template<std::size_t N>
-  using argument_type =
-      typename function_traits<decltype( & FunctionT::operator())>::template argument_type<N + 1>;
+  using argument_type = typename std::tuple_element<N, arguments>::type;
 };
 
+// Free functions
 template<typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT(Args ...)>
 {
-  static constexpr std::size_t arity = sizeof ... (Args);
+  using arguments = std::tuple<Args ...>;
+
+  static constexpr std::size_t arity = std::tuple_size<arguments>::value;
 
   template<std::size_t N>
-  using argument_type = typename std::tuple_element<N, std::tuple<Args ...>>::type;
+  using argument_type = typename std::tuple_element<N, arguments>::type;
 };
 
+// Function pointers
 template<typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (*)(Args ...)>: public function_traits<ReturnTypeT(Args ...)>
 {};
 
+// Lambdas
 template<typename ClassT, typename ReturnTypeT, typename ... Args>
 struct function_traits<ReturnTypeT (ClassT::*)(Args ...) const>
   : public function_traits<ReturnTypeT(ClassT &, Args ...)>
 {};
 
+/* NOTE(esteve):
+ * VS2015 does not support expression SFINAE, so we're using this template to evaluate
+ * the arity of a function.
+ */
+template<std::size_t Arity, typename FunctorT>
+struct arity_comparator : std::integral_constant<
+    bool, (Arity == function_traits<FunctorT>::arity)>{};
+
+template<typename FunctorT, typename ... Args>
+struct check_arguments : std::is_same<
+    typename function_traits<FunctorT>::arguments,
+    std::tuple<Args ...>
+  >
+{};
+
+template<typename FunctorAT, typename FunctorBT>
+struct same_arguments : std::is_same<
+    typename function_traits<FunctorAT>::arguments,
+    typename function_traits<FunctorBT>::arguments
+  >
+{};
 } /* namespace rclcpp */
 
 #endif /* RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ */

rclcpp/include/rclcpp/intra_process_manager.hpp

@@ -12,24 +12,26 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_
-#define RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_
+#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_
+#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_
 
-#include <rclcpp/mapped_ring_buffer.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/subscription.hpp>
+#include <rmw/types.h>
 
 #include <algorithm>
 #include <atomic>
 #include <cstdint>
 #include <exception>
-#include <limits>
 #include <map>
+#include <memory>
 #include <unordered_map>
 #include <set>
 
-#include <rmw/types.h>
+#include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/intra_process_manager_state.hpp"
+#include "rclcpp/mapped_ring_buffer.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/subscription.hpp"
 
 namespace rclcpp
 {
@@ -122,7 +124,12 @@ private:
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
 
-  IntraProcessManager() = default;
+  explicit IntraProcessManager(
+    IntraProcessManagerStateBase::SharedPtr state = create_default_state()
+  )
+  : state_(state)
+  {
+  }
 
   /// Register a subscription with the manager, returns subscriptions unique id.
   /* In addition to generating a unique intra process id for the subscription,
@@ -140,8 +147,7 @@ public:
   add_subscription(subscription::SubscriptionBase::SharedPtr subscription)
   {
     auto id = IntraProcessManager::get_next_unique_id();
-    subscriptions_[id] = subscription;
-    subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
+    state_->add_subscription(id, subscription);
     return id;
   }
 
@@ -153,17 +159,7 @@ public:
   void
   remove_subscription(uint64_t intra_process_subscription_id)
   {
-    subscriptions_.erase(intra_process_subscription_id);
-    for (auto & pair : subscription_ids_by_topic_) {
-      pair.second.erase(intra_process_subscription_id);
-    }
-    // Iterate over all publisher infos and all stored subscription id's and
-    // remove references to this subscription's id.
-    for (auto & publisher_pair : publishers_) {
-      for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) {
-        sub_pair.second.erase(intra_process_subscription_id);
-      }
-    }
+    state_->remove_subscription(intra_process_subscription_id);
   }
 
   /// Register a publisher with the manager, returns the publisher unique id.
@@ -188,20 +184,17 @@ public:
    * \param buffer_size if 0 (default) a size is calculated based on the QoS.
    * \return an unsigned 64-bit integer which is the publisher's unique id.
    */
-  template<typename MessageT>
+  template<typename MessageT, typename Alloc>
   uint64_t
-  add_publisher(publisher::Publisher::SharedPtr publisher, size_t buffer_size = 0)
+  add_publisher(typename publisher::Publisher<MessageT, Alloc>::SharedPtr publisher,
+    size_t buffer_size = 0)
   {
     auto id = IntraProcessManager::get_next_unique_id();
-    publishers_[id].publisher = publisher;
     size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size();
-    // As long as the size of the ring buffer is less than the max sequence number, we're safe.
-    if (size > std::numeric_limits<uint64_t>::max()) {
-      throw std::invalid_argument("the calculated buffer size is too large");
-    }
-    publishers_[id].sequence_number.store(0);
-    publishers_[id].buffer = mapped_ring_buffer::MappedRingBuffer<MessageT>::make_shared(size);
-    publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
+    auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
+      typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
+      size, publisher->get_allocator());
+    state_->add_publisher(id, publisher, mrb, size);
     return id;
   }
 
@@ -213,7 +206,7 @@ public:
   void
   remove_publisher(uint64_t intra_process_publisher_id)
   {
-    publishers_.erase(intra_process_publisher_id);
+    state_->remove_publisher(intra_process_publisher_id);
   }
 
   /// Store a message in the manager, and return the message sequence number.
@@ -246,42 +239,30 @@ public:
    * \param message the message that is being stored.
    * \return the message sequence number.
    */
-  template<typename MessageT>
+  template<typename MessageT, typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>>
   uint64_t
   store_intra_process_message(
     uint64_t intra_process_publisher_id,
-    std::unique_ptr<MessageT> & message)
+    std::unique_ptr<MessageT, Deleter> & message)
   {
-    auto it = publishers_.find(intra_process_publisher_id);
-    if (it == publishers_.end()) {
-      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
+    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
+    using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
+    uint64_t message_seq = 0;
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id(
+      intra_process_publisher_id, message_seq);
+    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
+    if (!typed_buffer) {
+      throw std::runtime_error("Typecast failed due to incorrect message type");
     }
-    PublisherInfo & info = it->second;
-    // Calculate the next message sequence number.
-    uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
+
     // Insert the message into the ring buffer using the message_seq to identify it.
-    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT> TypedMRB;
-    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info.buffer);
     bool did_replace = typed_buffer->push_and_replace(message_seq, message);
     // TODO(wjwwood): do something when a message was displaced. log debug?
     (void)did_replace;  // Avoid unused variable warning.
-    // Figure out what subscriptions should receive the message.
-    auto publisher = info.publisher.lock();
-    if (!publisher) {
-      throw std::runtime_error("publisher has unexpectedly gone out of scope");
-    }
-    auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
-    // Store the list for later comparison.
-    info.target_subscriptions_by_message_sequence[message_seq].clear();
-    std::copy(
-      destined_subscriptions.begin(), destined_subscriptions.end(),
-      // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
-      std::inserter(
-        info.target_subscriptions_by_message_sequence[message_seq],
-        // This ends up only being a hint to std::set, could also be .begin().
-        info.target_subscriptions_by_message_sequence[message_seq].end()
-      )
-    );
+
+    state_->store_intra_process_message(intra_process_publisher_id, message_seq);
+
     // Return the message sequence which is sent to the subscription.
     return message_seq;
   }
@@ -320,48 +301,32 @@ public:
    * \param requesting_subscriptions_intra_process_id the subscription's id.
    * \param message the message typed unique_ptr used to return the message.
    */
-  template<typename MessageT>
+  template<typename MessageT, typename Alloc = std::allocator<void>,
+  typename Deleter = std::default_delete<MessageT>>
   void
   take_intra_process_message(
     uint64_t intra_process_publisher_id,
     uint64_t message_sequence_number,
     uint64_t requesting_subscriptions_intra_process_id,
-    std::unique_ptr<MessageT> & message)
+    std::unique_ptr<MessageT, Deleter> & message)
   {
+    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
+    using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
     message = nullptr;
-    PublisherInfo * info;
-    {
-      auto it = publishers_.find(intra_process_publisher_id);
-      if (it == publishers_.end()) {
-        // Publisher is either invalid or no longer exists.
-        return;
-      }
-      info = &it->second;
+
+    size_t target_subs_size = 0;
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message(
+      intra_process_publisher_id,
+      message_sequence_number,
+      requesting_subscriptions_intra_process_id,
+      target_subs_size
+      );
+    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
+    if (!typed_buffer) {
+      return;
     }
-    // Figure out how many subscriptions are left.
-    std::set<uint64_t> * target_subs;
-    {
-      auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number);
-      if (it == info->target_subscriptions_by_message_sequence.end()) {
-        // Message is no longer being stored by this publisher.
-        return;
-      }
-      target_subs = &it->second;
-    }
-    {
-      auto it = std::find(
-        target_subs->begin(), target_subs->end(),
-        requesting_subscriptions_intra_process_id);
-      if (it == target_subs->end()) {
-        // This publisher id/message seq pair was not intended for this subscription.
-        return;
-      }
-      target_subs->erase(it);
-    }
-    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT> TypedMRB;
-    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info->buffer);
     // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left.
-    if (target_subs->size()) {
+    if (target_subs_size) {
       // There are more subscriptions to serve, return a copy.
       typed_buffer->get_copy_at_key(message_sequence_number, message);
     } else {
@@ -374,16 +339,7 @@ public:
   bool
   matches_any_publishers(const rmw_gid_t * id) const
   {
-    for (auto & publisher_pair : publishers_) {
-      auto publisher = publisher_pair.second.publisher.lock();
-      if (!publisher) {
-        continue;
-      }
-      if (*publisher.get() == id) {
-        return true;
-      }
-    }
-    return false;
+    return state_->matches_any_publishers(id);
   }
 
 private:
@@ -410,28 +366,12 @@ private:
 
   static std::atomic<uint64_t> next_unique_id_;
 
-  std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr> subscriptions_;
-  std::map<std::string, std::set<uint64_t>> subscription_ids_by_topic_;
-
-  struct PublisherInfo
-  {
-    RCLCPP_DISABLE_COPY(PublisherInfo);
-
-    PublisherInfo() = default;
-
-    publisher::Publisher::WeakPtr publisher;
-    std::atomic<uint64_t> sequence_number;
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
-    std::unordered_map<uint64_t, std::set<uint64_t>> target_subscriptions_by_message_sequence;
-  };
-
-  std::unordered_map<uint64_t, PublisherInfo> publishers_;
-
+  IntraProcessManagerStateBase::SharedPtr state_;
 };
 
 std::atomic<uint64_t> IntraProcessManager::next_unique_id_ {1};
 
-} /* namespace intra_process_manager */
-} /* namespace rclcpp */
+}  // namespace intra_process_manager
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_ */
+#endif  // RCLCPP__INTRA_PROCESS_MANAGER_HPP_

rclcpp/include/rclcpp/intra_process_manager_state.hpp

@@ -0,0 +1,270 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
+#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
+
+#include <rclcpp/mapped_ring_buffer.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include <algorithm>
+#include <atomic>
+#include <functional>
+#include <limits>
+#include <memory>
+#include <map>
+#include <unordered_map>
+#include <set>
+#include <string>
+#include <utility>
+
+namespace rclcpp
+{
+namespace intra_process_manager
+{
+
+class IntraProcessManagerStateBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
+
+  virtual void
+  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
+
+  virtual void
+  remove_subscription(uint64_t intra_process_subscription_id) = 0;
+
+  virtual void add_publisher(uint64_t id,
+    publisher::PublisherBase::WeakPtr publisher,
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
+    size_t size) = 0;
+
+  virtual void
+  remove_publisher(uint64_t intra_process_publisher_id) = 0;
+
+  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
+  get_publisher_info_for_id(
+    uint64_t intra_process_publisher_id,
+    uint64_t & message_seq) = 0;
+
+  virtual void
+  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
+
+  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
+  take_intra_process_message(uint64_t intra_process_publisher_id,
+    uint64_t message_sequence_number,
+    uint64_t requesting_subscriptions_intra_process_id,
+    size_t & size) = 0;
+
+  virtual bool
+  matches_any_publishers(const rmw_gid_t * id) const = 0;
+};
+
+template<typename Allocator = std::allocator<void>>
+class IntraProcessManagerState : public IntraProcessManagerStateBase
+{
+public:
+  void
+  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription)
+  {
+    subscriptions_[id] = subscription;
+    subscription_ids_by_topic_[subscription->get_topic_name()].insert(id);
+  }
+
+  void
+  remove_subscription(uint64_t intra_process_subscription_id)
+  {
+    subscriptions_.erase(intra_process_subscription_id);
+    for (auto & pair : subscription_ids_by_topic_) {
+      pair.second.erase(intra_process_subscription_id);
+    }
+    // Iterate over all publisher infos and all stored subscription id's and
+    // remove references to this subscription's id.
+    for (auto & publisher_pair : publishers_) {
+      for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) {
+        sub_pair.second.erase(intra_process_subscription_id);
+      }
+    }
+  }
+
+  void add_publisher(uint64_t id,
+    publisher::PublisherBase::WeakPtr publisher,
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
+    size_t size)
+  {
+    publishers_[id].publisher = publisher;
+    // As long as the size of the ring buffer is less than the max sequence number, we're safe.
+    if (size > std::numeric_limits<uint64_t>::max()) {
+      throw std::invalid_argument("the calculated buffer size is too large");
+    }
+    publishers_[id].sequence_number.store(0);
+
+    publishers_[id].buffer = mrb;
+    publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
+  }
+
+  void
+  remove_publisher(uint64_t intra_process_publisher_id)
+  {
+    publishers_.erase(intra_process_publisher_id);
+  }
+
+  // return message_seq and mrb
+  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
+  get_publisher_info_for_id(
+    uint64_t intra_process_publisher_id,
+    uint64_t & message_seq)
+  {
+    auto it = publishers_.find(intra_process_publisher_id);
+    if (it == publishers_.end()) {
+      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
+    }
+    PublisherInfo & info = it->second;
+    // Calculate the next message sequence number.
+    message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
+
+    return info.buffer;
+  }
+
+  void
+  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq)
+  {
+    auto it = publishers_.find(intra_process_publisher_id);
+    if (it == publishers_.end()) {
+      throw std::runtime_error("store_intra_process_message called with invalid publisher id");
+    }
+    PublisherInfo & info = it->second;
+    auto publisher = info.publisher.lock();
+    if (!publisher) {
+      throw std::runtime_error("publisher has unexpectedly gone out of scope");
+    }
+
+    // Figure out what subscriptions should receive the message.
+    auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()];
+    // Store the list for later comparison.
+    info.target_subscriptions_by_message_sequence[message_seq].clear();
+    std::copy(
+      destined_subscriptions.begin(), destined_subscriptions.end(),
+      // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq]
+      std::inserter(
+        info.target_subscriptions_by_message_sequence[message_seq],
+        // This ends up only being a hint to std::set, could also be .begin().
+        info.target_subscriptions_by_message_sequence[message_seq].end()
+      )
+    );
+  }
+
+  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
+  take_intra_process_message(uint64_t intra_process_publisher_id,
+    uint64_t message_sequence_number,
+    uint64_t requesting_subscriptions_intra_process_id,
+    size_t & size
+  )
+  {
+    PublisherInfo * info;
+    {
+      auto it = publishers_.find(intra_process_publisher_id);
+      if (it == publishers_.end()) {
+        // Publisher is either invalid or no longer exists.
+        return 0;
+      }
+      info = &it->second;
+    }
+    // Figure out how many subscriptions are left.
+    AllocSet * target_subs;
+    {
+      auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number);
+      if (it == info->target_subscriptions_by_message_sequence.end()) {
+        // Message is no longer being stored by this publisher.
+        return 0;
+      }
+      target_subs = &it->second;
+    }
+    {
+      auto it = std::find(
+        target_subs->begin(), target_subs->end(),
+        requesting_subscriptions_intra_process_id);
+      if (it == target_subs->end()) {
+        // This publisher id/message seq pair was not intended for this subscription.
+        return 0;
+      }
+      target_subs->erase(it);
+    }
+    size = target_subs->size();
+    return info->buffer;
+  }
+
+  bool
+  matches_any_publishers(const rmw_gid_t * id) const
+  {
+    for (auto & publisher_pair : publishers_) {
+      auto publisher = publisher_pair.second.publisher.lock();
+      if (!publisher) {
+        continue;
+      }
+      if (*publisher.get() == id) {
+        return true;
+      }
+    }
+    return false;
+  }
+
+private:
+  template<typename T>
+  using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
+
+  using AllocSet = std::set<uint64_t, std::less<uint64_t>, RebindAlloc<uint64_t>>;
+  using SubscriptionMap = std::unordered_map<uint64_t, subscription::SubscriptionBase::WeakPtr,
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
+  using IDTopicMap = std::map<std::string, AllocSet,
+      std::less<std::string>, RebindAlloc<std::pair<std::string, AllocSet>>>;
+
+  SubscriptionMap subscriptions_;
+
+  IDTopicMap subscription_ids_by_topic_;
+
+  struct PublisherInfo
+  {
+    RCLCPP_DISABLE_COPY(PublisherInfo);
+
+    PublisherInfo() = default;
+
+    publisher::PublisherBase::WeakPtr publisher;
+    std::atomic<uint64_t> sequence_number;
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
+
+    using TargetSubscriptionsMap = std::unordered_map<uint64_t, AllocSet,
+        std::hash<uint64_t>, std::equal_to<uint64_t>,
+        RebindAlloc<std::pair<const uint64_t, AllocSet>>>;
+    TargetSubscriptionsMap target_subscriptions_by_message_sequence;
+  };
+
+  using PublisherMap = std::unordered_map<uint64_t, PublisherInfo,
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
+
+  PublisherMap publishers_;
+};
+
+static IntraProcessManagerStateBase::SharedPtr create_default_state()
+{
+  return std::make_shared<IntraProcessManagerState<>>();
+}
+
+}  // namespace intra_process_manager
+}  // namespace rclcpp
+
+#endif  // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_

rclcpp/include/rclcpp/macros.hpp

@@ -19,19 +19,19 @@
  *
  * Use in the private section of the class.
  */
-#define RCLCPP_DISABLE_COPY(Class) \
-  Class(const Class &) = delete; \
-  Class & operator=(const Class &) = delete;
+#define RCLCPP_DISABLE_COPY(...) \
+  __VA_ARGS__(const __VA_ARGS__ &) = delete; \
+  __VA_ARGS__ & operator=(const __VA_ARGS__ &) = delete;
 
 /* Defines aliases and static functions for using the Class with smart pointers.
  *
  * Use in the public section of the class.
  * Make sure to include <memory> in the header when using this.
  */
-#define RCLCPP_SMART_PTR_DEFINITIONS(Class) \
-  RCLCPP_SHARED_PTR_DEFINITIONS(Class) \
-  RCLCPP_WEAK_PTR_DEFINITIONS(Class) \
-  RCLCPP_UNIQUE_PTR_DEFINITIONS(Class)
+#define RCLCPP_SMART_PTR_DEFINITIONS(...) \
+  RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
+  RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
+  RCLCPP_UNIQUE_PTR_DEFINITIONS(__VA_ARGS__)
 
 /* Defines aliases and static functions for using the Class with smart pointers.
  *
@@ -42,44 +42,44 @@
  * Use in the public section of the class.
  * Make sure to include <memory> in the header when using this.
  */
-#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Class) \
-  RCLCPP_SHARED_PTR_DEFINITIONS(Class) \
-  RCLCPP_WEAK_PTR_DEFINITIONS(Class) \
-  __RCLCPP_UNIQUE_PTR_ALIAS(Class)
+#define RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(...) \
+  RCLCPP_SHARED_PTR_DEFINITIONS(__VA_ARGS__) \
+  RCLCPP_WEAK_PTR_DEFINITIONS(__VA_ARGS__) \
+  __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
-#define __RCLCPP_SHARED_PTR_ALIAS(Class) using SharedPtr = std::shared_ptr<Class>;
+#define __RCLCPP_SHARED_PTR_ALIAS(...) using SharedPtr = std::shared_ptr<__VA_ARGS__>;
 
-#define __RCLCPP_MAKE_SHARED_DEFINITION(Class) \
+#define __RCLCPP_MAKE_SHARED_DEFINITION(...) \
   template<typename ... Args> \
-  static std::shared_ptr<Class> \
+  static std::shared_ptr<__VA_ARGS__> \
   make_shared(Args && ... args) \
   { \
-    return std::make_shared<Class>(std::forward<Args>(args) ...); \
+    return std::make_shared<__VA_ARGS__>(std::forward<Args>(args) ...); \
   }
 
 /// Defines aliases and static functions for using the Class with shared_ptrs.
-#define RCLCPP_SHARED_PTR_DEFINITIONS(Class) \
-  __RCLCPP_SHARED_PTR_ALIAS(Class) \
-  __RCLCPP_MAKE_SHARED_DEFINITION(Class)
+#define RCLCPP_SHARED_PTR_DEFINITIONS(...) \
+  __RCLCPP_SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_MAKE_SHARED_DEFINITION(__VA_ARGS__)
 
-#define __RCLCPP_WEAK_PTR_ALIAS(Class) using WeakPtr = std::weak_ptr<Class>;
+#define __RCLCPP_WEAK_PTR_ALIAS(...) using WeakPtr = std::weak_ptr<__VA_ARGS__>;
 
 /// Defines aliases and static functions for using the Class with weak_ptrs.
-#define RCLCPP_WEAK_PTR_DEFINITIONS(Class) __RCLCPP_WEAK_PTR_ALIAS(Class)
+#define RCLCPP_WEAK_PTR_DEFINITIONS(...) __RCLCPP_WEAK_PTR_ALIAS(__VA_ARGS__)
 
-#define __RCLCPP_UNIQUE_PTR_ALIAS(Class) using UniquePtr = std::unique_ptr<Class>;
+#define __RCLCPP_UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>;
 
-#define __RCLCPP_MAKE_UNIQUE_DEFINITION(Class) \
+#define __RCLCPP_MAKE_UNIQUE_DEFINITION(...) \
   template<typename ... Args> \
-  static std::unique_ptr<Class> \
+  static std::unique_ptr<__VA_ARGS__> \
   make_unique(Args && ... args) \
   { \
-    return std::unique_ptr<Class>(new Class(std::forward<Args>(args) ...)); \
+    return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
   }
 /// Defines aliases and static functions for using the Class with unique_ptrs.
-#define RCLCPP_UNIQUE_PTR_DEFINITIONS(Class) \
-  __RCLCPP_UNIQUE_PTR_ALIAS(Class) \
-  __RCLCPP_MAKE_UNIQUE_DEFINITION(Class)
+#define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \
+  __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
+  __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 
 #define RCLCPP_INFO(Args) std::cout << Args << std::endl;
 

rclcpp/include/rclcpp/mapped_ring_buffer.hpp

@@ -12,10 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_RING_BUFFER_HPP_
-#define RCLCPP_RCLCPP_RING_BUFFER_HPP_
-
-#include <rclcpp/macros.hpp>
+#ifndef RCLCPP__MAPPED_RING_BUFFER_HPP_
+#define RCLCPP__MAPPED_RING_BUFFER_HPP_
 
 #include <algorithm>
 #include <cstddef>
@@ -23,6 +21,9 @@
 #include <memory>
 #include <vector>
 
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/macros.hpp"
+
 namespace rclcpp
 {
 namespace mapped_ring_buffer
@@ -50,24 +51,35 @@ public:
  * there is no guarantee on which value is returned if a key is used multiple
  * times.
  */
-template<typename T>
+template<typename T, typename Alloc = std::allocator<void>>
 class MappedRingBuffer : public MappedRingBufferBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T>);
+  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Alloc>);
+  using ElemAllocTraits = allocator::AllocRebind<T, Alloc>;
+  using ElemAlloc = typename ElemAllocTraits::allocator_type;
+  using ElemDeleter = allocator::Deleter<ElemAlloc, T>;
+
+  using ElemUniquePtr = std::unique_ptr<T, ElemDeleter>;
 
   /// Constructor.
   /* The constructor will allocate memory while reserving space.
    *
    * \param size size of the ring buffer; must be positive and non-zero.
    */
-  MappedRingBuffer(size_t size)
+  explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
   : elements_(size), head_(0)
   {
     if (size == 0) {
       throw std::invalid_argument("size must be a positive, non-zero value");
     }
+    if (!allocator) {
+      allocator_ = std::make_shared<ElemAlloc>();
+    } else {
+      allocator_ = std::make_shared<ElemAlloc>(*allocator.get());
+    }
   }
+
   virtual ~MappedRingBuffer() {}
 
   /// Return a copy of the value stored in the ring buffer at the given key.
@@ -82,12 +94,14 @@ public:
    * \param value if the key is found, the value is stored in this parameter
    */
   void
-  get_copy_at_key(uint64_t key, std::unique_ptr<T> & value)
+  get_copy_at_key(uint64_t key, ElemUniquePtr & value)
   {
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
-      value = std::unique_ptr<T>(new T(*it->value));
+      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
+      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
+      value = ElemUniquePtr(ptr);
     }
   }
 
@@ -111,13 +125,15 @@ public:
    * \param value if the key is found, the value is stored in this parameter
    */
   void
-  get_ownership_at_key(uint64_t key, std::unique_ptr<T> & value)
+  get_ownership_at_key(uint64_t key, ElemUniquePtr & value)
   {
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
       // Make a copy.
-      auto copy = std::unique_ptr<T>(new T(*it->value));
+      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
+      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
+      auto copy = ElemUniquePtr(ptr);
       // Return the original.
       value.swap(it->value);
       // Store the copy.
@@ -136,7 +152,7 @@ public:
    * \param value if the key is found, the value is stored in this parameter
    */
   void
-  pop_at_key(uint64_t key, std::unique_ptr<T> & value)
+  pop_at_key(uint64_t key, ElemUniquePtr & value)
   {
     auto it = get_iterator_of_key(key);
     value = nullptr;
@@ -158,7 +174,7 @@ public:
    * \param value the value to store, and optionally the value displaced
    */
   bool
-  push_and_replace(uint64_t key, std::unique_ptr<T> & value)
+  push_and_replace(uint64_t key, ElemUniquePtr & value)
   {
     bool did_replace = elements_[head_].in_use;
     elements_[head_].key = key;
@@ -169,9 +185,9 @@ public:
   }
 
   bool
-  push_and_replace(uint64_t key, std::unique_ptr<T> && value)
+  push_and_replace(uint64_t key, ElemUniquePtr && value)
   {
-    std::unique_ptr<T> temp = std::move(value);
+    ElemUniquePtr temp = std::move(value);
     return push_and_replace(key, temp);
   }
 
@@ -183,16 +199,18 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T>);
+  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>);
 
   struct element
   {
     uint64_t key;
-    std::unique_ptr<T> value;
+    ElemUniquePtr value;
     bool in_use;
   };
 
-  typename std::vector<element>::iterator
+  using VectorAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<element>;
+
+  typename std::vector<element, VectorAlloc>::iterator
   get_iterator_of_key(uint64_t key)
   {
     // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
@@ -203,12 +221,12 @@ private:
     return it;
   }
 
-  std::vector<element> elements_;
+  std::vector<element, VectorAlloc> elements_;
   size_t head_;
-
+  std::shared_ptr<ElemAlloc> allocator_;
 };
 
-} /* namespace ring_buffer */
-} /* namespace rclcpp */
+}  // namespace mapped_ring_buffer
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_RING_BUFFER_HPP_ */
+#endif  // RCLCPP__MAPPED_RING_BUFFER_HPP_

rclcpp/include/rclcpp/memory_strategies.hpp

@@ -12,19 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_
-#define RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_
+#ifndef RCLCPP__MEMORY_STRATEGIES_HPP_
+#define RCLCPP__MEMORY_STRATEGIES_HPP_
 
-#include <rclcpp/strategies/static_memory_strategy.hpp>
+#include <rclcpp/memory_strategy.hpp>
+#include <rclcpp/strategies/allocator_memory_strategy.hpp>
 
 namespace rclcpp
 {
 namespace memory_strategies
 {
 
-using rclcpp::memory_strategies::static_memory_strategy::StaticMemoryStrategy;
+using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
 
-}  /* memory_strategies */
-}  /* rclcpp */
+static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
+{
+  return std::make_shared<AllocatorMemoryStrategy<>>();
+}
 
-#endif
+}  // namespace memory_strategies
+}  // namespace rclcpp
+
+#endif  // RCLCPP__MEMORY_STRATEGIES_HPP_

rclcpp/include/rclcpp/memory_strategy.hpp

@@ -12,21 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
-#include <rclcpp/any_executable.hpp>
+#ifndef RCLCPP__MEMORY_STRATEGY_HPP_
+#define RCLCPP__MEMORY_STRATEGY_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/any_executable.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
 
 namespace rclcpp
 {
-
-// TODO move HandleType somewhere where it makes sense
-enum class HandleType {subscription_handle, service_handle, client_handle, guard_condition_handle};
-
-namespace executor
-{
-class Executor;
-}
-
 namespace memory_strategy
 {
 
@@ -38,75 +35,221 @@ namespace memory_strategy
  */
 class MemoryStrategy
 {
-
-  friend class executor::Executor;
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MemoryStrategy);
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
+  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
 
-  /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
-  /**
-   * The default implementation ignores the handle type and dynamically allocates the memory.
-   * \param[in] The type of entity that this function is requesting for.
-   * \param[in] The number of handles to borrow.
-   * \return Pointer to the allocated handles.
-   */
-  virtual void ** borrow_handles(HandleType type, size_t number_of_handles)
-  {
-    (void)type;
-    return static_cast<void **>(alloc(sizeof(void *) * number_of_handles));
-  }
+  // return the new number of subscribers
+  virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
 
-  /// Return the memory borrowed in borrow_handles.
-  /**
-   * return_handles should always mirror the way memory was borrowed in borrow_handles.
-   * \param[in] The type of entity that this function is returning.
-   * \param[in] Pointer to the handles returned.
-   */
-  virtual void return_handles(HandleType type, void ** handles)
-  {
-    (void)type;
-    this->free(handles);
-  }
+  // return the new number of services
+  virtual size_t fill_service_handles(void ** & ptr) = 0;
+
+  // return the new number of clients
+  virtual size_t fill_client_handles(void ** & ptr) = 0;
+
+  virtual void clear_active_entities() = 0;
+
+  virtual void clear_handles() = 0;
+  virtual void revalidate_handles() = 0;
+  virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0;
 
   /// Provide a newly initialized AnyExecutable object.
   // \return Shared pointer to the fresh executable.
-  virtual executor::AnyExecutable::SharedPtr instantiate_next_executable()
-  {
-    return std::make_shared<executor::AnyExecutable>();
-  }
+  virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0;
 
-  /// Implementation of a general-purpose allocation function.
-  /**
-   * \param[in] size Number of bytes to allocate.
-   * \return Pointer to the allocated chunk of memory.
-   */
-  virtual void * alloc(size_t size)
+  virtual void
+  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
+  virtual void
+  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
+  virtual void
+  get_next_client(executor::AnyExecutable::SharedPtr any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
+
+  static rclcpp::subscription::SubscriptionBase::SharedPtr
+  get_subscription_by_handle(void * subscriber_handle,
+    const WeakNodeVector & weak_nodes)
   {
-    if (size == 0) {
-      return NULL;
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group) {
+          continue;
+        }
+        for (auto & weak_subscription : group->get_subscription_ptrs()) {
+          auto subscription = weak_subscription.lock();
+          if (subscription) {
+            if (subscription->get_subscription_handle()->data == subscriber_handle) {
+              return subscription;
+            }
+            if (subscription->get_intra_process_subscription_handle() &&
+              subscription->get_intra_process_subscription_handle()->data == subscriber_handle)
+            {
+              return subscription;
+            }
+          }
+        }
+      }
     }
-    return std::malloc(size);
+    return nullptr;
   }
 
-  /// Implementation of a general-purpose free.
-  /**
-   * \param[in] Pointer to deallocate.
-   */
-  virtual void free(void * ptr)
+  static rclcpp::service::ServiceBase::SharedPtr
+  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
   {
-    return std::free(ptr);
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group) {
+          continue;
+        }
+        for (auto & service : group->get_service_ptrs()) {
+          if (service->get_service_handle()->data == service_handle) {
+            return service;
+          }
+        }
+      }
+    }
+    return nullptr;
   }
+
+  static rclcpp::client::ClientBase::SharedPtr
+  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
+  {
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group) {
+          continue;
+        }
+        for (auto & client : group->get_client_ptrs()) {
+          if (client->get_client_handle()->data == client_handle) {
+            return client;
+          }
+        }
+      }
+    }
+    return nullptr;
+  }
+
+  static rclcpp::node::Node::SharedPtr
+  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
+    const WeakNodeVector & weak_nodes)
+  {
+    if (!group) {
+      return nullptr;
+    }
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto callback_group = weak_group.lock();
+        if (callback_group == group) {
+          return node;
+        }
+      }
+    }
+    return nullptr;
+  }
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_subscription(
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    const WeakNodeVector & weak_nodes)
+  {
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group) {
+          continue;
+        }
+        for (auto & weak_sub : group->get_subscription_ptrs()) {
+          auto sub = weak_sub.lock();
+          if (sub == subscription) {
+            return group;
+          }
+        }
+      }
+    }
+    return nullptr;
+  }
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_service(
+    rclcpp::service::ServiceBase::SharedPtr service,
+    const WeakNodeVector & weak_nodes)
+  {
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group) {
+          continue;
+        }
+        for (auto & serv : group->get_service_ptrs()) {
+          if (serv == service) {
+            return group;
+          }
+        }
+      }
+    }
+    return nullptr;
+  }
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_client(rclcpp::client::ClientBase::SharedPtr client,
+    const WeakNodeVector & weak_nodes)
+  {
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group) {
+          continue;
+        }
+        for (auto & cli : group->get_client_ptrs()) {
+          if (cli == client) {
+            return group;
+          }
+        }
+      }
+    }
+    return nullptr;
+  }
+
+
 };
 
+}  // namespace memory_strategy
 
-MemoryStrategy::SharedPtr create_default_strategy()
-{
-  return std::make_shared<MemoryStrategy>(MemoryStrategy());
-}
+}  // namespace rclcpp
 
-}  /* memory_strategy */
-
-}  /* rclcpp */
-
-#endif
+#endif  // RCLCPP__MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/message_memory_strategy.hpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_
+#ifndef RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
+#define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_
 
 #include <memory>
 
-#include <rclcpp/macros.hpp>
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/macros.hpp"
 
 namespace rclcpp
 {
@@ -26,24 +27,37 @@ namespace message_memory_strategy
 
 /// Default allocation strategy for messages received by subscriptions.
 // A message memory strategy must be templated on the type of the subscription it belongs to.
-template<typename MessageT>
+template<typename MessageT, typename Alloc = std::allocator<void>>
 class MessageMemoryStrategy
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
 
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+
+  MessageMemoryStrategy()
+  {
+    message_allocator_ = std::make_shared<MessageAlloc>();
+  }
+
+  explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
+  {
+    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+  }
+
   /// Default factory method
   static SharedPtr create_default()
   {
-    return SharedPtr(new MessageMemoryStrategy<MessageT>);
+    return std::make_shared<MessageMemoryStrategy<MessageT, Alloc>>(std::make_shared<Alloc>());
   }
 
   /// By default, dynamically allocate a new message.
   // \return Shared pointer to the new message.
   virtual std::shared_ptr<MessageT> borrow_message()
   {
-    return std::shared_ptr<MessageT>(new MessageT);
+    return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
   /// Release ownership of the message, which will deallocate it if it has no more owners.
@@ -52,9 +66,12 @@ public:
   {
     msg.reset();
   }
+
+  std::shared_ptr<MessageAlloc> message_allocator_;
+  MessageDeleter message_deleter_;
 };
 
-}  /* message_memory_strategy */
-}  /* rclcpp */
+}  // namespace message_memory_strategy
+}  // namespace rclcpp
 
-#endif  /* RCLCPP_RCLCPP_MSG_MEMORY_STRATEGY_HPP_ */
+#endif  // RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/node.hpp

@@ -12,55 +12,48 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_NODE_HPP_
-#define RCLCPP_RCLCPP_NODE_HPP_
+#ifndef RCLCPP__NODE_HPP_
+#define RCLCPP__NODE_HPP_
 
 #include <list>
+#include <map>
 #include <memory>
 #include <string>
 #include <tuple>
+#include <vector>
 
-#include <rcl_interfaces/msg/list_parameters_result.hpp>
-#include <rcl_interfaces/msg/parameter_descriptor.hpp>
-#include <rcl_interfaces/msg/parameter_event.hpp>
-#include <rcl_interfaces/msg/set_parameters_result.hpp>
-#include <rosidl_generator_cpp/message_type_support.hpp>
+#include "rcl_interfaces/msg/list_parameters_result.hpp"
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
+#include "rcl_interfaces/msg/parameter_event.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+#include "rosidl_generator_cpp/message_type_support.hpp"
 
-#include <rclcpp/callback_group.hpp>
-#include <rclcpp/client.hpp>
-#include <rclcpp/context.hpp>
-#include <rclcpp/function_traits.hpp>
-#include <rclcpp/macros.hpp>
-#include <rclcpp/message_memory_strategy.hpp>
-#include <rclcpp/parameter.hpp>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/service.hpp>
-#include <rclcpp/subscription.hpp>
-#include <rclcpp/timer.hpp>
+#include "rclcpp/callback_group.hpp"
+#include "rclcpp/client.hpp"
+#include "rclcpp/context.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/parameter.hpp"
+#include "rclcpp/publisher.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp/timer.hpp"
 
 
 // Forward declaration of ROS middleware class
 namespace rmw
 {
 struct rmw_node_t;
-} // namespace rmw
+}  // namespace rmw
 
 namespace rclcpp
 {
 
-// Forward declaration for friend statement
-namespace executor
-{
-class Executor;
-} // namespace executor
-
 namespace node
 {
 /// Node is the single point of entry for creating publishers and subscribers.
 class Node
 {
-  friend class rclcpp::executor::Executor;
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Node);
 
@@ -70,7 +63,7 @@ public:
    * \param[in] use_intra_process_comms True to use the optimized intra-process communication
    * pipeline to pass messages between nodes in the same process using shared memory.
    */
-  Node(const std::string & node_name, bool use_intra_process_comms = false);
+  explicit Node(const std::string & node_name, bool use_intra_process_comms = false);
 
   /// Create a node based on the node name and a rclcpp::context::Context.
   /**
@@ -92,21 +85,61 @@ public:
   rclcpp::callback_group::CallbackGroup::SharedPtr
   create_callback_group(rclcpp::callback_group::CallbackGroupType group_type);
 
+  /// Create and return a Publisher.
+  /**
+   * \param[in] topic_name The topic for this publisher to publish on.
+   * \param[in] qos_history_depth The depth of the publisher message queue.
+   * \return Shared pointer to the created publisher.
+   */
+  template<typename MessageT, typename Alloc = std::allocator<void>>
+  typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
+  create_publisher(
+    const std::string & topic_name, size_t qos_history_depth,
+    std::shared_ptr<Alloc> allocator = nullptr);
+
   /// Create and return a Publisher.
   /**
    * \param[in] topic_name The topic for this publisher to publish on.
    * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
    * \return Shared pointer to the created publisher.
    */
-  template<typename MessageT>
-  rclcpp::publisher::Publisher::SharedPtr
+  template<typename MessageT, typename Alloc = std::allocator<void>>
+  typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
   create_publisher(
-    const std::string & topic_name, const rmw_qos_profile_t & qos_profile);
+    const std::string & topic_name,
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
+    std::shared_ptr<Alloc> allocator = nullptr);
 
   /// Create and return a Subscription.
   /**
    * \param[in] topic_name The topic to subscribe on.
+   * \param[in] callback The user-defined callback function.
    * \param[in] qos_profile The quality of service profile to pass on to the rmw implementation.
+   * \param[in] group The callback group for this subscription. NULL for no callback group.
+   * \param[in] ignore_local_publications True to ignore local publications.
+   * \param[in] msg_mem_strat The message memory strategy to use for allocating messages.
+   * \return Shared pointer to the created subscription.
+   */
+  /* TODO(jacquelinekay):
+     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 rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+  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);
+
+  /// Create and return a Subscription.
+  /**
+   * \param[in] topic_name The topic to subscribe on.
+   * \param[in] qos_history_depth The depth of the subscription's incoming message queue.
    * \param[in] callback The user-defined callback function.
    * \param[in] group The callback group for this subscription. NULL for no callback group.
    * \param[in] ignore_local_publications True to ignore local publications.
@@ -117,27 +150,17 @@ public:
      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 rclcpp::subscription::Subscription<MessageT>::SharedPtr
+  template<typename MessageT, typename CallbackT, typename Alloc = std::allocator<void>>
+  typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
   create_subscription(
     const std::string & topic_name,
-    const rmw_qos_profile_t & qos_profile,
+    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>::SharedPtr
-    msg_mem_strat = nullptr);
-
-  template<typename MessageT>
-  typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
-  create_subscription_with_unique_ptr_callback(
-    const std::string & topic_name,
-    const rmw_qos_profile_t & qos_profile,
-    typename rclcpp::subscription::AnySubscriptionCallback<MessageT>::UniquePtrCallback callback,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr,
-    bool ignore_local_publications = false,
-    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr
-    msg_mem_strat = nullptr);
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    msg_mem_strat = nullptr,
+    std::shared_ptr<Alloc> allocator = nullptr);
 
   /// Create a timer.
   /**
@@ -207,6 +230,8 @@ public:
 
   size_t count_subscribers(const std::string & topic_name) const;
 
+  const CallbackGroupWeakPtrList & get_callback_groups() const;
+
 private:
   RCLCPP_DISABLE_COPY(Node);
 
@@ -235,101 +260,7 @@ private:
 
   std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
 
-  publisher::Publisher::SharedPtr events_publisher_;
-
-  template<typename MessageT>
-  typename subscription::Subscription<MessageT>::SharedPtr
-  create_subscription_internal(
-    const std::string & topic_name,
-    const rmw_qos_profile_t & qos_profile,
-    rclcpp::subscription::AnySubscriptionCallback<MessageT> callback,
-    rclcpp::callback_group::CallbackGroup::SharedPtr group,
-    bool ignore_local_publications,
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr msg_mem_strat);
-
-  template<
-    typename ServiceT,
-    typename FunctorT,
-    typename std::enable_if<
-      function_traits<FunctorT>::arity == 2
-    >::type * = nullptr,
-    typename std::enable_if<
-      std::is_same<
-        typename function_traits<FunctorT>::template argument_type<0>,
-        typename std::shared_ptr<typename ServiceT::Request>
-      >::value
-    >::type * = nullptr,
-    typename std::enable_if<
-      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(
-    std::shared_ptr<rmw_node_t> node_handle,
-    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(
-      node_handle, service_handle, service_name, callback_without_header);
-  }
-
-  template<
-    typename ServiceT,
-    typename FunctorT,
-    typename std::enable_if<
-      function_traits<FunctorT>::arity == 3
-    >::type * = nullptr,
-    typename std::enable_if<
-      std::is_same<
-        typename function_traits<FunctorT>::template argument_type<0>,
-        std::shared_ptr<rmw_request_id_t>
-      >::value
-    >::type * = nullptr,
-    typename std::enable_if<
-      std::is_same<
-        typename function_traits<FunctorT>::template argument_type<1>,
-        typename std::shared_ptr<typename ServiceT::Request>
-      >::value
-    >::type * = nullptr
-/*
-   TODO(esteve): reenable this block of code when VS2015 gets better support
-   for SFINAE and remove the static_assert from the body of this method. For
-   more info about the current support for SFINAE in VS2015 RC:
-
-   http://blogs.msdn.com/b/vcblog/archive/2015/04/29/c-11-14-17-features-in-vs-2015-rc.aspx
-   ,
-    typename std::enable_if<
-      std::is_same<
-        typename function_traits<FunctorT>::template argument_type<2>,
-        typename std::shared_ptr<typename ServiceT::Response>
-      >::value
-    >::type * = nullptr
- */
-  >
-  typename rclcpp::service::Service<ServiceT>::SharedPtr
-  create_service_internal(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
-    const std::string & service_name,
-    FunctorT callback)
-  {
-    static_assert(
-      std::is_same<
-        typename function_traits<FunctorT>::template argument_type<2>,
-        typename std::shared_ptr<typename ServiceT::Response>
-      >::value, "Third argument must be of type std::shared_ptr<ServiceT::Response>");
-
-    typename rclcpp::service::Service<ServiceT>::CallbackWithHeaderType callback_with_header =
-      callback;
-    return service::Service<ServiceT>::make_shared(
-      node_handle, service_handle, service_name, callback_with_header);
-  }
+  publisher::Publisher<rcl_interfaces::msg::ParameterEvent>::SharedPtr events_publisher_;
 };
 
 const rosidl_message_type_support_t * Node::ipm_ts_ =
@@ -347,9 +278,9 @@ const rosidl_message_type_support_t * Node::ipm_ts_ =
                make_shared())); \
   }
 
-#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
+#ifndef RCLCPP__NODE_IMPL_HPP_
 // Template implementations
 #include "node_impl.hpp"
 #endif
 
-#endif /* RCLCPP_RCLCPP_NODE_HPP_ */
+#endif  // RCLCPP__NODE_HPP_

rclcpp/include/rclcpp/node_impl.hpp

@@ -12,34 +12,38 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_
-#define RCLCPP_RCLCPP_NODE_IMPL_HPP_
+#ifndef RCLCPP__NODE_IMPL_HPP_
+#define RCLCPP__NODE_IMPL_HPP_
+
+#include <rmw/error_handling.h>
+#include <rmw/rmw.h>
 
 #include <algorithm>
 #include <cstdlib>
 #include <iostream>
 #include <limits>
+#include <map>
 #include <memory>
 #include <sstream>
 #include <stdexcept>
 #include <string>
+#include <utility>
+#include <vector>
 
-#include <rcl_interfaces/msg/intra_process_message.hpp>
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
-#include <rosidl_generator_cpp/message_type_support.hpp>
-#include <rosidl_generator_cpp/service_type_support.hpp>
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rosidl_generator_cpp/message_type_support.hpp"
+#include "rosidl_generator_cpp/service_type_support.hpp"
 
-#include <rclcpp/contexts/default_context.hpp>
-#include <rclcpp/intra_process_manager.hpp>
-#include <rclcpp/parameter.hpp>
+#include "rclcpp/contexts/default_context.hpp"
+#include "rclcpp/intra_process_manager.hpp"
+#include "rclcpp/parameter.hpp"
 
-#ifndef RCLCPP_RCLCPP_NODE_HPP_
+#ifndef RCLCPP__NODE_HPP_
 #include "node.hpp"
 #endif
 
 using namespace rclcpp;
-using namespace rclcpp::node;
+using namespace node;
 
 Node::Node(const std::string & node_name, bool use_intra_process_comms)
 : Node(
@@ -66,17 +70,15 @@ Node::Node(
   _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var);
 #endif
   if (ros_domain_id) {
-    unsigned long number = strtoul(ros_domain_id, NULL, 0);
-    if (number == (std::numeric_limits<unsigned long>::max)()) {
+    uint32_t number = strtoul(ros_domain_id, NULL, 0);
+    if (number == (std::numeric_limits<uint32_t>::max)()) {
       throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number");
     }
     domain_id = static_cast<size_t>(number);
-  }
 #ifdef _WIN32
-  if (ros_domain_id) {
     free(ros_domain_id);
-  }
 #endif
+  }
 
   auto node = rmw_create_node(name_.c_str(), domain_id);
   if (!node) {
@@ -115,11 +117,29 @@ Node::create_callback_group(
   return group;
 }
 
-template<typename MessageT>
-publisher::Publisher::SharedPtr
+template<typename MessageT, typename Alloc>
+typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
 Node::create_publisher(
-  const std::string & topic_name, const rmw_qos_profile_t & qos_profile)
+  const std::string & topic_name, size_t qos_history_depth,
+  std::shared_ptr<Alloc> allocator)
 {
+  if (!allocator) {
+    allocator = std::make_shared<Alloc>();
+  }
+  rmw_qos_profile_t qos = rmw_qos_profile_default;
+  qos.depth = qos_history_depth;
+  return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
+}
+
+template<typename MessageT, typename Alloc>
+typename publisher::Publisher<MessageT, Alloc>::SharedPtr
+Node::create_publisher(
+  const std::string & topic_name, const rmw_qos_profile_t & qos_profile,
+  std::shared_ptr<Alloc> allocator)
+{
+  if (!allocator) {
+    allocator = std::make_shared<Alloc>();
+  }
   using rosidl_generator_cpp::get_message_type_support_handle;
   auto type_support_handle = get_message_type_support_handle<MessageT>();
   rmw_publisher_t * publisher_handle = rmw_create_publisher(
@@ -132,8 +152,8 @@ Node::create_publisher(
     // *INDENT-ON*
   }
 
-  auto publisher = publisher::Publisher::make_shared(
-    node_handle_, publisher_handle, topic_name, qos_profile.depth);
+  auto publisher = publisher::Publisher<MessageT, Alloc>::make_shared(
+    node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator);
 
   if (use_intra_process_comms_) {
     rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher(
@@ -149,7 +169,7 @@ Node::create_publisher(
     auto intra_process_manager =
       context_->get_sub_context<rclcpp::intra_process_manager::IntraProcessManager>();
     uint64_t intra_process_publisher_id =
-      intra_process_manager->add_publisher<MessageT>(publisher);
+      intra_process_manager->add_publisher<MessageT, Alloc>(publisher);
     rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager;
     // *INDENT-OFF*
     auto shared_publish_callback =
@@ -171,8 +191,10 @@ Node::create_publisher(
           "' is incompatible from the publisher type '" + message_type_info.name() + "'");
       }
       MessageT * typed_message_ptr = static_cast<MessageT *>(msg);
-      std::unique_ptr<MessageT> unique_msg(typed_message_ptr);
-      uint64_t message_seq = ipm->store_intra_process_message(publisher_id, unique_msg);
+      using MessageDeleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
+      std::unique_ptr<MessageT, MessageDeleter> unique_msg(typed_message_ptr);
+      uint64_t message_seq =
+        ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
       return message_seq;
     };
     // *INDENT-ON*
@@ -197,65 +219,31 @@ Node::group_in_node(callback_group::CallbackGroup::SharedPtr group)
   return group_belongs_to_this_node;
 }
 
-template<typename MessageT, typename CallbackT>
-typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
+template<typename MessageT, typename CallbackT, typename Alloc>
+typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
 Node::create_subscription(
   const std::string & topic_name,
-  const rmw_qos_profile_t & qos_profile,
   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>::SharedPtr
-  msg_mem_strat)
+  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+  msg_mem_strat,
+  typename std::shared_ptr<Alloc> allocator)
 {
-  rclcpp::subscription::AnySubscriptionCallback<MessageT> any_subscription_callback;
+  if (!allocator) {
+    allocator = std::make_shared<Alloc>();
+  }
+
+  rclcpp::subscription::AnySubscriptionCallback<MessageT,
+  Alloc> any_subscription_callback(allocator);
   any_subscription_callback.set(callback);
-  return this->create_subscription_internal(
-    topic_name,
-    qos_profile,
-    any_subscription_callback,
-    group,
-    ignore_local_publications,
-    msg_mem_strat);
-}
 
-template<typename MessageT>
-typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
-Node::create_subscription_with_unique_ptr_callback(
-  const std::string & topic_name,
-  const rmw_qos_profile_t & qos_profile,
-  typename rclcpp::subscription::AnySubscriptionCallback<MessageT>::UniquePtrCallback callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr
-  msg_mem_strat)
-{
-  rclcpp::subscription::AnySubscriptionCallback<MessageT> any_subscription_callback;
-  any_subscription_callback.unique_ptr_callback = callback;
-  return this->create_subscription_internal(
-    topic_name,
-    qos_profile,
-    any_subscription_callback,
-    group,
-    ignore_local_publications,
-    msg_mem_strat);
-}
-
-template<typename MessageT>
-typename subscription::Subscription<MessageT>::SharedPtr
-Node::create_subscription_internal(
-  const std::string & topic_name,
-  const rmw_qos_profile_t & qos_profile,
-  rclcpp::subscription::AnySubscriptionCallback<MessageT> callback,
-  rclcpp::callback_group::CallbackGroup::SharedPtr group,
-  bool ignore_local_publications,
-  typename message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr msg_mem_strat)
-{
   using rosidl_generator_cpp::get_message_type_support_handle;
 
   if (!msg_mem_strat) {
     msg_mem_strat =
-      rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT>::create_default();
+      rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::create_default();
   }
 
   auto type_support_handle = get_message_type_support_handle<MessageT>();
@@ -271,12 +259,12 @@ Node::create_subscription_internal(
 
   using namespace rclcpp::subscription;
 
-  auto sub = Subscription<MessageT>::make_shared(
+  auto sub = Subscription<MessageT, Alloc>::make_shared(
     node_handle_,
     subscriber_handle,
     topic_name,
     ignore_local_publications,
-    callback,
+    any_subscription_callback,
     msg_mem_strat);
   auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(sub);
   // Setup intra process.
@@ -303,7 +291,7 @@ Node::create_subscription_internal(
         uint64_t publisher_id,
         uint64_t message_sequence,
         uint64_t subscription_id,
-        std::unique_ptr<MessageT> & message)
+        typename Subscription<MessageT, Alloc>::MessageUniquePtr & message)
       {
         auto ipm = weak_ipm.lock();
         if (!ipm) {
@@ -311,7 +299,8 @@ Node::create_subscription_internal(
           throw std::runtime_error(
             "intra process take called after destruction of intra process manager");
         }
-        ipm->take_intra_process_message(publisher_id, message_sequence, subscription_id, message);
+        ipm->take_intra_process_message<MessageT, Alloc>(
+          publisher_id, message_sequence, subscription_id, message);
       },
       [weak_ipm](const rmw_gid_t * sender_gid) -> bool {
         auto ipm = weak_ipm.lock();
@@ -327,7 +316,7 @@ Node::create_subscription_internal(
   // Assign to a group.
   if (group) {
     if (!group_in_node(group)) {
-      // TODO: use custom exception
+      // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create subscription, group not in node.");
     }
     group->add_subscription(sub_base_ptr);
@@ -338,6 +327,30 @@ Node::create_subscription_internal(
   return sub;
 }
 
+template<typename MessageT, typename CallbackT, typename Alloc>
+typename rclcpp::subscription::Subscription<MessageT, Alloc>::SharedPtr
+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)
+{
+  rmw_qos_profile_t qos = rmw_qos_profile_default;
+  qos.depth = qos_history_depth;
+  return this->create_subscription<MessageT, CallbackT, Alloc>(
+    topic_name,
+    callback,
+    qos,
+    group,
+    ignore_local_publications,
+    msg_mem_strat,
+    allocator);
+}
+
 rclcpp::timer::WallTimer::SharedPtr
 Node::create_wall_timer(
   std::chrono::nanoseconds period,
@@ -347,7 +360,7 @@ Node::create_wall_timer(
   auto timer = rclcpp::timer::WallTimer::make_shared(period, callback);
   if (group) {
     if (!group_in_node(group)) {
-      // TODO: use custom exception
+      // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create timer, group not in node.");
     }
     group->add_timer(timer);
@@ -424,6 +437,9 @@ Node::create_service(
   auto service_type_support_handle =
     get_service_type_support_handle<ServiceT>();
 
+  rclcpp::service::AnyServiceCallback<ServiceT> any_service_callback;
+  any_service_callback.set(callback);
+
   rmw_service_t * service_handle = rmw_create_service(
     node_handle_.get(), service_type_support_handle, service_name.c_str());
   if (!service_handle) {
@@ -434,12 +450,12 @@ Node::create_service(
     // *INDENT-ON*
   }
 
-  auto serv = create_service_internal<ServiceT>(
-    node_handle_, service_handle, service_name, callback);
+  auto serv = service::Service<ServiceT>::make_shared(
+    node_handle_, service_handle, service_name, any_service_callback);
   auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
   if (group) {
     if (!group_in_node(group)) {
-      // TODO: use custom exception
+      // TODO(jacquelinekay): use custom exception
       throw std::runtime_error("Cannot create service, group not in node.");
     }
     group->add_service(serv_base_ptr);
@@ -487,7 +503,7 @@ Node::set_parameters_atomically(
   tmp_map.insert(parameters_.begin(), parameters_.end());
   std::swap(tmp_map, parameters_);
 
-  // TODO: handle parameter constraints
+  // TODO(jacquelinekay): handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
 
@@ -650,4 +666,11 @@ Node::count_subscribers(const std::string & topic_name) const
   return count;
 }
 
-#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */
+
+const Node::CallbackGroupWeakPtrList &
+Node::get_callback_groups() const
+{
+  return callback_groups_;
+}
+
+#endif  // RCLCPP__NODE_IMPL_HPP_

rclcpp/include/rclcpp/parameter.hpp

@@ -224,7 +224,7 @@ public:
     return parameter;
   }
 
-  std::string to_string() const
+  std::string value_to_string() const
   {
     switch (get_type()) {
       case rclcpp::parameter::ParameterType::PARAMETER_BOOL:
@@ -265,9 +265,68 @@ private:
   rcl_interfaces::msg::ParameterValue value_;
 };
 
-std::ostream & operator<<(std::ostream & os, const ParameterVariant & pv)
+
+/* Return a json encoded version of the parameter intended for a dict. */
+std::string _to_json_dict_entry(const ParameterVariant & param)
 {
-  os << pv.to_string();
+  std::stringstream ss;
+  ss << "\"" << param.get_name() << "\": ";
+  ss << "{\"type\": \"" << param.get_type_name() << "\", ";
+  ss << "\"value\": \"" << param.value_to_string() << "\"}";
+  return ss.str();
+}
+
+
+} /* namespace parameter */
+
+} /* namespace rclcpp */
+
+namespace std
+{
+/* Return a json encoded version of the parameter intended for a list. */
+inline std::string to_string(const rclcpp::parameter::ParameterVariant & param)
+{
+  std::stringstream ss;
+  ss << "{\"name\": \"" << param.get_name() << "\", ";
+  ss << "\"type\": \"" << param.get_type_name() << "\", ";
+  ss << "\"value\": \"" << param.value_to_string() << "\"}";
+  return ss.str();
+}
+
+/* Return a json encoded version of a vector of parameters, as a string*/
+inline std::string to_string(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
+{
+  std::stringstream ss;
+  ss << "{";
+  bool first = true;
+  for (const auto & pv : parameters) {
+    if (first == false) {
+      ss << ", ";
+    } else {
+      first = false;
+    }
+    ss << rclcpp::parameter::_to_json_dict_entry(pv);
+  }
+  ss << "}";
+  return ss.str();
+}
+
+} /* namespace std */
+
+namespace rclcpp
+{
+namespace parameter
+{
+
+std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv)
+{
+  os << std::to_string(pv);
+  return os;
+}
+
+std::ostream & operator<<(std::ostream & os, const std::vector<ParameterVariant> & parameters)
+{
+  os << std::to_string(parameters);
   return os;
 }
 

rclcpp/include/rclcpp/parameter_client.hpp

@@ -69,7 +69,7 @@ public:
 
   std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>
   get_parameters(
-    std::vector<std::string> names,
+    const std::vector<std::string> & names,
     std::function<void(
       std::shared_future<std::vector<rclcpp::parameter::ParameterVariant>>)> callback = nullptr)
   {
@@ -108,7 +108,7 @@ public:
 
   std::shared_future<std::vector<rclcpp::parameter::ParameterType>>
   get_parameter_types(
-    std::vector<std::string> names,
+    const std::vector<std::string> & names,
     std::function<void(
       std::shared_future<std::vector<rclcpp::parameter::ParameterType>>)> callback = nullptr)
   {
@@ -140,7 +140,7 @@ public:
 
   std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
   set_parameters(
-    std::vector<rclcpp::parameter::ParameterVariant> parameters,
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
     std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
     nullptr)
   {
@@ -170,7 +170,7 @@ public:
 
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
-    std::vector<rclcpp::parameter::ParameterVariant> parameters,
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters,
     std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
     nullptr)
   {
@@ -200,7 +200,7 @@ public:
 
   std::shared_future<rcl_interfaces::msg::ListParametersResult>
   list_parameters(
-    std::vector<std::string> prefixes,
+    const std::vector<std::string> & prefixes,
     uint64_t depth,
     std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
     nullptr)
@@ -232,7 +232,7 @@ public:
   on_parameter_event(FunctorT callback)
   {
     return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>(
-      "parameter_events", rmw_qos_profile_parameter_events, callback);
+      "parameter_events", callback, rmw_qos_profile_parameter_events);
   }
 
 private:
@@ -271,45 +271,77 @@ public:
   }
 
   std::vector<rclcpp::parameter::ParameterVariant>
-  get_parameters(std::vector<std::string> parameter_names)
+  get_parameters(const std::vector<std::string> & parameter_names)
   {
     auto f = async_parameters_client_->get_parameters(parameter_names);
-    return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
+    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+      rclcpp::executors::FutureReturnCode::SUCCESS)
+    {
+      return f.get();
+    }
+    // Return an empty vector if unsuccessful
+    return std::vector<rclcpp::parameter::ParameterVariant>();
   }
 
   std::vector<rclcpp::parameter::ParameterType>
-  get_parameter_types(std::vector<std::string> parameter_names)
+  get_parameter_types(const std::vector<std::string> & parameter_names)
   {
     auto f = async_parameters_client_->get_parameter_types(parameter_names);
-    return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
+
+    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+      rclcpp::executors::FutureReturnCode::SUCCESS)
+    {
+      return f.get();
+    }
+    return std::vector<rclcpp::parameter::ParameterType>();
   }
 
   std::vector<rcl_interfaces::msg::SetParametersResult>
-  set_parameters(std::vector<rclcpp::parameter::ParameterVariant> parameters)
+  set_parameters(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
   {
     auto f = async_parameters_client_->set_parameters(parameters);
-    return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
+
+    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+      rclcpp::executors::FutureReturnCode::SUCCESS)
+    {
+      return f.get();
+    }
+    return std::vector<rcl_interfaces::msg::SetParametersResult>();
   }
 
   rcl_interfaces::msg::SetParametersResult
-  set_parameters_atomically(std::vector<rclcpp::parameter::ParameterVariant> parameters)
+  set_parameters_atomically(const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
   {
     auto f = async_parameters_client_->set_parameters_atomically(parameters);
-    return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
+
+    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+      rclcpp::executors::FutureReturnCode::SUCCESS)
+    {
+      return f.get();
+    }
+
+    throw std::runtime_error("Unable to get result of set parameters service call.");
   }
 
   rcl_interfaces::msg::ListParametersResult
   list_parameters(
-    std::vector<std::string> parameter_prefixes,
+    const std::vector<std::string> & parameter_prefixes,
     uint64_t depth)
   {
     auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth);
-    return rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f).get();
+
+    if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) ==
+      rclcpp::executors::FutureReturnCode::SUCCESS)
+    {
+      return f.get();
+    }
+
+    throw std::runtime_error("Unable to get result of list parameters service call.");
   }
 
   template<typename FunctorT>
   typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
-  on_parameter_event(FunctorT & callback)
+  on_parameter_event(FunctorT callback)
   {
     return async_parameters_client_->on_parameter_event(callback);
   }

rclcpp/include/rclcpp/parameter_service.hpp

@@ -46,12 +46,17 @@ public:
   ParameterService(const rclcpp::node::Node::SharedPtr node)
   : node_(node)
   {
+    std::weak_ptr<rclcpp::node::Node> captured_node = node_;
     get_parameters_service_ = node_->create_service<rcl_interfaces::srv::GetParameters>(
-      node_->get_name() + "__get_parameters", [&node](
+      node_->get_name() + "__get_parameters", [captured_node](
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<rcl_interfaces::srv::GetParameters::Request> request,
         std::shared_ptr<rcl_interfaces::srv::GetParameters::Response> response)
         {
+          auto node = captured_node.lock();
+          if (!node) {
+            return;
+          }
           auto values = node->get_parameters(request->names);
           for (auto & pvariant : values) {
             response->values.push_back(pvariant.get_parameter_value());
@@ -60,11 +65,15 @@ public:
       );
 
     get_parameter_types_service_ = node_->create_service<rcl_interfaces::srv::GetParameterTypes>(
-      node_->get_name() + "__get_parameter_types", [&node](
+      node_->get_name() + "__get_parameter_types", [captured_node](
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Request> request,
         std::shared_ptr<rcl_interfaces::srv::GetParameterTypes::Response> response)
         {
+          auto node = captured_node.lock();
+          if (!node) {
+            return;
+          }
           auto types = node->get_parameter_types(request->names);
           std::transform(types.cbegin(), types.cend(),
           std::back_inserter(response->types), [](const uint8_t & type) {
@@ -74,11 +83,15 @@ public:
       );
 
     set_parameters_service_ = node_->create_service<rcl_interfaces::srv::SetParameters>(
-      node_->get_name() + "__set_parameters", [&node](
+      node_->get_name() + "__set_parameters", [captured_node](
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<rcl_interfaces::srv::SetParameters::Request> request,
         std::shared_ptr<rcl_interfaces::srv::SetParameters::Response> response)
         {
+          auto node = captured_node.lock();
+          if (!node) {
+            return;
+          }
           std::vector<rclcpp::parameter::ParameterVariant> pvariants;
           for (auto & p : request->parameters) {
             pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p));
@@ -90,11 +103,15 @@ public:
 
     set_parameters_atomically_service_ =
       node_->create_service<rcl_interfaces::srv::SetParametersAtomically>(
-      node_->get_name() + "__set_parameters_atomically", [&node](
+      node_->get_name() + "__set_parameters_atomically", [captured_node](
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Request> request,
         std::shared_ptr<rcl_interfaces::srv::SetParametersAtomically::Response> response)
         {
+          auto node = captured_node.lock();
+          if (!node) {
+            return;
+          }
           std::vector<rclcpp::parameter::ParameterVariant> pvariants;
           std::transform(request->parameters.cbegin(), request->parameters.cend(),
           std::back_inserter(pvariants),
@@ -108,22 +125,30 @@ public:
       );
 
     describe_parameters_service_ = node_->create_service<rcl_interfaces::srv::DescribeParameters>(
-      node_->get_name() + "__describe_parameters", [&node](
+      node_->get_name() + "__describe_parameters", [captured_node](
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Request> request,
         std::shared_ptr<rcl_interfaces::srv::DescribeParameters::Response> response)
         {
+          auto node = captured_node.lock();
+          if (!node) {
+            return;
+          }
           auto descriptors = node->describe_parameters(request->names);
           response->descriptors = descriptors;
         }
       );
 
     list_parameters_service_ = node_->create_service<rcl_interfaces::srv::ListParameters>(
-      node_->get_name() + "__list_parameters", [&node](
+      node_->get_name() + "__list_parameters", [captured_node](
         const std::shared_ptr<rmw_request_id_t>,
         const std::shared_ptr<rcl_interfaces::srv::ListParameters::Request> request,
         std::shared_ptr<rcl_interfaces::srv::ListParameters::Response> response)
         {
+          auto node = captured_node.lock();
+          if (!node) {
+            return;
+          }
           auto result = node->list_parameters(request->prefixes, request->depth);
           response->result = result;
         }

rclcpp/include/rclcpp/publisher.hpp

@@ -12,10 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_
-#define RCLCPP_RCLCPP_PUBLISHER_HPP_
+#ifndef RCLCPP__PUBLISHER_HPP_
+#define RCLCPP__PUBLISHER_HPP_
 
-#include <rclcpp/macros.hpp>
+#include <rmw/error_handling.h>
+#include <rmw/rmw.h>
 
 #include <iostream>
 #include <memory>
@@ -23,10 +24,11 @@
 #include <sstream>
 #include <string>
 
-#include <rcl_interfaces/msg/intra_process_message.hpp>
-#include <rmw/impl/cpp/demangle.hpp>
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl_interfaces/msg/intra_process_message.hpp"
+#include "rmw/impl/cpp/demangle.hpp"
+
+#include "rclcpp/allocator/allocator_deleter.hpp"
+#include "rclcpp/macros.hpp"
 
 namespace rclcpp
 {
@@ -35,19 +37,15 @@ namespace rclcpp
 namespace node
 {
 class Node;
-} // namespace node
+}  // namespace node
 
 namespace publisher
 {
 
-/// A publisher publishes messages of any type to a topic.
-class Publisher
+class PublisherBase
 {
-  friend rclcpp::node::Node;
-
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher);
-
+  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase);
   /// Default constructor.
   /**
    * Typically, a publisher is not created through this method, but instead is created through a
@@ -57,7 +55,7 @@ public:
    * \param[in] topic The topic that this publisher publishes on.
    * \param[in] queue_size The maximum number of unpublished messages to queue.
    */
-  Publisher(
+  PublisherBase(
     std::shared_ptr<rmw_node_t> node_handle,
     rmw_publisher_t * publisher_handle,
     std::string topic,
@@ -77,7 +75,7 @@ public:
   }
 
   /// Default destructor.
-  virtual ~Publisher()
+  virtual ~PublisherBase()
   {
     if (intra_process_publisher_handle_) {
       if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) {
@@ -97,80 +95,6 @@ public:
     }
   }
 
-  /// Send a message to the topic for this publisher.
-  /**
-   * This function is templated on the input message type, MessageT.
-   * \param[in] msg A shared pointer to the message to send.
-   */
-  template<typename MessageT>
-  void
-  publish(std::unique_ptr<MessageT> & msg)
-  {
-    this->do_inter_process_publish<MessageT>(msg.get());
-    if (store_intra_process_message_) {
-      // Take the pointer from the unique_msg, release it and pass as a void *
-      // to the ipm. The ipm should then capture it again as a unique_ptr of
-      // the correct type.
-      // TODO(wjwwood):
-      //   investigate how to transfer the custom deleter (if there is one)
-      //   from the incoming unique_ptr through to the ipm's unique_ptr.
-      //   See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
-      MessageT * msg_ptr = msg.get();
-      msg.release();
-      uint64_t message_seq;
-      {
-        std::lock_guard<std::mutex> lock(intra_process_publish_mutex_);
-        message_seq =
-          store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
-      }
-      rcl_interfaces::msg::IntraProcessMessage ipm;
-      ipm.publisher_id = intra_process_publisher_id_;
-      ipm.message_sequence = message_seq;
-      auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
-      if (status != RMW_RET_OK) {
-        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-        throw std::runtime_error(
-          std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
-        // *INDENT-ON*
-      }
-    } else {
-      // Always destroy the message, even if we don't consume it, for consistency.
-      msg.reset();
-    }
-  }
-
-  template<typename MessageT>
-  void
-  publish(const std::shared_ptr<MessageT> & msg)
-  {
-    // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg.get());
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    // TODO(wjwwood):
-    //   The intra process manager should probably also be able to store
-    //   shared_ptr's and do the "smart" thing based on other intra process
-    //   subscriptions. For now call the other publish().
-    std::unique_ptr<MessageT> unique_msg(new MessageT(*msg.get()));
-    return this->publish(unique_msg);
-  }
-
-  template<typename MessageT>
-  void
-  publish(const MessageT & msg)
-  {
-    // Avoid allocating when not using intra process.
-    if (!store_intra_process_message_) {
-      // In this case we're not using intra process.
-      return this->do_inter_process_publish(msg.get());
-    }
-    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    std::unique_ptr<MessageT> unique_msg(new MessageT(msg));
-    return this->publish(unique_msg);
-  }
-
   /// Get the topic that this publisher publishes on.
   // \return The topic name.
   const std::string &
@@ -243,20 +167,6 @@ public:
   typedef std::function<uint64_t(uint64_t, void *, const std::type_info &)> StoreMessageCallbackT;
 
 protected:
-  template<typename MessageT>
-  void
-  do_inter_process_publish(MessageT * msg)
-  {
-    auto status = rmw_publish(publisher_handle_, msg);
-    if (status != RMW_RET_OK) {
-      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
-      throw std::runtime_error(
-        std::string("failed to publish message: ") + rmw_get_error_string_safe());
-      // *INDENT-ON*
-    }
-  }
-
-
   void
   setup_intra_process(
     uint64_t intra_process_publisher_id,
@@ -277,7 +187,6 @@ protected:
     }
   }
 
-private:
   std::shared_ptr<rmw_node_t> node_handle_;
 
   rmw_publisher_t * publisher_handle_;
@@ -293,10 +202,153 @@ private:
   rmw_gid_t intra_process_rmw_gid_;
 
   std::mutex intra_process_publish_mutex_;
-
 };
 
-} /* namespace publisher */
-} /* namespace rclcpp */
+/// A publisher publishes messages of any type to a topic.
+template<typename MessageT, typename Alloc = std::allocator<void>>
+class Publisher : public PublisherBase
+{
+  friend rclcpp::node::Node;
 
-#endif /* RCLCPP_RCLCPP_PUBLISHER_HPP_ */
+public:
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
+
+  Publisher(
+    std::shared_ptr<rmw_node_t> node_handle,
+    rmw_publisher_t * publisher_handle,
+    std::string topic,
+    size_t queue_size,
+    std::shared_ptr<Alloc> allocator)
+  : PublisherBase(node_handle, publisher_handle, topic, queue_size)
+  {
+    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
+  }
+
+
+  /// Send a message to the topic for this publisher.
+  /**
+   * This function is templated on the input message type, MessageT.
+   * \param[in] msg A shared pointer to the message to send.
+   */
+  void
+  publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
+  {
+    this->do_inter_process_publish(msg.get());
+    if (store_intra_process_message_) {
+      // Take the pointer from the unique_msg, release it and pass as a void *
+      // to the ipm. The ipm should then capture it again as a unique_ptr of
+      // the correct type.
+      // TODO(wjwwood):
+      //   investigate how to transfer the custom deleter (if there is one)
+      //   from the incoming unique_ptr through to the ipm's unique_ptr.
+      //   See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr
+      MessageT * msg_ptr = msg.get();
+      msg.release();
+      uint64_t message_seq;
+      {
+        std::lock_guard<std::mutex> lock(intra_process_publish_mutex_);
+        message_seq =
+          store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT));
+      }
+      rcl_interfaces::msg::IntraProcessMessage ipm;
+      ipm.publisher_id = intra_process_publisher_id_;
+      ipm.message_sequence = message_seq;
+      auto status = rmw_publish(intra_process_publisher_handle_, &ipm);
+      if (status != RMW_RET_OK) {
+        // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+        throw std::runtime_error(
+          std::string("failed to publish intra process message: ") + rmw_get_error_string_safe());
+        // *INDENT-ON*
+      }
+    } else {
+      // Always destroy the message, even if we don't consume it, for consistency.
+      msg.reset();
+    }
+  }
+
+  void
+  publish(const std::shared_ptr<MessageT> & msg)
+  {
+    // Avoid allocating when not using intra process.
+    if (!store_intra_process_message_) {
+      // In this case we're not using intra process.
+      return this->do_inter_process_publish(msg.get());
+    }
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
+    // TODO(wjwwood):
+    //   The intra process manager should probably also be able to store
+    //   shared_ptr's and do the "smart" thing based on other intra process
+    //   subscriptions. For now call the other publish().
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
+    return this->publish(unique_msg);
+  }
+
+  void
+  publish(std::shared_ptr<const MessageT> msg)
+  {
+    // Avoid allocating when not using intra process.
+    if (!store_intra_process_message_) {
+      // In this case we're not using intra process.
+      return this->do_inter_process_publish(msg.get());
+    }
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
+    // TODO(wjwwood):
+    //   The intra process manager should probably also be able to store
+    //   shared_ptr's and do the "smart" thing based on other intra process
+    //   subscriptions. For now call the other publish().
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get());
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
+    return this->publish(unique_msg);
+  }
+
+  void
+  publish(const MessageT & msg)
+  {
+    // Avoid allocating when not using intra process.
+    if (!store_intra_process_message_) {
+      // In this case we're not using intra process.
+      return this->do_inter_process_publish(&msg);
+    }
+    // Otherwise we have to allocate memory in a unique_ptr and pass it along.
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg);
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
+    return this->publish(unique_msg);
+  }
+
+  std::shared_ptr<MessageAlloc> get_allocator() const
+  {
+    return message_allocator_;
+  }
+
+protected:
+  void
+  do_inter_process_publish(const MessageT * msg)
+  {
+    auto status = rmw_publish(publisher_handle_, msg);
+    if (status != RMW_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      throw std::runtime_error(
+        std::string("failed to publish message: ") + rmw_get_error_string_safe());
+      // *INDENT-ON*
+    }
+  }
+
+  std::shared_ptr<MessageAlloc> message_allocator_;
+
+  MessageDeleter message_deleter_;
+};
+
+}  // namespace publisher
+}  // namespace rclcpp
+
+#endif  // RCLCPP__PUBLISHER_HPP_

rclcpp/include/rclcpp/rclcpp.hpp

@@ -28,7 +28,7 @@
 
 namespace rclcpp
 {
-
+// NOLINTNEXTLINE(runtime/int)
 const std::chrono::seconds operator"" _s(unsigned long long s)
 {
   return std::chrono::seconds(s);
@@ -40,6 +40,7 @@ const std::chrono::nanoseconds operator"" _s(long double s)
 }
 
 const std::chrono::nanoseconds
+// NOLINTNEXTLINE(runtime/int)
 operator"" _ms(unsigned long long ms)
 {
   return std::chrono::milliseconds(ms);
@@ -52,6 +53,7 @@ operator"" _ms(long double ms)
 }
 
 const std::chrono::nanoseconds
+// NOLINTNEXTLINE(runtime/int)
 operator"" _ns(unsigned long long ns)
 {
   return std::chrono::nanoseconds(ns);
@@ -97,15 +99,15 @@ void spin(Node::SharedPtr node_ptr)
   executor.spin();
 }
 
-template<typename FutureT>
-std::shared_future<FutureT> &
+template<typename FutureT, typename TimeT = std::milli>
+rclcpp::executors::FutureReturnCode
 spin_until_future_complete(
-  Node::SharedPtr node_ptr, std::shared_future<FutureT> & future)
+  Node::SharedPtr node_ptr, std::shared_future<FutureT> & future,
+  std::chrono::duration<int64_t, TimeT> timeout = std::chrono::duration<int64_t, TimeT>(-1))
 {
   rclcpp::executors::SingleThreadedExecutor executor;
-  rclcpp::executors::spin_node_until_future_complete<FutureT>(
-    executor, node_ptr, future);
-  return future;
+  return rclcpp::executors::spin_node_until_future_complete<FutureT>(
+    executor, node_ptr, future, timeout);
 }
 
 } /* namespace rclcpp */

rclcpp/include/rclcpp/service.hpp

@@ -25,23 +25,16 @@
 #include <rmw/rmw.h>
 
 #include <rclcpp/macros.hpp>
-
+#include <rclcpp/any_service_callback.hpp>
 
 namespace rclcpp
 {
 
-// Forward declaration for friend statement
-namespace executor
-{
-class Executor;
-} // namespace executor
-
 namespace service
 {
 
 class ServiceBase
 {
-  friend class rclcpp::executor::Executor;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase);
@@ -78,8 +71,8 @@ public:
   virtual std::shared_ptr<void> create_request() = 0;
   virtual std::shared_ptr<void> create_request_header() = 0;
   virtual void handle_request(
-    std::shared_ptr<void> & request_header,
-    std::shared_ptr<void> & request) = 0;
+    std::shared_ptr<void> request_header,
+    std::shared_ptr<void> request) = 0;
 
 private:
   RCLCPP_DISABLE_COPY(ServiceBase);
@@ -91,39 +84,33 @@ private:
 
 };
 
+using namespace any_service_callback;
+
 template<typename ServiceT>
 class Service : public ServiceBase
 {
 public:
   using CallbackType = std::function<
       void(
-        const std::shared_ptr<typename ServiceT::Request> &,
-        std::shared_ptr<typename ServiceT::Response> &)>;
+        const std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>)>;
 
   using CallbackWithHeaderType = std::function<
       void(
-        const std::shared_ptr<rmw_request_id_t> &,
-        const std::shared_ptr<typename ServiceT::Request> &,
-        std::shared_ptr<typename ServiceT::Response> &)>;
+        const std::shared_ptr<rmw_request_id_t>,
+        const std::shared_ptr<typename ServiceT::Request>,
+        std::shared_ptr<typename ServiceT::Response>)>;
   RCLCPP_SMART_PTR_DEFINITIONS(Service);
 
   Service(
     std::shared_ptr<rmw_node_t> node_handle,
     rmw_service_t * service_handle,
     const std::string & service_name,
-    CallbackType callback)
-  : ServiceBase(node_handle, service_handle, service_name), callback_(callback),
-    callback_with_header_(nullptr)
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback)
   {}
 
-  Service(
-    std::shared_ptr<rmw_node_t> node_handle,
-    rmw_service_t * service_handle,
-    const std::string & service_name,
-    CallbackWithHeaderType callback_with_header)
-  : ServiceBase(node_handle, service_handle, service_name), callback_(nullptr),
-    callback_with_header_(callback_with_header)
-  {}
+  Service() = delete;
 
   std::shared_ptr<void> create_request()
   {
@@ -137,22 +124,18 @@ public:
     return std::shared_ptr<void>(new rmw_request_id_t);
   }
 
-  void handle_request(std::shared_ptr<void> & request_header, std::shared_ptr<void> & request)
+  void handle_request(std::shared_ptr<void> request_header, std::shared_ptr<void> request)
   {
     auto typed_request = std::static_pointer_cast<typename ServiceT::Request>(request);
     auto typed_request_header = std::static_pointer_cast<rmw_request_id_t>(request_header);
     auto response = std::shared_ptr<typename ServiceT::Response>(new typename ServiceT::Response);
-    if (callback_with_header_ != nullptr) {
-      callback_with_header_(typed_request_header, typed_request, response);
-    } else {
-      callback_(typed_request, response);
-    }
+    any_callback_.dispatch(typed_request_header, typed_request, response);
     send_response(typed_request_header, response);
   }
 
   void send_response(
-    std::shared_ptr<rmw_request_id_t> & req_id,
-    std::shared_ptr<typename ServiceT::Response> & response)
+    std::shared_ptr<rmw_request_id_t> req_id,
+    std::shared_ptr<typename ServiceT::Response> response)
   {
     rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get());
     if (status != RMW_RET_OK) {
@@ -166,8 +149,7 @@ public:
 private:
   RCLCPP_DISABLE_COPY(Service);
 
-  CallbackType callback_;
-  CallbackWithHeaderType callback_with_header_;
+  AnyServiceCallback<ServiceT> any_callback_;
 };
 
 } /* namespace service */

rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

@@ -0,0 +1,318 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
+#define RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
+
+#include <memory>
+#include <vector>
+
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node.hpp"
+
+namespace rclcpp
+{
+
+namespace memory_strategies
+{
+
+namespace allocator_memory_strategy
+{
+
+/// Delegate for handling memory allocations while the Executor is executing.
+/**
+ * By default, the memory strategy dynamically allocates memory for structures that come in from
+ * the rmw implementation after the executor waits for work, based on the number of entities that
+ * come through.
+ */
+template<typename Alloc = std::allocator<void>>
+class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
+
+  using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
+  using ExecAlloc = typename ExecAllocTraits::allocator_type;
+  using ExecDeleter = allocator::Deleter<ExecAlloc, executor::AnyExecutable>;
+  using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
+  using VoidAlloc = typename VoidAllocTraits::allocator_type;
+
+  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
+
+  explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
+  {
+    executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
+    allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
+  }
+
+  AllocatorMemoryStrategy()
+  {
+    executable_allocator_ = std::make_shared<ExecAlloc>();
+    allocator_ = std::make_shared<VoidAlloc>();
+  }
+
+  size_t fill_subscriber_handles(void ** & ptr)
+  {
+    for (auto & subscription : subscriptions_) {
+      subscriber_handles_.push_back(subscription->get_subscription_handle()->data);
+      if (subscription->get_intra_process_subscription_handle()) {
+        subscriber_handles_.push_back(subscription->get_intra_process_subscription_handle()->data);
+      }
+    }
+    ptr = subscriber_handles_.data();
+    return subscriber_handles_.size();
+  }
+
+  // return the new number of services
+  size_t fill_service_handles(void ** & ptr)
+  {
+    for (auto & service : services_) {
+      service_handles_.push_back(service->get_service_handle()->data);
+    }
+    ptr = service_handles_.data();
+    return service_handles_.size();
+  }
+
+  // return the new number of clients
+  size_t fill_client_handles(void ** & ptr)
+  {
+    for (auto & client : clients_) {
+      client_handles_.push_back(client->get_client_handle()->data);
+    }
+    ptr = client_handles_.data();
+    return client_handles_.size();
+  }
+
+  void clear_active_entities()
+  {
+    subscriptions_.clear();
+    services_.clear();
+    clients_.clear();
+  }
+
+  void clear_handles()
+  {
+    subscriber_handles_.clear();
+    service_handles_.clear();
+    client_handles_.clear();
+  }
+
+  void revalidate_handles()
+  {
+    {
+      VectorRebind<void *> temp;
+      for (auto & subscriber_handle : subscriber_handles_) {
+        if (subscriber_handle) {
+          temp.push_back(subscriber_handle);
+        }
+      }
+      subscriber_handles_.swap(temp);
+    }
+    {
+      VectorRebind<void *> temp;
+      for (auto & service_handle : service_handles_) {
+        if (service_handle) {
+          temp.push_back(service_handle);
+        }
+      }
+      service_handles_.swap(temp);
+    }
+    {
+      VectorRebind<void *> temp;
+      for (auto & client_handle : client_handles_) {
+        if (client_handle) {
+          temp.push_back(client_handle);
+        }
+      }
+      client_handles_.swap(temp);
+    }
+  }
+
+  bool collect_entities(const WeakNodeVector & weak_nodes)
+  {
+    bool has_invalid_weak_nodes = false;
+    for (auto & weak_node : weak_nodes) {
+      auto node = weak_node.lock();
+      if (!node) {
+        has_invalid_weak_nodes = false;
+        continue;
+      }
+      for (auto & weak_group : node->get_callback_groups()) {
+        auto group = weak_group.lock();
+        if (!group || !group->can_be_taken_from().load()) {
+          continue;
+        }
+        for (auto & weak_subscription : group->get_subscription_ptrs()) {
+          auto subscription = weak_subscription.lock();
+          if (subscription) {
+            subscriptions_.push_back(subscription);
+          }
+        }
+        for (auto & service : group->get_service_ptrs()) {
+          if (service) {
+            services_.push_back(service);
+          }
+        }
+        for (auto & client : group->get_client_ptrs()) {
+          if (client) {
+            clients_.push_back(client);
+          }
+        }
+      }
+    }
+    return has_invalid_weak_nodes;
+  }
+
+
+  /// Provide a newly initialized AnyExecutable object.
+  // \return Shared pointer to the fresh executable.
+  executor::AnyExecutable::SharedPtr instantiate_next_executable()
+  {
+    return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
+  }
+
+  virtual void
+  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
+    const WeakNodeVector & weak_nodes)
+  {
+    auto it = subscriber_handles_.begin();
+    while (it != subscriber_handles_.end()) {
+      auto subscription = get_subscription_by_handle(*it, weak_nodes);
+      if (subscription) {
+        // Figure out if this is for intra-process or not.
+        bool is_intra_process = false;
+        if (subscription->get_intra_process_subscription_handle()) {
+          is_intra_process = subscription->get_intra_process_subscription_handle()->data == *it;
+        }
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_subscription(subscription, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the subscription is not valid...
+          // Remove it from the ready list and continue looking
+          subscriber_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        if (is_intra_process) {
+          any_exec->subscription_intra_process = subscription;
+        } else {
+          any_exec->subscription = subscription;
+        }
+        any_exec->callback_group = group;
+        any_exec->node = get_node_by_group(group, weak_nodes);
+        subscriber_handles_.erase(it);
+        return;
+      }
+      // Else, the subscription is no longer valid, remove it and continue
+      subscriber_handles_.erase(it);
+    }
+  }
+
+  virtual void
+  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
+    const WeakNodeVector & weak_nodes)
+  {
+    auto it = service_handles_.begin();
+    while (it != service_handles_.end()) {
+      auto service = get_service_by_handle(*it, weak_nodes);
+      if (service) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_service(service, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the service is not valid...
+          // Remove it from the ready list and continue looking
+          service_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec->service = service;
+        any_exec->callback_group = group;
+        any_exec->node = get_node_by_group(group, weak_nodes);
+        service_handles_.erase(it);
+        return;
+      }
+      // Else, the service is no longer valid, remove it and continue
+      service_handles_.erase(it);
+    }
+  }
+
+  virtual void
+  get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes)
+  {
+    auto it = client_handles_.begin();
+    while (it != client_handles_.end()) {
+      auto client = get_client_by_handle(*it, weak_nodes);
+      if (client) {
+        // Find the group for this handle and see if it can be serviced
+        auto group = get_group_by_client(client, weak_nodes);
+        if (!group) {
+          // Group was not found, meaning the service is not valid...
+          // Remove it from the ready list and continue looking
+          client_handles_.erase(it);
+          continue;
+        }
+        if (!group->can_be_taken_from().load()) {
+          // Group is mutually exclusive and is being used, so skip it for now
+          // Leave it to be checked next time, but continue searching
+          ++it;
+          continue;
+        }
+        // Otherwise it is safe to set and return the any_exec
+        any_exec->client = client;
+        any_exec->callback_group = group;
+        any_exec->node = get_node_by_group(group, weak_nodes);
+        client_handles_.erase(it);
+        return;
+      }
+      // Else, the service is no longer valid, remove it and continue
+      client_handles_.erase(it);
+    }
+  }
+
+private:
+  template<typename T>
+  using VectorRebind =
+      std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
+
+  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
+  VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
+  VectorRebind<rclcpp::client::ClientBase::SharedPtr> clients_;
+
+  VectorRebind<void *> subscriber_handles_;
+  VectorRebind<void *> service_handles_;
+  VectorRebind<void *> client_handles_;
+
+  std::shared_ptr<ExecAlloc> executable_allocator_;
+  std::shared_ptr<VoidAlloc> allocator_;
+};
+
+}  // namespace allocator_memory_strategy
+}  // namespace memory_strategies
+
+}  // namespace rclcpp
+
+#endif  // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_
+#ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
+#define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_
 
 #include <rclcpp/macros.hpp>
 #include <rclcpp/message_memory_strategy.hpp>
@@ -96,10 +96,9 @@ protected:
 
   std::array<PoolMember, Size> pool_;
   size_t next_array_index_;
-
 };
 
-}  /* message_pool_memory_strategy */
-}  /* strategies */
-}  /* rclcpp */
-#endif  /* RCLCPP_RCLCPP_MSG_POOL_MEMORY_STRATEGY_HPP_ */
+}  // namespace message_pool_memory_strategy
+}  // namespace strategies
+}  // namespace rclcpp
+#endif  // RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_

rclcpp/include/rclcpp/strategies/static_memory_strategy.hpp

@@ -1,358 +0,0 @@
-// Copyright 2015 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef RCLCPP_RCLCPP_STATIC_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_STATIC_MEMORY_STRATEGY_HPP_
-
-#include <unordered_map>
-
-#include <rclcpp/memory_strategy.hpp>
-
-namespace rclcpp
-{
-
-namespace memory_strategies
-{
-
-namespace static_memory_strategy
-{
-
-/// Representation of the upper bounds on the memory pools managed by StaticMemoryStrategy.
-struct ObjectPoolBounds
-{
-public:
-  size_t max_subscriptions;
-  size_t max_services;
-  size_t max_clients;
-  size_t max_executables;
-  size_t max_guard_conditions;
-  size_t pool_size;
-
-  /// Default constructor attempts to set reasonable default limits on the fields.
-  ObjectPoolBounds()
-  : max_subscriptions(10), max_services(10), max_clients(10),
-    max_executables(1), max_guard_conditions(2), pool_size(1024)
-  {}
-
-  // Setters implement named parameter idiom/method chaining
-
-  /// Set the maximum number of subscriptions.
-  /**
-   * \param[in] subscriptions Maximum number of subscriptions.
-   * \return Reference to this object, for method chaining.
-   */
-  ObjectPoolBounds & set_max_subscriptions(size_t subscriptions)
-  {
-    max_subscriptions = subscriptions;
-    return *this;
-  }
-
-  /// Set the maximum number of services.
-  /**
-   * \param[in] services Maximum number of services.
-   * \return Reference to this object, for method chaining.
-   */
-  ObjectPoolBounds & set_max_services(size_t services)
-  {
-    max_services = services;
-    return *this;
-  }
-
-  /// Set the maximum number of clients.
-  /**
-   * \param[in] clients Maximum number of clients.
-   * \return Reference to this object, for method chaining.
-   */
-  ObjectPoolBounds & set_max_clients(size_t clients)
-  {
-    max_clients = clients;
-    return *this;
-  }
-
-  /// Set the maximum number of guard conditions.
-  /**
-   * \param[in] guard conditions Maximum number of guard conditions.
-   * \return Reference to this object, for method chaining.
-   */
-  ObjectPoolBounds & set_max_guard_conditions(size_t guard_conditions)
-  {
-    max_guard_conditions = guard_conditions;
-    return *this;
-  }
-
-  /// Set the maximum number of executables.
-  /**
-   * \param[in] executables Maximum number of executables.
-   * \return Reference to this object, for method chaining.
-   */
-  ObjectPoolBounds & set_max_executables(size_t executables)
-  {
-    max_executables = executables;
-    return *this;
-  }
-
-  /// Set the maximum memory pool size.
-  /**
-   * \param[in] executables Maximum memory pool size.
-   * \return Reference to this object, for method chaining.
-   */
-  ObjectPoolBounds & set_memory_pool_size(size_t pool)
-  {
-    pool_size = pool;
-    return *this;
-  }
-};
-
-
-/// Static memory allocation alternative to the default memory strategy.
-/**
- * The name is a bit of a misnomer. The memory managed by this class is actually allocated
- * dynamically in the constructor, but no subsequent accesses to the class (besides the destructor)
- * allocate or free memory.
- * StaticMemoryStrategy puts a hard limit on the number of subscriptions, etc. that can be executed
- * in one iteration of `Executor::spin`. Thus it allows for memory allocation optimization for
- * situations where a limit on the number of such entities is known.
- */
-class StaticMemoryStrategy : public memory_strategy::MemoryStrategy
-{
-public:
-  /// Default constructor.
-  // \param[in] bounds Representation of the limits on memory managed by this class.
-  StaticMemoryStrategy(ObjectPoolBounds bounds = ObjectPoolBounds())
-  : bounds_(bounds), memory_pool_(nullptr), subscription_pool_(nullptr),
-    service_pool_(nullptr), guard_condition_pool_(nullptr), executable_pool_(nullptr)
-  {
-    if (bounds_.pool_size) {
-      memory_pool_ = new void *[bounds_.pool_size];
-      memset(memory_pool_, 0, bounds_.pool_size * sizeof(void *));
-    }
-
-    if (bounds_.max_subscriptions) {
-      subscription_pool_ = new void *[bounds_.max_subscriptions];
-      memset(subscription_pool_, 0, bounds_.max_subscriptions * sizeof(void *));
-    }
-
-    if (bounds_.max_services) {
-      service_pool_ = new void *[bounds_.max_services];
-      memset(service_pool_, 0, bounds_.max_services * sizeof(void *));
-    }
-
-    if (bounds_.max_clients) {
-      client_pool_ = new void *[bounds_.max_clients];
-      memset(client_pool_, 0, bounds_.max_clients * sizeof(void *));
-    }
-
-    if (bounds_.max_guard_conditions) {
-      guard_condition_pool_ = new void *[bounds_.max_guard_conditions];
-      memset(guard_condition_pool_, 0, bounds_.max_guard_conditions * sizeof(void *));
-    }
-
-    if (bounds_.max_executables) {
-      executable_pool_ = new executor::AnyExecutable::SharedPtr[bounds_.max_executables];
-    }
-
-    for (size_t i = 0; i < bounds_.max_executables; ++i) {
-      executable_pool_[i] = std::make_shared<executor::AnyExecutable>();
-    }
-
-    pool_seq_ = 0;
-    exec_seq_ = 0;
-
-    // Reserve pool_size_ buckets in the memory map.
-    memory_map_.reserve(bounds_.pool_size);
-    for (size_t i = 0; i < bounds_.pool_size; ++i) {
-      memory_map_[memory_pool_[i]] = 0;
-    }
-  }
-
-  /// Default destructor. Free all allocated memory.
-  ~StaticMemoryStrategy()
-  {
-    if (bounds_.pool_size) {
-      delete[] memory_pool_;
-    }
-    if (bounds_.max_subscriptions) {
-      delete[] subscription_pool_;
-    }
-    if (bounds_.max_services) {
-      delete[] service_pool_;
-    }
-    if (bounds_.max_clients) {
-      delete[] client_pool_;
-    }
-    if (bounds_.max_guard_conditions) {
-      delete[] guard_condition_pool_;
-    }
-  }
-
-  /// Borrow handles by returning a pointer to the preallocated object pool for the specified type.
-  /**
-   * \param[in] The type of entity that this function is requesting for.
-   * \param[in] The number of handles to borrow.
-   * \return Pointer to the allocated handles.
-   */
-  void ** borrow_handles(HandleType type, size_t number_of_handles)
-  {
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (number_of_handles > bounds_.max_subscriptions) {
-          throw std::runtime_error("Requested size exceeded maximum subscriptions.");
-        }
-
-        return subscription_pool_;
-      case HandleType::service_handle:
-        if (number_of_handles > bounds_.max_services) {
-          throw std::runtime_error("Requested size exceeded maximum services.");
-        }
-
-        return service_pool_;
-      case HandleType::client_handle:
-        if (number_of_handles > bounds_.max_clients) {
-          throw std::runtime_error("Requested size exceeded maximum clients.");
-        }
-
-        return client_pool_;
-      case HandleType::guard_condition_handle:
-        if (number_of_handles > bounds_.max_guard_conditions) {
-          throw std::runtime_error("Requested size exceeded maximum guard_conditions.");
-        }
-
-        return guard_condition_pool_;
-      default:
-        break;
-    }
-    throw std::runtime_error("Unrecognized enum, could not borrow handle memory.");
-  }
-
-  /// Return the borrowed handles by clearing the object pool for the correspondign type.
-  /**
-   * \param[in] The type of entity that this function is returning.
-   * \param[in] Pointer to the handles returned.
-   */
-  void return_handles(HandleType type, void ** handles)
-  {
-    (void)handles;
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (bounds_.max_subscriptions) {
-          memset(subscription_pool_, 0, bounds_.max_subscriptions * sizeof(void *));
-        }
-        break;
-      case HandleType::service_handle:
-        if (bounds_.max_services) {
-          memset(service_pool_, 0, bounds_.max_services * sizeof(void *));
-        }
-        break;
-      case HandleType::client_handle:
-        if (bounds_.max_clients) {
-          memset(client_pool_, 0, bounds_.max_clients * sizeof(void *));
-        }
-        break;
-      case HandleType::guard_condition_handle:
-        if (bounds_.max_guard_conditions) {
-          memset(guard_condition_pool_, 0, bounds_.max_guard_conditions * sizeof(void *));
-        }
-        break;
-      default:
-        throw std::runtime_error("Unrecognized enum, could not return handle memory.");
-    }
-  }
-
-  /// Instantiate the next executable by borrowing space from the preallocated executables pool.
-  // \return Shared pointer to the executable.
-  executor::AnyExecutable::SharedPtr instantiate_next_executable()
-  {
-    if (exec_seq_ >= bounds_.max_executables) {
-      // wrap around (ring buffer logic)
-      exec_seq_ = 0;
-    }
-    size_t prev_exec_seq_ = exec_seq_;
-    ++exec_seq_;
-
-    if (!executable_pool_[prev_exec_seq_]) {
-      throw std::runtime_error("Executable pool member was NULL");
-    }
-
-    // Make sure to clear the executable fields.
-    executable_pool_[prev_exec_seq_]->subscription.reset();
-    executable_pool_[prev_exec_seq_]->timer.reset();
-    executable_pool_[prev_exec_seq_]->service.reset();
-    executable_pool_[prev_exec_seq_]->client.reset();
-    executable_pool_[prev_exec_seq_]->callback_group.reset();
-    executable_pool_[prev_exec_seq_]->node.reset();
-
-    return executable_pool_[prev_exec_seq_];
-  }
-
-  /// General allocate: reserve space in the memory pool reserved by this function.
-  /**
-   * \param[in] size Number of bytes to allocate.
-   * \return Pointer to the allocated chunk of memory.
-   */
-  void * alloc(size_t size)
-  {
-    // Extremely naive static allocation strategy
-    // Keep track of block size at a given pointer
-    if (pool_seq_ + size > bounds_.pool_size) {
-      // Start at 0
-      pool_seq_ = 0;
-    }
-    void * ptr = memory_pool_[pool_seq_];
-    if (memory_map_.count(ptr) == 0) {
-      // We expect to have the state for all blocks pre-mapped into memory_map_
-      throw std::runtime_error("Unexpected pointer in StaticMemoryStrategy::alloc.");
-    }
-    memory_map_[ptr] = size;
-    size_t prev_pool_seq = pool_seq_;
-    pool_seq_ += size;
-    return memory_pool_[prev_pool_seq];
-  }
-
-  /// Release the allocated memory in the memory pool.
-  /**
-   * \param[in] Pointer to deallocate.
-   */
-  void free(void * ptr)
-  {
-    if (memory_map_.count(ptr) == 0) {
-      // We expect to have the state for all blocks pre-mapped into memory_map_
-      throw std::runtime_error("Unexpected pointer in StaticMemoryStrategy::free.");
-    }
-
-    memset(ptr, 0, memory_map_[ptr]);
-  }
-
-private:
-  ObjectPoolBounds bounds_;
-
-  void ** memory_pool_;
-  void ** subscription_pool_;
-  void ** service_pool_;
-  void ** client_pool_;
-  void ** guard_condition_pool_;
-  executor::AnyExecutable::SharedPtr * executable_pool_;
-
-  size_t pool_seq_;
-  size_t exec_seq_;
-
-  std::unordered_map<void *, size_t> memory_map_;
-};
-
-}  /* static_memory_strategy */
-
-}  /* memory_strategies */
-
-}  /* rclcpp */
-
-#endif

rclcpp/include/rclcpp/subscription.hpp

@@ -12,8 +12,11 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
-#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
+#ifndef RCLCPP__SUBSCRIPTION_HPP_
+#define RCLCPP__SUBSCRIPTION_HPP_
+
+#include <rmw/error_handling.h>
+#include <rmw/rmw.h>
 
 #include <functional>
 #include <iostream>
@@ -21,27 +24,19 @@
 #include <sstream>
 #include <string>
 
-#include <rcl_interfaces/msg/intra_process_message.hpp>
-#include <rmw/error_handling.h>
-#include <rmw/rmw.h>
+#include "rcl_interfaces/msg/intra_process_message.hpp"
 
-#include <rclcpp/macros.hpp>
-#include <rclcpp/message_memory_strategy.hpp>
-#include <rclcpp/any_subscription_callback.hpp>
+#include "rclcpp/macros.hpp"
+#include "rclcpp/message_memory_strategy.hpp"
+#include "rclcpp/any_subscription_callback.hpp"
 
 namespace rclcpp
 {
 
-// Forward declaration is for friend statement in SubscriptionBase
-namespace executor
-{
-class Executor;
-} // namespace executor
-
 namespace node
 {
 class Node;
-} // namespace node
+}  // namespace node
 
 namespace subscription
 {
@@ -50,8 +45,6 @@ namespace subscription
 /// specializations of Subscription, among other things.
 class SubscriptionBase
 {
-  friend class rclcpp::executor::Executor;
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
 
@@ -104,6 +97,16 @@ public:
     return this->topic_name_;
   }
 
+  const rmw_subscription_t * get_subscription_handle() const
+  {
+    return subscription_handle_;
+  }
+
+  const rmw_subscription_t * get_intra_process_subscription_handle() const
+  {
+    return intra_process_subscription_handle_;
+  }
+
   /// Borrow a new message.
   // \return Shared pointer to the fresh message.
   virtual std::shared_ptr<void> create_message() = 0;
@@ -135,18 +138,22 @@ private:
 
   std::string topic_name_;
   bool ignore_local_publications_;
-
 };
 
 using namespace any_subscription_callback;
 
 /// Subscription implementation, templated on the type of message this subscription receives.
-template<typename MessageT>
+template<typename MessageT, typename Alloc = std::allocator<void>>
 class Subscription : public SubscriptionBase
 {
   friend class rclcpp::node::Node;
 
 public:
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
 
   /// Default constructor.
@@ -164,15 +171,17 @@ public:
     rmw_subscription_t * subscription_handle,
     const std::string & topic_name,
     bool ignore_local_publications,
-    AnySubscriptionCallback<MessageT> callback,
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr memory_strategy =
-    message_memory_strategy::MessageMemoryStrategy<MessageT>::create_default())
+    AnySubscriptionCallback<MessageT, Alloc> callback,
+    typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr memory_strategy =
+    message_memory_strategy::MessageMemoryStrategy<MessageT,
+    Alloc>::create_default())
   : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications),
     any_callback_(callback),
     message_memory_strategy_(memory_strategy),
     get_intra_process_message_callback_(nullptr),
     matches_any_intra_process_publishers_(nullptr)
-  {}
+  {
+  }
 
   /// Support dynamically setting the message memory strategy.
   /**
@@ -180,7 +189,8 @@ public:
    * \param[in] message_memory_strategy Shared pointer to the memory strategy to set.
    */
   void set_message_memory_strategy(
-    typename message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr message_memory_strategy)
+    typename message_memory_strategy::MessageMemoryStrategy<MessageT,
+    Alloc>::SharedPtr message_memory_strategy)
   {
     message_memory_strategy_ = message_memory_strategy;
   }
@@ -203,19 +213,7 @@ public:
       }
     }
     auto typed_message = std::static_pointer_cast<MessageT>(message);
-    if (any_callback_.shared_ptr_callback) {
-      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_.unique_ptr_callback) {
-      std::unique_ptr<MessageT> unique_msg(new MessageT(*typed_message));
-      any_callback_.unique_ptr_callback(unique_msg);
-    } 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);
-    } else {
-      throw std::runtime_error("unexpected message without any callback set");
-    }
+    any_callback_.dispatch(typed_message, message_info);
   }
 
   void return_message(std::shared_ptr<void> & message)
@@ -235,7 +233,7 @@ public:
       // However, this can only really happen if this node has it disabled, but the other doesn't.
       return;
     }
-    std::unique_ptr<MessageT> msg;
+    MessageUniquePtr msg;
     get_intra_process_message_callback_(
       ipm.publisher_id,
       ipm.message_sequence,
@@ -247,25 +245,13 @@ public:
       // TODO(wjwwood): should we notify someone of this? log error, log warning?
       return;
     }
-    if (any_callback_.shared_ptr_callback) {
-      typename MessageT::SharedPtr shared_msg = std::move(msg);
-      any_callback_.shared_ptr_callback(shared_msg);
-    } 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_.unique_ptr_callback) {
-      any_callback_.unique_ptr_callback(msg);
-    } else if (any_callback_.unique_ptr_with_info_callback) {
-      any_callback_.unique_ptr_with_info_callback(msg, message_info);
-    } else {
-      throw std::runtime_error("unexpected message without any callback set");
-    }
+    any_callback_.dispatch_intra_process(msg, message_info);
   }
 
 private:
   typedef
     std::function<
-      void (uint64_t, uint64_t, uint64_t, std::unique_ptr<MessageT> &)
+      void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &)
     > GetMessageCallbackType;
   typedef std::function<bool (const rmw_gid_t *)> MatchesAnyPublishersCallbackType;
 
@@ -283,17 +269,16 @@ private:
 
   RCLCPP_DISABLE_COPY(Subscription);
 
-  AnySubscriptionCallback<MessageT> any_callback_;
-  typename message_memory_strategy::MessageMemoryStrategy<MessageT>::SharedPtr
+  AnySubscriptionCallback<MessageT, Alloc> any_callback_;
+  typename message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
   message_memory_strategy_;
 
   GetMessageCallbackType get_intra_process_message_callback_;
   MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_;
   uint64_t intra_process_subscription_id_;
-
 };
 
-} /* namespace subscription */
-} /* namespace rclcpp */
+}  // namespace subscription
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */
+#endif  // RCLCPP__SUBSCRIPTION_HPP_

rclcpp/include/rclcpp/timer.hpp

@@ -31,12 +31,6 @@
 namespace rclcpp
 {
 
-// Forward declaration is for friend statement in GenericTimer
-namespace executor
-{
-class Executor;
-} // namespace executor
-
 namespace timer
 {
 
@@ -44,7 +38,6 @@ using CallbackType = std::function<void()>;
 
 class TimerBase
 {
-  friend class rclcpp::executor::Executor;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase);
@@ -66,6 +59,16 @@ public:
     this->canceled_ = true;
   }
 
+  void execute_callback() const
+  {
+    callback_();
+  }
+
+  const CallbackType & get_callback() const
+  {
+    return callback_;
+  }
+
   /// Check how long the timer has until its next scheduled callback.
   // \return A std::chrono::duration representing the relative time until the next callback.
   virtual std::chrono::nanoseconds
@@ -95,7 +98,6 @@ protected:
 template<class Clock = std::chrono::high_resolution_clock>
 class GenericTimer : public TimerBase
 {
-  friend class rclcpp::executor::Executor;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer);

rclcpp/include/rclcpp/utilities.hpp

@@ -45,6 +45,7 @@ rmw_guard_condition_t * g_sigint_guard_cond_handle = \
   rmw_create_guard_condition();
 /// Condition variable for timed sleep (see sleep_for).
 std::condition_variable g_interrupt_condition_variable;
+std::atomic<bool> g_is_interrupted(false);
 /// Mutex for protecting the global condition variable.
 std::mutex g_interrupt_mutex;
 
@@ -95,6 +96,7 @@ signal_handler(int signal_value)
     fprintf(stderr,
       "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
   }
+  g_is_interrupted.store(true);
   g_interrupt_condition_variable.notify_all();
 }
 } // namespace
@@ -117,6 +119,7 @@ init(int argc, char * argv[])
 {
   (void)argc;
   (void)argv;
+  g_is_interrupted.store(false);
   rmw_ret_t status = rmw_init();
   if (status != RMW_RET_OK) {
     // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
@@ -140,7 +143,15 @@ init(int argc, char * argv[])
     const size_t error_length = 1024;
     char error_string[error_length];
 #ifndef _WIN32
-    strerror_r(errno, error_string, error_length);
+    auto rc = strerror_r(errno, error_string, error_length);
+    if (rc) {
+      // *INDENT-OFF*
+      throw std::runtime_error(
+        "Failed to set SIGINT signal handler: (" + std::to_string(errno) +
+        ") unable to retrieve error string");
+      // *INDENT-ON*
+    }
+
 #else
     strerror_s(error_string, error_length, errno);
 #endif
@@ -170,6 +181,7 @@ shutdown()
     fprintf(stderr,
       "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe());
   }
+  g_is_interrupted.store(true);
   g_interrupt_condition_variable.notify_all();
 }
 
@@ -190,10 +202,18 @@ bool
 sleep_for(const std::chrono::nanoseconds & nanoseconds)
 {
   // TODO: determine if posix's nanosleep(2) is more efficient here
-  std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
-  auto cvs = ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
+  std::chrono::nanoseconds time_left = nanoseconds;
+  {
+    std::unique_lock<std::mutex> lock(::g_interrupt_mutex);
+    auto start = std::chrono::steady_clock::now();
+    ::g_interrupt_condition_variable.wait_for(lock, nanoseconds);
+    time_left -= std::chrono::steady_clock::now() - start;
+  }
+  if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) {
+    return sleep_for(time_left);
+  }
   // Return true if the timeout elapsed successfully, otherwise false.
-  return cvs != std::cv_status::no_timeout;
+  return !g_is_interrupted;
 }
 
 } /* namespace utilities */

rclcpp/test/test_function_traits.cpp

@@ -0,0 +1,457 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/function_traits.hpp>
+
+int func_no_args()
+{
+  return 0;
+}
+
+int func_one_int(int)
+{
+  return 1;
+}
+
+int func_two_ints(int, int)
+{
+  return 2;
+}
+
+int func_one_int_one_char(int, char)
+{
+  return 3;
+}
+
+struct FunctionObjectNoArgs
+{
+  int operator()() const
+  {
+    return 0;
+  }
+};
+
+struct FunctionObjectOneInt
+{
+  int operator()(int) const
+  {
+    return 1;
+  }
+};
+
+struct FunctionObjectTwoInts
+{
+  int operator()(int, int) const
+  {
+    return 2;
+  }
+};
+
+struct FunctionObjectOneIntOneChar
+{
+  int operator()(int, char) const
+  {
+    return 3;
+  }
+};
+
+template<
+  typename FunctorT,
+  std::size_t Arity = 0,
+  typename std::enable_if<rclcpp::arity_comparator<Arity, FunctorT>::value>::type * = nullptr
+>
+int func_accept_callback(FunctorT callback)
+{
+  return callback();
+}
+
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::check_arguments<FunctorT, int>::value
+  >::type * = nullptr
+>
+int func_accept_callback(FunctorT callback)
+{
+  int a = 4;
+  return callback(a);
+}
+
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::check_arguments<FunctorT, int, int>::value
+  >::type * = nullptr
+>
+int func_accept_callback(FunctorT callback)
+{
+  int a = 5;
+  int b = 6;
+  return callback(a, b);
+}
+
+template<
+  typename FunctorT,
+  typename std::enable_if<
+    rclcpp::check_arguments<FunctorT, int, char>::value
+  >::type * = nullptr
+>
+int func_accept_callback(FunctorT callback)
+{
+  int a = 7;
+  char b = 8;
+  return callback(a, b);
+}
+
+/*
+   Tests that funcion_traits calculates arity of several functors.
+ */
+TEST(TestFunctionTraits, arity) {
+  // Test regular functions
+  static_assert(
+    rclcpp::function_traits<decltype(func_no_args)>::arity == 0,
+    "Functor does not accept arguments");
+
+  static_assert(
+    rclcpp::function_traits<decltype(func_one_int)>::arity == 1,
+    "Functor only accepts one argument");
+
+  static_assert(
+    rclcpp::function_traits<decltype(func_two_ints)>::arity == 2,
+    "Functor only accepts two arguments");
+
+  static_assert(
+    rclcpp::function_traits<decltype(func_one_int_one_char)>::arity == 2,
+    "Functor only accepts two arguments");
+
+  // Test lambdas
+  auto lambda_no_args = []() {
+      return 0;
+    };
+
+  auto lambda_one_int = [](int) {
+      return 1;
+    };
+
+  auto lambda_two_ints = [](int, int) {
+      return 2;
+    };
+
+  auto lambda_one_int_one_char = [](int, char) {
+      return 3;
+    };
+
+  static_assert(
+    rclcpp::function_traits<decltype(lambda_no_args)>::arity == 0,
+    "Functor does not accept arguments");
+
+  static_assert(
+    rclcpp::function_traits<decltype(lambda_one_int)>::arity == 1,
+    "Functor only accepts one argument");
+
+  static_assert(
+    rclcpp::function_traits<decltype(lambda_two_ints)>::arity == 2,
+    "Functor only accepts two arguments");
+
+  static_assert(
+    rclcpp::function_traits<decltype(lambda_one_int_one_char)>::arity == 2,
+    "Functor only accepts two arguments");
+
+  // Test objects that have a call operator
+  static_assert(
+    rclcpp::function_traits<FunctionObjectNoArgs>::arity == 0,
+    "Functor does not accept arguments");
+
+  static_assert(
+    rclcpp::function_traits<FunctionObjectOneInt>::arity == 1,
+    "Functor only accepts one argument");
+
+  static_assert(
+    rclcpp::function_traits<FunctionObjectTwoInts>::arity == 2,
+    "Functor only accepts two arguments");
+
+  static_assert(
+    rclcpp::function_traits<FunctionObjectOneIntOneChar>::arity == 2,
+    "Functor only accepts two arguments");
+}
+
+/*
+   Tests that funcion_traits deducts the type of the arguments of several functors.
+ */
+TEST(TestFunctionTraits, argument_types) {
+  // Test regular functions
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(func_one_int)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(func_two_ints)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(func_two_ints)>::template argument_type<1>
+    >::value, "Functor accepts an int as second argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(func_one_int_one_char)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      char,
+      rclcpp::function_traits<decltype(func_one_int_one_char)>::template argument_type<1>
+    >::value, "Functor accepts a char as second argument");
+
+  // Test lambdas
+  auto lambda_one_int = [](int) {
+      return 1;
+    };
+
+  auto lambda_two_ints = [](int, int) {
+      return 2;
+    };
+
+  auto lambda_one_int_one_char = [](int, char) {
+      return 3;
+    };
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(lambda_one_int)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(lambda_two_ints)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(lambda_two_ints)>::template argument_type<1>
+    >::value, "Functor accepts an int as second argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<decltype(lambda_one_int_one_char)>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      char,
+      rclcpp::function_traits<decltype(lambda_one_int_one_char)>::template argument_type<1>
+    >::value, "Functor accepts a char as second argument");
+
+  // Test objects that have a call operator
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<FunctionObjectOneInt>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<FunctionObjectTwoInts>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<FunctionObjectTwoInts>::template argument_type<1>
+    >::value, "Functor accepts an int as second argument");
+
+  static_assert(
+    std::is_same<
+      int,
+      rclcpp::function_traits<FunctionObjectOneIntOneChar>::template argument_type<0>
+    >::value, "Functor accepts an int as first argument");
+
+  static_assert(
+    std::is_same<
+      char,
+      rclcpp::function_traits<FunctionObjectOneIntOneChar>::template argument_type<1>
+    >::value, "Functor accepts a char as second argument");
+}
+
+/*
+   Tests that funcion_traits checks the types of the arguments of several functors.
+ */
+TEST(TestFunctionTraits, check_arguments) {
+  // Test regular functions
+  static_assert(
+    rclcpp::check_arguments<decltype(func_one_int), int>::value,
+    "Functor accepts a single int as arguments");
+
+  static_assert(
+    !rclcpp::check_arguments<decltype(func_one_int), char>::value,
+    "Functor does not accept a char as argument");
+
+  static_assert(
+    !rclcpp::check_arguments<decltype(func_one_int), char, int>::value,
+    "Functor does not accept two arguments");
+
+  static_assert(
+    !rclcpp::check_arguments<decltype(func_two_ints), int>::value,
+    "Functor does not accept a single int as argument, requires two ints");
+
+  static_assert(
+    rclcpp::check_arguments<decltype(func_two_ints), int, int>::value,
+    "Functor accepts two ints as arguments");
+
+  static_assert(
+    !rclcpp::check_arguments<decltype(func_two_ints), bool, int>::value,
+    "Functor does not accept a bool and an int as arguments, requires two ints");
+
+  static_assert(
+    !rclcpp::check_arguments<decltype(func_two_ints), int, char>::value,
+    "Functor does not accept an int and a char as arguments, requires two ints");
+
+  static_assert(
+    rclcpp::check_arguments<decltype(func_one_int_one_char), int, char>::value,
+    "Functor accepts an int and a char as arguments");
+
+  // Test lambdas
+  auto lambda_one_int = [](int) {
+      return 1;
+    };
+
+  auto lambda_two_ints = [](int, int) {
+      return 2;
+    };
+
+  auto lambda_one_int_one_char = [](int, char) {
+      return 3;
+    };
+
+  static_assert(
+    rclcpp::check_arguments<decltype(lambda_one_int), int>::value,
+    "Functor accepts an int as the only argument");
+
+  static_assert(
+    rclcpp::check_arguments<decltype(lambda_two_ints), int, int>::value,
+    "Functor accepts two ints as arguments");
+
+  static_assert(
+    rclcpp::check_arguments<decltype(lambda_one_int_one_char), int, char>::value,
+    "Functor accepts an int and a char as arguments");
+
+  // Test objects that have a call operator
+  static_assert(
+    rclcpp::check_arguments<FunctionObjectOneInt, int>::value,
+    "Functor accepts an int as the only argument");
+
+  static_assert(
+    rclcpp::check_arguments<FunctionObjectTwoInts, int, int>::value,
+    "Functor accepts two ints as arguments");
+
+  static_assert(
+    rclcpp::check_arguments<FunctionObjectOneIntOneChar, int, char>::value,
+    "Functor accepts an int and a char as arguments");
+}
+
+/*
+   Tests that same_arguments work.
+*/
+TEST(TestFunctionTraits, same_arguments) {
+  auto lambda_one_int = [](int) {
+      return 1;
+    };
+
+  auto lambda_two_ints = [](int, int) {
+      return 1;
+    };
+
+  static_assert(
+    rclcpp::same_arguments<decltype(lambda_one_int), decltype(func_one_int)>::value,
+    "Lambda and function have the same arguments");
+
+  static_assert(
+    !rclcpp::same_arguments<decltype(lambda_two_ints), decltype(func_one_int)>::value,
+    "Lambda and function have different arguments");
+
+  static_assert(
+    !rclcpp::same_arguments<decltype(func_one_int_one_char), decltype(func_two_ints)>::value,
+    "Functions have different arguments");
+
+  static_assert(
+    !rclcpp::same_arguments<decltype(lambda_one_int), decltype(lambda_two_ints)>::value,
+    "Lambdas have different arguments");
+
+  static_assert(
+    rclcpp::same_arguments<FunctionObjectTwoInts, decltype(func_two_ints)>::value,
+    "Functor and function have the same arguments");
+
+  static_assert(
+    rclcpp::same_arguments<FunctionObjectTwoInts, decltype(lambda_two_ints)>::value,
+    "Functor and lambda have the same arguments");
+}
+
+/*
+   Tests that functions are matched via SFINAE.
+ */
+TEST(TestFunctionTraits, sfinae_match) {
+  EXPECT_EQ(0, func_accept_callback(func_no_args));
+
+  EXPECT_EQ(1, func_accept_callback(func_one_int));
+
+  EXPECT_EQ(2, func_accept_callback(func_two_ints));
+
+  EXPECT_EQ(3, func_accept_callback(func_one_int_one_char));
+
+  auto lambda_no_args = []() {
+      return 0;
+    };
+
+  auto lambda_one_int = [](int) {
+      return 1;
+    };
+
+  auto lambda_two_ints = [](int, int) {
+      return 2;
+    };
+
+  auto lambda_one_int_one_char = [](int, char) {
+      return 3;
+    };
+
+  EXPECT_EQ(0, func_accept_callback(lambda_no_args));
+
+  EXPECT_EQ(1, func_accept_callback(lambda_one_int));
+
+  EXPECT_EQ(2, func_accept_callback(lambda_two_ints));
+
+  EXPECT_EQ(3, func_accept_callback(lambda_one_int_one_char));
+
+  EXPECT_EQ(0, func_accept_callback(FunctionObjectNoArgs()));
+
+  EXPECT_EQ(1, func_accept_callback(FunctionObjectOneInt()));
+
+  EXPECT_EQ(2, func_accept_callback(FunctionObjectTwoInts()));
+
+  EXPECT_EQ(3, func_accept_callback(FunctionObjectOneIntOneChar()));
+}

rclcpp/test/test_intra_process_manager.cpp

@@ -16,6 +16,7 @@
 
 #include <gtest/gtest.h>
 
+#include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/macros.hpp>
 #include <rmw/types.h>
 
@@ -27,12 +28,12 @@ namespace publisher
 namespace mock
 {
 
-class Publisher
+class PublisherBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher);
+  RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase);
 
-  Publisher()
+  PublisherBase()
   : mock_topic_name(""), mock_queue_size(0) {}
 
   const std::string & get_topic_name() const
@@ -55,6 +56,29 @@ public:
   size_t mock_queue_size;
 };
 
+template<typename T, typename Alloc = std::allocator<void>>
+class Publisher : public PublisherBase
+{
+public:
+  using MessageAllocTraits = allocator::AllocRebind<T, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, T>;
+  using MessageUniquePtr = std::unique_ptr<T, MessageDeleter>;
+  std::shared_ptr<MessageAlloc> allocator_;
+
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>);
+
+  Publisher()
+  {
+    allocator_ = std::make_shared<MessageAlloc>();
+  }
+
+  std::shared_ptr<MessageAlloc> get_allocator()
+  {
+    return allocator_;
+  }
+};
+
 }
 }
 }
@@ -92,14 +116,16 @@ public:
 }
 
 // Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported.
-#define RCLCPP_RCLCPP_PUBLISHER_HPP_
-#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_
+#define RCLCPP__PUBLISHER_HPP_
+#define RCLCPP__SUBSCRIPTION_HPP_
 // Force ipm to use our mock publisher class.
 #define Publisher mock::Publisher
+#define PublisherBase mock::PublisherBase
 #define SubscriptionBase mock::SubscriptionBase
 #include <rclcpp/intra_process_manager.hpp>
 #undef SubscriptionBase
 #undef Publisher
+#undef PublisherBase
 
 #include <rcl_interfaces/msg/intra_process_message.hpp>
 
@@ -116,11 +142,13 @@ public:
 TEST(TestIntraProcessManager, nominal) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 2;
 
-  auto p2 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p2 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p2->mock_topic_name = "nominal2";
   p2->mock_queue_size = 10;
 
@@ -128,8 +156,10 @@ TEST(TestIntraProcessManager, nominal) {
   s1->mock_topic_name = "nominal1";
   s1->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
-  auto p2_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p2);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
+  auto p2_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p2);
   auto s1_id = ipm.add_subscription(s1);
 
   auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();
@@ -201,7 +231,8 @@ TEST(TestIntraProcessManager, nominal) {
 TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
@@ -209,7 +240,8 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
   s1->mock_topic_name = "nominal1";
   s1->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
   auto s1_id = ipm.add_subscription(s1);
 
   auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();
@@ -240,7 +272,8 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) {
 TEST(TestIntraProcessManager, removed_subscription_affects_take) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
@@ -256,7 +289,8 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) {
   s3->mock_topic_name = "nominal1";
   s3->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
   auto s1_id = ipm.add_subscription(s1);
   auto s2_id = ipm.add_subscription(s2);
   auto s3_id = ipm.add_subscription(s3);
@@ -308,7 +342,8 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) {
 TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
@@ -324,7 +359,8 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
   s3->mock_topic_name = "nominal1";
   s3->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
   auto s1_id = ipm.add_subscription(s1);
   auto s2_id = ipm.add_subscription(s2);
   auto s3_id = ipm.add_subscription(s3);
@@ -377,15 +413,18 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) {
 TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
-  auto p2 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p2 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p2->mock_topic_name = "nominal1";
   p2->mock_queue_size = 10;
 
-  auto p3 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p3 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p3->mock_topic_name = "nominal1";
   p3->mock_queue_size = 10;
 
@@ -393,9 +432,12 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
   s1->mock_topic_name = "nominal1";
   s1->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
-  auto p2_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p2);
-  auto p3_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p3);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
+  auto p2_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p2);
+  auto p3_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p3);
   auto s1_id = ipm.add_subscription(s1);
 
   auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();
@@ -468,15 +510,18 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) {
 TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 10;
 
-  auto p2 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p2 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p2->mock_topic_name = "nominal1";
   p2->mock_queue_size = 10;
 
-  auto p3 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p3 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p3->mock_topic_name = "nominal1";
   p3->mock_queue_size = 10;
 
@@ -492,9 +537,12 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
   s3->mock_topic_name = "nominal1";
   s3->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
-  auto p2_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p2);
-  auto p3_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p3);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
+  auto p2_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p2);
+  auto p3_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p3);
   auto s1_id = ipm.add_subscription(s1);
   auto s2_id = ipm.add_subscription(s2);
   auto s3_id = ipm.add_subscription(s3);
@@ -626,7 +674,8 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) {
 TEST(TestIntraProcessManager, ring_buffer_displacement) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 2;
 
@@ -634,7 +683,8 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) {
   s1->mock_topic_name = "nominal1";
   s1->mock_queue_size = 10;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
   auto s1_id = ipm.add_subscription(s1);
 
   auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();
@@ -694,11 +744,13 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) {
 TEST(TestIntraProcessManager, subscription_creation_race_condition) {
   rclcpp::intra_process_manager::IntraProcessManager ipm;
 
-  auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+  auto p1 =
+    std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
   p1->mock_topic_name = "nominal1";
   p1->mock_queue_size = 2;
 
-  auto p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+  auto p1_id =
+    ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
 
   auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();
   ipm_msg->message_sequence = 42;
@@ -740,11 +792,12 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) {
   uint64_t p1_id;
   uint64_t p1_m1_id;
   {
-    auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+    auto p1 =
+      std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
     p1->mock_topic_name = "nominal1";
     p1->mock_queue_size = 2;
 
-    p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+    p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
 
     auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();
     ipm_msg->message_sequence = 42;
@@ -777,11 +830,12 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_store) {
 
   uint64_t p1_id;
   {
-    auto p1 = std::make_shared<rclcpp::publisher::mock::Publisher>();
+    auto p1 =
+      std::make_shared<rclcpp::publisher::mock::Publisher<rcl_interfaces::msg::IntraProcessMessage>>();
     p1->mock_topic_name = "nominal1";
     p1->mock_queue_size = 2;
 
-    p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage>(p1);
+    p1_id = ipm.add_publisher<rcl_interfaces::msg::IntraProcessMessage, std::allocator<void>>(p1);
   }
 
   auto ipm_msg = std::make_shared<rcl_interfaces::msg::IntraProcessMessage>();