From 32e7fe83862d3f6177e475c051e762c51c50c453 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Tue, 22 Sep 2020 08:18:37 +0000
Subject: [PATCH 1/9] Make get_rcl_allocator a function family

As explained in #1254, there's conceptually no way to implement RCL
allocators in terms of C++ allocators. In order to fix this behavior,
we have to delete the generic version of get_rcl_allocator. Since some
ROS code depends on this, we need to allow users to write their own
version of get_rcl_allocator for allocators that support the C-style
interface (most do).

So this CL changes get_rcl_allocator from a template function
into a family of (potentially templated) functions, which allows
users to add their own overloads and rely on the "most specialized"
mechanism for function specialization to select the right one. See
http://www.gotw.ca/publications/mill17.htm for details. This also
allows us to return get_rcl_default_allocator for all specializations
of std::allocator (previously, only for std::allocator), which will
already fix #1254 for pretty much all clients. I'll continue to work
on deleting the generic version, though, to make sure that nobody is
accidentally bitten by it.

I've tried to test this by doing a full ROS compilation following the
Dockerfile of the source Docker image, and all packages compile.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 .../rclcpp/allocator/allocator_common.hpp     | 31 +++++++------------
 .../rclcpp/message_memory_strategy.hpp        |  4 +--
 rclcpp/include/rclcpp/publisher_options.hpp   |  4 ++-
 .../strategies/allocator_memory_strategy.hpp  |  2 +-
 .../include/rclcpp/subscription_options.hpp   |  4 ++-
 5 files changed, 21 insertions(+), 24 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index d117376517..ff93e57c65 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -39,42 +39,40 @@ void * retyped_allocate(size_t size, void * untyped_allocator)
   return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }
 
-template<typename T, typename Alloc>
+template<typename Alloc>
 void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
 {
   auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
   if (!typed_allocator) {
     throw std::runtime_error("Received incorrect allocator type");
   }
-  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  auto typed_ptr = static_cast<typename Alloc::value_type *>(untyped_pointer);
   std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
 }
 
-template<typename T, typename Alloc>
+template<typename Alloc>
 void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator)
 {
   auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
   if (!typed_allocator) {
     throw std::runtime_error("Received incorrect allocator type");
   }
-  auto typed_ptr = static_cast<T *>(untyped_pointer);
+  auto typed_ptr = static_cast<typename Alloc::value_type *>(untyped_pointer);
   std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
   return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
 }
 
+}  // namespace allocator
 
 // Convert a std::allocator_traits-formatted Allocator into an rcl allocator
-template<
-  typename T,
-  typename Alloc,
-  typename std::enable_if<!std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
+template<typename Alloc>
 rcl_allocator_t get_rcl_allocator(Alloc & allocator)
 {
   rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
 #ifndef _WIN32
-  rcl_allocator.allocate = &retyped_allocate<Alloc>;
-  rcl_allocator.deallocate = &retyped_deallocate<T, Alloc>;
-  rcl_allocator.reallocate = &retyped_reallocate<T, Alloc>;
+  rcl_allocator.allocate = &allocator::retyped_allocate<Alloc>;
+  rcl_allocator.deallocate = &allocator::retyped_deallocate<Alloc>;
+  rcl_allocator.reallocate = &allocator::retyped_reallocate<Alloc>;
   rcl_allocator.state = &allocator;
 #else
   (void)allocator;  // Remove warning
@@ -82,18 +80,13 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   return rcl_allocator;
 }
 
-// TODO(jacquelinekay) Workaround for an incomplete implementation of std::allocator<void>
-template<
-  typename T,
-  typename Alloc,
-  typename std::enable_if<std::is_same<Alloc, std::allocator<void>>::value>::type * = nullptr>
-rcl_allocator_t get_rcl_allocator(Alloc & allocator)
+template <typename T>
+rcl_allocator_t get_rcl_allocator(std::allocator<T>& allocator)
 {
-  (void)allocator;
+  (void)allocator;  // Remove warning
   return rcl_get_default_allocator();
 }
 
-}  // namespace allocator
 }  // namespace rclcpp
 
 #endif  // RCLCPP__ALLOCATOR__ALLOCATOR_COMMON_HPP_
diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp
index f548d953c2..21adb0d2a7 100644
--- a/rclcpp/include/rclcpp/message_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp
@@ -61,7 +61,7 @@ class MessageMemoryStrategy
     message_allocator_ = std::make_shared<MessageAlloc>();
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>();
     buffer_allocator_ = std::make_shared<BufferAlloc>();
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    rcutils_allocator_ = get_rcl_allocator(*buffer_allocator_.get());
   }
 
   explicit MessageMemoryStrategy(std::shared_ptr<Alloc> allocator)
@@ -69,7 +69,7 @@ class MessageMemoryStrategy
     message_allocator_ = std::make_shared<MessageAlloc>(*allocator.get());
     serialized_message_allocator_ = std::make_shared<SerializedMessageAlloc>(*allocator.get());
     buffer_allocator_ = std::make_shared<BufferAlloc>(*allocator.get());
-    rcutils_allocator_ = allocator::get_rcl_allocator<char, BufferAlloc>(*buffer_allocator_.get());
+    rcutils_allocator_ = get_rcl_allocator(*buffer_allocator_.get());
   }
 
   virtual ~MessageMemoryStrategy() = default;
diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index 9547b349dd..e7b841b6e4 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -75,7 +75,9 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
-    result.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    // TODO: This is likely to invoke undefined behavior - message_alloc goes out of scope
+    // at the end of this function, but the allocator doesn't.
+    result.allocator = get_rcl_allocator(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
 
     // Apply payload to rcl_publisher_options if necessary.
diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
index 76790b1245..a4e5c8700c 100644
--- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
+++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp
@@ -437,7 +437,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy
 
   rcl_allocator_t get_allocator() override
   {
-    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    return get_rcl_allocator(*allocator_.get());
   }
 
   size_t number_of_ready_subscriptions() const override
diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp
index ebf4331c4f..d9d34d7ad8 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -102,7 +102,9 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
-    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    // TODO: This is likely to invoke undefined behavior - message_alloc goes out of scope
+    // at the end of this function, but the allocator doesn't.
+    result.allocator = get_rcl_allocator(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
 

From b98e90fc5d6ae4f3afe0b391b28580fa6efbc9bc Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Wed, 23 Sep 2020 19:39:24 +0000
Subject: [PATCH 2/9] Addressed code style test failures.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 rclcpp/include/rclcpp/allocator/allocator_common.hpp | 4 ++--
 rclcpp/include/rclcpp/publisher_options.hpp          | 4 ++--
 rclcpp/include/rclcpp/subscription_options.hpp       | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index ff93e57c65..6300913555 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -80,8 +80,8 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   return rcl_allocator;
 }
 
-template <typename T>
-rcl_allocator_t get_rcl_allocator(std::allocator<T>& allocator)
+template<typename T>
+rcl_allocator_t get_rcl_allocator(std::allocator<T> & allocator)
 {
   (void)allocator;  // Remove warning
   return rcl_get_default_allocator();
diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index e7b841b6e4..910d9410ba 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -75,8 +75,8 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
-    // TODO: This is likely to invoke undefined behavior - message_alloc goes out of scope
-    // at the end of this function, but the allocator doesn't.
+    // TODO(stevewolter): This is likely to invoke undefined behavior - message_alloc goes
+    // out of scope at the end of this function, but the allocator doesn't.
     result.allocator = get_rcl_allocator(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
 
diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp
index d9d34d7ad8..510b1f612e 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -102,8 +102,8 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     using AllocatorTraits = std::allocator_traits<Allocator>;
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
-    // TODO: This is likely to invoke undefined behavior - message_alloc goes out of scope
-    // at the end of this function, but the allocator doesn't.
+    // TODO(stevewolter): This is likely to invoke undefined behavior - message_alloc
+    // goes out of scope at the end of this function, but the allocator doesn't.
     result.allocator = get_rcl_allocator(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;

From e63917fb17dca6be8029f68abb7b3be837c64134 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Thu, 24 Sep 2020 21:44:49 +0000
Subject: [PATCH 3/9] Update comments.

Incidentally, this also reruns the flaky test suite, which
seems to be using the real DDS middleware and to be prone
to cross-talk.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 rclcpp/include/rclcpp/allocator/allocator_common.hpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index 6300913555..38dd96249a 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -64,7 +64,9 @@ void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_al
 
 }  // namespace allocator
 
-// Convert a std::allocator_traits-formatted Allocator into an rcl allocator
+// Deprecated: Generic converter from C++ allocator into RCL allocator.
+// This allocator overallocates memory by 100x and invokes UB on free,
+// see #1254.
 template<typename Alloc>
 rcl_allocator_t get_rcl_allocator(Alloc & allocator)
 {
@@ -80,6 +82,9 @@ rcl_allocator_t get_rcl_allocator(Alloc & allocator)
   return rcl_allocator;
 }
 
+// Builds the RCL default allocator for the C++ standard allocator.
+// This assumes that the user intent behind both allocators is the
+// same: Using system malloc for allocation.
 template<typename T>
 rcl_allocator_t get_rcl_allocator(std::allocator<T> & allocator)
 {

From 928bf4f6a4886936743f253965c30829d53efc50 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Thu, 24 Sep 2020 22:00:34 +0000
Subject: [PATCH 4/9] Also update recently added tests.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 .../test/rclcpp/allocator/test_allocator_common.cpp   | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
index d3270e8e12..2a656c114e 100644
--- a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
+++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
@@ -26,7 +26,7 @@ TEST(TestAllocatorCommon, retyped_allocate) {
   ASSERT_NE(nullptr, allocated_mem);
 
   auto code = [&untyped_allocator, allocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+      rclcpp::allocator::retyped_deallocate<std::allocator<int>>(
         allocated_mem, untyped_allocator);
     };
   EXPECT_NO_THROW(code());
@@ -34,12 +34,12 @@ TEST(TestAllocatorCommon, retyped_allocate) {
   allocated_mem = allocator.allocate(1);
   ASSERT_NE(nullptr, allocated_mem);
   void * reallocated_mem =
-    rclcpp::allocator::retyped_reallocate<int, std::allocator<int>>(
+    rclcpp::allocator::retyped_reallocate<std::allocator<int>>(
     allocated_mem, 2u, untyped_allocator);
   ASSERT_NE(nullptr, reallocated_mem);
 
   auto code2 = [&untyped_allocator, reallocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<int, std::allocator<int>>(
+      rclcpp::allocator::retyped_deallocate<std::allocator<int>>(
         reallocated_mem, untyped_allocator);
     };
   EXPECT_NO_THROW(code2());
@@ -47,7 +47,7 @@ TEST(TestAllocatorCommon, retyped_allocate) {
 
 TEST(TestAllocatorCommon, get_rcl_allocator) {
   std::allocator<int> allocator;
-  auto rcl_allocator = rclcpp::allocator::get_rcl_allocator<int>(allocator);
+  auto rcl_allocator = rclcpp::get_rcl_allocator(allocator);
   EXPECT_NE(nullptr, rcl_allocator.allocate);
   EXPECT_NE(nullptr, rcl_allocator.deallocate);
   EXPECT_NE(nullptr, rcl_allocator.reallocate);
@@ -57,8 +57,7 @@ TEST(TestAllocatorCommon, get_rcl_allocator) {
 
 TEST(TestAllocatorCommon, get_void_rcl_allocator) {
   std::allocator<void> allocator;
-  auto rcl_allocator =
-    rclcpp::allocator::get_rcl_allocator<void, std::allocator<void>>(allocator);
+  auto rcl_allocator = rclcpp::get_rcl_allocator(allocator);
   EXPECT_NE(nullptr, rcl_allocator.allocate);
   EXPECT_NE(nullptr, rcl_allocator.deallocate);
   EXPECT_NE(nullptr, rcl_allocator.reallocate);

From 3f97bc357d9fe19cd7fe94dc896acd14215be202 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Mon, 28 Sep 2020 18:32:14 +0000
Subject: [PATCH 5/9] Added reference to bug number.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 rclcpp/include/rclcpp/publisher_options.hpp    | 2 +-
 rclcpp/include/rclcpp/subscription_options.hpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index 910d9410ba..e873adf42a 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -76,7 +76,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
     // TODO(stevewolter): This is likely to invoke undefined behavior - message_alloc goes
-    // out of scope at the end of this function, but the allocator doesn't.
+    // out of scope at the end of this function, but the allocator doesn't. See #1339.
     result.allocator = get_rcl_allocator(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
 
diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp
index 510b1f612e..e21d6effdb 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -103,7 +103,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
     auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
     // TODO(stevewolter): This is likely to invoke undefined behavior - message_alloc
-    // goes out of scope at the end of this function, but the allocator doesn't.
+    // goes out of scope at the end of this function, but the allocator doesn't. See #1339.
     result.allocator = get_rcl_allocator(*message_alloc);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;

From fca62b2a76f9869c21178bb84c402ed90d33b464 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Tue, 29 Sep 2020 19:43:10 +0000
Subject: [PATCH 6/9] Extend allocator lifetime in options.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 rclcpp/include/rclcpp/publisher_options.hpp   | 38 +++++++++----------
 .../include/rclcpp/subscription_options.hpp   | 33 ++++++++--------
 2 files changed, 34 insertions(+), 37 deletions(-)

diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index e873adf42a..5be5846c4b 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -56,14 +56,16 @@ struct PublisherOptionsBase
 template<typename Allocator>
 struct PublisherOptionsWithAllocator : public PublisherOptionsBase
 {
-  /// Optional custom allocator.
-  std::shared_ptr<Allocator> allocator = nullptr;
-
-  PublisherOptionsWithAllocator<Allocator>() {}
+  PublisherOptionsWithAllocator<Allocator>()
+  : allocator_(new Allocator()),
+    message_allocator_(*allocator_)
+  {}
 
   /// Constructor using base class as input.
   explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)
-  : PublisherOptionsBase(publisher_options_base)
+  : PublisherOptionsBase(publisher_options_base),
+    allocator_(new Allocator()),
+    message_allocator_(*allocator_)
   {}
 
   /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t.
@@ -72,12 +74,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   to_rcl_publisher_options(const rclcpp::QoS & qos) const
   {
     rcl_publisher_options_t result = rcl_publisher_get_default_options();
-    using AllocatorTraits = std::allocator_traits<Allocator>;
-    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*this->get_allocator().get());
-    // TODO(stevewolter): This is likely to invoke undefined behavior - message_alloc goes
-    // out of scope at the end of this function, but the allocator doesn't. See #1339.
-    result.allocator = get_rcl_allocator(*message_alloc);
+    result.allocator = get_rcl_allocator(message_allocator_);
     result.qos = qos.get_rmw_qos_profile();
 
     // Apply payload to rcl_publisher_options if necessary.
@@ -89,20 +86,19 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   }
 
 
-  /// Get the allocator, creating one if needed.
+  /// Get the allocator
   std::shared_ptr<Allocator>
   get_allocator() const
   {
-    if (!this->allocator) {
-      // TODO(wjwwood): I would like to use the commented line instead, but
-      //   cppcheck 1.89 fails with:
-      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
-      // return std::make_shared<Allocator>();
-      std::shared_ptr<Allocator> tmp(new Allocator());
-      return tmp;
-    }
-    return this->allocator;
+    return allocator_;
   }
+
+ private:
+  using AllocatorTraits = std::allocator_traits<Allocator>;
+  using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
+
+  std::shared_ptr<Allocator> allocator_;
+  MessageAllocatorT message_allocator_;
 };
 
 using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;
diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp
index e21d6effdb..15b04dad8b 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -78,15 +78,17 @@ struct SubscriptionOptionsBase
 template<typename Allocator>
 struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
 {
-  /// Optional custom allocator.
-  std::shared_ptr<Allocator> allocator = nullptr;
-
-  SubscriptionOptionsWithAllocator<Allocator>() {}
+  SubscriptionOptionsWithAllocator<Allocator>()
+  : allocator_(new Allocator()),
+    message_allocator_(*allocator_)
+  {}
 
   /// Constructor using base class as input.
   explicit SubscriptionOptionsWithAllocator(
     const SubscriptionOptionsBase & subscription_options_base)
-  : SubscriptionOptionsBase(subscription_options_base)
+  : SubscriptionOptionsBase(subscription_options_base),
+    allocator_(new Allocator()),
+    message_allocator_(*allocator_)
   {}
 
   /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t.
@@ -99,12 +101,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
-    using AllocatorTraits = std::allocator_traits<Allocator>;
-    using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-    auto message_alloc = std::make_shared<MessageAllocatorT>(*allocator.get());
-    // TODO(stevewolter): This is likely to invoke undefined behavior - message_alloc
-    // goes out of scope at the end of this function, but the allocator doesn't. See #1339.
-    result.allocator = get_rcl_allocator(*message_alloc);
+    result.allocator = get_rcl_allocator(message_allocator_);
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
 
@@ -116,15 +113,19 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     return result;
   }
 
-  /// Get the allocator, creating one if needed.
+  /// Get the allocator
   std::shared_ptr<Allocator>
   get_allocator() const
   {
-    if (!this->allocator) {
-      return std::make_shared<Allocator>();
-    }
-    return this->allocator;
+    return allocator_
   }
+
+ private:
+  using AllocatorTraits = std::allocator_traits<Allocator>;
+  using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
+
+  std::shared_ptr<Allocator> allocator_;
+  MessageAllocatorT message_allocator_;
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;

From 7f0e5b8aedaaab6523ecdb88a6491a8f339e0fde Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Tue, 29 Sep 2020 20:34:55 +0000
Subject: [PATCH 7/9] Drop the generic version of get_rcl_allocator.

This allows us to simplify a bunch of code as well.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 .../rclcpp/allocator/allocator_common.hpp     | 56 ++-----------------
 rclcpp/include/rclcpp/publisher_options.hpp   | 33 ++++++-----
 .../include/rclcpp/subscription_options.hpp   | 28 ++++------
 .../node_interfaces/node_parameters.cpp       |  1 -
 .../allocator/test_allocator_common.cpp       | 27 ---------
 rclcpp/test/rclcpp/test_subscription.cpp      |  1 -
 6 files changed, 31 insertions(+), 115 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index 38dd96249a..15065920f4 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -29,64 +29,16 @@ namespace allocator
 template<typename T, typename Alloc>
 using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_traits<T>;
 
-template<typename Alloc>
-void * retyped_allocate(size_t size, void * untyped_allocator)
-{
-  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
-  if (!typed_allocator) {
-    throw std::runtime_error("Received incorrect allocator type");
-  }
-  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
-}
-
-template<typename Alloc>
-void retyped_deallocate(void * untyped_pointer, void * untyped_allocator)
-{
-  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
-  if (!typed_allocator) {
-    throw std::runtime_error("Received incorrect allocator type");
-  }
-  auto typed_ptr = static_cast<typename Alloc::value_type *>(untyped_pointer);
-  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
-}
-
-template<typename Alloc>
-void * retyped_reallocate(void * untyped_pointer, size_t size, void * untyped_allocator)
-{
-  auto typed_allocator = static_cast<Alloc *>(untyped_allocator);
-  if (!typed_allocator) {
-    throw std::runtime_error("Received incorrect allocator type");
-  }
-  auto typed_ptr = static_cast<typename Alloc::value_type *>(untyped_pointer);
-  std::allocator_traits<Alloc>::deallocate(*typed_allocator, typed_ptr, 1);
-  return std::allocator_traits<Alloc>::allocate(*typed_allocator, size);
-}
-
 }  // namespace allocator
 
-// Deprecated: Generic converter from C++ allocator into RCL allocator.
-// This allocator overallocates memory by 100x and invokes UB on free,
-// see #1254.
-template<typename Alloc>
-rcl_allocator_t get_rcl_allocator(Alloc & allocator)
-{
-  rcl_allocator_t rcl_allocator = rcl_get_default_allocator();
-#ifndef _WIN32
-  rcl_allocator.allocate = &allocator::retyped_allocate<Alloc>;
-  rcl_allocator.deallocate = &allocator::retyped_deallocate<Alloc>;
-  rcl_allocator.reallocate = &allocator::retyped_reallocate<Alloc>;
-  rcl_allocator.state = &allocator;
-#else
-  (void)allocator;  // Remove warning
-#endif
-  return rcl_allocator;
-}
-
 // Builds the RCL default allocator for the C++ standard allocator.
 // This assumes that the user intent behind both allocators is the
 // same: Using system malloc for allocation.
+//
+// If you're using a custom allocator in ROS, you'll need to provide
+// your own overload for this function.
 template<typename T>
-rcl_allocator_t get_rcl_allocator(std::allocator<T> & allocator)
+rcl_allocator_t get_rcl_allocator(std::allocator<T> allocator)
 {
   (void)allocator;  // Remove warning
   return rcl_get_default_allocator();
diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index 5be5846c4b..fe2c805030 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -56,16 +56,14 @@ struct PublisherOptionsBase
 template<typename Allocator>
 struct PublisherOptionsWithAllocator : public PublisherOptionsBase
 {
-  PublisherOptionsWithAllocator<Allocator>()
-  : allocator_(new Allocator()),
-    message_allocator_(*allocator_)
-  {}
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+
+  PublisherOptionsWithAllocator<Allocator>() {}
 
   /// Constructor using base class as input.
   explicit PublisherOptionsWithAllocator(const PublisherOptionsBase & publisher_options_base)
-  : PublisherOptionsBase(publisher_options_base),
-    allocator_(new Allocator()),
-    message_allocator_(*allocator_)
+  : PublisherOptionsBase(publisher_options_base)
   {}
 
   /// Convert this class, and a rclcpp::QoS, into an rcl_publisher_options_t.
@@ -74,7 +72,7 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   to_rcl_publisher_options(const rclcpp::QoS & qos) const
   {
     rcl_publisher_options_t result = rcl_publisher_get_default_options();
-    result.allocator = get_rcl_allocator(message_allocator_);
+    result.allocator = get_rcl_allocator(*this->get_allocator());
     result.qos = qos.get_rmw_qos_profile();
 
     // Apply payload to rcl_publisher_options if necessary.
@@ -86,19 +84,20 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   }
 
 
-  /// Get the allocator
+  /// Get the allocator, creating one if needed.
   std::shared_ptr<Allocator>
   get_allocator() const
   {
-    return allocator_;
+    if (!this->allocator) {
+      // TODO(wjwwood): I would like to use the commented line instead, but
+      //   cppcheck 1.89 fails with:
+      //     Syntax Error: AST broken, binary operator '>' doesn't have two operands.
+      // return std::make_shared<Allocator>();
+      std::shared_ptr<Allocator> tmp(new Allocator());
+      return tmp;
+    }
+    return this->allocator;
   }
-
- private:
-  using AllocatorTraits = std::allocator_traits<Allocator>;
-  using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-
-  std::shared_ptr<Allocator> allocator_;
-  MessageAllocatorT message_allocator_;
 };
 
 using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;
diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp
index 15b04dad8b..21fa109f61 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -78,17 +78,15 @@ struct SubscriptionOptionsBase
 template<typename Allocator>
 struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
 {
-  SubscriptionOptionsWithAllocator<Allocator>()
-  : allocator_(new Allocator()),
-    message_allocator_(*allocator_)
-  {}
+  /// Optional custom allocator.
+  std::shared_ptr<Allocator> allocator = nullptr;
+
+  SubscriptionOptionsWithAllocator<Allocator>() {}
 
   /// Constructor using base class as input.
   explicit SubscriptionOptionsWithAllocator(
     const SubscriptionOptionsBase & subscription_options_base)
-  : SubscriptionOptionsBase(subscription_options_base),
-    allocator_(new Allocator()),
-    message_allocator_(*allocator_)
+  : SubscriptionOptionsBase(subscription_options_base)
   {}
 
   /// Convert this class, with a rclcpp::QoS, into an rcl_subscription_options_t.
@@ -101,7 +99,7 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   to_rcl_subscription_options(const rclcpp::QoS & qos) const
   {
     rcl_subscription_options_t result = rcl_subscription_get_default_options();
-    result.allocator = get_rcl_allocator(message_allocator_);
+    result.allocator = get_rcl_allocator(*this->get_allocator());
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
 
@@ -113,19 +111,15 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     return result;
   }
 
-  /// Get the allocator
+  /// Get the allocator, creating one if needed.
   std::shared_ptr<Allocator>
   get_allocator() const
   {
-    return allocator_
+    if (!this->allocator) {
+      return std::make_shared<Allocator>();
+    }
+    return this->allocator;
   }
-
- private:
-  using AllocatorTraits = std::allocator_traits<Allocator>;
-  using MessageAllocatorT = typename AllocatorTraits::template rebind_alloc<MessageT>;
-
-  std::shared_ptr<Allocator> allocator_;
-  MessageAllocatorT message_allocator_;
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
index 90c851be90..2e5ebeb77c 100644
--- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
@@ -60,7 +60,6 @@ NodeParameters::NodeParameters(
   // TODO(wjwwood): expose this allocator through the Parameter interface.
   rclcpp::PublisherOptionsWithAllocator<AllocatorT> publisher_options(
     parameter_event_publisher_options);
-  publisher_options.allocator = std::make_shared<AllocatorT>();
 
   if (start_parameter_services) {
     parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
index 2a656c114e..ebea34c555 100644
--- a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
+++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp
@@ -18,33 +18,6 @@
 
 #include "rclcpp/allocator/allocator_common.hpp"
 
-TEST(TestAllocatorCommon, retyped_allocate) {
-  std::allocator<int> allocator;
-  void * untyped_allocator = &allocator;
-  void * allocated_mem =
-    rclcpp::allocator::retyped_allocate<std::allocator<char>>(1u, untyped_allocator);
-  ASSERT_NE(nullptr, allocated_mem);
-
-  auto code = [&untyped_allocator, allocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<std::allocator<int>>(
-        allocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code());
-
-  allocated_mem = allocator.allocate(1);
-  ASSERT_NE(nullptr, allocated_mem);
-  void * reallocated_mem =
-    rclcpp::allocator::retyped_reallocate<std::allocator<int>>(
-    allocated_mem, 2u, untyped_allocator);
-  ASSERT_NE(nullptr, reallocated_mem);
-
-  auto code2 = [&untyped_allocator, reallocated_mem]() {
-      rclcpp::allocator::retyped_deallocate<std::allocator<int>>(
-        reallocated_mem, untyped_allocator);
-    };
-  EXPECT_NO_THROW(code2());
-}
-
 TEST(TestAllocatorCommon, get_rcl_allocator) {
   std::allocator<int> allocator;
   auto rcl_allocator = rclcpp::get_rcl_allocator(allocator);
diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp
index 9559e69c4b..ad825e18b8 100644
--- a/rclcpp/test/rclcpp/test_subscription.cpp
+++ b/rclcpp/test/rclcpp/test_subscription.cpp
@@ -247,7 +247,6 @@ TEST_F(TestSubscription, various_creation_signatures) {
   }
   {
     rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
-    options.allocator = std::make_shared<std::allocator<void>>();
     EXPECT_NE(nullptr, options.get_allocator());
     auto sub = rclcpp::create_subscription<Empty>(
       node, "topic", 42, cb, options);

From af20431574ed59046d5c58278415b6589ccc9652 Mon Sep 17 00:00:00 2001
From: Steve Wolter <steve.wolter.de@gmail.com>
Date: Wed, 30 Sep 2020 20:34:23 +0200
Subject: [PATCH 8/9] Update
 rclcpp/include/rclcpp/allocator/allocator_common.hpp

Co-authored-by: William Woodall <william+github@osrfoundation.org>
Signed-off-by: Steve Wolter <swolter@google.com>
---
 .../include/rclcpp/allocator/allocator_common.hpp  | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/rclcpp/include/rclcpp/allocator/allocator_common.hpp b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
index 15065920f4..054c11c1a4 100644
--- a/rclcpp/include/rclcpp/allocator/allocator_common.hpp
+++ b/rclcpp/include/rclcpp/allocator/allocator_common.hpp
@@ -31,12 +31,14 @@ using AllocRebind = typename std::allocator_traits<Alloc>::template rebind_trait
 
 }  // namespace allocator
 
-// Builds the RCL default allocator for the C++ standard allocator.
-// This assumes that the user intent behind both allocators is the
-// same: Using system malloc for allocation.
-//
-// If you're using a custom allocator in ROS, you'll need to provide
-// your own overload for this function.
+/// Return the equivalent rcl_allocator_t for the C++ standard allocator.
+/**
+ * This assumes that the user intent behind both allocators is the
+ * same: Using system malloc for allocation.
+ *
+ * If you're using a custom allocator in ROS, you'll need to provide
+ * your own overload for this function.
+ */
 template<typename T>
 rcl_allocator_t get_rcl_allocator(std::allocator<T> allocator)
 {

From c51a72eae2b262cd88dc11897a4b76d5e355e683 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Wed, 30 Sep 2020 20:27:53 +0000
Subject: [PATCH 9/9] Revert accidental deletion of allocator creation.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 1 +
 rclcpp/test/rclcpp/test_subscription.cpp              | 1 +
 2 files changed, 2 insertions(+)

diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
index 2e5ebeb77c..90c851be90 100644
--- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
+++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp
@@ -60,6 +60,7 @@ NodeParameters::NodeParameters(
   // TODO(wjwwood): expose this allocator through the Parameter interface.
   rclcpp::PublisherOptionsWithAllocator<AllocatorT> publisher_options(
     parameter_event_publisher_options);
+  publisher_options.allocator = std::make_shared<AllocatorT>();
 
   if (start_parameter_services) {
     parameter_service_ = std::make_shared<ParameterService>(node_base, node_services, this);
diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp
index ad825e18b8..9559e69c4b 100644
--- a/rclcpp/test/rclcpp/test_subscription.cpp
+++ b/rclcpp/test/rclcpp/test_subscription.cpp
@@ -247,6 +247,7 @@ TEST_F(TestSubscription, various_creation_signatures) {
   }
   {
     rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> options;
+    options.allocator = std::make_shared<std::allocator<void>>();
     EXPECT_NE(nullptr, options.get_allocator());
     auto sub = rclcpp::create_subscription<Empty>(
       node, "topic", 42, cb, options);