Set allocator before goto fail.

Signed-off-by: Borja Outerelo <borjaouterelo@gmail.com>

rcl/src/rcl/init.c

@@ -83,6 +83,9 @@ rcl_init(
   // Zero initialize rmw context first so its validity can by checked in cleanup.
   context->impl->rmw_context = rmw_get_zero_initialized_context();
 
+  // Store the allocator.
+  context->impl->allocator = allocator;
+
   // Copy the options into the context for future reference.
   rcl_ret_t ret = rcl_init_options_copy(options, &(context->impl->init_options));
   if (RCL_RET_OK != ret) {
@@ -151,9 +154,6 @@ rcl_init(
     goto fail;
   }
 
-  // Store the allocator.
-  context->impl->allocator = allocator;
-
   TRACEPOINT(rcl_init, (const void *)context);
 
   return RCL_RET_OK;

rcl/src/rcl/init.c.orig

@@ -0,0 +1,205 @@
+// Copyright 2015 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl/init.h"
+
+#include "./arguments_impl.h"
+#include "./common.h"
+#include "./context_impl.h"
+#include "./init_options_impl.h"
+#include "rcl/arguments.h"
+#include "rcl/error_handling.h"
+#include "rcl/logging.h"
+#include "rcutils/logging_macros.h"
+#include "rcutils/stdatomic_helper.h"
+#include "rmw/error_handling.h"
+#include "tracetools/tracetools.h"
+
+static atomic_uint_least64_t __rcl_next_unique_id = ATOMIC_VAR_INIT(1);
+
+rcl_ret_t
+rcl_init(
+  int argc,
+  char const * const * argv,
+  const rcl_init_options_t * options,
+  rcl_context_t * context)
+{
+  rcl_ret_t fail_ret = RCL_RET_ERROR;
+
+  if (argc > 0) {
+    RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT);
+    for (int i = 0; i < argc; ++i) {
+      RCL_CHECK_ARGUMENT_FOR_NULL(argv[i], RCL_RET_INVALID_ARGUMENT);
+    }
+  } else {
+    if (NULL != argv) {
+      RCL_SET_ERROR_MSG("argc is <= 0, but argv is not NULL");
+      return RCL_RET_INVALID_ARGUMENT;
+    }
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options->impl, RCL_RET_INVALID_ARGUMENT);
+  rcl_allocator_t allocator = options->impl->allocator;
+  RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT);
+
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME,
+    "Initializing ROS client library, for context at address: %p", (void *) context);
+
+  // test expectation that given context is zero initialized
+  if (NULL != context->impl) {
+    // note that this can also occur when the given context is used before initialization
+    // i.e. it is declared on the stack but never defined or zero initialized
+    RCL_SET_ERROR_MSG("rcl_init called on an already initialized context");
+    return RCL_RET_ALREADY_INIT;
+  }
+
+  // Zero initialize global arguments.
+  context->global_arguments = rcl_get_zero_initialized_arguments();
+
+  // Setup impl for context.
+  // use zero_allocate so the cleanup function will not try to clean up uninitialized parts later
+  context->impl = allocator.zero_allocate(1, sizeof(rcl_context_impl_t), allocator.state);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    context->impl, "failed to allocate memory for context impl", return RCL_RET_BAD_ALLOC);
+
+  // Zero initialize rmw context first so its validity can by checked in cleanup.
+  context->impl->rmw_context = rmw_get_zero_initialized_context();
+
+  // Store the allocator.
+  context->impl->allocator = allocator;
+
+  // Copy the options into the context for future reference.
+  rcl_ret_t ret = rcl_init_options_copy(options, &(context->impl->init_options));
+  if (RCL_RET_OK != ret) {
+    fail_ret = ret;  // error message already set
+    goto fail;
+  }
+
+  // Copy the argc and argv into the context, if argc >= 0.
+  context->impl->argc = argc;
+  context->impl->argv = NULL;
+  if (0 != argc && argv != NULL) {
+    context->impl->argv = (char **)allocator.zero_allocate(argc, sizeof(char *), allocator.state);
+    RCL_CHECK_FOR_NULL_WITH_MSG(
+      context->impl->argv,
+      "failed to allocate memory for argv",
+      fail_ret = RCL_RET_BAD_ALLOC; goto fail);
+    int64_t i;
+    for (i = 0; i < argc; ++i) {
+      size_t argv_i_length = strlen(argv[i]);
+      context->impl->argv[i] = (char *)allocator.allocate(argv_i_length, allocator.state);
+      RCL_CHECK_FOR_NULL_WITH_MSG(
+        context->impl->argv[i],
+        "failed to allocate memory for string entry in argv",
+        fail_ret = RCL_RET_BAD_ALLOC; goto fail);
+      memcpy(context->impl->argv[i], argv[i], argv_i_length);
+    }
+  }
+
+  // Parse the ROS specific arguments.
+  ret = rcl_parse_arguments(argc, argv, allocator, &context->global_arguments);
+  if (RCL_RET_OK != ret) {
+    fail_ret = ret;
+    RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to parse global arguments");
+    goto fail;
+  }
+
+  ret = rcl_logging_configure(&context->global_arguments, &allocator);
+  if (RCL_RET_OK != ret) {
+    fail_ret = ret;
+    RCUTILS_LOG_ERROR_NAMED(
+      ROS_PACKAGE_NAME,
+      "Failed to configure logging: %s",
+      rcutils_get_error_string().str);
+    goto fail;
+  }
+
+  // Set the instance id.
+  uint64_t next_instance_id = rcutils_atomic_fetch_add_uint64_t(&__rcl_next_unique_id, 1);
+  if (0 == next_instance_id) {
+    // Roll over occurred, this is an extremely unlikely occurrence.
+    RCL_SET_ERROR_MSG("unique rcl instance ids exhausted");
+    // Roll back to try to avoid the next call succeeding, but there's a data race here.
+    rcutils_atomic_store(&__rcl_next_unique_id, -1);
+    goto fail;
+  }
+  rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), next_instance_id);
+  context->impl->init_options.impl->rmw_init_options.instance_id = next_instance_id;
+
+  // Initialize rmw_init.
+  rmw_ret_t rmw_ret = rmw_init(
+    &(context->impl->init_options.impl->rmw_init_options),
+    &(context->impl->rmw_context));
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    fail_ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+    goto fail;
+  }
+
+<<<<<<< HEAD
+  // Store the allocator.
+  context->impl->allocator = allocator;
+
+  TRACEPOINT(rcl_init, (const void *)context);
+
+=======
+>>>>>>> d0d08f2... Set allocator before goto fail.
+  return RCL_RET_OK;
+fail:
+  __cleanup_context(context);
+  return fail_ret;
+}
+
+rcl_ret_t
+rcl_shutdown(rcl_context_t * context)
+{
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME,
+    "Shutting down ROS client library, for context at address: %p", (void *) context);
+  RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    context->impl, "context is zero-initialized", return RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_context_is_valid(context)) {
+    RCL_SET_ERROR_MSG("rcl_shutdown already called on the given context");
+    return RCL_RET_ALREADY_SHUTDOWN;
+  }
+
+  // reset the instance id to 0 to indicate "invalid"
+  rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0);
+
+  rmw_ret_t rmw_ret = rmw_shutdown(&(context->impl->rmw_context));
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+  }
+
+  rcl_ret_t rcl_ret = rcl_logging_fini();
+  RCUTILS_LOG_ERROR_EXPRESSION_NAMED(RCL_RET_OK != rcl_ret, ROS_PACKAGE_NAME,
+    "Failed to fini logging, rcl_ret_t: %i, rcl_error_str: %s", rcl_ret,
+    rcl_get_error_string().str);
+  rcl_reset_error();
+
+  return RCL_RET_OK;
+}
+
+#ifdef __cplusplus
+}
+#endif