Implement allocator refactor for type support structs

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

Author: methylDragon

include/rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h

@@ -20,6 +20,7 @@ extern "C"
 {
 #endif
 
+#include <rcutils/allocator.h>
 #include <rcutils/types/rcutils_ret.h>
 #include <rosidl_runtime_c/message_type_support_struct.h>
 #include <rosidl_runtime_c/type_description/type_description__struct.h>
@@ -48,6 +49,8 @@ extern "C"
 // outlives those downstream classes.
 typedef struct rosidl_dynamic_message_type_support_impl_s
 {
+  rcutils_allocator_t allocator;
+
   rosidl_type_hash_t * type_hash;
   rosidl_runtime_c__type_description__TypeDescription * type_description;
 
@@ -68,21 +71,70 @@ typedef struct rosidl_dynamic_message_type_support_impl_s
   rosidl_dynamic_typesupport_dynamic_data_t * dynamic_message;
 } rosidl_dynamic_message_type_support_impl_t;
 
+/// Return a zero initialized rosidl_dynamic_message_type_support_impl_t
+/**
+ * \return A zero initialized dynamic message typesupport handle impl.
+ */
+ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
+rosidl_dynamic_message_type_support_impl_t
+rosidl_get_zero_initialized_dynamic_message_type_support_impl(void);
+
 /// Init a dynamic message type support handle
 ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
 rcutils_ret_t
+rosidl_dynamic_message_type_support_handle_init(
+  rosidl_dynamic_typesupport_serialization_support_t * serialization_support,
+  const rosidl_type_hash_t * type_hash,
+  const rosidl_runtime_c__type_description__TypeDescription * type_description,
+  const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources,
+  rcutils_allocator_t * allocator,
+  rosidl_message_type_support_t * ts);
+
+/// Finalize a dynamic message type support handle
+/**
+ * This should only be used to finalize a dynamic message type support handle obtained with
+ * `rosidl_dynamic_message_type_support_handle_create()`, which has dynamically allocated members.
+ * Using this on a statically allocated typesupport will cause undefined behavior!
+ * (Static memory will get freed in that case.)
+ */
+ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
+rcutils_ret_t
+rosidl_dynamic_message_type_support_handle_fini(rosidl_message_type_support_t * ts);
+
+/// Init a dynamic message type support handle impl
+ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
+rcutils_ret_t
+rosidl_dynamic_message_type_support_impl_init(
+  rosidl_dynamic_typesupport_serialization_support_t * serialization_support,
+  const rosidl_type_hash_t * type_hash,
+  const rosidl_runtime_c__type_description__TypeDescription * type_description,
+  const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources,
+  rcutils_allocator_t * allocator,
+  rosidl_dynamic_message_type_support_impl_t * ts);
+
+/// Finalize a dynamic message type support handle impl
+ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
+rcutils_ret_t
+rosidl_dynamic_message_type_support_impl_fini(rosidl_dynamic_message_type_support_impl_t * ts);
+
+/// Create, then init a dynamic message type support handle
+ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
+rcutils_ret_t
 rosidl_dynamic_message_type_support_handle_create(
   rosidl_dynamic_typesupport_serialization_support_t * serialization_support,
   const rosidl_type_hash_t * type_hash,
   const rosidl_runtime_c__type_description__TypeDescription * type_description,
   const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources,
+  rcutils_allocator_t * allocator,
   rosidl_message_type_support_t ** ts);
 
-/// Destroy a rosidl_message_type_support_t obtained with
-/// `rmw_dynamic_message_type_support_handle_create()`, which has dynamically allocated members
-///
-/// NOTE: Using this on a statically allocated typesupport will cause undefined behavior!
-///       (Static memory will get freed in that case.)
+/// Finilaize, then destroy a dynamic message type support handle
+/**
+ * This should only be used to destroy a dynamic message type support handle obtained with
+ * `rosidl_dynamic_message_type_support_handle_create()`, which has dynamically allocated members.
+ * Using this on a statically allocated typesupport will cause undefined behavior!
+ * (Static memory will get freed in that case.)
+ */
 ROSIDL_DYNAMIC_TYPESUPPORT_PUBLIC
 rcutils_ret_t
 rosidl_dynamic_message_type_support_handle_destroy(rosidl_message_type_support_t * ts);

src/dynamic_message_type_support_struct.c

@@ -25,47 +25,135 @@
 #include "rosidl_dynamic_typesupport/identifier.h"
 
 
+rosidl_dynamic_message_type_support_impl_t
+rosidl_get_zero_initialized_dynamic_message_type_support_impl(void)
+{
+  static rosidl_dynamic_message_type_support_impl_t zero_typesupport_handle_impl = {
+    // .allocator = {0, 0, 0, 0, 0},
+    .type_hash = NULL,
+    .type_description = NULL,
+    .type_description_sources = NULL,
+    .serialization_support = NULL,
+    .dynamic_message_type = NULL,
+    .dynamic_message = NULL
+  };
+  zero_typesupport_handle_impl.allocator = rcutils_get_default_allocator();
+  return zero_typesupport_handle_impl;
+}
+
 rcutils_ret_t
-rosidl_dynamic_message_type_support_handle_create(
+rosidl_dynamic_message_type_support_handle_init(
   rosidl_dynamic_typesupport_serialization_support_t * serialization_support,
   const rosidl_type_hash_t * type_hash,
   const rosidl_runtime_c__type_description__TypeDescription * type_description,
   const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources,
-  rosidl_message_type_support_t ** ts)
+  rcutils_allocator_t * allocator,
+  rosidl_message_type_support_t * ts)
 {
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_support, RCUTILS_RET_INVALID_ARGUMENT);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_hash, RCUTILS_RET_INVALID_ARGUMENT);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_description, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RCUTILS_RET_INVALID_ARGUMENT);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(ts, RCUTILS_RET_INVALID_ARGUMENT);
 
   // NOTE(methylDragon): Not supported for now
   // RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_description_sources, RCUTILS_RET_INVALID_ARGUMENT);
 
   rcutils_ret_t ret = RCUTILS_RET_ERROR;
-  rcutils_allocator_t allocator = rcutils_get_default_allocator();
-  *ts = allocator.zero_allocate(1, sizeof(rosidl_message_type_support_t), allocator.state);
-  if (!*ts) {
-    RCUTILS_SET_ERROR_MSG("Could not allocate rosidl_message_type_support_t struct");
-    return RCUTILS_RET_BAD_ALLOC;
-  }
 
-  (*ts)->data = allocator.zero_allocate(
-    1, sizeof(rosidl_dynamic_message_type_support_impl_t), allocator.state);
-  if (!ts) {
+  ts->data = allocator->zero_allocate(
+    1, sizeof(rosidl_dynamic_message_type_support_impl_t), allocator->state);
+  if (!ts->data) {
     RCUTILS_SET_ERROR_MSG("Could not allocate rosidl_dynamic_message_type_support_impl_t struct");
     ret = RCUTILS_RET_BAD_ALLOC;
     goto fail;
   }
 
+  if (rosidl_dynamic_message_type_support_impl_init(
+      serialization_support, type_hash, type_description, type_description_sources, allocator,
+      (rosidl_dynamic_message_type_support_impl_t *) ts->data) != RCUTILS_RET_OK)
+  {
+    if (rosidl_dynamic_message_type_support_impl_fini(
+        (rosidl_dynamic_message_type_support_impl_t *) ts->data) != RCUTILS_RET_OK)
+    {
+      rcutils_error_string_t error_string = rcutils_get_error_string();
+      rcutils_reset_error();
+      RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
+        "While handling another error, could not finalize dynamic message typesupport impl:\n%s",
+        error_string.str);
+    }
+    goto fail;
+  }
+
+  ts->typesupport_identifier = rosidl_dynamic_typesupport_c__identifier;
+  ts->func = get_message_typesupport_handle_function;
+  ts->get_type_hash_func =
+    rosidl_get_dynamic_message_type_support_type_hash_function;
+  ts->get_type_description_func =
+    rosidl_get_dynamic_message_type_support_type_description_function;
+  ts->get_type_description_sources_func =
+    rosidl_get_dynamic_message_type_support_type_description_sources_function;
+
+  return RCUTILS_RET_OK;
+
+fail:
+  if (rosidl_dynamic_message_type_support_handle_fini(ts) != RCUTILS_RET_OK) {
+    rcutils_error_string_t error_string = rcutils_get_error_string();
+    rcutils_reset_error();
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "While handling another error, could not finalize dynamic message typesupport:\n%s",
+      error_string.str);
+  }
+  return ret;
+}
+
+rcutils_ret_t
+rosidl_dynamic_message_type_support_handle_fini(rosidl_message_type_support_t * ts)
+{
+  if (!ts) {
+    return RCUTILS_RET_OK;
+  }
+
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(ts->data, RCUTILS_RET_INVALID_ARGUMENT);
+
   rosidl_dynamic_message_type_support_impl_t * ts_impl =
-    (rosidl_dynamic_message_type_support_impl_t *) (*ts)->data;
+    (rosidl_dynamic_message_type_support_impl_t *) ts->data;
+  rcutils_allocator_t allocator = ts_impl->allocator;
+
+  if (ts->typesupport_identifier != rosidl_dynamic_typesupport_c__identifier) {
+    RCUTILS_SET_ERROR_MSG("type support not from this implementation");
+    return RCUTILS_RET_INVALID_ARGUMENT;
+  }
+
+  allocator.deallocate((void *) ts->data, allocator.state);
+  return RCUTILS_RET_OK;
+}
+
+rcutils_ret_t
+rosidl_dynamic_message_type_support_impl_init(
+  rosidl_dynamic_typesupport_serialization_support_t * serialization_support,
+  const rosidl_type_hash_t * type_hash,
+  const rosidl_runtime_c__type_description__TypeDescription * type_description,
+  const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources,
+  rcutils_allocator_t * allocator,
+  rosidl_dynamic_message_type_support_impl_t * ts_impl)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_support, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_hash, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_description, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(ts_impl, RCUTILS_RET_INVALID_ARGUMENT);
+
+  // NOTE(methylDragon): Not supported for now
+  // RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_description_sources, RCUTILS_RET_INVALID_ARGUMENT);
+
+  rcutils_ret_t ret = RCUTILS_RET_ERROR;
 
   // Copy init type hash (never null (checked above))
-  ts_impl->type_hash = allocator.zero_allocate(1, sizeof(rosidl_type_hash_t), allocator.state);
+  ts_impl->type_hash = allocator->zero_allocate(1, sizeof(rosidl_type_hash_t), allocator->state);
   if (!ts_impl->type_hash) {
     RCUTILS_SET_ERROR_MSG("Could not create type hash to assign into");
     ret = RCUTILS_RET_BAD_ALLOC;
-    goto fail;
   }
 
   ts_impl->type_hash->version = type_hash->version;
@@ -108,15 +196,7 @@ rosidl_dynamic_message_type_support_handle_create(
   }
 
   ts_impl->serialization_support = serialization_support;
-
-  (*ts)->typesupport_identifier = rosidl_dynamic_typesupport_c__identifier;
-  (*ts)->func = get_message_typesupport_handle_function;
-  (*ts)->get_type_hash_func =
-    rosidl_get_dynamic_message_type_support_type_hash_function;
-  (*ts)->get_type_description_func =
-    rosidl_get_dynamic_message_type_support_type_description_function;
-  (*ts)->get_type_description_sources_func =
-    rosidl_get_dynamic_message_type_support_type_description_sources_function;
+  ts_impl->allocator = *allocator;
 
   ret = rosidl_dynamic_typesupport_dynamic_type_create_from_description(
     ts_impl->serialization_support, type_description, &ts_impl->dynamic_message_type);
@@ -140,46 +220,109 @@ rosidl_dynamic_message_type_support_handle_create(
   return RCUTILS_RET_OK;
 
 fail:
-  if (rosidl_dynamic_message_type_support_handle_destroy(*ts) != RCUTILS_RET_OK) {
-    RCUTILS_SAFE_FWRITE_TO_STDERR(
-      "While handling another error, could not destroy dynamic message typesupport");
+  if (rosidl_dynamic_message_type_support_impl_fini(ts_impl) != RCUTILS_RET_OK) {
+    rcutils_error_string_t error_string = rcutils_get_error_string();
+    rcutils_reset_error();
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "While handling another error, could not finalize dynamic message typesupport impl:\n%s",
+      error_string.str);
   }
   return ret;
 }
 
+rcutils_ret_t
+rosidl_dynamic_message_type_support_impl_fini(rosidl_dynamic_message_type_support_impl_t * ts_impl)
+{
+  if (!ts_impl) {
+    return RCUTILS_RET_OK;
+  }
+
+  rcutils_allocator_t allocator = ts_impl->allocator;
+
+  if (ts_impl->type_description) {
+    rosidl_runtime_c__type_description__TypeDescription__fini(ts_impl->type_description);
+  }
+  if (ts_impl->type_description_sources) {
+    rosidl_runtime_c__type_description__TypeSource__Sequence__fini(
+      ts_impl->type_description_sources);
+  }
+  if (ts_impl->dynamic_message_type) {
+    rosidl_dynamic_typesupport_dynamic_type_destroy(ts_impl->dynamic_message_type);
+  }
+  if (ts_impl->dynamic_message) {
+    rosidl_dynamic_typesupport_dynamic_data_destroy(ts_impl->dynamic_message);
+  }
+  allocator.deallocate(ts_impl->type_hash, allocator.state);
+  return RCUTILS_RET_OK;
+}
 
 rcutils_ret_t
-rosidl_dynamic_message_type_support_handle_destroy(rosidl_message_type_support_t * ts)
+rosidl_dynamic_message_type_support_handle_create(
+  rosidl_dynamic_typesupport_serialization_support_t * serialization_support,
+  const rosidl_type_hash_t * type_hash,
+  const rosidl_runtime_c__type_description__TypeDescription * type_description,
+  const rosidl_runtime_c__type_description__TypeSource__Sequence * type_description_sources,
+  rcutils_allocator_t * allocator,
+  rosidl_message_type_support_t ** ts)
 {
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(serialization_support, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_hash, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_description, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(allocator, RCUTILS_RET_INVALID_ARGUMENT);
   RCUTILS_CHECK_ARGUMENT_FOR_NULL(ts, RCUTILS_RET_INVALID_ARGUMENT);
 
-  // NOTE(methylDragon): Ignores const...
+  // NOTE(methylDragon): Not supported for now
+  // RCUTILS_CHECK_ARGUMENT_FOR_NULL(type_description_sources, RCUTILS_RET_INVALID_ARGUMENT);
+
+  rcutils_ret_t ret = RCUTILS_RET_ERROR;
+
+  *ts = allocator->zero_allocate(1, sizeof(rosidl_message_type_support_t), allocator->state);
+  if (!*ts) {
+    RCUTILS_SET_ERROR_MSG("Could not allocate rosidl_message_type_support_t struct");
+    return RCUTILS_RET_BAD_ALLOC;
+  }
+
+  ret = rosidl_dynamic_message_type_support_handle_init(
+    serialization_support, type_hash, type_description, type_description_sources, allocator, *ts);
+  if (ret != RCUTILS_RET_OK) {
+    if (rosidl_dynamic_message_type_support_handle_destroy(*ts) != RCUTILS_RET_OK) {
+      RCUTILS_SAFE_FWRITE_TO_STDERR(
+        "While handling another error, could not destroy dynamic message typesupport");
+    }
+    rcutils_error_string_t error_string = rcutils_get_error_string();
+    rcutils_reset_error();
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Could not init dynamic message type support handle map:\n%s", error_string.str);
+    *ts = NULL;
+    return ret;
+  }
+
+  return RCUTILS_RET_OK;
+}
+
+rcutils_ret_t
+rosidl_dynamic_message_type_support_handle_destroy(rosidl_message_type_support_t * ts)
+{
+  if (!ts) {
+    return RCUTILS_RET_OK;
+  }
+
   if (ts->typesupport_identifier != rosidl_dynamic_typesupport_c__identifier) {
     RCUTILS_SET_ERROR_MSG("type support not from this implementation");
     return RCUTILS_RET_INVALID_ARGUMENT;
   }
 
-  rcutils_allocator_t allocator = rcutils_get_default_allocator();
+  rosidl_dynamic_message_type_support_impl_t * ts_impl =
+    (rosidl_dynamic_message_type_support_impl_t *) ts->data;
+  rcutils_allocator_t allocator = ts_impl->allocator;
 
-  if (ts->data) {
-    rosidl_dynamic_message_type_support_impl_t * ts_impl =
-      (rosidl_dynamic_message_type_support_impl_t *)ts->data;
-    if (ts_impl->type_description) {
-      rosidl_runtime_c__type_description__TypeDescription__fini(ts_impl->type_description);
-    }
-    if (ts_impl->type_description_sources) {
-      rosidl_runtime_c__type_description__TypeSource__Sequence__fini(
-        ts_impl->type_description_sources);
-    }
-    if (ts_impl->dynamic_message_type) {
-      rosidl_dynamic_typesupport_dynamic_type_destroy(ts_impl->dynamic_message_type);
-    }
-    if (ts_impl->dynamic_message) {
-      rosidl_dynamic_typesupport_dynamic_data_destroy(ts_impl->dynamic_message);
-    }
-    allocator.deallocate(ts_impl->type_hash, allocator.state);
+  if (rosidl_dynamic_message_type_support_handle_fini(ts) != RCUTILS_RET_OK) {
+    rcutils_error_string_t error_string = rcutils_get_error_string();
+    rcutils_reset_error();
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Could not finalize dynamic message typesupport:\n%s", error_string.str);
   }
-  allocator.deallocate((void *)ts->data, allocator.state);
+
   allocator.deallocate(ts, allocator.state);
   return RCUTILS_RET_OK;
 }