From 135433bcdbc51ccac3a36af4ed07caa858116e50 Mon Sep 17 00:00:00 2001
From: Michel Hidalgo <michel@ekumenlabs.com>
Date: Wed, 5 May 2021 17:17:42 -0300
Subject: [PATCH] Do not attempt to use void allocators for memory allocation.
 (#1657)

Keep a rebound allocator for byte-sized memory blocks around
for publisher and subscription options.

Follow-up after 1fc2d58799a6d4530522be5c236c70be53455e60

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>
---
 rclcpp/include/rclcpp/publisher_options.hpp   | 27 ++++++++++++++---
 .../include/rclcpp/subscription_options.hpp   | 27 ++++++++++++++---
 ..._intra_process_manager_with_allocators.cpp | 29 ++++++++++++++++++-
 3 files changed, 74 insertions(+), 9 deletions(-)

diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index 8388f85e47..ddd94d4c8d 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -80,10 +80,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());
-    result.allocator = rclcpp::allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    result.allocator = this->get_rcl_allocator();
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_publisher_options.require_unique_network_flow_endpoints =
       this->require_unique_network_flow_endpoints;
@@ -106,6 +103,28 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
     }
     return this->allocator;
   }
+
+private:
+  using PlainAllocator =
+    typename std::allocator_traits<Allocator>::template rebind_alloc<char>;
+
+  rcl_allocator_t
+  get_rcl_allocator() const
+  {
+    if (!plain_allocator_storage_) {
+      plain_allocator_storage_ =
+        std::make_shared<PlainAllocator>(*this->get_allocator());
+    }
+    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
+  }
+
+  // This is a temporal workaround, to make sure that get_allocator()
+  // always returns a copy of the same allocator.
+  mutable std::shared_ptr<Allocator> allocator_storage_;
+
+  // This is a temporal workaround, to keep the plain allocator that backs
+  // up the rcl allocator returned in rcl_publisher_options_t alive.
+  mutable std::shared_ptr<PlainAllocator> plain_allocator_storage_;
 };
 
 using PublisherOptions = PublisherOptionsWithAllocator<std::allocator<void>>;
diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp
index d9d4085d85..a100c527f8 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -107,10 +107,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>(*this->get_allocator().get());
-    result.allocator = allocator::get_rcl_allocator<MessageT>(*message_alloc);
+    result.allocator = this->get_rcl_allocator();
     result.qos = qos.get_rmw_qos_profile();
     result.rmw_subscription_options.ignore_local_publications = this->ignore_local_publications;
     result.rmw_subscription_options.require_unique_network_flow_endpoints =
@@ -133,6 +130,28 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
     }
     return this->allocator;
   }
+
+private:
+  using PlainAllocator =
+    typename std::allocator_traits<Allocator>::template rebind_alloc<char>;
+
+  rcl_allocator_t
+  get_rcl_allocator() const
+  {
+    if (!plain_allocator_storage_) {
+      plain_allocator_storage_ =
+        std::make_shared<PlainAllocator>(*this->get_allocator());
+    }
+    return rclcpp::allocator::get_rcl_allocator<char>(*plain_allocator_storage_);
+  }
+
+  // This is a temporal workaround, to make sure that get_allocator()
+  // always returns a copy of the same allocator.
+  mutable std::shared_ptr<Allocator> allocator_storage_;
+
+  // This is a temporal workaround, to keep the plain allocator that backs
+  // up the rcl allocator returned in rcl_subscription_options_t alive.
+  mutable std::shared_ptr<PlainAllocator> plain_allocator_storage_;
 };
 
 using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
diff --git a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
index f8cb96a421..520e32d0ab 100644
--- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
+++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
@@ -30,7 +30,7 @@
 static uint32_t num_allocs = 0;
 static uint32_t num_deallocs = 0;
 // A very simple custom allocator. Counts calls to allocate and deallocate.
-template<typename T = void>
+template<typename T>
 struct MyAllocator
 {
 public:
@@ -77,6 +77,33 @@ struct MyAllocator
   };
 };
 
+// Explicit specialization for void
+template<>
+struct MyAllocator<void>
+{
+public:
+  using value_type = void;
+  using pointer = void *;
+  using const_pointer = const void *;
+
+  MyAllocator() noexcept
+  {
+  }
+
+  ~MyAllocator() noexcept {}
+
+  template<typename U>
+  MyAllocator(const MyAllocator<U> &) noexcept
+  {
+  }
+
+  template<typename U>
+  struct rebind
+  {
+    typedef MyAllocator<U> other;
+  };
+};
+
 template<typename T, typename U>
 constexpr bool operator==(
   const MyAllocator<T> &,