From 2e68bd54383494360ba07bdc4e933640663ebb57 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Wed, 14 Oct 2015 14:14:16 -0700
Subject: [PATCH 01/12] publisher template on allocator

---
 .../rclcpp/allocator/allocator_deleter.hpp    |  56 +++++++
 .../rclcpp/allocator/allocator_factory.hpp    |  69 +++++++++
 .../rclcpp/allocator/allocator_wrapper.hpp    | 141 ++++++++++++++++++
 rclcpp/include/rclcpp/publisher.hpp           |  26 +++-
 4 files changed, 286 insertions(+), 6 deletions(-)
 create mode 100644 rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
 create mode 100644 rclcpp/include/rclcpp/allocator/allocator_factory.hpp
 create mode 100644 rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp

diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
new file mode 100644
index 0000000..a757a8a
--- /dev/null
+++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
@@ -0,0 +1,56 @@
+// 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_ALLOCATOR_DELETER_HPP_
+#define RCLCPP_RCLCPP_ALLOCATOR_DELETER_HPP_
+
+#include <memory>
+
+template<typename T, typename Allocator>
+class AllocatorDeleter
+{
+public:
+  AllocatorDeleter()
+  : allocator_(new Allocator)
+  {
+  }
+
+  AllocatorDeleter(Allocator * a)
+  : allocator_(a)
+  {
+  }
+
+  template<typename U, typename B>
+  AllocatorDeleter(const AllocatorDeleter<U, B> & a)
+  {
+    allocator_ = a.get_allocator();
+  }
+
+  void operator()(T * ptr)
+  {
+    std::allocator_traits<Allocator>::destroy(*allocator_, ptr);
+    std::allocator_traits<Allocator>::deallocate(*allocator_, ptr, sizeof(T));
+    ptr = NULL;
+  }
+
+  Allocator * get_allocator() const
+  {
+    return allocator_;
+  }
+
+private:
+  Allocator * allocator_;
+};
+
+#endif
diff --git a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
new file mode 100644
index 0000000..4b58b74
--- /dev/null
+++ b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
@@ -0,0 +1,69 @@
+// 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_ALLOCATOR_FACTORY_HPP_
+#define RCLCPP_RCLCPP_ALLOCATOR_FACTORY_HPP_
+
+#include <memory>
+#include <iostream>
+#include <rclcpp/allocator/allocator_deleter.hpp>
+
+template<template<typename> class Alloc, typename T, typename D>
+D initialize_deleter(Alloc<T> * alloc)
+{
+  (void) alloc;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T>
+std::default_delete<T> initialize_deleter(std::allocator<T> * alloc)
+{
+  (void) alloc;
+  return std::default_delete<T>();
+}
+
+template<template<typename> class Alloc, typename T>
+AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
+{
+  if (!alloc) {
+    throw std::invalid_argument("Allocator argument was NULL");
+  }
+  return AllocatorDeleter<T, Alloc<T>>(alloc);
+}
+
+/*
+class AllocatorFactoryBase
+{
+public:
+  // Hmm
+  template<typename T, typename AllocT>
+  virtual void AllocT<typename T> * request_allocator(...) = 0;
+
+  template<typename T, typename AllocT>
+  virtual void return_allocator(AllocT<typename T> * allocator);
+
+  template<typename T, typename AllocT, typename DeleterT>
+  virtual DeleterT<T> * get_deleter_for_allocator(AllocT<typename T> * allocator) = 0;
+
+};
+*/
+
+template<typename T, template<typename> class Alloc>
+using Deleter = typename std::conditional<
+    std::is_same<Alloc<T>, std::allocator<T>>::value,
+    std::default_delete<T>,
+    AllocatorDeleter<T, Alloc<T>>
+    >::type;
+
+#endif
diff --git a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
new file mode 100644
index 0000000..30f6f7b
--- /dev/null
+++ b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
@@ -0,0 +1,141 @@
+// 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_ALLOCATOR_WRAPPER_HPP_
+#define RCLCPP_RCLCPP_ALLOCATOR_WRAPPER_HPP_
+
+#include <memory>
+#include <iostream>
+#include <rclcpp/allocator_deleter.hpp>
+
+
+template<typename Alloc, typename T, typename D>
+void initialize_deleter(D * deleter, Alloc * alloc)
+{
+  (void) deleter;
+  (void) alloc;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T>
+void initialize_deleter(std::default_delete<T> * deleter, std::allocator<T> * alloc)
+{
+  (void) alloc;
+  deleter = new std::default_delete<T>;
+  if (!deleter) {
+    throw std::runtime_error("initialize_deleter failed");
+  }
+}
+
+template<typename Alloc, typename T>
+void initialize_deleter(AllocatorDeleter<T, Alloc> * deleter, Alloc * alloc)
+{
+  if (!alloc) {
+    throw std::invalid_argument("Allocator argument was NULL");
+  }
+  deleter = new AllocatorDeleter<T, Alloc>(alloc);
+  if (!deleter) {
+    throw std::runtime_error("initialize_deleter failed");
+  }
+}
+
+template<typename T, typename Alloc>
+class AllocatorWrapper
+{
+public:
+  using Deleter = typename std::conditional<
+      std::is_same<Alloc, std::allocator<T>>::value,
+      std::default_delete<T>,
+      AllocatorDeleter<T, Alloc>
+      >::type;
+
+  AllocatorWrapper(Alloc * allocator)
+  : allocator_(allocator)
+  {
+    if (!allocator_) {
+      throw std::invalid_argument("Allocator argument was NULL");
+    }
+    initialize_deleter(deleter_, allocator_);
+    if (deleter_ == NULL) {
+      throw std::invalid_argument("Failed to initialize deleter");
+    }
+  }
+
+  AllocatorWrapper(Alloc * allocator, Deleter * deleter)
+  : allocator_(allocator), deleter_(deleter)
+  {
+    if (!allocator_) {
+      throw std::invalid_argument("Allocator argument was NULL");
+    }
+    if (!deleter_) {
+      throw std::invalid_argument("Deleter argument was NULL");
+    }
+  }
+
+  AllocatorWrapper(Alloc & allocator)
+  {
+    allocator_ = &allocator;
+    if (!allocator_) {
+      throw std::invalid_argument("Allocator argument was NULL");
+    }
+    initialize_deleter(deleter_, allocator_);
+    if (!deleter_) {
+      throw std::invalid_argument("Failed to initialize deleter");
+    }
+  }
+
+  AllocatorWrapper()
+  {
+    allocator_ = new Alloc();
+    initialize_deleter(deleter_, allocator_);
+    if (!deleter_) {
+      //throw std::invalid_argument("Failed to initialize deleter");
+      deleter_ = new Deleter;
+    }
+  }
+
+  T * allocate(size_t size)
+  {
+    return std::allocator_traits<Alloc>::allocate(*allocator_, size);
+  }
+
+  T * deallocate(void * pointer, size_t size)
+  {
+    std::allocator_traits<Alloc>::deallocate(*allocator_, pointer, size);
+  }
+
+  template<class ... Args>
+  void construct(T * pointer, Args && ... args)
+  {
+    std::allocator_traits<Alloc>::construct(*allocator_, pointer, std::forward<Args>(args)...);
+  }
+
+  Deleter * get_deleter() const
+  {
+    return deleter_;
+  }
+  Alloc * get_underlying_allocator() const
+  {
+    return allocator_;
+  }
+
+private:
+  Alloc * allocator_;
+  Deleter * deleter_;
+};
+
+template<typename T>
+using DefaultAllocator = AllocatorWrapper<T, std::allocator<T>>;
+
+#endif
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index d75ed05..f81e689 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -28,6 +28,8 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
+#include <rclcpp/allocator/allocator_factory.hpp>
+
 namespace rclcpp
 {
 
@@ -203,13 +205,13 @@ protected:
 };
 
 /// A publisher publishes messages of any type to a topic.
-template<typename MessageT>
+template<typename MessageT, template<typename> class Alloc = std::allocator>
 class Publisher : public PublisherBase
 {
   friend rclcpp::node::Node;
 
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT>);
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
 
   Publisher(
     std::shared_ptr<rmw_node_t> node_handle,
@@ -217,7 +219,10 @@ public:
     std::string topic,
     size_t queue_size)
   : PublisherBase(node_handle, publisher_handle, topic, queue_size)
-  {}
+  {
+    // TODO: avoid messy initialization
+    message_deleter_ = initialize_deleter(&message_allocator_);
+  }
 
 
   /// Send a message to the topic for this publisher.
@@ -274,7 +279,9 @@ public:
     //   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()));
+    auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
+    std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
+    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -291,7 +298,9 @@ public:
     //   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()));
+    auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
+    std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
+    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -304,7 +313,9 @@ public:
       return this->do_inter_process_publish(&msg);
     }
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    std::unique_ptr<MessageT> unique_msg(new MessageT(msg));
+    auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
+    std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, msg);
+    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -321,6 +332,9 @@ protected:
     }
   }
 
+  Alloc<MessageT> message_allocator_;
+  Deleter<MessageT, Alloc> message_deleter_;
+
 };
 
 } /* namespace publisher */

From 444e4fdae39147985e81fa319ab5e43ce989077f Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Thu, 15 Oct 2015 17:47:03 -0700
Subject: [PATCH 02/12] templated stl structures exposed in Node

---
 .../rclcpp/allocator/allocator_deleter.hpp    | 71 +++++++++++++++-
 .../rclcpp/allocator/allocator_factory.hpp    | 17 ----
 .../rclcpp/allocator/allocator_wrapper.hpp    | 80 ++++++++++---------
 rclcpp/include/rclcpp/node.hpp                | 29 ++++---
 rclcpp/include/rclcpp/node_impl.hpp           |  5 ++
 rclcpp/include/rclcpp/publisher.hpp           | 13 +--
 6 files changed, 145 insertions(+), 70 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
index a757a8a..160c4e5 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
@@ -17,12 +17,18 @@
 
 #include <memory>
 
+namespace rclcpp
+{
+
+namespace allocator
+{
+
 template<typename T, typename Allocator>
 class AllocatorDeleter
 {
 public:
   AllocatorDeleter()
-  : allocator_(new Allocator)
+  : allocator_(NULL)
   {
   }
 
@@ -49,8 +55,71 @@ public:
     return allocator_;
   }
 
+  void set_allocator(Allocator * alloc)
+  {
+    allocator_ = alloc;
+  }
+
 private:
   Allocator * allocator_;
 };
 
+/*
+template<template<typename> class Alloc, typename T, typename D>
+D initialize_deleter(Alloc<T> * alloc)
+{
+  (void) alloc;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T>
+std::default_delete<T> initialize_deleter(std::allocator<T> * alloc)
+{
+  (void) alloc;
+  return std::default_delete<T>();
+}
+
+template<template<typename> class Alloc, typename T>
+AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
+{
+  if (!alloc) {
+    throw std::invalid_argument("Allocator argument was NULL");
+  }
+  return AllocatorDeleter<T, Alloc<T>>(alloc);
+}
+*/
+template<template<typename> class Alloc, typename T, typename D>
+void set_allocator_for_deleter(D * deleter, Alloc<T> * alloc)
+{
+  (void) alloc;
+  throw std::runtime_error("Reached unexpected template specialization");
+}
+
+template<typename T>
+void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<T> * alloc)
+{
+  (void) deleter;
+  (void) alloc;
+}
+
+template<template<typename> class Alloc, typename T>
+void set_allocator_for_deleter(AllocatorDeleter<T, Alloc<T>> * deleter, Alloc<T> * alloc)
+{
+  if (!deleter || !alloc) {
+    throw std::invalid_argument("Argument was NULL to set_allocator_for_deleter");
+  }
+  //return AllocatorDeleter<T, Alloc<T>>(alloc);
+  deleter->set_allocator(alloc);
+}
+
+template<typename Alloc, typename T>
+using Deleter = typename std::conditional<
+    std::is_same<Alloc, std::allocator<T>>::value,
+    std::default_delete<T>,
+    AllocatorDeleter<T, Alloc>
+    >::type;
+
+}
+}
+
 #endif
diff --git a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
index 4b58b74..2a98b1f 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
@@ -42,23 +42,6 @@ AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
   return AllocatorDeleter<T, Alloc<T>>(alloc);
 }
 
-/*
-class AllocatorFactoryBase
-{
-public:
-  // Hmm
-  template<typename T, typename AllocT>
-  virtual void AllocT<typename T> * request_allocator(...) = 0;
-
-  template<typename T, typename AllocT>
-  virtual void return_allocator(AllocT<typename T> * allocator);
-
-  template<typename T, typename AllocT, typename DeleterT>
-  virtual DeleterT<T> * get_deleter_for_allocator(AllocT<typename T> * allocator) = 0;
-
-};
-*/
-
 template<typename T, template<typename> class Alloc>
 using Deleter = typename std::conditional<
     std::is_same<Alloc<T>, std::allocator<T>>::value,
diff --git a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
index 30f6f7b..99fc421 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
@@ -17,50 +17,39 @@
 
 #include <memory>
 #include <iostream>
-#include <rclcpp/allocator_deleter.hpp>
+#include <rclcpp/allocator/allocator_deleter.hpp>
 
-
-template<typename Alloc, typename T, typename D>
-void initialize_deleter(D * deleter, Alloc * alloc)
+namespace rclcpp
 {
-  (void) deleter;
-  (void) alloc;
-  throw std::runtime_error("Reached unexpected template specialization");
-}
 
-template<typename T>
-void initialize_deleter(std::default_delete<T> * deleter, std::allocator<T> * alloc)
+namespace allocator
 {
-  (void) alloc;
-  deleter = new std::default_delete<T>;
-  if (!deleter) {
-    throw std::runtime_error("initialize_deleter failed");
-  }
-}
 
-template<typename Alloc, typename T>
-void initialize_deleter(AllocatorDeleter<T, Alloc> * deleter, Alloc * alloc)
+// Type-erased interface to AllocatorWrapper
+class AllocatorWrapper
 {
-  if (!alloc) {
-    throw std::invalid_argument("Allocator argument was NULL");
-  }
-  deleter = new AllocatorDeleter<T, Alloc>(alloc);
-  if (!deleter) {
-    throw std::runtime_error("initialize_deleter failed");
-  }
+  virtual void * allocate(size_t size) = 0;
+  virtual void deallocate(void * pointer, size_t size) = 0;
+  // Construct will have to be through placement new, since pure virtual function can't be templated
+
+  virtual void destroy(T* pointer) = 0;
 }
 
 template<typename T, typename Alloc>
-class AllocatorWrapper
+class TypedAllocatorWrapper : public AllocatorWrapper
 {
 public:
+  /*
   using Deleter = typename std::conditional<
       std::is_same<Alloc, std::allocator<T>>::value,
       std::default_delete<T>,
       AllocatorDeleter<T, Alloc>
       >::type;
+  */
 
-  AllocatorWrapper(Alloc * allocator)
+  using DeleterT = Deleter<Alloc, T>;
+
+  TypedAllocatorWrapper(Alloc * allocator)
   : allocator_(allocator)
   {
     if (!allocator_) {
@@ -72,7 +61,7 @@ public:
     }
   }
 
-  AllocatorWrapper(Alloc * allocator, Deleter * deleter)
+  TypedAllocatorWrapper(Alloc * allocator, DeleterT * deleter)
   : allocator_(allocator), deleter_(deleter)
   {
     if (!allocator_) {
@@ -83,7 +72,7 @@ public:
     }
   }
 
-  AllocatorWrapper(Alloc & allocator)
+  TypedAllocatorWrapper(Alloc & allocator)
   {
     allocator_ = &allocator;
     if (!allocator_) {
@@ -95,22 +84,27 @@ public:
     }
   }
 
-  AllocatorWrapper()
+  TypedAllocatorWrapper()
   {
     allocator_ = new Alloc();
     initialize_deleter(deleter_, allocator_);
     if (!deleter_) {
       //throw std::invalid_argument("Failed to initialize deleter");
-      deleter_ = new Deleter;
+      deleter_ = new DeleterT;
     }
   }
 
-  T * allocate(size_t size)
+  void * allocate(size_t size)
   {
     return std::allocator_traits<Alloc>::allocate(*allocator_, size);
   }
 
-  T * deallocate(void * pointer, size_t size)
+  void deallocate(void * pointer, size_t size)
+  {
+    deallocate(static_cast<T*>(pointer), size);
+  }
+
+  void deallocate(T * pointer, size_t size)
   {
     std::allocator_traits<Alloc>::deallocate(*allocator_, pointer, size);
   }
@@ -121,7 +115,18 @@ public:
     std::allocator_traits<Alloc>::construct(*allocator_, pointer, std::forward<Args>(args)...);
   }
 
-  Deleter * get_deleter() const
+  void destroy(void * pointer)
+  {
+    destroy(static_cast<T*>(pointer));
+  }
+
+  template<class T>
+  void destroy(T * pointer)
+  {
+    std::allocator_traits<Alloc>::destroy(*allocator_, pointer);
+  }
+
+  DeleterT * get_deleter() const
   {
     return deleter_;
   }
@@ -132,10 +137,13 @@ public:
 
 private:
   Alloc * allocator_;
-  Deleter * deleter_;
+  DeleterT * deleter_;
 };
 
 template<typename T>
-using DefaultAllocator = AllocatorWrapper<T, std::allocator<T>>;
+using DefaultAllocator = TypedAllocatorWrapper<T, std::allocator<T>>;
+
+}
+}
 
 #endif
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 6b64bec..e320651 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -202,25 +202,34 @@ public:
     FunctorT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+  template<typename Alloc>
+  using StringRebind = typename Alloc::template rebind<std::string>::other;
 
+  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
+  typename std::vector<rcl_interfaces::msg::SetParametersResult, Alloc> set_parameters(
+    const typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> & parameters);
+
+  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
   rcl_interfaces::msg::SetParametersResult set_parameters_atomically(
-    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
+    const typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> & parameters);
 
-  std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
-    const std::vector<std::string> & names) const;
+  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
+  typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> get_parameters(
+    const typename std::vector<std::string, StringRebind<Alloc>> & names) const;
 
-  std::vector<rcl_interfaces::msg::ParameterDescriptor> describe_parameters(
-    const std::vector<std::string> & names) const;
+  template<typename Alloc = std::allocator<rcl_interfaces::msg::ParameterDescriptor>>
+  typename std::vector<rcl_interfaces::msg::ParameterDescriptor, Alloc> describe_parameters(
+    const typename std::vector<std::string, StringRebind<Alloc>>> & names) const;
 
-  std::vector<uint8_t> get_parameter_types(
-    const std::vector<std::string> & names) const;
+  template<typename Alloc = std::allocator<uint8_t>>
+  typename std::vector<uint8_t, Alloc> get_parameter_types(
+    const typename std::vector<std::string, StringRebind<Alloc>>> & names) const;
 
   rcl_interfaces::msg::ListParametersResult list_parameters(
     const std::vector<std::string> & prefixes, uint64_t depth) const;
 
-  std::map<std::string, std::string> get_topic_names_and_types() const;
+  template<typename Alloc = std::allocator<std::pair<const std::string, std::string>>>
+  typename std::map<std::string, std::string, Alloc> get_topic_names_and_types() const;
 
   size_t count_publishers(const std::string & topic_name) const;
 
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 8a9c730..f568536 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -443,6 +443,7 @@ Node::create_service(
   return serv;
 }
 
+template<typename Alloc>
 std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@@ -455,6 +456,7 @@ Node::set_parameters(
   return results;
 }
 
+template<typename Alloc>
 rcl_interfaces::msg::SetParametersResult
 Node::set_parameters_atomically(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@@ -489,6 +491,7 @@ Node::set_parameters_atomically(
   return result;
 }
 
+template<typename Alloc>
 std::vector<rclcpp::parameter::ParameterVariant>
 Node::get_parameters(
   const std::vector<std::string> & names) const
@@ -508,6 +511,7 @@ Node::get_parameters(
   return results;
 }
 
+template<typename Alloc>
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 Node::describe_parameters(
   const std::vector<std::string> & names) const
@@ -528,6 +532,7 @@ Node::describe_parameters(
   return results;
 }
 
+template<typename Alloc>
 std::vector<uint8_t>
 Node::get_parameter_types(
   const std::vector<std::string> & names) const
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index f81e689..5670059 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -28,7 +28,7 @@
 #include <rmw/error_handling.h>
 #include <rmw/rmw.h>
 
-#include <rclcpp/allocator/allocator_factory.hpp>
+#include <rclcpp/allocator/allocator_deleter.hpp>
 
 namespace rclcpp
 {
@@ -221,7 +221,7 @@ public:
   : PublisherBase(node_handle, publisher_handle, topic, queue_size)
   {
     // TODO: avoid messy initialization
-    message_deleter_ = initialize_deleter(&message_allocator_);
+    allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_);
   }
 
 
@@ -281,7 +281,7 @@ public:
     //   subscriptions. For now call the other publish().
     auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
     std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
-    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
+    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -300,7 +300,7 @@ public:
     //   subscriptions. For now call the other publish().
     auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
     std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
-    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
+    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -315,7 +315,7 @@ public:
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
     auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
     std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, msg);
-    std::unique_ptr<MessageT, Deleter<MessageT, Alloc>> unique_msg(ptr, message_deleter_);
+    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -333,7 +333,8 @@ protected:
   }
 
   Alloc<MessageT> message_allocator_;
-  Deleter<MessageT, Alloc> message_deleter_;
+
+  allocator::Deleter<Alloc<MessageT>, MessageT> message_deleter_;
 
 };
 

From 788be0009d62bf7596208b5c364d1cf0666ecd98 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Tue, 20 Oct 2015 11:26:00 -0700
Subject: [PATCH 03/12] template subscriber, using rebind semantics

---
 .../rclcpp/allocator/allocator_common.hpp     |  33 ++++
 .../rclcpp/allocator/allocator_deleter.hpp    |  59 +++----
 .../rclcpp/allocator/allocator_factory.hpp    |  52 ------
 .../rclcpp/allocator/allocator_wrapper.hpp    | 149 ------------------
 .../rclcpp/any_subscription_callback.hpp      |  34 ++--
 .../include/rclcpp/intra_process_manager.hpp  |  20 +--
 rclcpp/include/rclcpp/mapped_ring_buffer.hpp  |  25 +--
 .../rclcpp/message_memory_strategy.hpp        |  20 ++-
 rclcpp/include/rclcpp/node.hpp                |  61 ++++---
 rclcpp/include/rclcpp/node_impl.hpp           |  69 ++++----
 rclcpp/include/rclcpp/publisher.hpp           |  37 +++--
 rclcpp/include/rclcpp/subscription.hpp        |  28 ++--
 rclcpp/test/test_intra_process_manager.cpp    |  47 ++++--
 13 files changed, 253 insertions(+), 381 deletions(-)
 create mode 100644 rclcpp/include/rclcpp/allocator/allocator_common.hpp
 delete mode 100644 rclcpp/include/rclcpp/allocator/allocator_factory.hpp
 delete mode 100644 rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
new file mode 100644
index 0000000..6f980b6
--- /dev/null
+++ b/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_RCLCPP_ALLOCATOR_COMMON_HPP_
+#define RCLCPP_RCLCPP_ALLOCATOR_COMMON_HPP_
+
+#include <memory>
+
+#include <rclcpp/allocator/allocator_deleter.hpp>
+
+namespace rclcpp
+{
+namespace allocator
+{
+
+template<typename U, typename Alloc>
+using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<U>;
+
+}
+}
+
+#endif
diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
index 160c4e5..d725f2b 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
@@ -23,9 +23,12 @@ namespace rclcpp
 namespace allocator
 {
 
-template<typename T, typename Allocator>
+template<typename Allocator>
 class AllocatorDeleter
 {
+  template<typename U>
+  using AllocRebind = typename std::allocator_traits<Allocator>::template rebind_alloc<U>;
+
 public:
   AllocatorDeleter()
   : allocator_(NULL)
@@ -37,16 +40,17 @@ public:
   {
   }
 
-  template<typename U, typename B>
-  AllocatorDeleter(const AllocatorDeleter<U, B> & a)
+  template<typename U>
+  AllocatorDeleter(const AllocatorDeleter<U> & a)
   {
     allocator_ = a.get_allocator();
   }
 
-  void operator()(T * ptr)
+  template<typename U>
+  void operator()(U * ptr)
   {
-    std::allocator_traits<Allocator>::destroy(*allocator_, ptr);
-    std::allocator_traits<Allocator>::deallocate(*allocator_, ptr, sizeof(T));
+    std::allocator_traits<AllocRebind<U>>::destroy(*allocator_, ptr);
+    std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, sizeof(U));
     ptr = NULL;
   }
 
@@ -64,61 +68,36 @@ private:
   Allocator * allocator_;
 };
 
-/*
-template<template<typename> class Alloc, typename T, typename D>
-D initialize_deleter(Alloc<T> * alloc)
+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>
-std::default_delete<T> initialize_deleter(std::allocator<T> * alloc)
-{
-  (void) alloc;
-  return std::default_delete<T>();
-}
-
-template<template<typename> class Alloc, typename T>
-AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
-{
-  if (!alloc) {
-    throw std::invalid_argument("Allocator argument was NULL");
-  }
-  return AllocatorDeleter<T, Alloc<T>>(alloc);
-}
-*/
-template<template<typename> class Alloc, typename T, typename D>
-void set_allocator_for_deleter(D * deleter, Alloc<T> * alloc)
-{
-  (void) alloc;
-  throw std::runtime_error("Reached unexpected template specialization");
-}
-
-template<typename T>
-void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<T> * alloc)
+template<typename T, typename U>
+void set_allocator_for_deleter(std::default_delete<T> * deleter, std::allocator<U> * alloc)
 {
   (void) deleter;
   (void) alloc;
 }
 
-template<template<typename> class Alloc, typename T>
-void set_allocator_for_deleter(AllocatorDeleter<T, Alloc<T>> * deleter, Alloc<T> * 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");
   }
-  //return AllocatorDeleter<T, Alloc<T>>(alloc);
   deleter->set_allocator(alloc);
 }
 
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
-    std::is_same<Alloc, std::allocator<T>>::value,
+    std::is_same<Alloc, std::allocator<void>>::value,
     std::default_delete<T>,
-    AllocatorDeleter<T, Alloc>
+    AllocatorDeleter<Alloc>
     >::type;
-
 }
 }
 
diff --git a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp b/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
deleted file mode 100644
index 2a98b1f..0000000
--- a/rclcpp/include/rclcpp/allocator/allocator_factory.hpp
+++ /dev/null
@@ -1,52 +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_ALLOCATOR_FACTORY_HPP_
-#define RCLCPP_RCLCPP_ALLOCATOR_FACTORY_HPP_
-
-#include <memory>
-#include <iostream>
-#include <rclcpp/allocator/allocator_deleter.hpp>
-
-template<template<typename> class Alloc, typename T, typename D>
-D initialize_deleter(Alloc<T> * alloc)
-{
-  (void) alloc;
-  throw std::runtime_error("Reached unexpected template specialization");
-}
-
-template<typename T>
-std::default_delete<T> initialize_deleter(std::allocator<T> * alloc)
-{
-  (void) alloc;
-  return std::default_delete<T>();
-}
-
-template<template<typename> class Alloc, typename T>
-AllocatorDeleter<T, Alloc<T>> initialize_deleter(Alloc<T> * alloc)
-{
-  if (!alloc) {
-    throw std::invalid_argument("Allocator argument was NULL");
-  }
-  return AllocatorDeleter<T, Alloc<T>>(alloc);
-}
-
-template<typename T, template<typename> class Alloc>
-using Deleter = typename std::conditional<
-    std::is_same<Alloc<T>, std::allocator<T>>::value,
-    std::default_delete<T>,
-    AllocatorDeleter<T, Alloc<T>>
-    >::type;
-
-#endif
diff --git a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp b/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
deleted file mode 100644
index 99fc421..0000000
--- a/rclcpp/include/rclcpp/allocator/allocator_wrapper.hpp
+++ /dev/null
@@ -1,149 +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_ALLOCATOR_WRAPPER_HPP_
-#define RCLCPP_RCLCPP_ALLOCATOR_WRAPPER_HPP_
-
-#include <memory>
-#include <iostream>
-#include <rclcpp/allocator/allocator_deleter.hpp>
-
-namespace rclcpp
-{
-
-namespace allocator
-{
-
-// Type-erased interface to AllocatorWrapper
-class AllocatorWrapper
-{
-  virtual void * allocate(size_t size) = 0;
-  virtual void deallocate(void * pointer, size_t size) = 0;
-  // Construct will have to be through placement new, since pure virtual function can't be templated
-
-  virtual void destroy(T* pointer) = 0;
-}
-
-template<typename T, typename Alloc>
-class TypedAllocatorWrapper : public AllocatorWrapper
-{
-public:
-  /*
-  using Deleter = typename std::conditional<
-      std::is_same<Alloc, std::allocator<T>>::value,
-      std::default_delete<T>,
-      AllocatorDeleter<T, Alloc>
-      >::type;
-  */
-
-  using DeleterT = Deleter<Alloc, T>;
-
-  TypedAllocatorWrapper(Alloc * allocator)
-  : allocator_(allocator)
-  {
-    if (!allocator_) {
-      throw std::invalid_argument("Allocator argument was NULL");
-    }
-    initialize_deleter(deleter_, allocator_);
-    if (deleter_ == NULL) {
-      throw std::invalid_argument("Failed to initialize deleter");
-    }
-  }
-
-  TypedAllocatorWrapper(Alloc * allocator, DeleterT * deleter)
-  : allocator_(allocator), deleter_(deleter)
-  {
-    if (!allocator_) {
-      throw std::invalid_argument("Allocator argument was NULL");
-    }
-    if (!deleter_) {
-      throw std::invalid_argument("Deleter argument was NULL");
-    }
-  }
-
-  TypedAllocatorWrapper(Alloc & allocator)
-  {
-    allocator_ = &allocator;
-    if (!allocator_) {
-      throw std::invalid_argument("Allocator argument was NULL");
-    }
-    initialize_deleter(deleter_, allocator_);
-    if (!deleter_) {
-      throw std::invalid_argument("Failed to initialize deleter");
-    }
-  }
-
-  TypedAllocatorWrapper()
-  {
-    allocator_ = new Alloc();
-    initialize_deleter(deleter_, allocator_);
-    if (!deleter_) {
-      //throw std::invalid_argument("Failed to initialize deleter");
-      deleter_ = new DeleterT;
-    }
-  }
-
-  void * allocate(size_t size)
-  {
-    return std::allocator_traits<Alloc>::allocate(*allocator_, size);
-  }
-
-  void deallocate(void * pointer, size_t size)
-  {
-    deallocate(static_cast<T*>(pointer), size);
-  }
-
-  void deallocate(T * pointer, size_t size)
-  {
-    std::allocator_traits<Alloc>::deallocate(*allocator_, pointer, size);
-  }
-
-  template<class ... Args>
-  void construct(T * pointer, Args && ... args)
-  {
-    std::allocator_traits<Alloc>::construct(*allocator_, pointer, std::forward<Args>(args)...);
-  }
-
-  void destroy(void * pointer)
-  {
-    destroy(static_cast<T*>(pointer));
-  }
-
-  template<class T>
-  void destroy(T * pointer)
-  {
-    std::allocator_traits<Alloc>::destroy(*allocator_, pointer);
-  }
-
-  DeleterT * get_deleter() const
-  {
-    return deleter_;
-  }
-  Alloc * get_underlying_allocator() const
-  {
-    return allocator_;
-  }
-
-private:
-  Alloc * allocator_;
-  DeleterT * deleter_;
-};
-
-template<typename T>
-using DefaultAllocator = TypedAllocatorWrapper<T, std::allocator<T>>;
-
-}
-}
-
-#endif
diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
index fdec603..7b420f6 100644
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -15,6 +15,7 @@
 #ifndef RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_
 #define RCLCPP_RCLCPP_ANY_SUBSCRIPTION_CALLBACK_HPP_
 
+#include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/function_traits.hpp>
 
 #include <functional>
@@ -29,18 +30,21 @@ namespace rclcpp
 namespace any_subscription_callback
 {
 
-template<typename MessageT>
+template<typename MessageT, typename Alloc>
 class AnySubscriptionCallback
 {
+  using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
+  using UniquePtr = 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 ConstSharedPtrCallback = std::function<void(const std::shared_ptr<const MessageT>)>;
   using ConstSharedPtrWithInfoCallback =
       std::function<void(const std::shared_ptr<const MessageT>, const rmw_message_info_t &)>;
-  using UniquePtrCallback = std::function<void(std::unique_ptr<MessageT>)>;
-  using UniquePtrWithInfoCallback =
-      std::function<void(std::unique_ptr<MessageT>, const rmw_message_info_t &)>;
+  using UniquePtrCallback = std::function<void(UniquePtr)>;
+  using UniquePtrWithInfoCallback = std::function<void(UniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -50,11 +54,14 @@ class AnySubscriptionCallback
   UniquePtrWithInfoCallback unique_ptr_with_info_callback_;
 
 public:
-  AnySubscriptionCallback()
+  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_ = new typename MessageAlloc::allocator_type(*allocator.get());
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_);
+  }
 
   AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
 
@@ -155,17 +162,20 @@ public:
     } else if (const_shared_ptr_with_info_callback_) {
       const_shared_ptr_with_info_callback_(message, message_info);
     } else if (unique_ptr_callback_) {
-      unique_ptr_callback_(std::unique_ptr<MessageT>(new MessageT(*message)));
+      auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
+      MessageAlloc::construct(*message_allocator_, ptr, *message);
+      unique_ptr_callback_(UniquePtr(ptr, message_deleter_));
     } else if (unique_ptr_with_info_callback_) {
-      unique_ptr_with_info_callback_(std::unique_ptr<MessageT>(new MessageT(*
-        message)), message_info);
+      auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
+      MessageAlloc::construct(*message_allocator_, ptr, *message);
+      unique_ptr_with_info_callback_(UniquePtr(ptr, message_deleter_), message_info);
     } else {
       throw std::runtime_error("unexpected message without any callback set");
     }
   }
 
   void dispatch_intra_process(
-    std::unique_ptr<MessageT> & message, const rmw_message_info_t & message_info)
+    UniquePtr & message, const rmw_message_info_t & message_info)
   {
     (void)message_info;
     if (shared_ptr_callback_) {
@@ -188,6 +198,10 @@ public:
       throw std::runtime_error("unexpected message without any callback set");
     }
   }
+
+private:
+  typename MessageAlloc::allocator_type * message_allocator_;
+  MessageDeleter message_deleter_;
 };
 
 } /* namespace any_subscription_callback */
diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp
index 340b527..c66d7b6 100644
--- a/rclcpp/include/rclcpp/intra_process_manager.hpp
+++ b/rclcpp/include/rclcpp/intra_process_manager.hpp
@@ -188,9 +188,9 @@ 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(typename publisher::Publisher<MessageT>::SharedPtr publisher,
+  add_publisher(typename publisher::Publisher<MessageT, Alloc>::SharedPtr publisher,
     size_t buffer_size = 0)
   {
     auto id = IntraProcessManager::get_next_unique_id();
@@ -201,7 +201,9 @@ public:
       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);
+    using Deleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
+    publishers_[id].buffer = mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter>::make_shared(
+      size);
     publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
     return id;
   }
@@ -247,11 +249,11 @@ public:
    * \param message the message that is being stored.
    * \return the message sequence number.
    */
-  template<typename MessageT>
+  template<typename MessageT, 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()) {
@@ -261,7 +263,7 @@ public:
     // 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;
+    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter> 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?
@@ -321,13 +323,13 @@ 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 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)
   {
     message = nullptr;
     PublisherInfo * info;
@@ -359,7 +361,7 @@ public:
       }
       target_subs->erase(it);
     }
-    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT> TypedMRB;
+    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter> 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()) {
diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
index 4b5bb47..daf6792 100644
--- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
+++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
@@ -50,11 +50,12 @@ public:
  * there is no guarantee on which value is returned if a key is used multiple
  * times.
  */
-template<typename T>
+template<typename T, typename Deleter = std::default_delete<T>>
 class MappedRingBuffer : public MappedRingBufferBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T>);
+  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Deleter>);
+  using ElemUniquePtr = std::unique_ptr<T, Deleter>;
 
   /// Constructor.
   /* The constructor will allocate memory while reserving space.
@@ -82,12 +83,12 @@ 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));
+      value = ElemUniquePtr(new T(*it->value));
     }
   }
 
@@ -111,13 +112,13 @@ 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 copy = ElemUniquePtr(new T(*it->value));
       // Return the original.
       value.swap(it->value);
       // Store the copy.
@@ -136,7 +137,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 +159,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 +170,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,12 +184,12 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T>);
+  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Deleter>);
 
   struct element
   {
     uint64_t key;
-    std::unique_ptr<T> value;
+    ElemUniquePtr value;
     bool in_use;
   };
 
diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index 3ef46d8..25c8c2d 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -17,6 +17,7 @@
 
 #include <memory>
 
+#include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/macros.hpp>
 
 namespace rclcpp
@@ -26,24 +27,34 @@ 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 MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
+
+  MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
+  {
+    message_allocator_ = new typename MessageAlloc::allocator_type(*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);
+    auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
+    MessageAlloc::construct(*message_allocator_, ptr);
+    return std::shared_ptr<MessageT>(ptr, message_deleter_);
   }
 
   /// Release ownership of the message, which will deallocate it if it has no more owners.
@@ -52,6 +63,9 @@ public:
   {
     msg.reset();
   }
+
+  typename MessageAlloc::allocator_type * message_allocator_;
+  MessageDeleter message_deleter_;
 };
 
 }  /* message_memory_strategy */
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index e320651..e7d3e28 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -91,10 +91,11 @@ public:
    * \param[in] qos_history_depth The depth of the publisher message queue.
    * \return Shared pointer to the created publisher.
    */
-  template<typename MessageT>
-  typename rclcpp::publisher::Publisher<MessageT>::SharedPtr
+  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);
+    const std::string & topic_name, size_t qos_history_depth,
+    std::shared_ptr<Alloc> allocator = nullptr);
 
   /// Create and return a Publisher.
   /**
@@ -102,11 +103,12 @@ public:
    * \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>
-  typename rclcpp::publisher::Publisher<MessageT>::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 = rmw_qos_profile_default);
+    const rmw_qos_profile_t & qos_profile = rmw_qos_profile_default,
+    std::shared_ptr<Alloc> allocator = nullptr);
 
   /// Create and return a Subscription.
   /**
@@ -122,16 +124,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,
     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>::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 and return a Subscription.
   /**
@@ -147,16 +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,
     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);
+    typename rclcpp::message_memory_strategy::MessageMemoryStrategy<MessageT, Alloc>::SharedPtr
+    msg_mem_strat = nullptr,
+    std::shared_ptr<Alloc> allocator = nullptr);
 
   /// Create a timer.
   /**
@@ -202,34 +206,25 @@ public:
     FunctorT callback,
     rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
 
-  template<typename Alloc>
-  using StringRebind = typename Alloc::template rebind<std::string>::other;
+  std::vector<rcl_interfaces::msg::SetParametersResult> set_parameters(
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
-  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
-  typename std::vector<rcl_interfaces::msg::SetParametersResult, Alloc> set_parameters(
-    const typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> & parameters);
-
-  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
   rcl_interfaces::msg::SetParametersResult set_parameters_atomically(
-    const typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> & parameters);
+    const std::vector<rclcpp::parameter::ParameterVariant> & parameters);
 
-  template<typename Alloc = std::allocator<rclcpp::parameter::ParameterVariant>>
-  typename std::vector<rclcpp::parameter::ParameterVariant, Alloc> get_parameters(
-    const typename std::vector<std::string, StringRebind<Alloc>> & names) const;
+  std::vector<rclcpp::parameter::ParameterVariant> get_parameters(
+    const std::vector<std::string> & names) const;
 
-  template<typename Alloc = std::allocator<rcl_interfaces::msg::ParameterDescriptor>>
-  typename std::vector<rcl_interfaces::msg::ParameterDescriptor, Alloc> describe_parameters(
-    const typename std::vector<std::string, StringRebind<Alloc>>> & names) const;
+  std::vector<rcl_interfaces::msg::ParameterDescriptor> describe_parameters(
+    const std::vector<std::string> & names) const;
 
-  template<typename Alloc = std::allocator<uint8_t>>
-  typename std::vector<uint8_t, Alloc> get_parameter_types(
-    const typename std::vector<std::string, StringRebind<Alloc>>> & names) const;
+  std::vector<uint8_t> get_parameter_types(
+    const std::vector<std::string> & names) const;
 
   rcl_interfaces::msg::ListParametersResult list_parameters(
     const std::vector<std::string> & prefixes, uint64_t depth) const;
 
-  template<typename Alloc = std::allocator<std::pair<const std::string, std::string>>>
-  typename std::map<std::string, std::string, Alloc> get_topic_names_and_types() const;
+  std::map<std::string, std::string> get_topic_names_and_types() const;
 
   size_t count_publishers(const std::string & topic_name) const;
 
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index f568536..9f82a31 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -113,21 +113,29 @@ Node::create_callback_group(
   return group;
 }
 
-template<typename MessageT>
-typename rclcpp::publisher::Publisher<MessageT>::SharedPtr
+template<typename MessageT, typename Alloc>
+typename rclcpp::publisher::Publisher<MessageT, Alloc>::SharedPtr
 Node::create_publisher(
-  const std::string & topic_name, size_t qos_history_depth)
+  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>(topic_name, qos);
+  return this->create_publisher<MessageT, Alloc>(topic_name, qos, allocator);
 }
 
-template<typename MessageT>
-typename publisher::Publisher<MessageT>::SharedPtr
+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)
+  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(
@@ -140,8 +148,8 @@ Node::create_publisher(
     // *INDENT-ON*
   }
 
-  auto publisher = publisher::Publisher<MessageT>::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(
@@ -157,7 +165,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 =
@@ -205,25 +213,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,
   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);
 
   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>();
@@ -239,7 +253,7 @@ Node::create_subscription(
 
   using namespace rclcpp::subscription;
 
-  auto sub = Subscription<MessageT>::make_shared(
+  auto sub = Subscription<MessageT, Alloc>::make_shared(
     node_handle_,
     subscriber_handle,
     topic_name,
@@ -271,7 +285,7 @@ Node::create_subscription(
         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) {
@@ -306,26 +320,28 @@ Node::create_subscription(
   return sub;
 }
 
-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,
   size_t qos_history_depth,
   CallbackT callback,
   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,
+  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>(
+  return this->create_subscription<MessageT, CallbackT, Alloc>(
     topic_name,
     callback,
     qos,
     group,
     ignore_local_publications,
-    msg_mem_strat);
+    msg_mem_strat,
+    allocator);
 }
 
 rclcpp::timer::WallTimer::SharedPtr
@@ -443,7 +459,6 @@ Node::create_service(
   return serv;
 }
 
-template<typename Alloc>
 std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@@ -456,7 +471,6 @@ Node::set_parameters(
   return results;
 }
 
-template<typename Alloc>
 rcl_interfaces::msg::SetParametersResult
 Node::set_parameters_atomically(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
@@ -491,7 +505,6 @@ Node::set_parameters_atomically(
   return result;
 }
 
-template<typename Alloc>
 std::vector<rclcpp::parameter::ParameterVariant>
 Node::get_parameters(
   const std::vector<std::string> & names) const
@@ -511,7 +524,6 @@ Node::get_parameters(
   return results;
 }
 
-template<typename Alloc>
 std::vector<rcl_interfaces::msg::ParameterDescriptor>
 Node::describe_parameters(
   const std::vector<std::string> & names) const
@@ -532,7 +544,6 @@ Node::describe_parameters(
   return results;
 }
 
-template<typename Alloc>
 std::vector<uint8_t>
 Node::get_parameter_types(
   const std::vector<std::string> & names) const
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 5670059..0c706e1 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -205,23 +205,28 @@ protected:
 };
 
 /// A publisher publishes messages of any type to a topic.
-template<typename MessageT, template<typename> class Alloc = std::allocator>
+template<typename MessageT, typename Alloc = std::allocator<void>>
 class Publisher : public PublisherBase
 {
   friend rclcpp::node::Node;
 
 public:
+  using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, 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)
+    size_t queue_size,
+    std::shared_ptr<Alloc> allocator)
   : PublisherBase(node_handle, publisher_handle, topic, queue_size)
   {
-    // TODO: avoid messy initialization
-    allocator::set_allocator_for_deleter(&message_deleter_, &message_allocator_);
+    message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_);
   }
 
 
@@ -231,7 +236,7 @@ public:
    * \param[in] msg A shared pointer to the message to send.
    */
   void
-  publish(std::unique_ptr<MessageT> & msg)
+  publish(MessageUniquePtr & msg)
   {
     this->do_inter_process_publish(msg.get());
     if (store_intra_process_message_) {
@@ -279,9 +284,9 @@ public:
     //   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 = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
-    std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
-    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
+    auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
+    MessageAlloc::construct(*message_allocator_, ptr, *msg.get());
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -298,9 +303,9 @@ public:
     //   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 = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
-    std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, *msg.get());
-    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
+    auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
+    MessageAlloc::construct(*message_allocator_, ptr, *msg.get());
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -313,9 +318,9 @@ public:
       return this->do_inter_process_publish(&msg);
     }
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    auto ptr = std::allocator_traits<Alloc<MessageT>>::allocate(message_allocator_, 1);
-    std::allocator_traits<Alloc<MessageT>>::construct(message_allocator_, ptr, msg);
-    std::unique_ptr<MessageT, allocator::Deleter<Alloc<MessageT>, MessageT>> unique_msg(ptr, message_deleter_);
+    auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
+    MessageAlloc::construct(*message_allocator_, ptr, msg);
+    MessageUniquePtr unique_msg(ptr, message_deleter_);
     return this->publish(unique_msg);
   }
 
@@ -332,9 +337,9 @@ protected:
     }
   }
 
-  Alloc<MessageT> message_allocator_;
+  typename MessageAlloc::allocator_type * message_allocator_;
 
-  allocator::Deleter<Alloc<MessageT>, MessageT> message_deleter_;
+  MessageDeleter message_deleter_;
 
 };
 
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index b7250e6..7f148c3 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -144,12 +144,16 @@ private:
 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 MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
+  using MessageUniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
 
   /// Default constructor.
@@ -167,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.
   /**
@@ -183,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;
   }
@@ -226,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,
@@ -244,7 +251,7 @@ public:
 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;
 
@@ -262,14 +269,13 @@ 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 */
diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp
index 4ce7f90..3cae9bf 100644
--- a/rclcpp/test/test_intra_process_manager.cpp
+++ b/rclcpp/test/test_intra_process_manager.cpp
@@ -55,11 +55,11 @@ public:
   size_t mock_queue_size;
 };
 
-template<typename T>
+template<typename T, typename Alloc = std::allocator<void>>
 class Publisher : public PublisherBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T>);
+  RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>);
 };
 
 }
@@ -139,8 +139,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>();
@@ -221,7 +223,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>();
@@ -269,7 +272,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);
@@ -338,7 +342,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);
@@ -410,9 +415,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>();
@@ -512,9 +520,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);
@@ -655,7 +666,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>();
@@ -720,7 +732,8 @@ TEST(TestIntraProcessManager, subscription_creation_race_condition) {
   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;
@@ -767,7 +780,7 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) {
     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;
@@ -805,7 +818,7 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_store) {
     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>();

From 0cd13608f77bc7662f53c900cf80a7dd890c31a0 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Thu, 22 Oct 2015 11:15:19 -0700
Subject: [PATCH 04/12] Add allocator memory strategy

---
 .../rclcpp/allocator/allocator_deleter.hpp    |   3 +-
 .../rclcpp/message_memory_strategy.hpp        |   5 +
 rclcpp/include/rclcpp/publisher.hpp           |   3 +-
 .../strategies/allocator_memory_strategy.hpp  | 187 ++++++++++++++++++
 rclcpp/test/test_intra_process_manager.cpp    |   5 +-
 5 files changed, 200 insertions(+), 3 deletions(-)
 create mode 100644 rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp

diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
index d725f2b..2eb00ed 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
@@ -94,7 +94,8 @@ void set_allocator_for_deleter(AllocatorDeleter<T> * deleter, Alloc * alloc)
 
 template<typename Alloc, typename T>
 using Deleter = typename std::conditional<
-    std::is_same<Alloc, std::allocator<void>>::value,
+    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;
diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index 25c8c2d..9076c93 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -37,6 +37,11 @@ public:
   using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
   using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
 
+  MessageMemoryStrategy()
+  {
+    message_allocator_ = new typename MessageAlloc::allocator_type();
+  }
+
   MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 0c706e1..92249d7 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -236,7 +236,8 @@ public:
    * \param[in] msg A shared pointer to the message to send.
    */
   void
-  publish(MessageUniquePtr & msg)
+  publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
+  //publish(MessageUniquePtr & msg)
   {
     this->do_inter_process_publish(msg.get());
     if (store_intra_process_message_) {
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
new file mode 100644
index 0000000..daa2e0f
--- /dev/null
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -0,0 +1,187 @@
+// 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_ALLOCATOR_MEMORY_STRATEGY_HPP_
+#define RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
+
+#include <rclcpp/allocator/allocator_common.hpp>
+#include <rclcpp/memory_strategy.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>
+class AllocatorMemoryStrategy : public MemoryStrategy
+{
+
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
+
+  using ExecAlloc = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
+  using ExecDeleter =
+      allocator::Deleter<typename ExecAlloc::allocator_type, executor::AnyExecutable>;
+  using VoidAlloc = allocator::AllocRebind<void *, Alloc>;
+
+  AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
+  {
+    executable_allocator_ = new ExecAlloc(*allocator.get());
+    allocator_ = new VoidAlloc(*allocator.get());
+  }
+
+  /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
+  /**
+   * The default implementation stores std::vectors for each handle type and resizes the vectors
+   * as necessary based on the requested number of handles.
+   * \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)
+  {
+    switch (type) {
+      case HandleType::subscription_handle:
+        if (subscription_handles.size() < number_of_handles) {
+          subscription_handles.resize(number_of_handles, 0);
+        }
+        return static_cast<void **>(subscription_handles.data());
+      case HandleType::service_handle:
+        if (service_handles.size() < number_of_handles) {
+          service_handles.resize(number_of_handles, 0);
+        }
+        return static_cast<void **>(service_handles.data());
+      case HandleType::client_handle:
+        if (client_handles.size() < number_of_handles) {
+          client_handles.resize(number_of_handles, 0);
+        }
+        return static_cast<void **>(client_handles.data());
+      case HandleType::guard_condition_handle:
+        if (number_of_handles > 2) {
+          throw std::runtime_error("Too many guard condition handles requested!");
+        }
+        return guard_cond_handles.data();
+      default:
+        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
+                ", could not borrow handle memory.");
+    }
+  }
+
+  /// 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)
+  {
+    switch (type) {
+      case HandleType::subscription_handle:
+        if (handles != subscription_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
+        }
+        memset(handles, 0, subscription_handles.size());
+        break;
+      case HandleType::service_handle:
+        if (handles != service_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
+        }
+        memset(handles, 0, service_handles.size());
+        break;
+      case HandleType::client_handle:
+        if (handles != client_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
+        }
+        memset(handles, 0, client_handles.size());
+        break;
+      case HandleType::guard_condition_handle:
+        if (handles != guard_cond_handles.data()) {
+          throw std::runtime_error(
+                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
+        }
+        guard_cond_handles.fill(0);
+        break;
+      default:
+        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
+                ", could not borrow handle memory.");
+    }
+  }
+
+  /// 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>();
+    auto ptr = ExecAlloc::allocate(*executable_allocator_, 1);
+    ExecAlloc::construct(*executable_allocator_);
+    return std::shared_ptr<executor::AnyExecutable>(ptr, executable_deleter_);
+  }
+
+  /// 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)
+  {
+    if (size == 0) {
+      return NULL;
+    }
+    return VoidAlloc::allocate(*allocator_, size);
+  }
+
+  /// Implementation of a general-purpose free.
+  /**
+   * \param[in] Pointer to deallocate.
+   */
+  virtual void free(void * ptr)
+  {
+    VoidAlloc::deallocate(*allocator, ptr);
+  }
+
+  std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr, Alloc> subs;
+  std::vector<rclcpp::service::ServiceBase::SharedPtr, Alloc> services;
+  std::vector<rclcpp::client::ClientBase::SharedPtr, Alloc> clients;
+
+  std::vector<void *, Alloc> subscription_handles;
+  std::vector<void *, Alloc> service_handles;
+  std::vector<void *, Alloc> client_handles;
+  std::array<void *, 2> guard_cond_handles;
+
+private:
+  typename ExecAlloc::allocator_type * executable_allocator_;
+  ExecDeleter executable_deleter_;
+  typename VoidAlloc::allocator_type * allocator_;
+};
+
+}  /* allocator_memory_strategy */
+}  /* memory_strategies */
+
+}  /* rclcpp */
+
+#endif
diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp
index 3cae9bf..df460c9 100644
--- a/rclcpp/test/test_intra_process_manager.cpp
+++ b/rclcpp/test/test_intra_process_manager.cpp
@@ -15,7 +15,7 @@
 #include <memory>
 
 #include <gtest/gtest.h>
-
+#include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/macros.hpp>
 #include <rmw/types.h>
 
@@ -59,6 +59,9 @@ template<typename T, typename Alloc = std::allocator<void>>
 class Publisher : public PublisherBase
 {
 public:
+  using MessageAlloc = allocator::AllocRebind<T, Alloc>;
+  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, T>;
+
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>);
 };
 

From ea21d9263a1932dad2cba224d3ab1c58e4311a78 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Thu, 22 Oct 2015 15:58:00 -0700
Subject: [PATCH 05/12] rebind allocator in mapped_ring_buffer

---
 .../rclcpp/allocator/allocator_common.hpp     |  4 +-
 .../rclcpp/allocator/allocator_deleter.hpp    |  3 +-
 .../rclcpp/any_subscription_callback.hpp      | 31 +++++------
 .../include/rclcpp/intra_process_manager.hpp  | 25 +++++----
 rclcpp/include/rclcpp/mapped_ring_buffer.hpp  | 36 +++++++++----
 .../rclcpp/message_memory_strategy.hpp        | 16 +++---
 rclcpp/include/rclcpp/node_impl.hpp           |  7 +--
 rclcpp/include/rclcpp/publisher.hpp           | 29 +++++-----
 .../strategies/allocator_memory_strategy.hpp  | 53 ++++++++++++-------
 rclcpp/include/rclcpp/subscription.hpp        |  5 +-
 rclcpp/test/test_intra_process_manager.cpp    | 15 +++++-
 11 files changed, 143 insertions(+), 81 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index 6f980b6..5d493b3 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ALLOCATOR_COMMON_HPP_
-#define RCLCPP_RCLCPP_ALLOCATOR_COMMON_HPP_
+#ifndef RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
+#define RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
 
 #include <memory>
 
diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
index 2eb00ed..8e948b6 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
@@ -50,7 +50,8 @@ public:
   void operator()(U * ptr)
   {
     std::allocator_traits<AllocRebind<U>>::destroy(*allocator_, ptr);
-    std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, sizeof(U));
+    // TODO: figure out if we're guaranteed to be destroying only 1 item here
+    std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, 1);
     ptr = NULL;
   }
 
diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
index 7b420f6..cb9a23c 100644
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -33,9 +33,10 @@ namespace any_subscription_callback
 template<typename MessageT, typename Alloc>
 class AnySubscriptionCallback
 {
-  using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
-  using UniquePtr = std::unique_ptr<MessageT, MessageDeleter>;
+  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 =
@@ -43,8 +44,8 @@ class AnySubscriptionCallback
   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(UniquePtr)>;
-  using UniquePtrWithInfoCallback = std::function<void(UniquePtr, const rmw_message_info_t &)>;
+  using UniquePtrCallback = std::function<void(MessageUniquePtr)>;
+  using UniquePtrWithInfoCallback = std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
@@ -59,8 +60,8 @@ public:
     const_shared_ptr_callback_(nullptr), const_shared_ptr_with_info_callback_(nullptr),
     unique_ptr_callback_(nullptr), unique_ptr_with_info_callback_(nullptr)
   {
-    message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
-    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_);
+    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
   }
 
   AnySubscriptionCallback(const AnySubscriptionCallback &) = default;
@@ -162,20 +163,20 @@ public:
     } else if (const_shared_ptr_with_info_callback_) {
       const_shared_ptr_with_info_callback_(message, message_info);
     } else if (unique_ptr_callback_) {
-      auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
-      MessageAlloc::construct(*message_allocator_, ptr, *message);
-      unique_ptr_callback_(UniquePtr(ptr, message_deleter_));
+      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 = MessageAlloc::allocate(*message_allocator_, 1);
-      MessageAlloc::construct(*message_allocator_, ptr, *message);
-      unique_ptr_with_info_callback_(UniquePtr(ptr, message_deleter_), message_info);
+      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(
-    UniquePtr & message, const rmw_message_info_t & message_info)
+    MessageUniquePtr & message, const rmw_message_info_t & message_info)
   {
     (void)message_info;
     if (shared_ptr_callback_) {
@@ -200,7 +201,7 @@ public:
   }
 
 private:
-  typename MessageAlloc::allocator_type * message_allocator_;
+  std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;
 };
 
diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp
index c66d7b6..21739cf 100644
--- a/rclcpp/include/rclcpp/intra_process_manager.hpp
+++ b/rclcpp/include/rclcpp/intra_process_manager.hpp
@@ -201,9 +201,10 @@ public:
       throw std::invalid_argument("the calculated buffer size is too large");
     }
     publishers_[id].sequence_number.store(0);
-    using Deleter = typename publisher::Publisher<MessageT, Alloc>::MessageDeleter;
-    publishers_[id].buffer = mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter>::make_shared(
-      size);
+    publishers_[id].buffer =
+      mapped_ring_buffer::MappedRingBuffer<MessageT,
+      typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
+      size, publisher->get_allocator());
     publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
     return id;
   }
@@ -249,11 +250,13 @@ public:
    * \param message the message that is being stored.
    * \return the message sequence number.
    */
-  template<typename MessageT, typename Deleter = std::default_delete<MessageT>>
+  template<typename MessageT, typename Alloc = std::allocator<void>>
   uint64_t
   store_intra_process_message(
     uint64_t intra_process_publisher_id,
-    std::unique_ptr<MessageT, Deleter> & message)
+    std::unique_ptr<MessageT,
+    typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
+    MessageT>> & message)
   {
     auto it = publishers_.find(intra_process_publisher_id);
     if (it == publishers_.end()) {
@@ -263,7 +266,8 @@ public:
     // 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, Deleter> TypedMRB;
+    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
+    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> 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?
@@ -323,13 +327,15 @@ 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, typename Deleter = std::default_delete<MessageT>>
+  template<typename MessageT, typename Alloc = std::allocator<void>>
   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, Deleter> & message)
+    std::unique_ptr<MessageT,
+    typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
+    MessageT>> & message)
   {
     message = nullptr;
     PublisherInfo * info;
@@ -361,7 +367,8 @@ public:
       }
       target_subs->erase(it);
     }
-    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, Deleter> TypedMRB;
+    using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
+    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> 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()) {
diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
index daf6792..9e6d611 100644
--- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
+++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
@@ -15,6 +15,7 @@
 #ifndef RCLCPP_RCLCPP_RING_BUFFER_HPP_
 #define RCLCPP_RCLCPP_RING_BUFFER_HPP_
 
+#include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/macros.hpp>
 
 #include <algorithm>
@@ -50,25 +51,35 @@ public:
  * there is no guarantee on which value is returned if a key is used multiple
  * times.
  */
-template<typename T, typename Deleter = std::default_delete<T>>
+template<typename T, typename Alloc = std::allocator<void>>
 class MappedRingBuffer : public MappedRingBufferBase
 {
 public:
-  RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBuffer<T, Deleter>);
-  using ElemUniquePtr = std::unique_ptr<T, Deleter>;
+  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)
+  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.
@@ -88,7 +99,9 @@ public:
     auto it = get_iterator_of_key(key);
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
-      value = ElemUniquePtr(new T(*it->value));
+      auto ptr = ElemAllocTraits::allocate(*allocator_.get(), 1);
+      ElemAllocTraits::construct(*allocator_.get(), ptr, *it->value);
+      value = ElemUniquePtr(ptr);
     }
   }
 
@@ -118,7 +131,9 @@ public:
     value = nullptr;
     if (it != elements_.end() && it->in_use) {
       // Make a copy.
-      auto copy = ElemUniquePtr(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.
@@ -184,7 +199,7 @@ public:
   }
 
 private:
-  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Deleter>);
+  RCLCPP_DISABLE_COPY(MappedRingBuffer<T, Alloc>);
 
   struct element
   {
@@ -193,7 +208,9 @@ private:
     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)
@@ -204,8 +221,9 @@ private:
     return it;
   }
 
-  std::vector<element> elements_;
+  std::vector<element, VectorAlloc> elements_;
   size_t head_;
+  std::shared_ptr<ElemAlloc> allocator_;
 
 };
 
diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index 9076c93..e88c077 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -34,17 +34,18 @@ class MessageMemoryStrategy
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
 
-  using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, MessageT>;
+  using MessageAllocTraits = allocator::AllocRebind<MessageT, Alloc>;
+  using MessageAlloc = typename MessageAllocTraits::allocator_type;
+  using MessageDeleter = allocator::Deleter<MessageAlloc, MessageT>;
 
   MessageMemoryStrategy()
   {
-    message_allocator_ = new typename MessageAlloc::allocator_type();
+    message_allocator_ = std::make_shared<MessageAlloc>();
   }
 
   MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
-    message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
+    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
   }
 
   /// Default factory method
@@ -57,9 +58,10 @@ public:
   // \return Shared pointer to the new message.
   virtual std::shared_ptr<MessageT> borrow_message()
   {
-    auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
-    MessageAlloc::construct(*message_allocator_, ptr);
+    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
+    MessageAllocTraits::construct(*message_allocator_.get(), ptr);
     return std::shared_ptr<MessageT>(ptr, message_deleter_);
+    //return std::allocate_shared<MessageT, typename MessageAlloc::allocator_type>(*message_allocator_);
   }
 
   /// Release ownership of the message, which will deallocate it if it has no more owners.
@@ -69,7 +71,7 @@ public:
     msg.reset();
   }
 
-  typename MessageAlloc::allocator_type * message_allocator_;
+  std::shared_ptr<MessageAlloc> message_allocator_;
   MessageDeleter message_deleter_;
 };
 
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 9f82a31..e82a383 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -187,8 +187,9 @@ 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*
@@ -293,7 +294,7 @@ Node::create_subscription(
           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();
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 92249d7..f08a4a8 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -211,8 +211,9 @@ class Publisher : public PublisherBase
   friend rclcpp::node::Node;
 
 public:
-  using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, 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>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<MessageT, Alloc>);
@@ -225,8 +226,8 @@ public:
     std::shared_ptr<Alloc> allocator)
   : PublisherBase(node_handle, publisher_handle, topic, queue_size)
   {
-    message_allocator_ = new typename MessageAlloc::allocator_type(*allocator.get());
-    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_);
+    message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
+    allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get());
   }
 
 
@@ -237,7 +238,6 @@ public:
    */
   void
   publish(std::unique_ptr<MessageT, MessageDeleter> & msg)
-  //publish(MessageUniquePtr & msg)
   {
     this->do_inter_process_publish(msg.get());
     if (store_intra_process_message_) {
@@ -285,8 +285,8 @@ public:
     //   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 = MessageAlloc::allocate(*message_allocator_, 1);
-    MessageAlloc::construct(*message_allocator_, ptr, *msg.get());
+    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);
   }
@@ -304,8 +304,8 @@ public:
     //   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 = MessageAlloc::allocate(*message_allocator_, 1);
-    MessageAlloc::construct(*message_allocator_, ptr, *msg.get());
+    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);
   }
@@ -319,12 +319,17 @@ public:
       return this->do_inter_process_publish(&msg);
     }
     // Otherwise we have to allocate memory in a unique_ptr and pass it along.
-    auto ptr = MessageAlloc::allocate(*message_allocator_, 1);
-    MessageAlloc::construct(*message_allocator_, ptr, msg);
+    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)
@@ -338,7 +343,7 @@ protected:
     }
   }
 
-  typename MessageAlloc::allocator_type * message_allocator_;
+  std::shared_ptr<MessageAlloc> message_allocator_;
 
   MessageDeleter message_deleter_;
 
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index daa2e0f..90c6dee 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -35,21 +35,22 @@ namespace allocator_memory_strategy
  * come through.
  */
 template<typename Alloc>
-class AllocatorMemoryStrategy : public MemoryStrategy
+class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 {
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
 
-  using ExecAlloc = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
-  using ExecDeleter =
-      allocator::Deleter<typename ExecAlloc::allocator_type, executor::AnyExecutable>;
-  using VoidAlloc = allocator::AllocRebind<void *, 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;
 
   AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
-    executable_allocator_ = new ExecAlloc(*allocator.get());
-    allocator_ = new VoidAlloc(*allocator.get());
+    executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
+    allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
   }
 
   /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
@@ -137,8 +138,8 @@ public:
   virtual executor::AnyExecutable::SharedPtr instantiate_next_executable()
   {
     //return std::make_shared<executor::AnyExecutable>();
-    auto ptr = ExecAlloc::allocate(*executable_allocator_, 1);
-    ExecAlloc::construct(*executable_allocator_);
+    auto ptr = ExecAllocTraits::allocate(*executable_allocator_.get(), 1);
+    ExecAllocTraits::construct(*executable_allocator_.get(), ptr);
     return std::shared_ptr<executor::AnyExecutable>(ptr, executable_deleter_);
   }
 
@@ -152,7 +153,9 @@ public:
     if (size == 0) {
       return NULL;
     }
-    return VoidAlloc::allocate(*allocator_, size);
+    auto ptr = VoidAllocTraits::allocate(*allocator_.get(), size);
+    alloc_map[ptr] = size;
+    return ptr;
   }
 
   /// Implementation of a general-purpose free.
@@ -161,22 +164,34 @@ public:
    */
   virtual void free(void * ptr)
   {
-    VoidAlloc::deallocate(*allocator, ptr);
+    if (alloc_map.count(ptr) == 0) {
+      // do nothing, the pointer is not in the alloc'd map
+      return;
+    }
+    VoidAllocTraits::deallocate(*allocator_.get(), &ptr, alloc_map[ptr]);
   }
 
-  std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr, Alloc> subs;
-  std::vector<rclcpp::service::ServiceBase::SharedPtr, Alloc> services;
-  std::vector<rclcpp::client::ClientBase::SharedPtr, Alloc> clients;
+  template<typename U>
+  using VectorRebind = typename std::allocator_traits<Alloc>::template rebind_alloc<U>;
 
-  std::vector<void *, Alloc> subscription_handles;
-  std::vector<void *, Alloc> service_handles;
-  std::vector<void *, Alloc> client_handles;
+  std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr,
+  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr>> subs;
+  std::vector<rclcpp::service::ServiceBase::SharedPtr,
+  VectorRebind<rclcpp::service::ServiceBase::SharedPtr>> services;
+  std::vector<rclcpp::client::ClientBase::SharedPtr,
+  VectorRebind<rclcpp::client::ClientBase::SharedPtr>> clients;
+
+  std::vector<void *, VoidAlloc> subscription_handles;
+  std::vector<void *, VoidAlloc> service_handles;
+  std::vector<void *, VoidAlloc> client_handles;
   std::array<void *, 2> guard_cond_handles;
 
+  std::unordered_map<void *, size_t> alloc_map;
+
 private:
-  typename ExecAlloc::allocator_type * executable_allocator_;
+  std::shared_ptr<ExecAlloc> executable_allocator_;
   ExecDeleter executable_deleter_;
-  typename VoidAlloc::allocator_type * allocator_;
+  std::shared_ptr<VoidAlloc> allocator_;
 };
 
 }  /* allocator_memory_strategy */
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index 7f148c3..fbdb100 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/rclcpp/include/rclcpp/subscription.hpp
@@ -150,8 +150,9 @@ class Subscription : public SubscriptionBase
   friend class rclcpp::node::Node;
 
 public:
-  using MessageAlloc = allocator::AllocRebind<MessageT, Alloc>;
-  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, 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>;
 
   RCLCPP_SMART_PTR_DEFINITIONS(Subscription);
diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp
index df460c9..24d3f9b 100644
--- a/rclcpp/test/test_intra_process_manager.cpp
+++ b/rclcpp/test/test_intra_process_manager.cpp
@@ -59,10 +59,21 @@ template<typename T, typename Alloc = std::allocator<void>>
 class Publisher : public PublisherBase
 {
 public:
-  using MessageAlloc = allocator::AllocRebind<T, Alloc>;
-  using MessageDeleter = allocator::Deleter<typename MessageAlloc::allocator_type, T>;
+  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_;
+  }
 };
 
 }

From 4138e6be1d73311d2b0d056c841d67dfeca8404a Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Fri, 23 Oct 2015 11:41:45 -0700
Subject: [PATCH 06/12] Use allocate_shared

---
 rclcpp/include/rclcpp/message_memory_strategy.hpp           | 5 +----
 .../include/rclcpp/strategies/allocator_memory_strategy.hpp | 6 +-----
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index e88c077..9860303 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -58,10 +58,7 @@ public:
   // \return Shared pointer to the new message.
   virtual std::shared_ptr<MessageT> borrow_message()
   {
-    auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1);
-    MessageAllocTraits::construct(*message_allocator_.get(), ptr);
-    return std::shared_ptr<MessageT>(ptr, message_deleter_);
-    //return std::allocate_shared<MessageT, typename MessageAlloc::allocator_type>(*message_allocator_);
+    return std::allocate_shared<MessageT, MessageAlloc>(*message_allocator_.get());
   }
 
   /// Release ownership of the message, which will deallocate it if it has no more owners.
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index 90c6dee..a6ebfc3 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -137,10 +137,7 @@ public:
   // \return Shared pointer to the fresh executable.
   virtual executor::AnyExecutable::SharedPtr instantiate_next_executable()
   {
-    //return std::make_shared<executor::AnyExecutable>();
-    auto ptr = ExecAllocTraits::allocate(*executable_allocator_.get(), 1);
-    ExecAllocTraits::construct(*executable_allocator_.get(), ptr);
-    return std::shared_ptr<executor::AnyExecutable>(ptr, executable_deleter_);
+    return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
   }
 
   /// Implementation of a general-purpose allocation function.
@@ -190,7 +187,6 @@ public:
 
 private:
   std::shared_ptr<ExecAlloc> executable_allocator_;
-  ExecDeleter executable_deleter_;
   std::shared_ptr<VoidAlloc> allocator_;
 };
 

From c663d892a4a15c6555e051d5584182dabc1c2e4f Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Mon, 26 Oct 2015 17:01:40 -0700
Subject: [PATCH 07/12] templatize IPM state

---
 .../include/rclcpp/intra_process_manager.hpp  | 146 +++-------
 .../rclcpp/intra_process_manager_state.hpp    | 271 ++++++++++++++++++
 2 files changed, 308 insertions(+), 109 deletions(-)
 create mode 100644 rclcpp/include/rclcpp/intra_process_manager_state.hpp

diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp
index 21739cf..6ef4f18 100644
--- a/rclcpp/include/rclcpp/intra_process_manager.hpp
+++ b/rclcpp/include/rclcpp/intra_process_manager.hpp
@@ -15,6 +15,7 @@
 #ifndef RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_
 #define RCLCPP_RCLCPP_INTRA_PROCESS_MANAGER_HPP_
 
+#include <rclcpp/intra_process_manager_state.hpp>
 #include <rclcpp/mapped_ring_buffer.hpp>
 #include <rclcpp/macros.hpp>
 #include <rclcpp/publisher.hpp>
@@ -24,7 +25,6 @@
 #include <atomic>
 #include <cstdint>
 #include <exception>
-#include <limits>
 #include <map>
 #include <unordered_map>
 #include <set>
@@ -122,7 +122,10 @@ private:
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
 
-  IntraProcessManager() = default;
+  //IntraProcessManager() = default;
+  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 +143,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 +155,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.
@@ -194,18 +186,11 @@ public:
     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,
+    auto mrb = mapped_ring_buffer::MappedRingBuffer<MessageT,
       typename publisher::Publisher<MessageT, Alloc>::MessageAlloc>::make_shared(
       size, publisher->get_allocator());
-    publishers_[id].target_subscriptions_by_message_sequence.reserve(size);
+    state_->add_publisher(id, publisher, mrb, size);
     return id;
   }
 
@@ -217,7 +202,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.
@@ -258,37 +243,23 @@ public:
     typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
     MessageT>> & 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");
-    }
-    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.
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
     typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
-    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info.buffer);
+
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
+    auto message_seq = state_->get_publisher_info_for_id(intra_process_publisher_id, buffer);
+    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");
+    }
+
+    // Insert the message into the ring buffer using the message_seq to identify it.
     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;
   }
@@ -337,41 +308,22 @@ public:
     typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
     MessageT>> & message)
   {
-    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;
-    }
-    // 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);
-    }
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
     typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
-    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(info->buffer);
+
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
+    size_t target_subs_size = state_->take_intra_process_message(
+      intra_process_publisher_id,
+      message_sequence_number,
+      requesting_subscriptions_intra_process_id,
+      buffer
+    );
+    typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
+    if (!typed_buffer) {
+      return;
+    }
     // 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 {
@@ -384,16 +336,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:
@@ -420,22 +363,7 @@ 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::PublisherBase::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_;
 
 };
 
diff --git a/rclcpp/include/rclcpp/intra_process_manager_state.hpp b/rclcpp/include/rclcpp/intra_process_manager_state.hpp
new file mode 100644
index 0000000..906d9a2
--- /dev/null
+++ b/rclcpp/include/rclcpp/intra_process_manager_state.hpp
@@ -0,0 +1,271 @@
+// 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__INTRA_PROCESS_MANAGER_STATE_HPP_
+#define RCLCPP__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 <limits>
+#include <memory>
+#include <map>
+#include <unordered_map>
+#include <set>
+
+namespace rclcpp
+{
+namespace intra_process_manager
+{
+
+class IntraProcessManagerStateBase {
+public:
+  //RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManagerStateBase);
+  using SharedPtr = std::shared_ptr<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 uint64_t
+  get_publisher_info_for_id(
+    uint64_t intra_process_publisher_id,
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb) = 0;
+
+  virtual void
+  store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
+
+  virtual size_t
+  take_intra_process_message(uint64_t intra_process_publisher_id,
+    uint64_t message_sequence_number,
+    uint64_t requesting_subscriptions_intra_process_id,
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb) = 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);
+  }
+
+  // TODO
+  // return message_seq and mrb
+  uint64_t
+  get_publisher_info_for_id(
+    uint64_t intra_process_publisher_id,
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb)
+  {
+    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;
+    mrb = info.buffer;
+    // Calculate the next message sequence number.
+    uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
+
+    return message_seq;
+  }
+
+  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()
+      )
+    );
+
+  }
+
+  size_t
+  take_intra_process_message(uint64_t intra_process_publisher_id,
+    uint64_t message_sequence_number,
+    uint64_t requesting_subscriptions_intra_process_id,
+    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb
+  )
+  {
+    mrb = nullptr;
+    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);
+    }
+    mrb = info->buffer;
+    return target_subs->size();
+  }
+
+  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 AllocString = std::basic_string<char, std::char_traits<char>, RebindAlloc<char>>;
+  using AllocString = std::string;
+  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<AllocString, AllocSet,
+    std::less<AllocString>, RebindAlloc<std::pair<AllocString, 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<>>();
+}
+
+}
+}
+
+#endif

From 06818ee78c9641f383ac6c603fcb85cc4a79711b Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Mon, 26 Oct 2015 18:29:53 -0700
Subject: [PATCH 08/12] move a lot to memory strategy

---
 rclcpp/include/rclcpp/executor.hpp            |  53 +-
 rclcpp/include/rclcpp/memory_strategies.hpp   |  17 +-
 rclcpp/include/rclcpp/memory_strategy.hpp     | 158 ++----
 .../strategies/allocator_memory_strategy.hpp  | 495 ++++++++++++++----
 4 files changed, 478 insertions(+), 245 deletions(-)

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index 9a1658b..97fdd33 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -27,7 +27,7 @@
 
 #include <rclcpp/any_executable.hpp>
 #include <rclcpp/macros.hpp>
-#include <rclcpp/memory_strategy.hpp>
+#include <rclcpp/memory_strategies.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/utilities.hpp>
 
@@ -343,10 +343,17 @@ protected:
   void
   wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
+    /*
     memory_strategy_->subs.clear();
     memory_strategy_->services.clear();
     memory_strategy_->clients.clear();
+    */
+    memory_strategy_->clear_active_entities();
+
     // Collect the subscriptions and timers to be waited on
+    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+
+    /*
     bool has_invalid_weak_nodes = false;
     for (auto & weak_node : weak_nodes_) {
       auto node = weak_node.lock();
@@ -373,6 +380,8 @@ protected:
         }
       }
     }
+    */
+
     // Clean up any invalid nodes, if they were detected
     if (has_invalid_weak_nodes) {
       weak_nodes_.erase(
@@ -382,7 +391,22 @@ protected:
             return i.expired();
           }));
     }
+
+
     // Use the number of subscriptions to allocate memory in the handles
+    rmw_subscriptions_t subscriber_handles;
+    subscriber_handles.subscriber_count =
+      memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers);
+
+    rmw_services_t service_handles;
+    service_handles.service_count =
+      memory_strategy_->fill_service_handles(service_handles.services);
+
+    rmw_clients_t client_handles;
+    client_handles.client_count =
+      memory_strategy_->fill_client_handles(client_handles.clients);
+
+    /*
     size_t max_number_of_subscriptions = memory_strategy_->subs.size() * 2;  // Times two for intra-process.
     rmw_subscriptions_t subscriber_handles;
     subscriber_handles.subscriber_count = 0;
@@ -442,14 +466,16 @@ protected:
         client->get_client_handle()->data;
       client_handle_index += 1;
     }
+    */
 
     // 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 =
+      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)
     {
@@ -500,6 +526,7 @@ 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,
@@ -508,10 +535,13 @@ protected:
         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) {
@@ -545,10 +575,13 @@ protected:
     memory_strategy_->subs.clear();
     memory_strategy_->services.clear();
     memory_strategy_->clients.clear();
+    */
+    memory_strategy_->clear_active_entities();
   }
 
 /******************************/
 
+/*
   rclcpp::subscription::SubscriptionBase::SharedPtr
   get_subscription_by_handle(void * subscriber_handle)
   {
@@ -579,6 +612,7 @@ protected:
     }
     return nullptr;
   }
+*/
 
   rclcpp::service::ServiceBase::SharedPtr
   get_service_by_handle(void * service_handle)
@@ -729,6 +763,7 @@ protected:
     return latest;
   }
 
+  /*
   rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_subscription(
     rclcpp::subscription::SubscriptionBase::SharedPtr subscription)
@@ -909,28 +944,29 @@ protected:
       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;
     }
@@ -980,12 +1016,15 @@ private:
   RCLCPP_DISABLE_COPY(Executor);
 
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
+  std::array<void*, 2> guard_cond_handles_;
+  /*
   using SubscriberHandles = std::list<void *>;
   SubscriberHandles subscriber_handles_;
   using ServiceHandles = std::list<void *>;
   ServiceHandles service_handles_;
   using ClientHandles = std::list<void *>;
   ClientHandles client_handles_;
+  */
 };
 
 } /* executor */
diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp
index 598da69..2887426 100644
--- a/rclcpp/include/rclcpp/memory_strategies.hpp
+++ b/rclcpp/include/rclcpp/memory_strategies.hpp
@@ -15,18 +15,27 @@
 #ifndef RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_
 #define RCLCPP_RCLCPP_MEMORY_STRATEGIES_HPP_
 
-#include <rclcpp/strategies/heap_pool_memory_strategy.hpp>
-#include <rclcpp/strategies/stack_pool_memory_strategy.hpp>
+//#include <rclcpp/strategies/heap_pool_memory_strategy.hpp>
+//#include <rclcpp/strategies/stack_pool_memory_strategy.hpp>
+#include <rclcpp/strategies/allocator_memory_strategy.hpp>
 
 namespace rclcpp
 {
 namespace memory_strategies
 {
 
-using rclcpp::memory_strategies::heap_pool_memory_strategy::HeapPoolMemoryStrategy;
-using rclcpp::memory_strategies::stack_pool_memory_strategy::StackPoolMemoryStrategy;
+using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
+//using rclcpp::memory_strategies::heap_pool_memory_strategy::HeapPoolMemoryStrategy;
+//using rclcpp::memory_strategies::stack_pool_memory_strategy::StackPoolMemoryStrategy;
 
 }  /* memory_strategies */
+
+namespace memory_strategy {
+  MemoryStrategy::SharedPtr create_default_strategy() {
+    return std::make_shared<memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+  }
+}
+
 }  /* rclcpp */
 
 #endif
diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp
index eed0e0e..86acfb8 100644
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -14,19 +14,17 @@
 
 #ifndef RCLCPP_RCLCPP_MEMORY_STRATEGY_HPP_
 #define RCLCPP_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,137 +36,43 @@ 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<rclcpp::node::Node>>>;
 
-  /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
-  /**
-   * The default implementation stores std::vectors for each handle type and resizes the vectors
-   * as necessary based on the requested number of handles.
-   * \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)
-  {
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (subscription_handles.size() < number_of_handles) {
-          subscription_handles.resize(number_of_handles, 0);
-        }
-        return static_cast<void **>(subscription_handles.data());
-      case HandleType::service_handle:
-        if (service_handles.size() < number_of_handles) {
-          service_handles.resize(number_of_handles, 0);
-        }
-        return static_cast<void **>(service_handles.data());
-      case HandleType::client_handle:
-        if (client_handles.size() < number_of_handles) {
-          client_handles.resize(number_of_handles, 0);
-        }
-        return static_cast<void **>(client_handles.data());
-      case HandleType::guard_condition_handle:
-        if (number_of_handles > 2) {
-          throw std::runtime_error("Too many guard condition handles requested!");
-        }
-        return guard_cond_handles.data();
-      default:
-        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
-                ", could not borrow handle memory.");
-    }
-  }
+  // 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)
-  {
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (handles != subscription_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this MemoryStrategy");
-        }
-        memset(handles, 0, subscription_handles.size());
-        break;
-      case HandleType::service_handle:
-        if (handles != service_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this MemoryStrategy");
-        }
-        memset(handles, 0, service_handles.size());
-        break;
-      case HandleType::client_handle:
-        if (handles != client_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this MemoryStrategy");
-        }
-        memset(handles, 0, client_handles.size());
-        break;
-      case HandleType::guard_condition_handle:
-        if (handles != guard_cond_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this MemoryStrategy");
-        }
-        guard_cond_handles.fill(0);
-        break;
-      default:
-        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
-                ", could not borrow handle memory.");
-    }
-  }
+  // 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 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)
-  {
-    if (size == 0) {
-      return NULL;
-    }
-    return std::malloc(size);
-  }
+  virtual void
+  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
+    const WeakNodeVector & weak_nodes) = 0;
 
-  /// Implementation of a general-purpose free.
-  /**
-   * \param[in] Pointer to deallocate.
-   */
-  virtual void free(void * ptr)
-  {
-    return std::free(ptr);
-  }
+  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;
 
-  std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr> subs;
-  std::vector<rclcpp::service::ServiceBase::SharedPtr> services;
-  std::vector<rclcpp::client::ClientBase::SharedPtr> clients;
 
-  std::vector<void *> subscription_handles;
-  std::vector<void *> service_handles;
-  std::vector<void *> client_handles;
-  std::array<void *, 2> guard_cond_handles;
 };
 
-
-MemoryStrategy::SharedPtr create_default_strategy()
-{
-  return std::make_shared<MemoryStrategy>(MemoryStrategy());
-}
-
 }  /* memory_strategy */
 
 }  /* rclcpp */
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index a6ebfc3..f47b95c 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -15,13 +15,15 @@
 #ifndef RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
 #define RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
 
+#include <memory>
+#include <vector>
+
 #include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/memory_strategy.hpp>
 
 namespace rclcpp
 {
 
-
 namespace memory_strategies
 {
 
@@ -34,13 +36,16 @@ namespace allocator_memory_strategy
  * the rmw implementation after the executor waits for work, based on the number of entities that
  * come through.
  */
-template<typename Alloc>
+template<typename Alloc = std::allocator<void>>
 class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 {
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
 
+  using WeakNode = std::weak_ptr<rclcpp::node::Node>>;
+  using NodeVector = std::vector<WeakNode>;
+
   using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
   using ExecAlloc = typename ExecAllocTraits::allocator_type;
   using ExecDeleter = allocator::Deleter<ExecAlloc, executor::AnyExecutable>;
@@ -53,139 +58,415 @@ public:
     allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
   }
 
-  /// Borrow memory for storing data for subscriptions, services, clients, or guard conditions.
-  /**
-   * The default implementation stores std::vectors for each handle type and resizes the vectors
-   * as necessary based on the requested number of handles.
-   * \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)
+  AllocatorMemoryStrategy()
   {
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (subscription_handles.size() < number_of_handles) {
-          subscription_handles.resize(number_of_handles, 0);
-        }
-        return static_cast<void **>(subscription_handles.data());
-      case HandleType::service_handle:
-        if (service_handles.size() < number_of_handles) {
-          service_handles.resize(number_of_handles, 0);
-        }
-        return static_cast<void **>(service_handles.data());
-      case HandleType::client_handle:
-        if (client_handles.size() < number_of_handles) {
-          client_handles.resize(number_of_handles, 0);
-        }
-        return static_cast<void **>(client_handles.data());
-      case HandleType::guard_condition_handle:
-        if (number_of_handles > 2) {
-          throw std::runtime_error("Too many guard condition handles requested!");
-        }
-        return guard_cond_handles.data();
-      default:
-        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
-                ", could not borrow handle memory.");
-    }
+    executable_allocator_ = std::make_shared<ExecAlloc>();
+    allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  /// 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)
-  {
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (handles != subscription_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
-        }
-        memset(handles, 0, subscription_handles.size());
-        break;
-      case HandleType::service_handle:
-        if (handles != service_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
-        }
-        memset(handles, 0, service_handles.size());
-        break;
-      case HandleType::client_handle:
-        if (handles != client_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
-        }
-        memset(handles, 0, client_handles.size());
-        break;
-      case HandleType::guard_condition_handle:
-        if (handles != guard_cond_handles.data()) {
-          throw std::runtime_error(
-                  "tried to return memory that isn't handled by this AllocatorMemoryStrategy");
-        }
-        guard_cond_handles.fill(0);
-        break;
-      default:
-        throw std::runtime_error("Unknown HandleType " + std::to_string(static_cast<int>(type)) +
-                ", could not borrow handle memory.");
+  size_t fill_subscriber_handles(void ** ptr) {
+    size_t max_size = subscriptions_.size();
+    if (subscriber_handles_.size() < max_size) {
+      subscriber_handles_.resize(max_size);
     }
+    size_t count = 0;
+    for (auto & subscription : subscriptions_) {
+      subscriber_handles_[count++] = subscription->get_subscription_handle()->data;
+      if (subscription->get_intra_process_subscription_handle()) {
+        subscriber_handles_[count++] =
+          subscription->get_intra_process_subscription_handle()->data;
+      }
+    }
+    ptr = static_cast<void**>(subscriber_handles_.data());
+    return count;
   }
 
+  // return the new number of services
+  size_t fill_service_handles(void ** ptr) {
+    size_t max_size = services_.size();
+    if (service_handles_.size() < max_size) {
+      service_handles_.resize(max_size);
+    }
+    size_t count = 0;
+    for (auto & service : services_) {
+      service_handles_[count++] = service->get_service_handle()->data;
+    }
+    ptr = static_cast<void**>(service_handles_.data());
+    return count;
+  }
+
+  // return the new number of clients
+  size_t fill_client_handles(void ** ptr) {
+    size_t max_size = clients_.size();
+    if (client_handles_.size() < max_size) {
+      client_handles_.resize(max_size);
+    }
+    size_t count = 0;
+    for (auto & client : clients_) {
+      client_handles_[count++] = client->get_client_handle()->data;
+    }
+    ptr = static_cast<void**>(client_handles_.data());
+    return count;
+  }
+
+  void clear_active_entities() {
+    subscriptions_.clear();
+    services_.clear();
+    clients_.clear();
+  }
+
+  void clear_handles() {
+    subscriber_handles_.clear();
+    service_handles_.clear();
+    client_handles_.clear();
+  }
+
+  bool collect_entities(const NodeVector & 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()) {
+          services_.push_back(service);
+        }
+        for (auto & client : group->get_client_ptrs()) {
+          clients_.push_back(client);
+        }
+      }
+    }
+    return has_invalid_weak_nodes;
+  }
+
+
   /// Provide a newly initialized AnyExecutable object.
   // \return Shared pointer to the fresh executable.
-  virtual executor::AnyExecutable::SharedPtr instantiate_next_executable()
+  executor::AnyExecutable::SharedPtr instantiate_next_executable()
   {
     return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
   }
 
-  /// 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)
+  static rclcpp::subscription::SubscriptionBase::SharedPtr
+  get_subscription_by_handle(void * subscriber_handle,
+    const NodeVector & 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;
+            }
+          }
+        }
+      }
     }
-    auto ptr = VoidAllocTraits::allocate(*allocator_.get(), size);
-    alloc_map[ptr] = size;
-    return ptr;
+    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 NodeVector & weak_nodes)
   {
-    if (alloc_map.count(ptr) == 0) {
-      // do nothing, the pointer is not in the alloc'd map
-      return;
+    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;
+          }
+        }
+      }
     }
-    VoidAllocTraits::deallocate(*allocator_.get(), &ptr, alloc_map[ptr]);
+    return rclcpp::service::ServiceBase::SharedPtr();
   }
 
+  static rclcpp::client::ClientBase::SharedPtr
+  get_client_by_handle(void * client_handle,
+    const NodeVector & 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 rclcpp::client::ClientBase::SharedPtr();
+  }
+
+  static rclcpp::node::Node::SharedPtr
+  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
+    const NodeVector & weak_nodes)
+  {
+    if (!group) {
+      return rclcpp::node::Node::SharedPtr();
+    }
+    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 rclcpp::node::Node::SharedPtr();
+  }
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_subscription(
+    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
+    const NodeVector & 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 rclcpp::callback_group::CallbackGroup::SharedPtr();
+  }
+
+
+
+  virtual void
+  get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
+    const NodeVector & 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++);
+    }
+  }
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_service(
+    rclcpp::service::ServiceBase::SharedPtr service,
+    const NodeVector & 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 rclcpp::callback_group::CallbackGroup::SharedPtr();
+  }
+
+  virtual void
+  get_next_service(executor::AnyExecutable::SharedPtr any_exec,
+    const NodeVector & 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++);
+    }
+  }
+
+  static rclcpp::callback_group::CallbackGroup::SharedPtr
+  get_group_by_client(
+    rclcpp::client::ClientBase::SharedPtr client,
+    const NodeVector & 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 rclcpp::callback_group::CallbackGroup::SharedPtr();
+  }
+
+  virtual void
+  get_next_client(executor::AnyExecutable::SharedPtr any_exec,
+    const NodeVector & 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 U>
   using VectorRebind = typename std::allocator_traits<Alloc>::template rebind_alloc<U>;
 
   std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr,
-  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr>> subs;
+  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr>> subscriptions_;
   std::vector<rclcpp::service::ServiceBase::SharedPtr,
-  VectorRebind<rclcpp::service::ServiceBase::SharedPtr>> services;
+  VectorRebind<rclcpp::service::ServiceBase::SharedPtr>> services_;
   std::vector<rclcpp::client::ClientBase::SharedPtr,
-  VectorRebind<rclcpp::client::ClientBase::SharedPtr>> clients;
+  VectorRebind<rclcpp::client::ClientBase::SharedPtr>> clients_;
 
-  std::vector<void *, VoidAlloc> subscription_handles;
-  std::vector<void *, VoidAlloc> service_handles;
-  std::vector<void *, VoidAlloc> client_handles;
-  std::array<void *, 2> guard_cond_handles;
+  std::vector<void *, VoidAlloc> subscriber_handles_;
+  std::vector<void *, VoidAlloc> service_handles_;
+  std::vector<void *, VoidAlloc> client_handles_;
 
-  std::unordered_map<void *, size_t> alloc_map;
-
-private:
   std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;
 };

From 0e78ea0512f55dad8783b205764746c7117a4545 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Tue, 27 Oct 2015 10:57:57 -0700
Subject: [PATCH 09/12] static factory method

---
 rclcpp/include/rclcpp/memory_strategies.hpp                   | 4 ++--
 rclcpp/include/rclcpp/memory_strategy.hpp                     | 2 +-
 .../include/rclcpp/strategies/allocator_memory_strategy.hpp   | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp
index 2887426..b69ed51 100644
--- a/rclcpp/include/rclcpp/memory_strategies.hpp
+++ b/rclcpp/include/rclcpp/memory_strategies.hpp
@@ -31,10 +31,10 @@ using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrat
 }  /* memory_strategies */
 
 namespace memory_strategy {
-  MemoryStrategy::SharedPtr create_default_strategy() {
+  static MemoryStrategy::SharedPtr create_default_strategy() {
     return std::make_shared<memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
   }
-}
+} /* memory_strategy */
 
 }  /* rclcpp */
 
diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp
index 86acfb8..22291bd 100644
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -38,7 +38,7 @@ class MemoryStrategy
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
-  using WeakNodeVector = std::vector<std::weak_ptr<rclcpp::node::Node>>>;
+  using WeakNodeVector = typename std::vector<std::weak_ptr<rclcpp::node::Node>>>;
 
   // return the new number of subscribers
   virtual size_t fill_subscriber_handles(void ** ptr) = 0;
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index f47b95c..c0f86fc 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -43,8 +43,6 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
 
-  using WeakNode = std::weak_ptr<rclcpp::node::Node>>;
-  using NodeVector = std::vector<WeakNode>;
 
   using ExecAllocTraits = allocator::AllocRebind<executor::AnyExecutable, Alloc>;
   using ExecAlloc = typename ExecAllocTraits::allocator_type;
@@ -52,6 +50,8 @@ public:
   using VoidAllocTraits = typename allocator::AllocRebind<void *, Alloc>;
   using VoidAlloc = typename VoidAllocTraits::allocator_type;
 
+  using NodeVector = typename std::vector<std::weak_ptr<rclcpp::node::Node>>>;
+
   AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());

From fc48cf5fa2fbaae4e50262586eb0ae4b746b4ab3 Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Tue, 27 Oct 2015 11:55:01 -0700
Subject: [PATCH 10/12] remove pool memory strategies

---
 .../rclcpp/any_subscription_callback.hpp      |   3 +-
 rclcpp/include/rclcpp/executor.hpp            | 422 +-----------------
 .../executors/multi_threaded_executor.hpp     |   2 +-
 .../executors/single_threaded_executor.hpp    |   2 +-
 .../include/rclcpp/intra_process_manager.hpp  |   5 +-
 .../rclcpp/intra_process_manager_state.hpp    |  46 +-
 rclcpp/include/rclcpp/memory_strategies.hpp   |  13 +-
 rclcpp/include/rclcpp/memory_strategy.hpp     | 138 +++++-
 .../strategies/allocator_memory_strategy.hpp  | 214 ++-------
 .../strategies/heap_pool_memory_strategy.hpp  | 336 --------------
 .../strategies/stack_pool_memory_strategy.hpp | 219 ---------
 rclcpp/test/test_intra_process_manager.cpp    |   6 +-
 12 files changed, 227 insertions(+), 1179 deletions(-)
 delete mode 100644 rclcpp/include/rclcpp/strategies/heap_pool_memory_strategy.hpp
 delete mode 100644 rclcpp/include/rclcpp/strategies/stack_pool_memory_strategy.hpp

diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
index cb9a23c..bdce88b 100644
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -45,7 +45,8 @@ class AnySubscriptionCallback
   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(MessageUniquePtr, const rmw_message_info_t &)>;
+  using UniquePtrWithInfoCallback =
+      std::function<void(MessageUniquePtr, const rmw_message_info_t &)>;
 
   SharedPtrCallback shared_ptr_callback_;
   SharedPtrWithInfoCallback shared_ptr_with_info_callback_;
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index 97fdd33..b567aef 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -27,6 +27,7 @@
 
 #include <rclcpp/any_executable.hpp>
 #include <rclcpp/macros.hpp>
+#include <rclcpp/memory_strategy.hpp>
 #include <rclcpp/memory_strategies.hpp>
 #include <rclcpp/node.hpp>
 #include <rclcpp/utilities.hpp>
@@ -48,7 +49,6 @@ namespace executor
  */
 class Executor
 {
-  friend class memory_strategy::MemoryStrategy;
 
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
@@ -56,7 +56,7 @@ public:
   /// 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)
   {
@@ -343,45 +343,11 @@ protected:
   void
   wait_for_work(std::chrono::duration<int64_t, T> timeout = std::chrono::duration<int64_t, T>(-1))
   {
-    /*
-    memory_strategy_->subs.clear();
-    memory_strategy_->services.clear();
-    memory_strategy_->clients.clear();
-    */
     memory_strategy_->clear_active_entities();
 
     // Collect the subscriptions and timers to be waited on
     bool has_invalid_weak_nodes = memory_strategy_->collect_entities(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) {
-            memory_strategy_->subs.push_back(subscription);
-          }
-        }
-        for (auto & service : group->get_service_ptrs()) {
-          memory_strategy_->services.push_back(service);
-        }
-        for (auto & client : group->get_client_ptrs()) {
-          memory_strategy_->clients.push_back(client);
-        }
-      }
-    }
-    */
-
     // Clean up any invalid nodes, if they were detected
     if (has_invalid_weak_nodes) {
       weak_nodes_.erase(
@@ -406,76 +372,12 @@ protected:
     client_handles.client_count =
       memory_strategy_->fill_client_handles(client_handles.clients);
 
-    /*
-    size_t max_number_of_subscriptions = memory_strategy_->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 : memory_strategy_->subs) {
-      subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
-        subscription->get_subscription_handle()->data;
-      if (subscription->get_intra_process_subscription_handle()) {
-        subscriber_handles.subscribers[subscriber_handles.subscriber_count++] =
-          subscription->get_intra_process_subscription_handle()->data;
-      }
-    }
-
-    // Use the number of services to allocate memory in the handles
-    size_t number_of_services = memory_strategy_->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 : memory_strategy_->services) {
-      service_handles.services[service_handle_index] = \
-        service->get_service_handle()->data;
-      service_handle_index += 1;
-    }
-
-    // Use the number of clients to allocate memory in the handles
-    size_t number_of_clients = memory_strategy_->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 : memory_strategy_->clients) {
-      client_handles.clients[client_handle_index] = \
-        client->get_client_handle()->data;
-      client_handle_index += 1;
-    }
-    */
-
     // 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());
+    guard_condition_handles.guard_conditions = static_cast<void **>(guard_cond_handles_.data());
     if (guard_condition_handles.guard_conditions == NULL &&
       number_of_guard_conds > 0)
     {
@@ -526,140 +428,15 @@ 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_->subs.clear();
-    memory_strategy_->services.clear();
-    memory_strategy_->clients.clear();
-    */
     memory_strategy_->clear_active_entities();
   }
 
 /******************************/
-
-/*
-  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->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 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->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 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->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 rclcpp::client::ClientBase::SharedPtr();
-  }
-
   rclcpp::node::Node::SharedPtr
   get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group)
   {
@@ -763,189 +540,6 @@ 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->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 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->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);
-        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->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & serv : group->get_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->get_callback_groups()) {
-        auto group = weak_group.lock();
-        if (!group) {
-          continue;
-        }
-        for (auto & cli : group->get_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()
   {
@@ -1016,15 +610,7 @@ private:
   RCLCPP_DISABLE_COPY(Executor);
 
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
-  std::array<void*, 2> guard_cond_handles_;
-  /*
-  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 */
diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
index a251560..7cfea87 100644
--- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
+++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
@@ -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();
diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
index 9155956..c082296 100644
--- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
+++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
@@ -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.
diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp
index 6ef4f18..4790ae1 100644
--- a/rclcpp/include/rclcpp/intra_process_manager.hpp
+++ b/rclcpp/include/rclcpp/intra_process_manager.hpp
@@ -124,7 +124,8 @@ public:
 
   //IntraProcessManager() = default;
   IntraProcessManager(IntraProcessManagerStateBase::SharedPtr state = create_default_state())
-    : state_(state) {
+  : state_(state)
+  {
   }
 
   /// Register a subscription with the manager, returns subscriptions unique id.
@@ -317,7 +318,7 @@ public:
       message_sequence_number,
       requesting_subscriptions_intra_process_id,
       buffer
-    );
+      );
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
       return;
diff --git a/rclcpp/include/rclcpp/intra_process_manager_state.hpp b/rclcpp/include/rclcpp/intra_process_manager_state.hpp
index 906d9a2..cc59e6f 100644
--- a/rclcpp/include/rclcpp/intra_process_manager_state.hpp
+++ b/rclcpp/include/rclcpp/intra_process_manager_state.hpp
@@ -32,7 +32,8 @@ namespace rclcpp
 namespace intra_process_manager
 {
 
-class IntraProcessManagerStateBase {
+class IntraProcessManagerStateBase
+{
 public:
   //RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManagerStateBase);
   using SharedPtr = std::shared_ptr<IntraProcessManagerStateBase>;
@@ -44,9 +45,9 @@ public:
   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;
+    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;
@@ -70,17 +71,19 @@ public:
 };
 
 template<typename Allocator = std::allocator<void>>
-class IntraProcessManagerState : public IntraProcessManagerStateBase {
+class IntraProcessManagerState : public IntraProcessManagerStateBase
+{
 public:
-
   void
-  add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) {
+  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) {
+  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);
@@ -96,9 +99,10 @@ public:
   }
 
   void add_publisher(uint64_t id,
-      publisher::PublisherBase::WeakPtr publisher,
-      mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb,
-      size_t size) {
+    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.
@@ -112,7 +116,8 @@ public:
   }
 
   void
-  remove_publisher(uint64_t intra_process_publisher_id) {
+  remove_publisher(uint64_t intra_process_publisher_id)
+  {
     publishers_.erase(intra_process_publisher_id);
   }
 
@@ -220,7 +225,6 @@ public:
     return false;
   }
 
-
 private:
   template<typename T>
   using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
@@ -229,10 +233,10 @@ private:
   using AllocString = std::string;
   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>>>;
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, subscription::SubscriptionBase::WeakPtr>>>;
   using IDTopicMap = std::map<AllocString, AllocSet,
-    std::less<AllocString>, RebindAlloc<std::pair<AllocString, AllocSet>>>;
+      std::less<AllocString>, RebindAlloc<std::pair<AllocString, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -249,19 +253,21 @@ private:
     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>>>;
+        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>>>;
+      std::hash<uint64_t>, std::equal_to<uint64_t>,
+      RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
 
 };
 
-static IntraProcessManagerStateBase::SharedPtr create_default_state() {
+static IntraProcessManagerStateBase::SharedPtr create_default_state()
+{
   return std::make_shared<IntraProcessManagerState<>>();
 }
 
diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp
index b69ed51..fee7c25 100644
--- a/rclcpp/include/rclcpp/memory_strategies.hpp
+++ b/rclcpp/include/rclcpp/memory_strategies.hpp
@@ -17,6 +17,7 @@
 
 //#include <rclcpp/strategies/heap_pool_memory_strategy.hpp>
 //#include <rclcpp/strategies/stack_pool_memory_strategy.hpp>
+#include <rclcpp/memory_strategy.hpp>
 #include <rclcpp/strategies/allocator_memory_strategy.hpp>
 
 namespace rclcpp
@@ -28,14 +29,12 @@ using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrat
 //using rclcpp::memory_strategies::heap_pool_memory_strategy::HeapPoolMemoryStrategy;
 //using rclcpp::memory_strategies::stack_pool_memory_strategy::StackPoolMemoryStrategy;
 
+static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
+{
+  return std::make_shared<memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+}
+
 }  /* memory_strategies */
-
-namespace memory_strategy {
-  static MemoryStrategy::SharedPtr create_default_strategy() {
-    return std::make_shared<memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
-  }
-} /* memory_strategy */
-
 }  /* rclcpp */
 
 #endif
diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp
index 22291bd..cdc27cf 100644
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -24,10 +24,10 @@
 
 namespace rclcpp
 {
-
 namespace 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
@@ -38,16 +38,16 @@ class MemoryStrategy
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy);
-  using WeakNodeVector = typename std::vector<std::weak_ptr<rclcpp::node::Node>>>;
+  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
 
   // return the new number of subscribers
-  virtual size_t fill_subscriber_handles(void ** ptr) = 0;
+  virtual size_t fill_subscriber_handles(void ** & ptr) = 0;
 
   // return the new number of services
-  virtual size_t fill_service_handles(void ** ptr) = 0;
+  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 size_t fill_client_handles(void ** & ptr) = 0;
 
   virtual void clear_active_entities() = 0;
 
@@ -70,6 +70,134 @@ public:
   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)
+  {
+    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 nullptr;
+  }
+
+  static rclcpp::service::ServiceBase::SharedPtr
+  get_service_by_handle(void * service_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 & service : group->get_service_ptrs()) {
+          if (service->get_service_handle()->data == service_handle) {
+            return service;
+          }
+        }
+      }
+    }
+    return rclcpp::service::ServiceBase::SharedPtr();
+  }
+
+  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 rclcpp::client::ClientBase::SharedPtr();
+  }
+
+  static rclcpp::node::Node::SharedPtr
+  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
+    const WeakNodeVector & weak_nodes)
+  {
+    if (!group) {
+      return rclcpp::node::Node::SharedPtr();
+    }
+    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 rclcpp::node::Node::SharedPtr();
+  }
+
+  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 rclcpp::callback_group::CallbackGroup::SharedPtr();
+  }
+
 
 };
 
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index c0f86fc..1117ca6 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -20,6 +20,7 @@
 
 #include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/memory_strategy.hpp>
+#include <rclcpp/node.hpp>
 
 namespace rclcpp
 {
@@ -43,14 +44,13 @@ 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 NodeVector = typename std::vector<std::weak_ptr<rclcpp::node::Node>>>;
+  using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
 
   AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
@@ -64,11 +64,10 @@ public:
     allocator_ = std::make_shared<VoidAlloc>();
   }
 
-  size_t fill_subscriber_handles(void ** ptr) {
+  size_t fill_subscriber_handles(void ** & ptr)
+  {
     size_t max_size = subscriptions_.size();
-    if (subscriber_handles_.size() < max_size) {
-      subscriber_handles_.resize(max_size);
-    }
+    subscriber_handles_.resize(max_size);
     size_t count = 0;
     for (auto & subscription : subscriptions_) {
       subscriber_handles_[count++] = subscription->get_subscription_handle()->data;
@@ -77,12 +76,13 @@ public:
           subscription->get_intra_process_subscription_handle()->data;
       }
     }
-    ptr = static_cast<void**>(subscriber_handles_.data());
+    ptr = subscriber_handles_.data();
     return count;
   }
 
   // return the new number of services
-  size_t fill_service_handles(void ** ptr) {
+  size_t fill_service_handles(void ** & ptr)
+  {
     size_t max_size = services_.size();
     if (service_handles_.size() < max_size) {
       service_handles_.resize(max_size);
@@ -91,12 +91,13 @@ public:
     for (auto & service : services_) {
       service_handles_[count++] = service->get_service_handle()->data;
     }
-    ptr = static_cast<void**>(service_handles_.data());
+    ptr = service_handles_.data();
     return count;
   }
 
   // return the new number of clients
-  size_t fill_client_handles(void ** ptr) {
+  size_t fill_client_handles(void ** & ptr)
+  {
     size_t max_size = clients_.size();
     if (client_handles_.size() < max_size) {
       client_handles_.resize(max_size);
@@ -105,23 +106,26 @@ public:
     for (auto & client : clients_) {
       client_handles_[count++] = client->get_client_handle()->data;
     }
-    ptr = static_cast<void**>(client_handles_.data());
+    ptr = client_handles_.data();
     return count;
   }
 
-  void clear_active_entities() {
+  void clear_active_entities()
+  {
     subscriptions_.clear();
     services_.clear();
     clients_.clear();
   }
 
-  void clear_handles() {
+  void clear_handles()
+  {
     subscriber_handles_.clear();
     service_handles_.clear();
     client_handles_.clear();
   }
 
-  bool collect_entities(const NodeVector & weak_nodes) {
+  bool collect_entities(const WeakNodeVector & weak_nodes)
+  {
     bool has_invalid_weak_nodes = false;
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
@@ -159,143 +163,15 @@ public:
     return std::allocate_shared<executor::AnyExecutable>(*executable_allocator_.get());
   }
 
-  static rclcpp::subscription::SubscriptionBase::SharedPtr
-  get_subscription_by_handle(void * subscriber_handle,
-    const NodeVector & 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_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 nullptr;
-  }
-
-  static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle,
-    const NodeVector & 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 & service : group->get_service_ptrs()) {
-          if (service->get_service_handle()->data == service_handle) {
-            return service;
-          }
-        }
-      }
-    }
-    return rclcpp::service::ServiceBase::SharedPtr();
-  }
-
-  static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle,
-    const NodeVector & 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 rclcpp::client::ClientBase::SharedPtr();
-  }
-
-  static rclcpp::node::Node::SharedPtr
-  get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group,
-    const NodeVector & weak_nodes)
-  {
-    if (!group) {
-      return rclcpp::node::Node::SharedPtr();
-    }
-    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 rclcpp::node::Node::SharedPtr();
-  }
-
-  static rclcpp::callback_group::CallbackGroup::SharedPtr
-  get_group_by_subscription(
-    rclcpp::subscription::SubscriptionBase::SharedPtr subscription,
-    const NodeVector & 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 rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
-
-
-
   virtual void
   get_next_subscription(executor::AnyExecutable::SharedPtr any_exec,
-    const NodeVector & weak_nodes)
+    const WeakNodeVector & weak_nodes)
   {
     auto it = subscriber_handles_.begin();
+    VectorRebind<typename VectorRebind<void *>::iterator> to_erase;
     while (it != subscriber_handles_.end()) {
-      auto subscription = get_subscription_by_handle(*it, weak_nodes);
+      auto subscription = memory_strategy::MemoryStrategy::get_subscription_by_handle(*it,
+          weak_nodes);
       if (subscription) {
         // Figure out if this is for intra-process or not.
         bool is_intra_process = false;
@@ -307,7 +183,8 @@ public:
         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++);
+          //subscriber_handles_.erase(it++);
+          to_erase.push_back(it++);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -324,18 +201,25 @@ public:
         }
         any_exec->callback_group = group;
         any_exec->node = get_node_by_group(group, weak_nodes);
-        subscriber_handles_.erase(it++);
+        to_erase.push_back(it++);
         return;
       }
       // Else, the subscription is no longer valid, remove it and continue
-      subscriber_handles_.erase(it++);
+      // TODO
+      to_erase.push_back(it++);
     }
+    for (auto & entry : to_erase) {
+      if (entry != subscriber_handles_.end()) {
+        subscriber_handles_.erase(entry);
+      }
+    }
+
   }
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_service(
     rclcpp::service::ServiceBase::SharedPtr service,
-    const NodeVector & weak_nodes)
+    const WeakNodeVector & weak_nodes)
   {
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
@@ -359,11 +243,11 @@ public:
 
   virtual void
   get_next_service(executor::AnyExecutable::SharedPtr any_exec,
-    const NodeVector & weak_nodes)
+    const WeakNodeVector & weak_nodes)
   {
     auto it = service_handles_.begin();
     while (it != service_handles_.end()) {
-      auto service = get_service_by_handle(*it, weak_nodes);
+      auto service = memory_strategy::MemoryStrategy::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);
@@ -394,7 +278,7 @@ public:
   static rclcpp::callback_group::CallbackGroup::SharedPtr
   get_group_by_client(
     rclcpp::client::ClientBase::SharedPtr client,
-    const NodeVector & weak_nodes)
+    const WeakNodeVector & weak_nodes)
   {
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
@@ -418,11 +302,11 @@ public:
 
   virtual void
   get_next_client(executor::AnyExecutable::SharedPtr any_exec,
-    const NodeVector & weak_nodes)
+    const WeakNodeVector & weak_nodes)
   {
     auto it = client_handles_.begin();
     while (it != client_handles_.end()) {
-      auto client = get_client_by_handle(*it, weak_nodes);
+      auto client = memory_strategy::MemoryStrategy::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);
@@ -450,22 +334,18 @@ public:
     }
   }
 
-
-
 private:
   template<typename U>
-  using VectorRebind = typename std::allocator_traits<Alloc>::template rebind_alloc<U>;
+  using VectorRebind =
+      std::vector<U, typename std::allocator_traits<Alloc>::template rebind_alloc<U>>;
 
-  std::vector<rclcpp::subscription::SubscriptionBase::SharedPtr,
-  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr>> subscriptions_;
-  std::vector<rclcpp::service::ServiceBase::SharedPtr,
-  VectorRebind<rclcpp::service::ServiceBase::SharedPtr>> services_;
-  std::vector<rclcpp::client::ClientBase::SharedPtr,
-  VectorRebind<rclcpp::client::ClientBase::SharedPtr>> clients_;
+  VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
+  VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
+  VectorRebind<rclcpp::client::ClientBase::SharedPtr> clients_;
 
-  std::vector<void *, VoidAlloc> subscriber_handles_;
-  std::vector<void *, VoidAlloc> service_handles_;
-  std::vector<void *, VoidAlloc> client_handles_;
+  VectorRebind<void *> subscriber_handles_;
+  VectorRebind<void *> service_handles_;
+  VectorRebind<void *> client_handles_;
 
   std::shared_ptr<ExecAlloc> executable_allocator_;
   std::shared_ptr<VoidAlloc> allocator_;
diff --git a/rclcpp/include/rclcpp/strategies/heap_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/heap_pool_memory_strategy.hpp
deleted file mode 100644
index 8e7151c..0000000
--- a/rclcpp/include/rclcpp/strategies/heap_pool_memory_strategy.hpp
+++ /dev/null
@@ -1,336 +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_HEAP_POOL_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_HEAP_POOL_MEMORY_STRATEGY_HPP_
-
-#include <unordered_map>
-
-#include <rclcpp/memory_strategy.hpp>
-
-namespace rclcpp
-{
-
-namespace memory_strategies
-{
-
-namespace heap_pool_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;
-  }
-};
-
-
-/// Heap-based memory pool allocation strategy, an alternative to the default memory strategy.
-/**
- * The memory managed by this class is allocated dynamically in the constructor, but no subsequent
- * accesses to the class (besides the destructor) allocate or free memory.
- * HeapPoolMemoryStrategy 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 HeapPoolMemoryStrategy : public memory_strategy::MemoryStrategy
-{
-public:
-  HeapPoolMemoryStrategy(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;
-    }
-
-    subs.reserve(bounds_.max_subscriptions);
-    clients.reserve(bounds_.max_clients);
-    services.reserve(bounds_.max_services);
-  }
-
-  /// Default destructor. Free all allocated memory.
-  ~HeapPoolMemoryStrategy()
-  {
-    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_;
-    }
-  }
-
-  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.");
-  }
-
-  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.");
-    }
-  }
-
-  executor::AnyExecutable::SharedPtr instantiate_next_executable()
-  {
-    if (exec_seq_ >= bounds_.max_executables) {
-      // wrap around
-      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");
-    }
-
-    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_];
-  }
-
-  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 HeapPoolMemoryStrategy::alloc.");
-    }
-    memory_map_[ptr] = size;
-    size_t prev_pool_seq = pool_seq_;
-    pool_seq_ += size;
-    return memory_pool_[prev_pool_seq];
-  }
-
-  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 HeapPoolMemoryStrategy::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_;
-};
-
-}  /* heap_pool_memory_strategy */
-
-}  /* memory_strategies */
-
-}  /* rclcpp */
-
-#endif
diff --git a/rclcpp/include/rclcpp/strategies/stack_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/stack_pool_memory_strategy.hpp
deleted file mode 100644
index a7fd0a9..0000000
--- a/rclcpp/include/rclcpp/strategies/stack_pool_memory_strategy.hpp
+++ /dev/null
@@ -1,219 +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_STACK_POOL_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_STACK_POOL_MEMORY_STRATEGY_HPP_
-
-#include <unordered_map>
-
-#include <rclcpp/memory_strategy.hpp>
-
-namespace rclcpp
-{
-
-namespace memory_strategies
-{
-
-namespace stack_pool_memory_strategy
-{
-
-/// Stack-based memory pool allocation strategy, an alternative to the default memory strategy.
-/**
- * The memory managed by this class is allocated statically (on the heap) in the constructor.
- * StackPoolMemoryStrategy 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.
- * Because this class is templated on the sizes of the memory pools for each entity, the amount of
- * memory required by this class is known at compile time.
- */
-template<size_t MaxSubscriptions = 10, size_t MaxServices = 10, size_t MaxClients = 10,
-size_t MaxExecutables = 1, size_t MaxGuardConditions = 2, size_t PoolSize = 0>
-class StackPoolMemoryStrategy : public memory_strategy::MemoryStrategy
-{
-public:
-  StackPoolMemoryStrategy()
-  {
-    if (PoolSize) {
-      memory_pool_.fill(0);
-    }
-
-    if (MaxSubscriptions) {
-      subscription_pool_.fill(0);
-    }
-
-    if (MaxServices) {
-      service_pool_.fill(0);
-    }
-
-    if (MaxClients) {
-      client_pool_.fill(0);
-    }
-
-    if (MaxGuardConditions) {
-      guard_condition_pool_.fill(0);
-    }
-
-    for (size_t i = 0; i < MaxExecutables; ++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(PoolSize);
-    for (size_t i = 0; i < PoolSize; ++i) {
-      memory_map_[memory_pool_[i]] = 0;
-    }
-    subs.reserve(MaxSubscriptions);
-    clients.reserve(MaxClients);
-    services.reserve(MaxServices);
-  }
-
-  void ** borrow_handles(HandleType type, size_t number_of_handles)
-  {
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (number_of_handles > MaxSubscriptions) {
-          throw std::runtime_error("Requested size exceeded maximum subscriptions.");
-        }
-
-        return subscription_pool_.data();
-      case HandleType::service_handle:
-        if (number_of_handles > MaxServices) {
-          throw std::runtime_error("Requested size exceeded maximum services.");
-        }
-
-        return service_pool_.data();
-      case HandleType::client_handle:
-        if (number_of_handles > MaxClients) {
-          throw std::runtime_error("Requested size exceeded maximum clients.");
-        }
-
-        return client_pool_.data();
-      case HandleType::guard_condition_handle:
-        if (number_of_handles > MaxGuardConditions) {
-          throw std::runtime_error("Requested size exceeded maximum guard_conditions.");
-        }
-
-        return guard_condition_pool_.data();
-      default:
-        break;
-    }
-    throw std::runtime_error("Unrecognized enum, could not borrow handle memory.");
-  }
-
-  void return_handles(HandleType type, void ** handles)
-  {
-    (void)handles;
-    switch (type) {
-      case HandleType::subscription_handle:
-        if (MaxSubscriptions) {
-          subscription_pool_.fill(0);
-        }
-        break;
-      case HandleType::service_handle:
-        if (MaxServices) {
-          service_pool_.fill(0);
-        }
-        break;
-      case HandleType::client_handle:
-        if (MaxClients) {
-          client_pool_.fill(0);
-        }
-        break;
-      case HandleType::guard_condition_handle:
-        if (MaxGuardConditions) {
-          guard_condition_pool_.fill(0);
-        }
-        break;
-      default:
-        throw std::runtime_error("Unrecognized enum, could not return handle memory.");
-    }
-  }
-
-  executor::AnyExecutable::SharedPtr instantiate_next_executable()
-  {
-    if (exec_seq_ >= MaxExecutables) {
-      // wrap around
-      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_];
-  }
-
-  void * alloc(size_t size)
-  {
-    // Extremely naive static allocation strategy
-    // Keep track of block size at a given pointer
-    if (pool_seq_ + size > PoolSize) {
-      // 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 StackPoolMemoryStrategy::alloc.");
-    }
-    memory_map_[ptr] = size;
-    size_t prev_pool_seq = pool_seq_;
-    pool_seq_ += size;
-    return memory_pool_[prev_pool_seq];
-  }
-
-  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 StackPoolMemoryStrategy::free.");
-    }
-
-    memset(ptr, 0, memory_map_[ptr]);
-  }
-
-private:
-  std::array<void *, PoolSize> memory_pool_;
-  std::array<void *, MaxSubscriptions> subscription_pool_;
-  std::array<void *, MaxServices> service_pool_;
-  std::array<void *, MaxClients> client_pool_;
-  std::array<void *, MaxGuardConditions> guard_condition_pool_;
-  std::array<executor::AnyExecutable::SharedPtr, MaxExecutables> executable_pool_;
-
-  size_t pool_seq_;
-  size_t exec_seq_;
-
-  std::unordered_map<void *, size_t> memory_map_;
-};
-
-}  /* stack_pool_memory_strategy */
-
-}  /* memory_strategies */
-
-}  /* rclcpp */
-
-#endif
diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp
index 24d3f9b..8291668 100644
--- a/rclcpp/test/test_intra_process_manager.cpp
+++ b/rclcpp/test/test_intra_process_manager.cpp
@@ -67,11 +67,13 @@ public:
 
   RCLCPP_SMART_PTR_DEFINITIONS(Publisher<T, Alloc>);
 
-  Publisher() {
+  Publisher()
+  {
     allocator_ = std::make_shared<MessageAlloc>();
   }
 
-  std::shared_ptr<MessageAlloc> get_allocator() {
+  std::shared_ptr<MessageAlloc> get_allocator()
+  {
     return allocator_;
   }
 };

From 24fb204192068069543e36b2e1563dd881257f4b Mon Sep 17 00:00:00 2001
From: Jackie Kay <jackie@osrfoundation.org>
Date: Tue, 27 Oct 2015 19:08:21 -0700
Subject: [PATCH 11/12] Cpplint and cleanup

---
 .../rclcpp/allocator/allocator_common.hpp     |  16 +-
 .../rclcpp/allocator/allocator_deleter.hpp    |  33 ++--
 .../rclcpp/any_subscription_callback.hpp      |  18 +-
 rclcpp/include/rclcpp/executor.hpp            |  35 ++--
 .../executors/multi_threaded_executor.hpp     |  25 ++-
 .../executors/single_threaded_executor.hpp    |  29 ++-
 .../include/rclcpp/intra_process_manager.hpp  |  61 +++---
 .../rclcpp/intra_process_manager_state.hpp    |  53 +++---
 rclcpp/include/rclcpp/mapped_ring_buffer.hpp  |  19 +-
 rclcpp/include/rclcpp/memory_strategies.hpp   |  16 +-
 rclcpp/include/rclcpp/memory_strategy.hpp     |  83 ++++++--
 .../rclcpp/message_memory_strategy.hpp        |  17 +-
 rclcpp/include/rclcpp/node.hpp                |  46 ++---
 rclcpp/include/rclcpp/node_impl.hpp           |  44 +++--
 rclcpp/include/rclcpp/publisher.hpp           |  25 ++-
 .../strategies/allocator_memory_strategy.hpp  | 179 +++++++-----------
 .../message_pool_memory_strategy.hpp          |  13 +-
 rclcpp/include/rclcpp/subscription.hpp        |  27 ++-
 rclcpp/test/test_intra_process_manager.cpp    |   5 +-
 19 files changed, 368 insertions(+), 376 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index 5d493b3..d32b200 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -12,22 +12,22 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
-#define RCLCPP_RCLCPP__ALLOCATOR_COMMON__HPP_
+#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
+#define RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
 
 #include <memory>
 
-#include <rclcpp/allocator/allocator_deleter.hpp>
+#include "rclcpp/allocator/allocator_deleter.hpp"
 
 namespace rclcpp
 {
 namespace allocator
 {
 
-template<typename U, typename Alloc>
-using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<U>;
+template<typename T, typename Alloc>
+using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
 
-}
-}
+}  // namespace allocator
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
diff --git a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
index 8e948b6..c3b59f2 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ALLOCATOR_DELETER_HPP_
-#define RCLCPP_RCLCPP_ALLOCATOR_DELETER_HPP_
+#ifndef RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
+#define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
 
 #include <memory>
 
@@ -26,33 +26,32 @@ namespace allocator
 template<typename Allocator>
 class AllocatorDeleter
 {
-  template<typename U>
-  using AllocRebind = typename std::allocator_traits<Allocator>::template rebind_alloc<U>;
+  template<typename T>
+  using AllocRebind = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
 
 public:
   AllocatorDeleter()
-  : allocator_(NULL)
+  : allocator_(nullptr)
   {
   }
 
-  AllocatorDeleter(Allocator * a)
+  explicit AllocatorDeleter(Allocator * a)
   : allocator_(a)
   {
   }
 
-  template<typename U>
-  AllocatorDeleter(const AllocatorDeleter<U> & a)
+  template<typename T>
+  AllocatorDeleter(const AllocatorDeleter<T> & a)
   {
     allocator_ = a.get_allocator();
   }
 
-  template<typename U>
-  void operator()(U * ptr)
+  template<typename T>
+  void operator()(T * ptr)
   {
-    std::allocator_traits<AllocRebind<U>>::destroy(*allocator_, ptr);
-    // TODO: figure out if we're guaranteed to be destroying only 1 item here
-    std::allocator_traits<AllocRebind<U>>::deallocate(*allocator_, ptr, 1);
-    ptr = NULL;
+    std::allocator_traits<AllocRebind<T>>::destroy(*allocator_, ptr);
+    std::allocator_traits<AllocRebind<T>>::deallocate(*allocator_, ptr, 1);
+    ptr = nullptr;
   }
 
   Allocator * get_allocator() const
@@ -100,7 +99,7 @@ using Deleter = typename std::conditional<
     std::default_delete<T>,
     AllocatorDeleter<Alloc>
     >::type;
-}
-}
+}  // namespace allocator
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_
diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp
index bdce88b..94e26a1 100644
--- a/rclcpp/include/rclcpp/any_subscription_callback.hpp
+++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp
@@ -12,17 +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/allocator/allocator_common.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
 {
@@ -56,7 +56,7 @@ class AnySubscriptionCallback
   UniquePtrWithInfoCallback unique_ptr_with_info_callback_;
 
 public:
-  AnySubscriptionCallback(std::shared_ptr<Alloc> allocator)
+  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)
@@ -206,7 +206,7 @@ private:
   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_
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index b567aef..cc3fe42 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/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,14 +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/memory_strategies.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
 {
@@ -49,7 +49,6 @@ namespace executor
  */
 class Executor
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor);
 
@@ -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.");
       }
     }
@@ -358,7 +357,6 @@ protected:
           }));
     }
 
-
     // Use the number of subscriptions to allocate memory in the handles
     rmw_subscriptions_t subscriber_handles;
     subscriber_handles.subscriber_count =
@@ -431,9 +429,8 @@ protected:
       memory_strategy_->clear_handles();
       return;
     }
-    // Add the new work to the class's list of things waiting to be executed
-    // Starting with the subscribers
-    memory_strategy_->clear_active_entities();
+
+    memory_strategy_->revalidate_handles();
   }
 
 /******************************/
@@ -565,7 +562,7 @@ protected:
       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>
@@ -613,7 +610,7 @@ private:
   std::array<void *, 2> guard_cond_handles_;
 };
 
-} /* executor */
-} /* rclcpp */
+}  // namespace executor
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */
+#endif  // RCLCPP__EXECUTOR_HPP_
diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
index 7cfea87..82b1f84 100644
--- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp
+++ b/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
 {
@@ -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_
diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
index c082296..be87e19 100644
--- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp
+++ b/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
 {
@@ -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_
diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp
index 4790ae1..d49d91c 100644
--- a/rclcpp/include/rclcpp/intra_process_manager.hpp
+++ b/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/intra_process_manager_state.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 <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,8 +124,9 @@ private:
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager);
 
-  //IntraProcessManager() = default;
-  IntraProcessManager(IntraProcessManagerStateBase::SharedPtr state = create_default_state())
+  explicit IntraProcessManager(
+    IntraProcessManagerStateBase::SharedPtr state = create_default_state()
+  )
   : state_(state)
   {
   }
@@ -236,19 +239,18 @@ public:
    * \param message the message that is being stored.
    * \return the message sequence number.
    */
-  template<typename MessageT, typename Alloc = std::allocator<void>>
+  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,
-    typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
-    MessageT>> & message)
+    std::unique_ptr<MessageT, Deleter> & message)
   {
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
-    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
-
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
-    auto message_seq = state_->get_publisher_info_for_id(intra_process_publisher_id, buffer);
+    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");
@@ -299,25 +301,25 @@ 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, typename Alloc = std::allocator<void>>
+  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,
-    typename allocator::Deleter<typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>,
-    MessageT>> & message)
+    std::unique_ptr<MessageT, Deleter> & message)
   {
     using MRBMessageAlloc = typename std::allocator_traits<Alloc>::template rebind_alloc<MessageT>;
-    typedef typename mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc> TypedMRB;
+    using TypedMRB = mapped_ring_buffer::MappedRingBuffer<MessageT, MRBMessageAlloc>;
+    message = nullptr;
 
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer;
-    size_t target_subs_size = state_->take_intra_process_message(
+    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,
-      buffer
+      target_subs_size
       );
     typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast<TypedMRB>(buffer);
     if (!typed_buffer) {
@@ -365,12 +367,11 @@ private:
   static std::atomic<uint64_t> next_unique_id_;
 
   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_
diff --git a/rclcpp/include/rclcpp/intra_process_manager_state.hpp b/rclcpp/include/rclcpp/intra_process_manager_state.hpp
index cc59e6f..8a4bc5e 100644
--- a/rclcpp/include/rclcpp/intra_process_manager_state.hpp
+++ b/rclcpp/include/rclcpp/intra_process_manager_state.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP__RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
-#define RCLCPP__RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
+#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
+#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
 
 #include <rclcpp/mapped_ring_buffer.hpp>
 #include <rclcpp/publisher.hpp>
@@ -21,11 +21,14 @@
 
 #include <algorithm>
 #include <atomic>
+#include <functional>
 #include <limits>
 #include <memory>
 #include <map>
 #include <unordered_map>
 #include <set>
+#include <string>
+#include <utility>
 
 namespace rclcpp
 {
@@ -35,8 +38,7 @@ namespace intra_process_manager
 class IntraProcessManagerStateBase
 {
 public:
-  //RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManagerStateBase);
-  using SharedPtr = std::shared_ptr<IntraProcessManagerStateBase>;
+  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase);
 
   virtual void
   add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0;
@@ -52,19 +54,19 @@ public:
   virtual void
   remove_publisher(uint64_t intra_process_publisher_id) = 0;
 
-  virtual uint64_t
+  virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr
   get_publisher_info_for_id(
     uint64_t intra_process_publisher_id,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb) = 0;
+    uint64_t & message_seq) = 0;
 
   virtual void
   store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0;
 
-  virtual size_t
+  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,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb) = 0;
+    size_t & size) = 0;
 
   virtual bool
   matches_any_publishers(const rmw_gid_t * id) const = 0;
@@ -95,7 +97,6 @@ public:
         sub_pair.second.erase(intra_process_subscription_id);
       }
     }
-
   }
 
   void add_publisher(uint64_t id,
@@ -103,7 +104,6 @@ public:
     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()) {
@@ -121,23 +121,21 @@ public:
     publishers_.erase(intra_process_publisher_id);
   }
 
-  // TODO
   // return message_seq and mrb
-  uint64_t
+  mapped_ring_buffer::MappedRingBufferBase::SharedPtr
   get_publisher_info_for_id(
     uint64_t intra_process_publisher_id,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb)
+    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;
-    mrb = info.buffer;
     // Calculate the next message sequence number.
-    uint64_t message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
+    message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed);
 
-    return message_seq;
+    return info.buffer;
   }
 
   void
@@ -166,17 +164,15 @@ public:
         info.target_subscriptions_by_message_sequence[message_seq].end()
       )
     );
-
   }
 
-  size_t
+  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,
-    mapped_ring_buffer::MappedRingBufferBase::SharedPtr & mrb
+    size_t & size
   )
   {
-    mrb = nullptr;
     PublisherInfo * info;
     {
       auto it = publishers_.find(intra_process_publisher_id);
@@ -206,8 +202,8 @@ public:
       }
       target_subs->erase(it);
     }
-    mrb = info->buffer;
-    return target_subs->size();
+    size = target_subs->size();
+    return info->buffer;
   }
 
   bool
@@ -229,14 +225,12 @@ private:
   template<typename T>
   using RebindAlloc = typename std::allocator_traits<Allocator>::template rebind_alloc<T>;
 
-  //using AllocString = std::basic_string<char, std::char_traits<char>, RebindAlloc<char>>;
-  using AllocString = std::string;
   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<AllocString, AllocSet,
-      std::less<AllocString>, RebindAlloc<std::pair<AllocString, AllocSet>>>;
+  using IDTopicMap = std::map<std::string, AllocSet,
+      std::less<std::string>, RebindAlloc<std::pair<std::string, AllocSet>>>;
 
   SubscriptionMap subscriptions_;
 
@@ -263,7 +257,6 @@ private:
       RebindAlloc<std::pair<const uint64_t, PublisherInfo>>>;
 
   PublisherMap publishers_;
-
 };
 
 static IntraProcessManagerStateBase::SharedPtr create_default_state()
@@ -271,7 +264,7 @@ static IntraProcessManagerStateBase::SharedPtr create_default_state()
   return std::make_shared<IntraProcessManagerState<>>();
 }
 
-}
-}
+}  // namespace intra_process_manager
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_
diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
index 9e6d611..8a9b7f9 100644
--- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
+++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp
@@ -12,11 +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/allocator/allocator_common.hpp>
-#include <rclcpp/macros.hpp>
+#ifndef RCLCPP__MAPPED_RING_BUFFER_HPP_
+#define RCLCPP__MAPPED_RING_BUFFER_HPP_
 
 #include <algorithm>
 #include <cstddef>
@@ -24,6 +21,9 @@
 #include <memory>
 #include <vector>
 
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/macros.hpp"
+
 namespace rclcpp
 {
 namespace mapped_ring_buffer
@@ -67,7 +67,7 @@ public:
    *
    * \param size size of the ring buffer; must be positive and non-zero.
    */
-  MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
+  explicit MappedRingBuffer(size_t size, std::shared_ptr<Alloc> allocator = nullptr)
   : elements_(size), head_(0)
   {
     if (size == 0) {
@@ -224,10 +224,9 @@ private:
   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_
diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp
index fee7c25..f5a1a2a 100644
--- a/rclcpp/include/rclcpp/memory_strategies.hpp
+++ b/rclcpp/include/rclcpp/memory_strategies.hpp
@@ -12,11 +12,9 @@
 // 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/heap_pool_memory_strategy.hpp>
-//#include <rclcpp/strategies/stack_pool_memory_strategy.hpp>
 #include <rclcpp/memory_strategy.hpp>
 #include <rclcpp/strategies/allocator_memory_strategy.hpp>
 
@@ -26,15 +24,13 @@ namespace memory_strategies
 {
 
 using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy;
-//using rclcpp::memory_strategies::heap_pool_memory_strategy::HeapPoolMemoryStrategy;
-//using rclcpp::memory_strategies::stack_pool_memory_strategy::StackPoolMemoryStrategy;
 
 static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy()
 {
-  return std::make_shared<memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>();
+  return std::make_shared<AllocatorMemoryStrategy<>>();
 }
 
-}  /* memory_strategies */
-}  /* rclcpp */
+}  // namespace memory_strategies
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__MEMORY_STRATEGIES_HPP_
diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp
index cdc27cf..895ea9d 100644
--- a/rclcpp/include/rclcpp/memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/memory_strategy.hpp
@@ -12,22 +12,21 @@
 // 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_
+#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>
+#include "rclcpp/any_executable.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/node.hpp"
 
 namespace rclcpp
 {
 namespace 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
@@ -52,6 +51,7 @@ public:
   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.
@@ -103,8 +103,7 @@ public:
   }
 
   static rclcpp::service::ServiceBase::SharedPtr
-  get_service_by_handle(void * service_handle,
-    const WeakNodeVector & weak_nodes)
+  get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes)
   {
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
@@ -123,12 +122,11 @@ public:
         }
       }
     }
-    return rclcpp::service::ServiceBase::SharedPtr();
+    return nullptr;
   }
 
   static rclcpp::client::ClientBase::SharedPtr
-  get_client_by_handle(void * client_handle,
-    const WeakNodeVector & weak_nodes)
+  get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes)
   {
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
@@ -147,7 +145,7 @@ public:
         }
       }
     }
-    return rclcpp::client::ClientBase::SharedPtr();
+    return nullptr;
   }
 
   static rclcpp::node::Node::SharedPtr
@@ -155,7 +153,7 @@ public:
     const WeakNodeVector & weak_nodes)
   {
     if (!group) {
-      return rclcpp::node::Node::SharedPtr();
+      return nullptr;
     }
     for (auto & weak_node : weak_nodes) {
       auto node = weak_node.lock();
@@ -169,7 +167,7 @@ public:
         }
       }
     }
-    return rclcpp::node::Node::SharedPtr();
+    return nullptr;
   }
 
   static rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -195,14 +193,63 @@ public:
         }
       }
     }
-    return rclcpp::callback_group::CallbackGroup::SharedPtr();
+    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;
   }
 
 
 };
 
-}  /* memory_strategy */
+}  // namespace memory_strategy
 
-}  /* rclcpp */
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__MEMORY_STRATEGY_HPP_
diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index 9860303..61b582d 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -12,13 +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/allocator/allocator_common.hpp>
-#include <rclcpp/macros.hpp>
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/macros.hpp"
 
 namespace rclcpp
 {
@@ -30,7 +30,6 @@ namespace message_memory_strategy
 template<typename MessageT, typename Alloc = std::allocator<void>>
 class MessageMemoryStrategy
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(MessageMemoryStrategy);
 
@@ -43,7 +42,7 @@ public:
     message_allocator_ = std::make_shared<MessageAlloc>();
   }
 
-  MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
+  explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
   }
@@ -72,7 +71,7 @@ public:
   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_
diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index e7d3e28..0e78b10 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -12,48 +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/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
 {
 
-
 namespace node
 {
 /// Node is the single point of entry for creating publishers and subscribers.
 class Node
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Node);
 
@@ -63,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.
   /**
@@ -278,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_
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index e82a383..b55d87f 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/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(
@@ -189,7 +193,8 @@ Node::create_publisher(
       MessageT * typed_message_ptr = static_cast<MessageT *>(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);
+      uint64_t message_seq =
+        ipm->store_intra_process_message<MessageT, Alloc>(publisher_id, unique_msg);
       return message_seq;
     };
     // *INDENT-ON*
@@ -294,7 +299,8 @@ Node::create_subscription(
           throw std::runtime_error(
             "intra process take called after destruction of intra process manager");
         }
-        ipm->take_intra_process_message<MessageT, Alloc>(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();
@@ -310,7 +316,7 @@ Node::create_subscription(
   // 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);
@@ -354,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);
@@ -449,7 +455,7 @@ Node::create_service(
   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);
@@ -497,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;
 
@@ -667,4 +673,4 @@ Node::get_callback_groups() const
   return callback_groups_;
 }
 
-#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */
+#endif  // RCLCPP__NODE_IMPL_HPP_
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index f08a4a8..d0acec5 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/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,12 +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/allocator/allocator_deleter.hpp"
+#include "rclcpp/macros.hpp"
 
 namespace rclcpp
 {
@@ -37,7 +37,7 @@ namespace rclcpp
 namespace node
 {
 class Node;
-} // namespace node
+}  // namespace node
 
 namespace publisher
 {
@@ -346,10 +346,9 @@ protected:
   std::shared_ptr<MessageAlloc> message_allocator_;
 
   MessageDeleter message_deleter_;
-
 };
 
-} /* namespace publisher */
-} /* namespace rclcpp */
+}  // namespace publisher
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_PUBLISHER_HPP_ */
+#endif  // RCLCPP__PUBLISHER_HPP_
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index 1117ca6..61d612d 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
-#define RCLCPP_RCLCPP_ALLOCATOR_MEMORY_STRATEGY_HPP_
+#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>
+#include "rclcpp/allocator/allocator_common.hpp"
+#include "rclcpp/memory_strategy.hpp"
+#include "rclcpp/node.hpp"
 
 namespace rclcpp
 {
@@ -40,7 +40,6 @@ namespace allocator_memory_strategy
 template<typename Alloc = std::allocator<void>>
 class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(AllocatorMemoryStrategy<Alloc>);
 
@@ -52,7 +51,7 @@ public:
 
   using WeakNodeVector = std::vector<std::weak_ptr<node::Node>>;
 
-  AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
+  explicit AllocatorMemoryStrategy(std::shared_ptr<Alloc> allocator)
   {
     executable_allocator_ = std::make_shared<ExecAlloc>(*allocator.get());
     allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
@@ -66,48 +65,34 @@ public:
 
   size_t fill_subscriber_handles(void ** & ptr)
   {
-    size_t max_size = subscriptions_.size();
-    subscriber_handles_.resize(max_size);
-    size_t count = 0;
     for (auto & subscription : subscriptions_) {
-      subscriber_handles_[count++] = subscription->get_subscription_handle()->data;
+      subscriber_handles_.push_back(subscription->get_subscription_handle()->data);
       if (subscription->get_intra_process_subscription_handle()) {
-        subscriber_handles_[count++] =
-          subscription->get_intra_process_subscription_handle()->data;
+        subscriber_handles_.push_back(subscription->get_intra_process_subscription_handle()->data);
       }
     }
     ptr = subscriber_handles_.data();
-    return count;
+    return subscriber_handles_.size();
   }
 
   // return the new number of services
   size_t fill_service_handles(void ** & ptr)
   {
-    size_t max_size = services_.size();
-    if (service_handles_.size() < max_size) {
-      service_handles_.resize(max_size);
-    }
-    size_t count = 0;
     for (auto & service : services_) {
-      service_handles_[count++] = service->get_service_handle()->data;
+      service_handles_.push_back(service->get_service_handle()->data);
     }
     ptr = service_handles_.data();
-    return count;
+    return service_handles_.size();
   }
 
   // return the new number of clients
   size_t fill_client_handles(void ** & ptr)
   {
-    size_t max_size = clients_.size();
-    if (client_handles_.size() < max_size) {
-      client_handles_.resize(max_size);
-    }
-    size_t count = 0;
     for (auto & client : clients_) {
-      client_handles_[count++] = client->get_client_handle()->data;
+      client_handles_.push_back(client->get_client_handle()->data);
     }
     ptr = client_handles_.data();
-    return count;
+    return client_handles_.size();
   }
 
   void clear_active_entities()
@@ -124,6 +109,37 @@ public:
     client_handles_.clear();
   }
 
+  void revalidate_handles()
+  {
+    {
+      VectorRebind<void *> temp;
+      for (size_t i = 0; i < subscriptions_.size() * 2; i++) {
+        if (subscriber_handles_[i]) {
+          temp.push_back(subscriber_handles_[i]);
+        }
+      }
+      subscriber_handles_.swap(temp);
+    }
+    {
+      VectorRebind<void *> temp;
+      for (size_t i = 0; i < services_.size(); i++) {
+        if (service_handles_[i]) {
+          temp.push_back(service_handles_[i]);
+        }
+      }
+      service_handles_.swap(temp);
+    }
+    {
+      VectorRebind<void *> temp;
+      for (size_t i = 0; i < clients_.size(); i++) {
+        if (client_handles_[i]) {
+          temp.push_back(client_handles_[i]);
+        }
+      }
+      client_handles_.swap(temp);
+    }
+  }
+
   bool collect_entities(const WeakNodeVector & weak_nodes)
   {
     bool has_invalid_weak_nodes = false;
@@ -145,10 +161,14 @@ public:
           }
         }
         for (auto & service : group->get_service_ptrs()) {
-          services_.push_back(service);
+          if (service) {
+            services_.push_back(service);
+          }
         }
         for (auto & client : group->get_client_ptrs()) {
-          clients_.push_back(client);
+          if (client) {
+            clients_.push_back(client);
+          }
         }
       }
     }
@@ -168,10 +188,8 @@ public:
     const WeakNodeVector & weak_nodes)
   {
     auto it = subscriber_handles_.begin();
-    VectorRebind<typename VectorRebind<void *>::iterator> to_erase;
     while (it != subscriber_handles_.end()) {
-      auto subscription = memory_strategy::MemoryStrategy::get_subscription_by_handle(*it,
-          weak_nodes);
+      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;
@@ -183,8 +201,7 @@ public:
         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++);
-          to_erase.push_back(it++);
+          subscriber_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -201,44 +218,12 @@ public:
         }
         any_exec->callback_group = group;
         any_exec->node = get_node_by_group(group, weak_nodes);
-        to_erase.push_back(it++);
+        subscriber_handles_.erase(it);
         return;
       }
       // Else, the subscription is no longer valid, remove it and continue
-      // TODO
-      to_erase.push_back(it++);
+      subscriber_handles_.erase(it);
     }
-    for (auto & entry : to_erase) {
-      if (entry != subscriber_handles_.end()) {
-        subscriber_handles_.erase(entry);
-      }
-    }
-
-  }
-
-  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 rclcpp::callback_group::CallbackGroup::SharedPtr();
   }
 
   virtual void
@@ -247,14 +232,14 @@ public:
   {
     auto it = service_handles_.begin();
     while (it != service_handles_.end()) {
-      auto service = memory_strategy::MemoryStrategy::get_service_by_handle(*it, weak_nodes);
+      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++);
+          service_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -267,53 +252,27 @@ public:
         any_exec->service = service;
         any_exec->callback_group = group;
         any_exec->node = get_node_by_group(group, weak_nodes);
-        service_handles_.erase(it++);
+        service_handles_.erase(it);
         return;
       }
       // Else, the service is no longer valid, remove it and continue
-      service_handles_.erase(it++);
+      service_handles_.erase(it);
     }
   }
 
-  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 rclcpp::callback_group::CallbackGroup::SharedPtr();
-  }
-
   virtual void
-  get_next_client(executor::AnyExecutable::SharedPtr any_exec,
-    const WeakNodeVector & weak_nodes)
+  get_next_client(executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes)
   {
     auto it = client_handles_.begin();
     while (it != client_handles_.end()) {
-      auto client = memory_strategy::MemoryStrategy::get_client_by_handle(*it, weak_nodes);
+      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++);
+          client_handles_.erase(it);
           continue;
         }
         if (!group->can_be_taken_from().load()) {
@@ -326,18 +285,18 @@ public:
         any_exec->client = client;
         any_exec->callback_group = group;
         any_exec->node = get_node_by_group(group, weak_nodes);
-        client_handles_.erase(it++);
+        client_handles_.erase(it);
         return;
       }
       // Else, the service is no longer valid, remove it and continue
-      client_handles_.erase(it++);
+      client_handles_.erase(it);
     }
   }
 
 private:
-  template<typename U>
+  template<typename T>
   using VectorRebind =
-      std::vector<U, typename std::allocator_traits<Alloc>::template rebind_alloc<U>>;
+      std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
   VectorRebind<rclcpp::subscription::SubscriptionBase::SharedPtr> subscriptions_;
   VectorRebind<rclcpp::service::ServiceBase::SharedPtr> services_;
@@ -351,9 +310,9 @@ private:
   std::shared_ptr<VoidAlloc> allocator_;
 };
 
-}  /* allocator_memory_strategy */
-}  /* memory_strategies */
+}  // namespace allocator_memory_strategy
+}  // namespace memory_strategies
 
-}  /* rclcpp */
+}  // namespace rclcpp
 
-#endif
+#endif  // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp
index 43e518c..8f9438c 100644
--- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp
+++ b/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_
diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp
index fbdb100..fb9c952 100644
--- a/rclcpp/include/rclcpp/subscription.hpp
+++ b/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,13 +24,11 @@
 #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
 {
@@ -35,7 +36,7 @@ namespace rclcpp
 namespace node
 {
 class Node;
-} // namespace node
+}  // namespace node
 
 namespace subscription
 {
@@ -44,7 +45,6 @@ namespace subscription
 /// specializations of Subscription, among other things.
 class SubscriptionBase
 {
-
 public:
   RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase);
 
@@ -138,7 +138,6 @@ private:
 
   std::string topic_name_;
   bool ignore_local_publications_;
-
 };
 
 using namespace any_subscription_callback;
@@ -279,7 +278,7 @@ private:
   uint64_t intra_process_subscription_id_;
 };
 
-} /* namespace subscription */
-} /* namespace rclcpp */
+}  // namespace subscription
+}  // namespace rclcpp
 
-#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */
+#endif  // RCLCPP__SUBSCRIPTION_HPP_
diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp
index 8291668..6f47c83 100644
--- a/rclcpp/test/test_intra_process_manager.cpp
+++ b/rclcpp/test/test_intra_process_manager.cpp
@@ -15,6 +15,7 @@
 #include <memory>
 
 #include <gtest/gtest.h>
+
 #include <rclcpp/allocator/allocator_common.hpp>
 #include <rclcpp/macros.hpp>
 #include <rmw/types.h>
@@ -115,8 +116,8 @@ 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

From 142c9abbaab3d382d86e3e8e3887533f9b04469e Mon Sep 17 00:00:00 2001
From: William Woodall <wjwwood@gmail.com>
Date: Thu, 29 Oct 2015 15:35:32 -0700
Subject: [PATCH 12/12] Fix windows vector issue

---
 .../strategies/allocator_memory_strategy.hpp   | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index 61d612d..7622123 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -113,27 +113,27 @@ public:
   {
     {
       VectorRebind<void *> temp;
-      for (size_t i = 0; i < subscriptions_.size() * 2; i++) {
-        if (subscriber_handles_[i]) {
-          temp.push_back(subscriber_handles_[i]);
+      for (auto & subscriber_handle : subscriber_handles_) {
+        if (subscriber_handle) {
+          temp.push_back(subscriber_handle);
         }
       }
       subscriber_handles_.swap(temp);
     }
     {
       VectorRebind<void *> temp;
-      for (size_t i = 0; i < services_.size(); i++) {
-        if (service_handles_[i]) {
-          temp.push_back(service_handles_[i]);
+      for (auto & service_handle : service_handles_) {
+        if (service_handle) {
+          temp.push_back(service_handle);
         }
       }
       service_handles_.swap(temp);
     }
     {
       VectorRebind<void *> temp;
-      for (size_t i = 0; i < clients_.size(); i++) {
-        if (client_handles_[i]) {
-          temp.push_back(client_handles_[i]);
+      for (auto & client_handle : client_handles_) {
+        if (client_handle) {
+          temp.push_back(client_handle);
         }
       }
       client_handles_.swap(temp);