diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index dc71eb6b0..fa09fe77e 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -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); @@ -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); @@ -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(); @@ -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; } @@ -235,11 +235,14 @@ 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); @@ -247,20 +250,19 @@ rcl_node_init( 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( @@ -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); @@ -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; }