From 0659d829cec5a39c6ad861d597b4aa9aee224a25 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   | 19 ++++++++++-
 .../include/rclcpp/subscription_options.hpp   | 19 ++++++++++-
 ..._intra_process_manager_with_allocators.cpp | 34 ++++++++++++++++---
 3 files changed, 65 insertions(+), 7 deletions(-)

diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp
index efaa4b48c8..48250307e9 100644
--- a/rclcpp/include/rclcpp/publisher_options.hpp
+++ b/rclcpp/include/rclcpp/publisher_options.hpp
@@ -85,7 +85,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 = rclcpp::allocator::get_rcl_allocator<void>(*this->get_allocator());
+    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;
@@ -113,9 +113,26 @@ struct PublisherOptionsWithAllocator : public PublisherOptionsBase
   }
 
 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 373850e00b..d6d2d4c60d 100644
--- a/rclcpp/include/rclcpp/subscription_options.hpp
+++ b/rclcpp/include/rclcpp/subscription_options.hpp
@@ -112,7 +112,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 = allocator::get_rcl_allocator<void>(*this->get_allocator());
+    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 =
@@ -139,9 +139,26 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
   }
 
 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 8443706126..9bef3c2c0f 100644
--- a/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
+++ b/rclcpp/test/rclcpp/test_intra_process_manager_with_allocators.cpp
@@ -31,7 +31,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:
@@ -58,10 +58,7 @@ struct MyAllocator
       return nullptr;
     }
     num_allocs++;
-    // Use sizeof(char) in place for sizeof(void)
-    constexpr size_t value_size = sizeof(
-      typename std::conditional<!std::is_void<T>::value, T, char>::type);
-    return static_cast<T *>(std::malloc(size * value_size));
+    return static_cast<T *>(std::malloc(size * sizeof(T)));
   }
 
   void deallocate(T * ptr, size_t size)
@@ -81,6 +78,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> &,