From 8aa8a71375e740c212e65e46ca7d0528d0654a4f Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Wed, 23 Sep 2020 12:28:04 +0000
Subject: [PATCH 1/2] Override get_rcl_allocator for TLSF allocators.

This removes one more user of the broken generic version of
get_rcl_allocator, which is causing
https://github.com/ros2/rclcpp/issues/1254.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 tlsf_cpp/include/tlsf_cpp/tlsf.hpp | 41 ++++++++++++++++++++++++------
 1 file changed, 33 insertions(+), 8 deletions(-)

diff --git a/tlsf_cpp/include/tlsf_cpp/tlsf.hpp b/tlsf_cpp/include/tlsf_cpp/tlsf.hpp
index 077b225..0fd856f 100644
--- a/tlsf_cpp/include/tlsf_cpp/tlsf.hpp
+++ b/tlsf_cpp/include/tlsf_cpp/tlsf.hpp
@@ -20,9 +20,12 @@
 
 #include <cstring>
 #include <iostream>
+#include <memory>
 #include <new>
 #include <stdexcept>
 
+#include "rcl/allocator.h"
+#include "rclcpp/allocator/allocator_common.hpp"
 #include "tlsf/tlsf.h"
 
 template<typename T, size_t DefaultPoolSize = 1024 * 1024>
@@ -61,19 +64,21 @@ struct tlsf_heap_allocator
   {
     pool_size = size;
     if (!memory_pool) {
-      memory_pool = new char[pool_size];
-      memset(memory_pool, 0, pool_size);
-      init_memory_pool(pool_size, memory_pool);
+      auto memory = new char[pool_size];
+      memset(memory, 0, pool_size);
+      init_memory_pool(pool_size, memory);
+      memory_pool = std::shared_ptr<char>(
+        memory,
+        [](char * pool) {
+          destroy_memory_pool(pool);
+          delete[] pool;
+        });
     }
     return pool_size;
   }
 
   ~tlsf_heap_allocator()
   {
-    if (memory_pool) {
-      destroy_memory_pool(memory_pool);
-      memory_pool = nullptr;
-    }
   }
 
   // Needed for std::allocator_traits
@@ -98,7 +103,7 @@ struct tlsf_heap_allocator
     typedef tlsf_heap_allocator<U> other;
   };
 
-  char * memory_pool;
+  std::shared_ptr<char> memory_pool;
   size_t pool_size;
 };
 
@@ -137,4 +142,24 @@ constexpr bool operator!=(
   return a.memory_pool != b.memory_pool;
 }
 
+template<typename T, size_t PoolSize>
+rcl_allocator_t get_rcl_allocator(tlsf_heap_allocator<T, PoolSize> allocator)
+{
+  rcl_allocator_t rcl_allocator;
+  rcl_allocator.allocate = [](size_t size, void *) {
+      return tlsf_malloc(size);
+    };
+  rcl_allocator.deallocate = [](void * pointer, void *) {
+      return tlsf_free(pointer);
+    };
+  rcl_allocator.reallocate = [](void * pointer, size_t size, void *) {
+      return tlsf_realloc(pointer, size);
+    };
+  rcl_allocator.zero_allocate = [](size_t nmemb, size_t size, void *) {
+      return tlsf_calloc(nmemb, size);
+    };
+  (void)allocator;  // unused
+  return rcl_allocator;
+}
+
 #endif  // TLSF_CPP__TLSF_HPP_

From 2d64cf7ffa35f4e4eeacdb62c4d5732d4b4aa0a5 Mon Sep 17 00:00:00 2001
From: Steve Wolter <swolter@google.com>
Date: Tue, 27 Oct 2020 12:15:50 +0000
Subject: [PATCH 2/2] Move get_rcl_allocator to rclcpp namespace.

Signed-off-by: Steve Wolter <swolter@google.com>
---
 tlsf_cpp/include/tlsf_cpp/tlsf.hpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/tlsf_cpp/include/tlsf_cpp/tlsf.hpp b/tlsf_cpp/include/tlsf_cpp/tlsf.hpp
index 0fd856f..8a00750 100644
--- a/tlsf_cpp/include/tlsf_cpp/tlsf.hpp
+++ b/tlsf_cpp/include/tlsf_cpp/tlsf.hpp
@@ -142,6 +142,9 @@ constexpr bool operator!=(
   return a.memory_pool != b.memory_pool;
 }
 
+namespace rclcpp {
+
+// Overload rclcpp::get_rcl_allocator for TLSF allocators.
 template<typename T, size_t PoolSize>
 rcl_allocator_t get_rcl_allocator(tlsf_heap_allocator<T, PoolSize> allocator)
 {
@@ -162,4 +165,6 @@ rcl_allocator_t get_rcl_allocator(tlsf_heap_allocator<T, PoolSize> allocator)
   return rcl_allocator;
 }
 
+}  // namespace rclcpp
+
 #endif  // TLSF_CPP__TLSF_HPP_