Skip to content

Commit

Permalink
Cleanup the error handling in rcl_node_init. (#1099)
Browse files Browse the repository at this point in the history 
* Cleanup the error handling in rcl_node_init.

Only undo things that we've done before.  Not only does
this result in less code, it also avoids spurious warnings
from trying to cleanup things that we haven't initialized.

Signed-off-by: Chris Lalancette <[email protected]>
  • Loading branch information
@clalancette
clalancette authored Sep 20, 2023
1 parent feb775a commit 7b9c1ec
Showing 1 changed file with 58 additions and 70 deletions.
128 changes: 58 additions & 70 deletions rcl/src/rcl/node.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,7 @@ rcl_node_init(
rcl_ret_t ret;
rcl_ret_t fail_ret = RCL_RET_ERROR;
char * remapped_node_name = NULL;
const char * local_namespace_ = NULL;

// Check options and allocator first, so allocator can be used for errors.
RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
Expand Down Expand Up @@ -157,7 +158,6 @@ rcl_node_init(
}

// Process the namespace.
const char * local_namespace_ = NULL;
if (namespace_[0] == '\0') {
// If the namespace is just an empty string, replace with "/"
local_namespace_ = rcutils_strdup("/", *allocator);
Expand All @@ -177,24 +177,21 @@ rcl_node_init(
ret = rmw_validate_namespace(local_namespace_, &validation_result, NULL);
if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string().str);
goto cleanup;
goto fail;
}
if (validation_result != RMW_NAMESPACE_VALID) {
const char * msg = rmw_namespace_validation_result_string(validation_result);
RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("%s, result: %d", msg, validation_result);

ret = RCL_RET_NODE_INVALID_NAMESPACE;
goto cleanup;
goto fail;
}

// Allocate space for the implementation struct.
node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state);
node->impl = (rcl_node_impl_t *)allocator->zero_allocate(
1, sizeof(rcl_node_impl_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
node->impl->rmw_node_handle = NULL;
node->impl->graph_guard_condition = NULL;
node->impl->logger_name = NULL;
node->impl->fq_name = NULL;
node->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto fail);
node->impl->options = rcl_node_get_default_options();
node->impl->registered_types_by_type_hash = rcutils_get_zero_initialized_hash_map();
node->impl->get_type_description_service = rcl_get_zero_initialized_service();
Expand All @@ -215,16 +212,19 @@ rcl_node_init(
&remapped_node_name);
if (RCL_RET_OK != ret) {
goto fail;
} else if (NULL != remapped_node_name) {
}
if (NULL != remapped_node_name) {
name = remapped_node_name;
}

char * remapped_namespace = NULL;
ret = rcl_remap_node_namespace(
&(node->impl->options.arguments), global_args, name,
*allocator, &remapped_namespace);
if (RCL_RET_OK != ret) {
goto fail;
} else if (NULL != remapped_namespace) {
}
if (NULL != remapped_namespace) {
allocator->deallocate((char *)local_namespace_, allocator->state);
local_namespace_ = remapped_namespace;
}
Expand All @@ -235,32 +235,34 @@ rcl_node_init(
} else {
node->impl->fq_name = rcutils_format_string(*allocator, "%s/%s", local_namespace_, name);
}
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->fq_name, "creating fully qualified name failed",
ret = RCL_RET_BAD_ALLOC; goto fail);

// node logger name
node->impl->logger_name = rcl_create_node_logger_name(name, local_namespace_, allocator);
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->logger_name, "creating logger name failed", goto fail);
node->impl->logger_name, "creating logger name failed", ret = RCL_RET_ERROR; goto fail);

RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Using domain ID of '%zu'", context->impl->rmw_context.actual_domain_id);

node->impl->rmw_node_handle = rmw_create_node(
&(node->context->impl->rmw_context),
name, local_namespace_);

RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->rmw_node_handle, rmw_get_error_string().str, goto fail);
node->impl->rmw_node_handle, rmw_get_error_string().str, ret = RCL_RET_ERROR; goto fail);

// graph guard condition
rmw_graph_guard_condition = rmw_node_get_graph_guard_condition(node->impl->rmw_node_handle);
RCL_CHECK_FOR_NULL_WITH_MSG(
rmw_graph_guard_condition, rmw_get_error_string().str, goto fail);
rmw_graph_guard_condition, rmw_get_error_string().str, ret = RCL_RET_ERROR; goto fail);

node->impl->graph_guard_condition = (rcl_guard_condition_t *)allocator->allocate(
sizeof(rcl_guard_condition_t), allocator->state);
RCL_CHECK_FOR_NULL_WITH_MSG(
node->impl->graph_guard_condition,
"allocating memory failed",
goto fail);
node->impl->graph_guard_condition, "allocating memory failed",
ret = RCL_RET_BAD_ALLOC; goto fail);
*node->impl->graph_guard_condition = rcl_get_zero_initialized_guard_condition();
graph_guard_condition_options.allocator = *allocator;
ret = rcl_guard_condition_init_from_rmw(
Expand All @@ -280,7 +282,7 @@ rcl_node_init(
goto fail;
}

// The initialization for the rosout publisher requires the node to be in initialized to a point
// The initialization for the rosout publisher requires the node to be initialized to a point
// that it can create new topic publishers
if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) {
ret = rcl_logging_rosout_init_publisher_for_node(node);
Expand All @@ -289,77 +291,63 @@ rcl_node_init(
goto fail;
}
}

RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized");
ret = RCL_RET_OK;
TRACETOOLS_TRACEPOINT(
rcl_node_init,
(const void *)node,
(const void *)rcl_node_get_rmw_handle(node),
rcl_node_get_name(node),
rcl_node_get_namespace(node));
goto cleanup;

allocator->deallocate(remapped_node_name, allocator->state);
allocator->deallocate((char *)local_namespace_, allocator->state);

return RCL_RET_OK;

fail:
if (node->impl) {
if (rcl_logging_rosout_enabled() &&
node->impl->options.enable_rosout &&
node->impl->logger_name)
{
ret = rcl_logging_rosout_fini_publisher_for_node(node);
RCUTILS_LOG_ERROR_EXPRESSION_NAMED(
(ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT),
ROS_PACKAGE_NAME, "Failed to fini publisher for node: %i", ret);
allocator->deallocate((char *)node->impl->logger_name, allocator->state);
}
if (node->impl->registered_types_by_type_hash.impl) {
ret = rcl_node_type_cache_fini(node);
if (NULL != node->impl->registered_types_by_type_hash.impl) {
fail_ret = rcl_node_type_cache_fini(node);
RCUTILS_LOG_ERROR_EXPRESSION_NAMED(
(ret != RCL_RET_OK),
ROS_PACKAGE_NAME, "Failed to fini type cache for node: %i", ret);
}
if (node->impl->fq_name) {
allocator->deallocate((char *)node->impl->fq_name, allocator->state);
}
if (node->impl->rmw_node_handle) {
ret = rmw_destroy_node(node->impl->rmw_node_handle);
if (ret != RMW_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME,
"failed to fini rmw node in error recovery: %s", rmw_get_error_string().str
);
}
(fail_ret != RCL_RET_OK),
ROS_PACKAGE_NAME, "Failed to fini type cache for node: %s", rcl_get_error_string().str);
}

if (node->impl->graph_guard_condition) {
ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME,
"failed to fini guard condition in error recovery: %s", rcl_get_error_string().str
);
}
fail_ret = rcl_guard_condition_fini(node->impl->graph_guard_condition);
RCUTILS_LOG_ERROR_EXPRESSION_NAMED(
fail_ret != RCL_RET_OK, ROS_PACKAGE_NAME,
"failed to fini guard condition in error recovery: %s", rcl_get_error_string().str);

allocator->deallocate(node->impl->graph_guard_condition, allocator->state);
}
if (NULL != node->impl->options.arguments.impl) {
ret = rcl_arguments_fini(&(node->impl->options.arguments));
if (ret != RCL_RET_OK) {
RCUTILS_LOG_ERROR_NAMED(
ROS_PACKAGE_NAME,
"failed to fini arguments in error recovery: %s", rcl_get_error_string().str
);
}

if (node->impl->rmw_node_handle) {
fail_ret = rmw_destroy_node(node->impl->rmw_node_handle);
RCUTILS_LOG_ERROR_EXPRESSION_NAMED(
fail_ret != RMW_RET_OK, ROS_PACKAGE_NAME,
"failed to fini rmw node in error recovery: %s", rmw_get_error_string().str);
}

allocator->deallocate((char *)node->impl->logger_name, allocator->state);

allocator->deallocate((char *)node->impl->fq_name, allocator->state);

fail_ret = rcl_node_options_fini(&(node->impl->options));
RCUTILS_LOG_ERROR_EXPRESSION_NAMED(
fail_ret != RCL_RET_OK, ROS_PACKAGE_NAME,
"failed to fini node options: %s", rcl_get_error_string().str);

allocator->deallocate(node->impl, allocator->state);
}
*node = rcl_get_zero_initialized_node();

ret = fail_ret;
// fall through from fail -> cleanup
cleanup:
allocator->deallocate(remapped_node_name, allocator->state);

allocator->deallocate((char *)local_namespace_, allocator->state);
local_namespace_ = NULL;

if (NULL != remapped_node_name) {
allocator->deallocate(remapped_node_name, allocator->state);
}
*node = rcl_get_zero_initialized_node();

return ret;
}

Expand Down

0 comments on commit 7b9c1ec

Please sign in to comment.