Skip to content

Commit

Permalink
Implement allocator refactor for type support structs
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
@methylDragon
methylDragon committed Apr 7, 2023
1 parent 8f55ada commit b988c8d
Show file tree
Hide file tree
Showing 2 changed files with 247 additions and 52 deletions.
62 changes: 57 additions & 5 deletions include/rosidl_dynamic_typesupport/dynamic_message_type_support_struct.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;

Expand All @@ -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);
Expand Down
237 changes: 190 additions & 47 deletions src/dynamic_message_type_support_struct.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 = {
// Can't do it via rcutils_get_zero_initialized_allocator() because it isn't constexpr
.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
};
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;
Expand Down Expand Up @@ -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);
Expand All @@ -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;
}
Expand Down

0 comments on commit b988c8d

Please sign in to comment.