Implement allocator refactor for type support structs

Signed-off-by: methylDragon <[email protected]>

Author: methylDragon

rclcpp/include/rclcpp/dynamic_typesupport/dynamic_message_type_support.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
 #define RCLCPP__DYNAMIC_TYPESUPPORT__DYNAMIC_MESSAGE_TYPE_SUPPORT_HPP_
 
+#include <rcl/allocator.h>
 #include <rosidl_dynamic_typesupport/types.h>
 #include <rosidl_runtime_c/message_type_support_struct.h>
 #include <rosidl_runtime_c/type_description/type_description__struct.h>
@@ -67,15 +68,17 @@ class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMes
   RCLCPP_PUBLIC
   DynamicMessageTypeSupport(
     const rosidl_runtime_c__type_description__TypeDescription & description,
-    const std::string & serialization_library_name = "");
+    const std::string & serialization_library_name = "",
+    rcl_allocator_t allocator = rcl_get_default_allocator());
 
   /// From description, for provided serialization support
   /// Does NOT take ownership of the description (copies instead.)
   /// Constructs type support top-down (calling `rmw_dynamic_message_type_support_handle_create()`)
   RCLCPP_PUBLIC
   DynamicMessageTypeSupport(
     DynamicSerializationSupport::SharedPtr serialization_support,
-    const rosidl_runtime_c__type_description__TypeDescription & description);
+    const rosidl_runtime_c__type_description__TypeDescription & description,
+    rcl_allocator_t allocator = rcl_get_default_allocator());
 
   /// Assume ownership of managed types
   /// Does NOT take ownership of the description (copies instead.)
@@ -158,7 +161,6 @@ class DynamicMessageTypeSupport : public std::enable_shared_from_this<DynamicMes
   DynamicMessage::ConstSharedPtr
   get_shared_dynamic_message() const;
 
-
   // METHODS =======================================================================================
   RCLCPP_PUBLIC
   void

rclcpp/src/rclcpp/dynamic_typesupport/dynamic_message_type_support.cpp

@@ -23,8 +23,10 @@
 
 #include "rcl/allocator.h"
 #include "rcl/dynamic_message_type_support.h"
+#include "rcl/error_handling.h"
 #include "rcl/type_hash.h"
 #include "rcl/types.h"
+#include "rcutils/allocator.h"
 #include "rcutils/logging_macros.h"
 #include "rmw/dynamic_message_type_support.h"
 
@@ -46,7 +48,8 @@ using rclcpp::dynamic_typesupport::DynamicSerializationSupport;
 // CONSTRUCTION ====================================================================================
 DynamicMessageTypeSupport::DynamicMessageTypeSupport(
   const rosidl_runtime_c__type_description__TypeDescription & description,
-  const std::string & serialization_library_name)
+  const std::string & serialization_library_name,
+  rcl_allocator_t allocator)
 : serialization_support_(nullptr),
   dynamic_message_type_(nullptr),
   dynamic_message_(nullptr),
@@ -57,13 +60,16 @@ DynamicMessageTypeSupport::DynamicMessageTypeSupport(
   rcl_ret_t ret;
 
   if (serialization_library_name.empty()) {
-    ret = rcl_dynamic_message_type_support_handle_create(nullptr, &description, &ts);
+    ret = rcl_dynamic_message_type_support_handle_create(nullptr, &description, &allocator, &ts);
   } else {
     ret = rcl_dynamic_message_type_support_handle_create(
-      serialization_library_name.c_str(), &description, &ts);
+      serialization_library_name.c_str(), &description, &allocator, &ts);
   }
   if (ret != RCL_RET_OK) {
-    throw std::runtime_error("error initializing rosidl message type support");
+    auto msg = std::string("error initializing rosidl message type support:\n") +
+      rcl_get_error_string().str;
+    rcl_reset_error();
+    throw std::runtime_error(msg);
   }
   if (!ts) {
     throw std::runtime_error("could not init rosidl message type support");
@@ -85,13 +91,16 @@ DynamicMessageTypeSupport::DynamicMessageTypeSupport(
     ts,
     [](rosidl_message_type_support_t * ts) -> void {
       auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
-      auto allocator = rcl_get_default_allocator();
+      // User might have passed in a different allocator, but the type hash was allocated using
+      // the default
+      rcutils_allocator_t type_hash_allocator = rcutils_get_default_allocator();
+      auto impl_allocator = ts_impl->allocator;
 
       // These are all C allocated
-      allocator.deallocate(ts_impl->type_hash, &allocator.state);
-      allocator.deallocate(
-        const_cast<rosidl_dynamic_message_type_support_impl_t *>(ts_impl), &allocator.state);
-      allocator.deallocate(ts, &allocator.state);
+      type_hash_allocator.deallocate(ts_impl->type_hash, &type_hash_allocator.state);
+      impl_allocator.deallocate(
+        const_cast<rosidl_dynamic_message_type_support_impl_t *>(ts_impl), &impl_allocator.state);
+      impl_allocator.deallocate(ts, &impl_allocator.state);
     }
   );
 
@@ -112,7 +121,8 @@ DynamicMessageTypeSupport::DynamicMessageTypeSupport(
 
 DynamicMessageTypeSupport::DynamicMessageTypeSupport(
   DynamicSerializationSupport::SharedPtr serialization_support,
-  const rosidl_runtime_c__type_description__TypeDescription & description)
+  const rosidl_runtime_c__type_description__TypeDescription & description,
+  rcl_allocator_t allocator)
 : serialization_support_(serialization_support),
   dynamic_message_type_(nullptr),
   dynamic_message_(nullptr),
@@ -141,6 +151,7 @@ DynamicMessageTypeSupport::DynamicMessageTypeSupport(
     type_hash.get(),  // type_hash
     &description,     // type_description
     nullptr,          // type_description_sources (not implemented for dynamic types)
+    &allocator,       // allocator
     &ts);
   if (ret != RMW_RET_OK || !ts) {
     throw std::runtime_error("could not init rosidl message type support");
@@ -158,13 +169,16 @@ DynamicMessageTypeSupport::DynamicMessageTypeSupport(
   //                     are managed by the passed in SharedPtr wrapper classes. We just delete it.
   rosidl_message_type_support_.reset(
     ts,
-    [](rosidl_message_type_support_t * ts) -> void {
+    [&](rosidl_message_type_support_t * ts) -> void {
       auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+      auto impl_allocator = ts_impl->allocator;
 
-      // These are allocated with new
-      delete ts_impl->type_hash;  // Only because we should've allocated it here
-      delete ts_impl;
-      delete ts;
+      delete ts_impl->type_hash;  // Only because we should've allocated it here with the unique ptr
+
+      // These are allocated with the allocator by rmw_dynamic_message_type_support_handle_create
+      impl_allocator.deallocate(
+        const_cast<rosidl_dynamic_message_type_support_impl_t *>(ts_impl), &impl_allocator.state);
+      impl_allocator.deallocate(ts, &impl_allocator.state);
     }
   );
   manage_description_(ts_impl->type_description);
@@ -214,8 +228,7 @@ DynamicMessageTypeSupport::DynamicMessageTypeSupport(
     throw std::runtime_error("could not init type description.");
   }
   if (!rosidl_runtime_c__type_description__TypeDescription__copy(
-      &description,
-      description_.get()))
+      &description, description_.get()))
   {
     throw std::runtime_error("could not copy type description.");
   }
@@ -311,12 +324,13 @@ DynamicMessageTypeSupport::init_rosidl_message_type_support_(
 
   rosidl_dynamic_message_type_support_impl_t * ts_impl =
     new rosidl_dynamic_message_type_support_impl_t {
-    type_hash.get(),                                              // type_hash
-    description,                                                  // type_description
-    nullptr,    // NOTE(methylDragon): Not supported for now      // type_description_sources
-    serialization_support->get_rosidl_serialization_support(),    // serialization_support
-    dynamic_message_type->get_rosidl_dynamic_type(),              // dynamic_message_type
-    dynamic_message->get_rosidl_dynamic_data()                    // dynamic_message
+    rcutils_get_default_allocator(),                            // allocator
+    type_hash.get(),                                            // type_hash
+    description,                                                // type_description
+    nullptr,    // NOTE(methylDragon): Not supported for now    // type_description_sources
+    serialization_support->get_rosidl_serialization_support(),  // serialization_support
+    dynamic_message_type->get_rosidl_dynamic_type(),            // dynamic_message_type
+    dynamic_message->get_rosidl_dynamic_data()                  // dynamic_message
   };
   if (!ts_impl) {
     throw std::runtime_error(
@@ -326,26 +340,29 @@ DynamicMessageTypeSupport::init_rosidl_message_type_support_(
 
   // NOTE(methylDragon): We don't finalize the rosidl_message_type_support->data since its members
   //                     are managed by the passed in SharedPtr wrapper classes. We just delete it.
+  /* *INDENT-OFF* */
   rosidl_message_type_support_.reset(
     new rosidl_message_type_support_t{
-    rmw_get_dynamic_typesupport_identifier(),             // typesupport_identifier
-    ts_impl,                                              // data
-    get_message_typesupport_handle_function,              // func
-    // get_type_hash_func
-    rosidl_get_dynamic_message_type_support_type_hash_function,
-    // get_type_description_func
-    rosidl_get_dynamic_message_type_support_type_description_function,
+    rmw_get_dynamic_typesupport_identifier(),                           // typesupport_identifier
+    ts_impl,                                                            // data
+    get_message_typesupport_handle_function,                            // func
+    rosidl_get_dynamic_message_type_support_type_hash_function,         // get_type_hash_func
+    rosidl_get_dynamic_message_type_support_type_description_function,  // get_type_description_func
     // get_type_description_sources_func
     rosidl_get_dynamic_message_type_support_type_description_sources_function
-  },
+    },
+    // Custom deleter
     [](rosidl_message_type_support_t * ts) -> void {
       auto ts_impl = static_cast<const rosidl_dynamic_message_type_support_impl_t *>(ts->data);
+      // User might have passed in a different allocator, but the type hash was allocated using
+      // the default
       auto allocator = rcl_get_default_allocator();
       // Only because we should've allocated it here (also it's C allocated)
       allocator.deallocate(ts_impl->type_hash, &allocator.state);
       delete ts_impl;
     }
   );
+  /* *INDENT-ON* */
 
   if (!rosidl_message_type_support_) {
     throw std::runtime_error("Could not allocate rosidl_message_type_support_t struct");