move allocator and error handling to c utilities (#122)

* moved allocator to c_utilities

* moved error_handling to c_utilities

* refactor uses of RCL_SET_ERROR_MSG with allocator

* add missing guard condition functions and tests

* add missing timer functions

* refactor rcl_lifecycyle

* missed an instance of RCL_SET_ERROR_MSG

* fix segfaults in error cases for rcl_lifecycle

* remove extra header

* check return code of rcl_lifecycle_init_default_state_machine

rcl/CMakeLists.txt

@@ -23,7 +23,6 @@ else()
 endif()
 
 set(${PROJECT_NAME}_sources
-  src/rcl/allocator.c
   src/rcl/client.c
   src/rcl/common.c
   src/rcl/graph.c
@@ -66,6 +65,7 @@ install(
 set(rcl_lib_dir "$<TARGET_FILE_DIR:${PROJECT_NAME}>")
 
 ament_export_dependencies(ament_cmake)
+ament_export_dependencies(c_utilities)
 ament_export_dependencies(rcl_interfaces)
 ament_export_dependencies(rmw)
 ament_export_dependencies(rmw_implementation)

rcl/include/rcl/allocator.h

@@ -20,79 +20,13 @@ extern "C"
 {
 #endif
 
-#include "rcl/macros.h"
+#include "c_utilities/allocator.h"
-#include "rcl/types.h"
-#include "rcl/visibility_control.h"
 
-/// Encapsulation of an allocator.
+typedef utilities_allocator_t rcl_allocator_t;
-/**
- * The default allocator uses std::malloc(), std::free(), and std::realloc().
- * It can be obtained using rcl_get_default_allocator().
- *
- * The allocator should be trivially copyable.
- * Meaning that the struct should continue to work after being assignment
- * copied into a new struct.
- * Specifically the object pointed to by the state pointer should remain valid
- * until all uses of the allocator have been made.
- * Particular care should be taken when giving an allocator to rcl_init_* where
- * it is stored within another object and used later.
- */
-typedef struct rcl_allocator_t
-{
-  /// Allocate memory, given a size and the `state` pointer.
-  /** An error should be indicated by returning `NULL`. */
-  void * (*allocate)(size_t size, void * state);
-  /// Deallocate previously allocated memory, mimicking std::free().
-  /** Also takes the `state` pointer. */
-  void (* deallocate)(void * pointer, void * state);
-  /// Reallocate if possible, otherwise it deallocates and allocates.
-  /**
-   * Also takes the `state` pointer.
-   *
-   * If unsupported then do deallocate and then allocate.
-   * This should behave as std::realloc() does, as opposed to posix's
-   * [reallocf](https://linux.die.net/man/3/reallocf), i.e. the memory given
-   * by pointer will not be free'd automatically if std::realloc() fails.
-   * For reallocf-like behavior use rcl_reallocf().
-   * This function must be able to take an input pointer of `NULL` and succeed.
-   */
-  void * (*reallocate)(void * pointer, size_t size, void * state);
-  /// Implementation defined state storage.
-  /** This is passed as the final parameter to other allocator functions. */
-  void * state;
-} rcl_allocator_t;
 
-/// Return a properly initialized rcl_allocator_t with default values.
+#define rcl_get_default_allocator utilities_get_default_allocator
-/**
- * This defaults to:
- *
- * - allocate = wraps std::malloc()
- * - deallocate = wraps std::free()
- * - reallocate = wrapps std::realloc()
- * - state = `NULL`
- *
- * <hr>
- * Attribute          | Adherence
- * ------------------ | -------------
- * Allocates Memory   | No
- * Thread-Safe        | Yes
- * Uses Atomics       | No
- * Lock-Free          | Yes
- */
-RCL_PUBLIC
-RCL_WARN_UNUSED
-rcl_allocator_t
-rcl_get_default_allocator(void);
 
-/// Emulate the behavior of [reallocf](https://linux.die.net/man/3/reallocf).
+#define rcl_reallocf utilities_reallocf
-/**
- * This function will return `NULL` if the allocator is `NULL` or has `NULL` for
- * function pointer fields.
- */
-RCL_PUBLIC
-RCL_WARN_UNUSED
-void *
-rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator);
 
 #if __cplusplus
 }

rcl/include/rcl/error_handling.h

@@ -15,24 +15,34 @@
 #ifndef RCL__ERROR_HANDLING_H_
 #define RCL__ERROR_HANDLING_H_
 
-#include "rmw/error_handling.h"
+#include "c_utilities/error_handling.h"
 
-/* The error handling in RCL is just an alias to the error handling in RMW. */
+/// The error handling in RCL is just an alias to the error handling in c_utilities.
+/**
+ * Allocators given to functions in rcl are passed along to the error handling
+ * on a "best effort" basis.
+ * In some situations, like when NULL is passed for the allocator or something
+ * else that contains it, the allocator is not available to be passed to the
+ * RCL_SET_ERROR_MSG macro.
+ * In these cases, the default allocator rcl_get_default_allocator() is used.
+ * Since these are considered fatal errors, as opposed to errors that might
+ * occur during normal runtime, is should be okay to use the default allocator.
+ */
 
-typedef rmw_error_state_t rcl_error_state_t;
+typedef utilities_error_state_t rcl_error_state_t;
 
-#define rcl_set_error_state rmw_set_error_state
+#define rcl_set_error_state utilities_set_error_state
 
-#define RCL_SET_ERROR_MSG(msg) RMW_SET_ERROR_MSG(msg)
+#define RCL_SET_ERROR_MSG(msg, allocator) UTILITIES_SET_ERROR_MSG(msg, allocator)
 
-#define rcl_error_is_set rmw_error_is_set
+#define rcl_error_is_set utilities_error_is_set
 
-#define rcl_get_error_state rmw_get_error_state
+#define rcl_get_error_state utilities_get_error_state
 
-#define rcl_get_error_string rmw_get_error_string
+#define rcl_get_error_string utilities_get_error_string
 
-#define rcl_get_error_string_safe rmw_get_error_string_safe
+#define rcl_get_error_string_safe utilities_get_error_string_safe
 
-#define rcl_reset_error rmw_reset_error
+#define rcl_reset_error utilities_reset_error
 
 #endif  // RCL__ERROR_HANDLING_H_

rcl/include/rcl/graph.h

@@ -63,6 +63,7 @@ rcl_get_zero_initialized_topic_names_and_types(void);
  * <i>[1] implementation may need to protect the data structure with a lock</i>
  *
  * \param[in] node the handle to the node being used to query the ROS graph
+ * \param[in] allocator allocator to be used when allocating space for strings
  * \param[out] topic_names_and_types list of topic names and their types
  * \return `RCL_RET_OK` if the query was successful, or
  * \return `RCL_RET_NODE_INVALID` if the node is invalid, or
@@ -74,6 +75,7 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_get_topic_names_and_types(
   const rcl_node_t * node,
+  rcl_allocator_t allocator,
   rcl_topic_names_and_types_t * topic_names_and_types);
 
 /// Destroy a struct which was previously given to rcl_get_topic_names_and_types.
@@ -151,6 +153,7 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_get_node_names(
   const rcl_node_t * node,
+  rcl_allocator_t allocator,
   utilities_string_array_t * node_names);
 
 /// Return the number of publishers on a given topic.
@@ -166,6 +169,9 @@ rcl_get_node_names(
  * The count parameter must not be `NULL`, and must point to a valid bool.
  * The count parameter is the output for this function and will be set.
  *
+ * In the event that error handling needs to allocate memory, this function
+ * will try to use the node's allocator.
+ *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -204,6 +210,9 @@ rcl_count_publishers(
  * The count parameter must not be `NULL`, and must point to a valid bool.
  * The count parameter is the output for this function and will be set.
  *
+ * In the event that error handling needs to allocate memory, this function
+ * will try to use the node's allocator.
+ *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -244,6 +253,9 @@ rcl_count_subscribers(
  * The is_available parameter must not be `NULL`, and must point a bool variable.
  * The result of the check will be stored in the is_available parameter.
  *
+ * In the event that error handling needs to allocate memory, this function
+ * will try to use the node's allocator.
+ *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------

rcl/include/rcl/guard_condition.h

@@ -189,6 +189,31 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_trigger_guard_condition(rcl_guard_condition_t * guard_condition);
 
+/// Return the guard condition options.
+/**
+ * Returned is a pointer to the internally held rcl_guard_condition_options_t.
+ * This function can fail, and therefore return `NULL`, if the:
+ *   - guard_condition is `NULL`
+ *   - guard_condition is invalid (never called init, called fini, or invalid node)
+ *
+ * The returned pointer is made invalid if the guard condition is finalized.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] guard_condition pointer to the rcl guard_condition
+ * \return rcl guard condition options if successful, otherwise `NULL`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+const rcl_guard_condition_options_t *
+rcl_guard_condition_get_options(const rcl_guard_condition_t * guard_condition);
+
 /// Return the rmw guard condition handle.
 /**
  * The handle returned is a pointer to the internally held rmw handle.
@@ -207,7 +232,7 @@ rcl_trigger_guard_condition(rcl_guard_condition_t * guard_condition);
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
- * Allocates Memory   | Yes
+ * Allocates Memory   | No
  * Thread-Safe        | No
  * Uses Atomics       | No
  * Lock-Free          | Yes

rcl/include/rcl/timer.h

@@ -504,6 +504,28 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_timer_reset(rcl_timer_t * timer);
 
+/// Return the allocator for the timer.
+/**
+ * This function can fail, and therefore return `NULL`, if:
+ *   - timer is `NULL`
+ *   - timer has not been initialized (the implementation is invalid)
+ *
+ * The returned pointer is only valid as long as the timer object is valid.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[inout] timer handle to the timer object
+ * \return pointer to the allocator, or `NULL` if an error occurred
+ */
+const rcl_allocator_t *
+rcl_timer_get_allocator(const rcl_timer_t * timer);
+
 #if __cplusplus
 }
 #endif

rcl/package.xml

@@ -13,12 +13,15 @@
 
   <build_export_depend>rmw</build_export_depend>
 
+  <build_depend>c_utilities</build_depend>
   <build_depend>rcl_interfaces</build_depend>
   <build_depend>rosidl_generator_c</build_depend>
+  <build_export_depend>c_utilities</build_export_depend>
   <build_export_depend>rcl_interfaces</build_export_depend>
   <build_export_depend>rosidl_generator_c</build_export_depend>
 
   <exec_depend>ament_cmake</exec_depend>
+  <exec_depend>c_utilities</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <depend>rmw_implementation</depend>

rcl/src/rcl/allocator.c

@@ -1,65 +0,0 @@
-// 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.
-
-#include <stdlib.h>
-
-#include "rcl/allocator.h"
-#include "rcl/error_handling.h"
-
-static void *
-__default_allocate(size_t size, void * state)
-{
-  (void)state;  // unused
-  return malloc(size);
-}
-
-static void
-__default_deallocate(void * pointer, void * state)
-{
-  (void)state;  // unused
-  free(pointer);
-}
-
-static void *
-__default_reallocate(void * pointer, size_t size, void * state)
-{
-  (void)state;  // unused
-  return realloc(pointer, size);
-}
-
-rcl_allocator_t
-rcl_get_default_allocator()
-{
-  static rcl_allocator_t default_allocator = {
-    __default_allocate,
-    __default_deallocate,
-    __default_reallocate,
-    NULL
-  };
-  return default_allocator;
-}
-
-void *
-rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator)
-{
-  if (!allocator || !allocator->reallocate || !allocator->deallocate) {
-    RCL_SET_ERROR_MSG("invalid allocator or allocator function pointers");
-    return NULL;
-  }
-  void * new_pointer = allocator->reallocate(pointer, size, allocator->state);
-  if (!new_pointer) {
-    allocator->deallocate(pointer, allocator->state);
-  }
-  return new_pointer;
-}

rcl/src/rcl/client.c

@@ -21,6 +21,7 @@ extern "C"
 
 #include <string.h>
 
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 #include "./common.h"
@@ -49,29 +50,34 @@ rcl_client_init(
   const rcl_client_options_t * options)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
+
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  // check the options and allocator first, so the allocator can be passed to errors
-  if (!node->impl) {
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-    RCL_SET_ERROR_MSG("invalid node");
-    return RCL_RET_NODE_INVALID;
-  }
-  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
-  if (client->impl) {
-    RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized");
-    return RCL_RET_ALREADY_INIT;
-  }
   const rcl_allocator_t * allocator = &options->allocator;
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (!node->impl) {
+    RCL_SET_ERROR_MSG("invalid node", *allocator);
+    return RCL_RET_NODE_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (client->impl) {
+    RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized", *allocator);
+    return RCL_RET_ALREADY_INIT;
+  }
   // Allocate space for the implementation struct.
   client->impl = (rcl_client_impl_t *)allocator->allocate(
     sizeof(rcl_client_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    client->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+    client->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
   // Fill out implementation struct.
   // rmw handle (create rmw client)
   // TODO(wjwwood): pass along the allocator to rmw when it supports it
@@ -81,7 +87,7 @@ rcl_client_init(
     service_name,
     &options->qos);
   if (!client->impl->rmw_handle) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     goto fail;
   }
   // options
@@ -99,16 +105,16 @@ rcl_client_fini(rcl_client_t * client, rcl_node_t * node)
 {
   (void)node;
   rcl_ret_t result = RCL_RET_OK;
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (client->impl) {
+    rcl_allocator_t allocator = client->impl->options.allocator;
     rmw_ret_t ret =
       rmw_destroy_client(rcl_node_get_rmw_handle(node), client->impl->rmw_handle);
     if (ret != RMW_RET_OK) {
-      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
       result = RCL_RET_ERROR;
     }
-    rcl_allocator_t allocator = client->impl->options.allocator;
     allocator.deallocate(client->impl, allocator.state);
   }
   return result;
@@ -128,29 +134,31 @@ rcl_client_get_default_options()
 const char *
 rcl_client_get_service_name(const rcl_client_t * client)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL);
+  const rcl_client_options_t * options = rcl_client_get_options(client);
+  if (!options) {
+    return NULL;  // error already set
+  }
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    client->impl, "client is invalid", return NULL);
+    client->impl->rmw_handle, "client is invalid", return NULL, options->allocator);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    client->impl->rmw_handle, "client is invalid", return NULL);
   return client->impl->rmw_handle->service_name;
 }
 
 const rcl_client_options_t *
 rcl_client_get_options(const rcl_client_t * client)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    client->impl, "client is invalid", return NULL);
+    client->impl, "client is invalid", return NULL, rcl_get_default_allocator());
   return &client->impl->options;
 }
 
 rmw_client_t *
 rcl_client_get_rmw_handle(const rcl_client_t * client)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, NULL);
+  const rcl_client_options_t * options = rcl_client_get_options(client);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+  if (!options) {
-    client->impl, "client is invalid", return NULL);
+    return NULL;  // error already set
+  }
   return client->impl->rmw_handle;
 }
 
@@ -159,16 +167,17 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    sequence_number, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    client->impl, "client is invalid", return RCL_RET_INVALID_ARGUMENT);
+    client->impl, "client is invalid", return RCL_RET_CLIENT_INVALID, rcl_get_default_allocator());
   *sequence_number = rcl_atomic_load_int64_t(&client->impl->sequence_number);
   if (rmw_send_request(
       client->impl->rmw_handle, ros_request, sequence_number) != RMW_RET_OK)
   {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), client->impl->options.allocator);
     return RCL_RET_ERROR;
   }
   rcl_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number);
@@ -183,21 +192,22 @@ rcl_take_response(
   rmw_request_id_t * request_header,
   void * ros_response)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    client->impl, "client is invalid", return RCL_RET_INVALID_ARGUMENT);
+    client->impl, "client is invalid", return RCL_RET_CLIENT_INVALID, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT);
+    request_header, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
 
   bool taken = false;
   if (rmw_take_response(
       client->impl->rmw_handle, request_header, ros_response, &taken) != RMW_RET_OK)
   {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), client->impl->options.allocator);
     return RCL_RET_ERROR;
   }
   if (!taken) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), client->impl->options.allocator);
     return RCL_RET_CLIENT_TAKE_FAILED;
   }
   return RCL_RET_OK;

rcl/src/rcl/common.c

@@ -21,6 +21,8 @@ extern "C"
 
 #include <stdlib.h>
 
+#include "rcl/allocator.h"
+
 #if defined(WIN32)
 # define WINDOWS_ENV_BUFFER_SIZE 2048
 static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE];
@@ -29,8 +31,8 @@ static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE];
 rcl_ret_t
 rcl_impl_getenv(const char * env_name, const char ** env_value)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(env_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(env_name, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(env_value, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(env_value, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   *env_value = NULL;
 #if !defined(WIN32)
   *env_value = getenv(env_name);
@@ -41,7 +43,7 @@ rcl_impl_getenv(const char * env_name, const char ** env_value)
   size_t required_size;
   errno_t ret = getenv_s(&required_size, __env_buffer, sizeof(__env_buffer), env_name);
   if (ret != 0) {
-    RCL_SET_ERROR_MSG("value in env variable too large to read in");
+    RCL_SET_ERROR_MSG("value in env variable too large to read in", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   __env_buffer[WINDOWS_ENV_BUFFER_SIZE - 1] = '\0';

rcl/src/rcl/common.h

@@ -23,12 +23,13 @@ extern "C"
 #include "rcl/error_handling.h"
 #include "rcl/types.h"
 
-#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) \
+#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type, allocator) \
-  RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return error_return_type)
+  RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", \
+    return error_return_type, allocator)
 
-#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) \
+#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement, allocator) \
   if (!(value)) { \
-    RCL_SET_ERROR_MSG(msg); \
+    RCL_SET_ERROR_MSG(msg, allocator); \
     error_statement; \
   }
 

rcl/src/rcl/graph.c

@@ -33,23 +33,24 @@ rcl_get_zero_initialized_topic_names_and_types(void)
 rcl_ret_t
 rcl_get_topic_names_and_types(
   const rcl_node_t * node,
+  rcl_allocator_t allocator,
   rcl_topic_names_and_types_t * topic_names_and_types)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, allocator);
   if (!rcl_node_is_valid(node)) {
     return RCL_RET_NODE_INVALID;
   }
-  RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT, allocator);
   if (topic_names_and_types->topic_count != 0) {
-    RCL_SET_ERROR_MSG("topic count is not zero");
+    RCL_SET_ERROR_MSG("topic count is not zero", allocator);
     return RCL_RET_INVALID_ARGUMENT;
   }
   if (topic_names_and_types->topic_names) {
-    RCL_SET_ERROR_MSG("topic names is not null");
+    RCL_SET_ERROR_MSG("topic names is not null", allocator);
     return RCL_RET_INVALID_ARGUMENT;
   }
   if (topic_names_and_types->type_names) {
-    RCL_SET_ERROR_MSG("type names is not null");
+    RCL_SET_ERROR_MSG("type names is not null", allocator);
     return RCL_RET_INVALID_ARGUMENT;
   }
   return rmw_get_topic_names_and_types(
@@ -62,26 +63,28 @@ rcl_ret_t
 rcl_destroy_topic_names_and_types(
   rcl_topic_names_and_types_t * topic_names_and_types)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    topic_names_and_types, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   return rmw_destroy_topic_names_and_types(topic_names_and_types);
 }
 
 rcl_ret_t
 rcl_get_node_names(
   const rcl_node_t * node,
+  rcl_allocator_t allocator,
   utilities_string_array_t * node_names)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, allocator);
   if (!rcl_node_is_valid(node)) {
     return RCL_RET_NODE_INVALID;
   }
-  RCL_CHECK_ARGUMENT_FOR_NULL(node_names, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node_names, RCL_RET_INVALID_ARGUMENT, allocator);
   if (node_names->size != 0) {
-    RCL_SET_ERROR_MSG("node_names size is not zero");
+    RCL_SET_ERROR_MSG("node_names size is not zero", allocator);
     return RCL_RET_INVALID_ARGUMENT;
   }
   if (node_names->data) {
-    RCL_SET_ERROR_MSG("node_names is not null");
+    RCL_SET_ERROR_MSG("node_names is not null", allocator);
     return RCL_RET_INVALID_ARGUMENT;
   }
   return rmw_get_node_names(
@@ -95,12 +98,16 @@ rcl_count_publishers(
   const char * topic_name,
   size_t * count)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!rcl_node_is_valid(node)) {
     return RCL_RET_NODE_INVALID;
   }
-  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
-  RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;  // shouldn't happen, but error is already set if so
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
   return rmw_count_publishers(rcl_node_get_rmw_handle(node), topic_name, count);
 }
 
@@ -110,12 +117,16 @@ rcl_count_subscribers(
   const char * topic_name,
   size_t * count)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!rcl_node_is_valid(node)) {
     return RCL_RET_NODE_INVALID;
   }
-  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
-  RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;  // shouldn't happen, but error is already set if so
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
   return rmw_count_subscribers(rcl_node_get_rmw_handle(node), topic_name, count);
 }
 
@@ -125,12 +136,16 @@ rcl_service_server_is_available(
   const rcl_client_t * client,
   bool * is_available)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!rcl_node_is_valid(node)) {
     return RCL_RET_NODE_INVALID;
   }
-  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
-  RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;  // shouldn't happen, but error is already set if so
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
   return rmw_service_server_is_available(
     rcl_node_get_rmw_handle(node),
     rcl_client_get_rmw_handle(client),

rcl/src/rcl/guard_condition.c

@@ -21,6 +21,7 @@ extern "C"
 
 #include "./common.h"
 #include "rcl/rcl.h"
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 typedef struct rcl_guard_condition_impl_t
@@ -45,22 +46,30 @@ __rcl_guard_condition_init_from_rmw_impl(
   // This function will create an rmw_guard_condition if the parameter is null.
 
   // Perform argument validation.
-  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT);
   const rcl_allocator_t * allocator = &options.allocator;
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT, *allocator);
   // Ensure the guard_condition handle is zero initialized.
   if (guard_condition->impl) {
-    RCL_SET_ERROR_MSG("guard_condition already initialized, or memory was unintialized");
+    RCL_SET_ERROR_MSG(
+      "guard_condition already initialized, or memory was unintialized", *allocator);
     return RCL_RET_ALREADY_INIT;
   }
+  // Make sure rcl has been initialized.
+  if (!rcl_ok()) {
+    RCL_SET_ERROR_MSG("rcl_init() has not been called", *allocator);
+    return RCL_RET_NOT_INIT;
+  }
   // Allocate space for the guard condition impl.
   guard_condition->impl = (rcl_guard_condition_impl_t *)allocator->allocate(
     sizeof(rcl_guard_condition_impl_t), allocator->state);
   if (!guard_condition->impl) {
-    RCL_SET_ERROR_MSG("allocating memory failed");
+    RCL_SET_ERROR_MSG("allocating memory failed", *allocator);
     return RCL_RET_BAD_ALLOC;
   }
   // Create the rmw guard condition.
@@ -73,7 +82,7 @@ __rcl_guard_condition_init_from_rmw_impl(
     if (!guard_condition->impl->rmw_handle) {
       // Deallocate impl and exit.
       allocator->deallocate(guard_condition->impl, allocator->state);
-      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
       return RCL_RET_ERROR;
     }
   }
@@ -104,22 +113,20 @@ rcl_ret_t
 rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition)
 {
   // Perform argument validation.
-  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    guard_condition, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   rcl_ret_t result = RCL_RET_OK;
   if (guard_condition->impl) {
+    // assuming the allocator is valid because it is checked in rcl_guard_condition_init()
+    rcl_allocator_t allocator = guard_condition->impl->options.allocator;
     if (guard_condition->impl->rmw_handle) {
       if (rmw_destroy_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) {
-        RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+        RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
         result = RCL_RET_ERROR;
       }
     }
-    rcl_allocator_t allocator = guard_condition->impl->options.allocator;
+    allocator.deallocate(guard_condition->impl, allocator.state);
-    if (allocator.deallocate) {
+    guard_condition->impl = NULL;
-      allocator.deallocate(guard_condition->impl, allocator.state);
-    } else {
-      RCL_SET_ERROR_MSG("deallocate not set");
-      result = RCL_RET_ERROR;
-    }
   }
   return result;
 }
@@ -136,26 +143,38 @@ rcl_guard_condition_get_default_options()
 rcl_ret_t
 rcl_trigger_guard_condition(rcl_guard_condition_t * guard_condition)
 {
-  // Perform argument validation.
+  const rcl_guard_condition_options_t * options = rcl_guard_condition_get_options(guard_condition);
-  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT);
+  if (!options) {
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+    return RCL_RET_INVALID_ARGUMENT;  // error already set
-    guard_condition->impl,
+  }
-    "guard condition implementation is invalid",
-    return RCL_RET_INVALID_ARGUMENT);
   // Trigger the guard condition.
   if (rmw_trigger_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
   return RCL_RET_OK;
 }
 
+const rcl_guard_condition_options_t *
+rcl_guard_condition_get_options(const rcl_guard_condition_t * guard_condition)
+{
+  // Perform argument validation.
+  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL, rcl_get_default_allocator());
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    guard_condition->impl,
+    "guard condition implementation is invalid",
+    return NULL,
+    rcl_get_default_allocator());
+  return &guard_condition->impl->options;
+}
+
 rmw_guard_condition_t *
 rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL);
+  const rcl_guard_condition_options_t * options = rcl_guard_condition_get_options(guard_condition);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+  if (!options) {
-    guard_condition->impl, "guard condition implementation is invalid", return NULL);
+    return NULL;  // error already set
+  }
   return guard_condition->impl->rmw_handle;
 }
 

rcl/src/rcl/node.c

@@ -25,6 +25,7 @@ extern "C"
 #include <string.h>
 
 #include "rcl/rcl.h"
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "rmw/validate_namespace.h"
 #include "rmw/validate_node_name.h"
@@ -69,29 +70,34 @@ rcl_node_init(
     rcl_guard_condition_get_default_options();
   rcl_ret_t ret;
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT);
+
-  RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT);
+  // Check options and allocator first, so allocator can be used for errors.
-  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = &options->allocator;
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
   if (node->impl) {
-    RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized");
+    RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized", *allocator);
     return RCL_RET_ALREADY_INIT;
   }
   // Make sure rcl has been initialized.
   if (!rcl_ok()) {
-    RCL_SET_ERROR_MSG("rcl_init() has not been called");
+    RCL_SET_ERROR_MSG("rcl_init() has not been called", *allocator);
     return RCL_RET_NOT_INIT;
   }
-  const rcl_allocator_t * allocator = &options->allocator;
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
   // Make sure the node name is valid before allocating memory.
   int validation_result = 0;
   ret = rmw_validate_node_name(name, &validation_result, NULL);
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     return ret;
   }
   if (validation_result != RMW_NODE_NAME_VALID) {
@@ -99,7 +105,7 @@ rcl_node_init(
     if (!msg) {
       msg = "unknown validation_result, this should not happen";
     }
-    RCL_SET_ERROR_MSG(msg);
+    RCL_SET_ERROR_MSG(msg, *allocator);
     return RCL_RET_NODE_INVALID_NAME;
   }
 
@@ -117,7 +123,8 @@ rcl_node_init(
     // TODO(wjwwood): replace with generic strcat that takes an allocator once available
     // length + 2, because new leading / and terminating \0
     char * temp = (char *)allocator->allocate(namespace_length + 2, allocator->state);
-    RCL_CHECK_FOR_NULL_WITH_MSG(temp, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+    RCL_CHECK_FOR_NULL_WITH_MSG(
+      temp, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
     temp[0] = '/';
     memcpy(temp + 1, namespace_, strlen(namespace_) + 1);
     local_namespace_ = temp;
@@ -127,7 +134,7 @@ rcl_node_init(
   validation_result = 0;
   ret = rmw_validate_namespace(local_namespace_, &validation_result, NULL);
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     return ret;
   }
   if (validation_result != RMW_NAMESPACE_VALID) {
@@ -137,16 +144,17 @@ rcl_node_init(
       LOCAL_SNPRINTF(
         fixed_msg, sizeof(fixed_msg),
         "unknown validation_result '%d', this should not happen", validation_result);
-      RCL_SET_ERROR_MSG(fixed_msg);
+      RCL_SET_ERROR_MSG(fixed_msg, *allocator);
     } else {
-      RCL_SET_ERROR_MSG(msg);
+      RCL_SET_ERROR_MSG(msg, *allocator);
     }
     return RCL_RET_NODE_INVALID_NAMESPACE;
   }
 
   // Allocate space for the implementation struct.
   node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state);
-  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    node->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
   node->impl->rmw_node_handle = NULL;
   node->impl->graph_guard_condition = NULL;
   // Initialize node impl.
@@ -162,7 +170,7 @@ rcl_node_init(
     if (ros_domain_id) {
       unsigned long number = strtoul(ros_domain_id, NULL, 0);  // NOLINT(runtime/int)
       if (number == ULONG_MAX) {
-        RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number");
+        RCL_SET_ERROR_MSG("failed to interpret ROS_DOMAIN_ID as integral number", *allocator);
         goto fail;
       }
       domain_id = (size_t)number;
@@ -174,7 +182,7 @@ rcl_node_init(
   node->impl->actual_domain_id = domain_id;
   node->impl->rmw_node_handle = rmw_create_node(name, local_namespace_, domain_id);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail);
+    node->impl->rmw_node_handle, rmw_get_error_string_safe(), goto fail, *allocator);
   // free local_namespace_ if necessary
   if (should_free_local_namespace_) {
     allocator->deallocate((char *)local_namespace_, allocator->state);
@@ -184,7 +192,7 @@ rcl_node_init(
   // graph guard condition
   rmw_graph_guard_condition = rmw_node_get_graph_guard_condition(node->impl->rmw_node_handle);
   if (!rmw_graph_guard_condition) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     goto fail;
   }
   node->impl->graph_guard_condition = (rcl_guard_condition_t *)allocator->allocate(
@@ -192,7 +200,8 @@ rcl_node_init(
   RCL_CHECK_FOR_NULL_WITH_MSG(
     node->impl->graph_guard_condition,
     "allocating memory failed",
-    goto fail
+    goto fail,
+    *allocator
   );
   *node->impl->graph_guard_condition = rcl_get_zero_initialized_guard_condition();
   graph_guard_condition_options.allocator = *allocator;
@@ -233,20 +242,19 @@ fail:
 rcl_ret_t
 rcl_node_fini(rcl_node_t * node)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!node->impl) {
     // Repeat calls to fini or calling fini on a zero initialized node is ok.
     return RCL_RET_OK;
   }
+  rcl_allocator_t allocator = node->impl->options.allocator;
   rcl_ret_t result = RCL_RET_OK;
   rmw_ret_t ret = rmw_destroy_node(node->impl->rmw_node_handle);
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
     result = RCL_RET_ERROR;
   }
-  rcl_allocator_t allocator = node->impl->options.allocator;
+  // assuming that allocate and deallocate are ok since they are checked in init
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
   allocator.deallocate(node->impl, allocator.state);
   node->impl = NULL;
   return result;
@@ -255,10 +263,12 @@ rcl_node_fini(rcl_node_t * node)
 bool
 rcl_node_is_valid(const rcl_node_t * node)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, false);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, false, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "rcl node implementation is invalid", return false);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    node->impl, "rcl node implementation is invalid", return false, rcl_get_default_allocator());
   if (node->impl->rcl_instance_id != rcl_get_instance_id()) {
-    RCL_SET_ERROR_MSG("rcl node is invalid, rcl instance id does not match");
+    RCL_SET_ERROR_MSG(
+      "rcl node is invalid, rcl instance id does not match", rcl_get_default_allocator());
     return false;
   }
   return true;
@@ -306,11 +316,12 @@ rcl_node_get_options(const rcl_node_t * node)
 rcl_ret_t
 rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT);
+  const rcl_node_options_t * node_options = rcl_node_get_options(node);
-  if (!rcl_node_is_valid(node)) {
+  if (!node_options) {
     return RCL_RET_NODE_INVALID;
   }
+  RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT, node_options->allocator);
   *domain_id = node->impl->actual_domain_id;
   return RCL_RET_OK;
 }
@@ -327,8 +338,9 @@ rcl_node_get_rmw_handle(const rcl_node_t * node)
 uint64_t
 rcl_node_get_rcl_instance_id(const rcl_node_t * node)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, 0);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, 0, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return 0);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    node->impl, "node implementation is invalid", return 0, rcl_get_default_allocator());
   return node->impl->rcl_instance_id;
 }
 

rcl/src/rcl/publisher.c

@@ -22,6 +22,7 @@ extern "C"
 #include <string.h>
 
 #include "./common.h"
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 typedef struct rcl_publisher_impl_t
@@ -46,29 +47,35 @@ rcl_publisher_init(
   const rcl_publisher_options_t * options)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  // Check options and allocator first, so allocator can be used with errors.
-  if (!node->impl) {
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-    RCL_SET_ERROR_MSG("invalid node");
-    return RCL_RET_NODE_INVALID;
-  }
-  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
-  if (publisher->impl) {
-    RCL_SET_ERROR_MSG("publisher already initialized, or memory was unintialized");
-    return RCL_RET_ALREADY_INIT;
-  }
   const rcl_allocator_t * allocator = &options->allocator;
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (publisher->impl) {
+    RCL_SET_ERROR_MSG(
+      "publisher already initialized, or memory was unintialized", rcl_get_default_allocator());
+    return RCL_RET_ALREADY_INIT;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (!node->impl) {
+    RCL_SET_ERROR_MSG("invalid node", *allocator);
+    return RCL_RET_NODE_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator);
   // Allocate space for the implementation struct.
   publisher->impl = (rcl_publisher_impl_t *)allocator->allocate(
     sizeof(rcl_publisher_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    publisher->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+    publisher->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
   // Fill out implementation struct.
   // rmw handle (create rmw publisher)
   // TODO(wjwwood): pass along the allocator to rmw when it supports it
@@ -78,7 +85,7 @@ rcl_publisher_init(
     topic_name,
     &(options->qos));
   if (!publisher->impl->rmw_handle) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     goto fail;
   }
   // options
@@ -95,16 +102,16 @@ rcl_ret_t
 rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
 {
   rcl_ret_t result = RCL_RET_OK;
-  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (publisher->impl) {
+    rcl_allocator_t allocator = publisher->impl->options.allocator;
     rmw_ret_t ret =
       rmw_destroy_publisher(rcl_node_get_rmw_handle(node), publisher->impl->rmw_handle);
     if (ret != RMW_RET_OK) {
-      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
       result = RCL_RET_ERROR;
     }
-    rcl_allocator_t allocator = publisher->impl->options.allocator;
     allocator.deallocate(publisher->impl, allocator.state);
   }
   return result;
@@ -124,12 +131,16 @@ rcl_publisher_get_default_options()
 rcl_ret_t
 rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
+  const rcl_publisher_options_t * options = rcl_publisher_get_options(publisher);
+  if (!options) {
+    return RCL_RET_PUBLISHER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT, options->allocator);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID);
+    publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID, options->allocator);
   if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
   return RCL_RET_OK;
@@ -138,29 +149,32 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
 const char *
 rcl_publisher_get_topic_name(const rcl_publisher_t * publisher)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  const rcl_publisher_options_t * options = rcl_publisher_get_options(publisher);
+  if (!options) {
+    return NULL;
+  }
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    publisher->impl, "publisher is invalid", return NULL);
+    publisher->impl, "publisher is invalid", return NULL, options->allocator);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    publisher->impl->rmw_handle, "publisher is invalid", return NULL);
+    publisher->impl->rmw_handle, "publisher is invalid", return NULL, options->allocator);
   return publisher->impl->rmw_handle->topic_name;
 }
 
 const rcl_publisher_options_t *
 rcl_publisher_get_options(const rcl_publisher_t * publisher)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    publisher->impl, "publisher is invalid", return NULL);
+    publisher->impl, "publisher is invalid", return NULL, rcl_get_default_allocator());
   return &publisher->impl->options;
 }
 
 rmw_publisher_t *
 rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    publisher->impl, "publisher is invalid", return NULL);
+    publisher->impl, "publisher is invalid", return NULL, rcl_get_default_allocator());
   return publisher->impl->rmw_handle;
 }
 

rcl/src/rcl/rcl.c

@@ -24,6 +24,7 @@ extern "C"
 #include "./common.h"
 #include "./stdatomic_helper.h"
 #include "rcl/error_handling.h"
+#include "rmw/error_handling.h"
 
 static atomic_bool __rcl_is_initialized = ATOMIC_VAR_INIT(false);
 static rcl_allocator_t __rcl_allocator;
@@ -56,17 +57,22 @@ rcl_ret_t
 rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  if (argc > 0) {
+
-    RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT);
+  // Check allocator first, so it can be used in other errors.
-  }
   RCL_CHECK_FOR_NULL_WITH_MSG(
     allocator.allocate,
-    "invalid allocator, allocate not set", return RCL_RET_INVALID_ARGUMENT);
+    "invalid allocator, allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
     allocator.deallocate,
-    "invalid allocator, deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+    "invalid allocator, deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  if (argc > 0) {
+    RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT, allocator);
+  }
   if (rcl_atomic_exchange_bool(&__rcl_is_initialized, true)) {
-    RCL_SET_ERROR_MSG("rcl_init called while already initialized");
+    RCL_SET_ERROR_MSG("rcl_init called while already initialized", allocator);
     return RCL_RET_ALREADY_INIT;
   }
   // There is a race condition between the time __rcl_is_initialized is set true,
@@ -78,7 +84,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
   // Initialize rmw_init.
   rmw_ret_t rmw_ret = rmw_init();
   if (rmw_ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
     fail_ret = RCL_RET_ERROR;
     goto fail;
   }
@@ -87,7 +93,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
   __rcl_argc = argc;
   __rcl_argv = (char **)__rcl_allocator.allocate(sizeof(char *) * argc, __rcl_allocator.state);
   if (!__rcl_argv) {
-    RCL_SET_ERROR_MSG("allocation failed");
+    RCL_SET_ERROR_MSG("allocation failed", allocator);
     fail_ret = RCL_RET_BAD_ALLOC;
     goto fail;
   }
@@ -96,7 +102,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
   for (i = 0; i < argc; ++i) {
     __rcl_argv[i] = (char *)__rcl_allocator.allocate(strlen(argv[i]), __rcl_allocator.state);
     if (!__rcl_argv[i]) {
-      RCL_SET_ERROR_MSG("allocation failed");
+      RCL_SET_ERROR_MSG("allocation failed", allocator);
       fail_ret = RCL_RET_BAD_ALLOC;
       goto fail;
     }
@@ -106,7 +112,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator)
   if (rcl_atomic_load_uint64_t(&__rcl_instance_id) == 0) {
     // Roll over occurred.
     __rcl_next_unique_id--;  // roll back to avoid the next call succeeding.
-    RCL_SET_ERROR_MSG("unique rcl instance ids exhausted");
+    RCL_SET_ERROR_MSG("unique rcl instance ids exhausted", allocator);
     goto fail;
   }
   return RCL_RET_OK;
@@ -119,7 +125,8 @@ rcl_ret_t
 rcl_shutdown()
 {
   if (!rcl_ok()) {
-    RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init");
+    // must use default allocator here because __rcl_allocator may not be set yet
+    RCL_SET_ERROR_MSG("rcl_shutdown called before rcl_init", rcl_get_default_allocator());
     return RCL_RET_NOT_INIT;
   }
   __clean_up_init();

rcl/src/rcl/service.c

@@ -23,6 +23,7 @@ extern "C"
 #include <string.h>
 
 #include "./common.h"
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 typedef struct rcl_service_impl_t
@@ -47,29 +48,34 @@ rcl_service_init(
   const rcl_service_options_t * options)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
+
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  // Check options and allocator first, so the allocator can be used in errors.
-  if (!node->impl) {
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-    RCL_SET_ERROR_MSG("invalid node");
-    return RCL_RET_NODE_INVALID;
-  }
-  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
-  if (service->impl) {
-    RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized");
-    return RCL_RET_ALREADY_INIT;
-  }
   const rcl_allocator_t * allocator = &options->allocator;
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (!node->impl) {
+    RCL_SET_ERROR_MSG("invalid node", *allocator);
+    return RCL_RET_NODE_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (service->impl) {
+    RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized", *allocator);
+    return RCL_RET_ALREADY_INIT;
+  }
   // Allocate space for the implementation struct.
   service->impl = (rcl_service_impl_t *)allocator->allocate(
     sizeof(rcl_service_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+    service->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
 
   if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) {
     fprintf(stderr, "Warning: Setting QoS durability to 'transient local' for service servers "
@@ -84,7 +90,7 @@ rcl_service_init(
     service_name,
     &options->qos);
   if (!service->impl->rmw_handle) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     goto fail;
   }
   // options
@@ -101,16 +107,16 @@ rcl_ret_t
 rcl_service_fini(rcl_service_t * service, rcl_node_t * node)
 {
   rcl_ret_t result = RCL_RET_OK;
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (service->impl) {
+    rcl_allocator_t allocator = service->impl->options.allocator;
     rmw_ret_t ret =
       rmw_destroy_service(rcl_node_get_rmw_handle(node), service->impl->rmw_handle);
     if (ret != RMW_RET_OK) {
-      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
       result = RCL_RET_ERROR;
     }
-    rcl_allocator_t allocator = service->impl->options.allocator;
     allocator.deallocate(service->impl, allocator.state);
   }
   return result;
@@ -130,29 +136,30 @@ rcl_service_get_default_options()
 const char *
 rcl_service_get_service_name(const rcl_service_t * service)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL);
+  const rcl_service_options_t * options = rcl_service_get_options(service);
+  if (!options) {
+    return NULL;
+  }
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    service->impl, "service is invalid", return NULL);
+    service->impl->rmw_handle, "service is invalid", return NULL, options->allocator);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    service->impl->rmw_handle, "service is invalid", return NULL);
   return service->impl->rmw_handle->service_name;
 }
 
 const rcl_service_options_t *
 rcl_service_get_options(const rcl_service_t * service)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    service->impl, "service is invalid", return NULL);
+    service->impl, "service is invalid", return NULL, rcl_get_default_allocator());
   return &service->impl->options;
 }
 
 rmw_service_t *
 rcl_service_get_rmw_handle(const rcl_service_t * service)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    service->impl, "service is invalid", return NULL);
+    service->impl, "service is invalid", return NULL, rcl_get_default_allocator());
   return service->impl->rmw_handle;
 }
 
@@ -162,17 +169,19 @@ rcl_take_request(
   rmw_request_id_t * request_header,
   void * ros_request)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+  const rcl_service_options_t * options = rcl_service_get_options(service);
-    service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT);
+  if (!options) {
-  RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT);
+    return RCL_RET_SERVICE_INVALID;
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT);
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT, options->allocator);
 
   bool taken = false;
   if (rmw_take_request(
       service->impl->rmw_handle, request_header, ros_request, &taken) != RMW_RET_OK)
   {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
   if (!taken) {
@@ -187,16 +196,18 @@ rcl_send_response(
   rmw_request_id_t * request_header,
   void * ros_response)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+  const rcl_service_options_t * options = rcl_service_get_options(service);
-    service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT);
+  if (!options) {
-  RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT);
+    return RCL_RET_SERVICE_INVALID;
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT);
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT, options->allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT, options->allocator);
 
   if (rmw_send_response(
       service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK)
   {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
 

rcl/src/rcl/subscription.c

@@ -19,6 +19,7 @@ extern "C"
 
 #include "rcl/subscription.h"
 
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "./common.h"
 
@@ -44,25 +45,30 @@ rcl_subscription_init(
   const rcl_subscription_options_t * options)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
+
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  // Check options and allocator first, so the allocator can be used in errors.
-  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
-  if (subscription->impl) {
-    RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized");
-    return RCL_RET_ALREADY_INIT;
-  }
   const rcl_allocator_t * allocator = &options->allocator;
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+    allocator->deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT, *allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT, *allocator);
+  if (subscription->impl) {
+    RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized", *allocator);
+    return RCL_RET_ALREADY_INIT;
+  }
   // Allocate memory for the implementation struct.
   subscription->impl = (rcl_subscription_impl_t *)allocator->allocate(
     sizeof(rcl_subscription_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+    subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, *allocator);
   // Fill out the implemenation struct.
   // rmw_handle
   // TODO(wjwwood): pass allocator once supported in rmw api.
@@ -73,7 +79,7 @@ rcl_subscription_init(
     &(options->qos),
     options->ignore_local_publications);
   if (!subscription->impl->rmw_handle) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), *allocator);
     goto fail;
   }
   // options
@@ -90,16 +96,16 @@ rcl_ret_t
 rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
 {
   rcl_ret_t result = RCL_RET_OK;
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (subscription->impl) {
+    rcl_allocator_t allocator = subscription->impl->options.allocator;
     rmw_ret_t ret =
       rmw_destroy_subscription(rcl_node_get_rmw_handle(node), subscription->impl->rmw_handle);
     if (ret != RMW_RET_OK) {
-      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), allocator);
       result = RCL_RET_ERROR;
     }
-    rcl_allocator_t allocator = subscription->impl->options.allocator;
     allocator.deallocate(subscription->impl, allocator.state);
   }
   return result;
@@ -124,13 +130,18 @@ rcl_take(
   void * ros_message,
   rmw_message_info_t * message_info)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
+  const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription);
+  if (!options) {
+    return RCL_RET_SUBSCRIPTION_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT, options->allocator);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    subscription->impl, "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID);
+    subscription->impl, "subscription is invalid",
+    return RCL_RET_SUBSCRIPTION_INVALID, options->allocator);
   RCL_CHECK_FOR_NULL_WITH_MSG(
     subscription->impl->rmw_handle,
-    "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID);
+    "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID, options->allocator);
   // If message_info is NULL, use a place holder which can be discarded.
   rmw_message_info_t dummy_message_info;
   rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
@@ -139,7 +150,7 @@ rcl_take(
   rmw_ret_t ret =
     rmw_take_with_info(subscription->impl->rmw_handle, ros_message, &taken, message_info_local);
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
     return RCL_RET_ERROR;
   }
   if (!taken) {
@@ -151,30 +162,31 @@ rcl_take(
 const char *
 rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
+  const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
+  if (!options) {
-    subscription->impl, "subscription is invalid", return NULL);
+    return NULL;
+  }
   RCL_CHECK_FOR_NULL_WITH_MSG(
     subscription->impl->rmw_handle,
-    "subscription is invalid", return NULL);
+    "subscription is invalid", return NULL, options->allocator);
   return subscription->impl->rmw_handle->topic_name;
 }
 
 const rcl_subscription_options_t *
 rcl_subscription_get_options(const rcl_subscription_t * subscription)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    subscription->impl, "subscription is invalid", return NULL);
+    subscription->impl, "subscription is invalid", return NULL, rcl_get_default_allocator());
   return &subscription->impl->options;
 }
 
 rmw_subscription_t *
 rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL, rcl_get_default_allocator());
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    subscription->impl, "subscription is invalid", return NULL);
+    subscription->impl, "subscription is invalid", return NULL, rcl_get_default_allocator());
   return subscription->impl->rmw_handle;
 }
 

rcl/src/rcl/time.c

@@ -19,6 +19,7 @@
 
 #include "./common.h"
 #include "./stdatomic_helper.h"
+#include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 
 // Process default ROS time sources
@@ -91,9 +92,10 @@ rcl_init_time_source(
   enum rcl_time_source_type_t time_source_type, rcl_time_source_t * time_source
 )
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
   switch (time_source_type) {
     case RCL_TIME_SOURCE_UNINITIALIZED:
+      RCL_CHECK_ARGUMENT_FOR_NULL(
+        time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
       rcl_init_generic_time_source(time_source);
       return RCL_RET_OK;
     case RCL_ROS_TIME:
@@ -110,7 +112,6 @@ rcl_init_time_source(
 rcl_ret_t
 rcl_fini_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
   switch (time_source->type) {
     case RCL_ROS_TIME:
       return rcl_fini_ros_time_source(time_source);
@@ -128,7 +129,7 @@ rcl_fini_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_init_ros_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   rcl_init_generic_time_source(time_source);
   time_source->data = calloc(1, sizeof(rcl_ros_time_source_storage_t));
   time_source->get_now = rcl_get_ros_time;
@@ -139,9 +140,9 @@ rcl_init_ros_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_fini_ros_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_ROS_TIME) {
-    RCL_SET_ERROR_MSG("time_source not of type RCL_ROS_TIME");
+    RCL_SET_ERROR_MSG("time_source not of type RCL_ROS_TIME", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   free((rcl_ros_time_source_storage_t *)time_source->data);
@@ -151,7 +152,7 @@ rcl_fini_ros_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_init_steady_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   rcl_init_generic_time_source(time_source);
   time_source->get_now = rcl_get_steady_time;
   time_source->type = RCL_STEADY_TIME;
@@ -161,9 +162,9 @@ rcl_init_steady_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_fini_steady_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_STEADY_TIME) {
-    RCL_SET_ERROR_MSG("time_source not of type RCL_STEADY_TIME");
+    RCL_SET_ERROR_MSG("time_source not of type RCL_STEADY_TIME", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   return RCL_RET_OK;
@@ -172,7 +173,7 @@ rcl_fini_steady_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_init_system_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   rcl_init_generic_time_source(time_source);
   time_source->get_now = rcl_get_system_time;
   time_source->type = RCL_SYSTEM_TIME;
@@ -182,9 +183,9 @@ rcl_init_system_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_fini_system_time_source(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_SYSTEM_TIME) {
-    RCL_SET_ERROR_MSG("time_source not of type RCL_SYSTEM_TIME");
+    RCL_SET_ERROR_MSG("time_source not of type RCL_SYSTEM_TIME", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   return RCL_RET_OK;
@@ -193,7 +194,7 @@ rcl_fini_system_time_source(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!time_source) {
     time_point->time_source = rcl_get_default_ros_time_source();
     return RCL_RET_OK;
@@ -206,7 +207,7 @@ rcl_init_time_point(rcl_time_point_t * time_point, rcl_time_source_t * time_sour
 rcl_ret_t
 rcl_fini_time_point(rcl_time_point_t * time_point)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   (void)time_point;
   return RCL_RET_OK;
 }
@@ -214,7 +215,7 @@ rcl_fini_time_point(rcl_time_point_t * time_point)
 rcl_ret_t
 rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!time_source) {
     duration->time_source = rcl_get_default_ros_time_source();
     return RCL_RET_OK;
@@ -227,7 +228,7 @@ rcl_init_duration(rcl_duration_t * duration, rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_fini_duration(rcl_duration_t * duration)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(duration, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   (void)duration;
   return RCL_RET_OK;
 }
@@ -274,7 +275,8 @@ rcl_get_default_system_time_source(void)
 rcl_ret_t
 rcl_set_default_ros_time_source(rcl_time_source_t * process_time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(process_time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    process_time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (rcl_default_ros_time_source) {
     free(rcl_default_ros_time_source);
   }
@@ -287,7 +289,9 @@ rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish,
   rcl_duration_t * delta)
 {
   if (start->time_source->type != finish->time_source->type) {
-    RCL_SET_ERROR_MSG("Cannot difference between time points with time_sources types.");
+    RCL_SET_ERROR_MSG(
+      "Cannot difference between time points with time_sources types.",
+      rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   if (finish->nanoseconds < start->nanoseconds) {
@@ -301,27 +305,30 @@ rcl_difference_times(rcl_time_point_t * start, rcl_time_point_t * finish,
 rcl_ret_t
 rcl_get_time_point_now(rcl_time_point_t * time_point)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_point->time_source && time_point->time_source->get_now) {
     return time_point->time_source->get_now(time_point->time_source->data,
              &(time_point->nanoseconds));
   }
-  RCL_SET_ERROR_MSG("time_source is not initialized or does not have get_now registered.");
+  RCL_SET_ERROR_MSG(
+    "time_source is not initialized or does not have get_now registered.",
+    rcl_get_default_allocator());
   return RCL_RET_ERROR;
 }
 
 rcl_ret_t
 rcl_enable_ros_time_override(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_ROS_TIME) {
-    RCL_SET_ERROR_MSG("Time source is not RCL_ROS_TIME cannot enable override.")
+    RCL_SET_ERROR_MSG(
+      "Time source is not RCL_ROS_TIME cannot enable override.", rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
   rcl_ros_time_source_storage_t * storage = \
     (rcl_ros_time_source_storage_t *)time_source->data;
   if (!storage) {
-    RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.")
+    RCL_SET_ERROR_MSG("Storage not initialized, cannot enable.", rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
   storage->active = true;
@@ -331,14 +338,14 @@ rcl_enable_ros_time_override(rcl_time_source_t * time_source)
 rcl_ret_t
 rcl_disable_ros_time_override(rcl_time_source_t * time_source)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_ROS_TIME) {
     return RCL_RET_ERROR;
   }
   rcl_ros_time_source_storage_t * storage = \
     (rcl_ros_time_source_storage_t *)time_source->data;
   if (!storage) {
-    RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.")
+    RCL_SET_ERROR_MSG("Storage not initialized, cannot disable.", rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
   storage->active = false;
@@ -350,15 +357,15 @@ rcl_is_enabled_ros_time_override(
   rcl_time_source_t * time_source,
   bool * is_enabled)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(is_enabled, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_enabled, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_ROS_TIME) {
     return RCL_RET_ERROR;
   }
   rcl_ros_time_source_storage_t * storage = \
     (rcl_ros_time_source_storage_t *)time_source->data;
   if (!storage) {
-    RCL_SET_ERROR_MSG("Storage not initialized, cannot query.")
+    RCL_SET_ERROR_MSG("Storage not initialized, cannot query.", rcl_get_default_allocator())
     return RCL_RET_ERROR;
   }
   *is_enabled = storage->active;
@@ -370,7 +377,7 @@ rcl_set_ros_time_override(
   rcl_time_source_t * time_source,
   rcl_time_point_value_t time_value)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_source, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (time_source->type != RCL_ROS_TIME) {
     return RCL_RET_ERROR;
   }

rcl/src/rcl/time_unix.c

@@ -32,6 +32,7 @@ extern "C"
 #include <unistd.h>
 
 #include "./common.h"
+#include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 
 #if !defined(__MACH__)  // Assume clock_get_time is available on OS X.
@@ -47,7 +48,7 @@ extern "C"
 rcl_ret_t
 rcl_system_time_now(rcl_time_point_value_t * now)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   struct timespec timespec_now;
 #if defined(__MACH__)
   // On OS X use clock_get_time.
@@ -63,7 +64,7 @@ rcl_system_time_now(rcl_time_point_value_t * now)
   clock_gettime(CLOCK_REALTIME, &timespec_now);
 #endif  // defined(__MACH__)
   if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) {
-    RCL_SET_ERROR_MSG("unexpected negative time");
+    RCL_SET_ERROR_MSG("unexpected negative time", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   *now = RCL_S_TO_NS((uint64_t)timespec_now.tv_sec) + timespec_now.tv_nsec;
@@ -73,7 +74,7 @@ rcl_system_time_now(rcl_time_point_value_t * now)
 rcl_ret_t
 rcl_steady_time_now(rcl_time_point_value_t * now)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   // If clock_gettime is available or on OS X, use a timespec.
   struct timespec timespec_now;
 #if defined(__MACH__)
@@ -94,7 +95,7 @@ rcl_steady_time_now(rcl_time_point_value_t * now)
 #endif  // defined(CLOCK_MONOTONIC_RAW)
 #endif  // defined(__MACH__)
   if (__WOULD_BE_NEGATIVE(timespec_now.tv_sec, timespec_now.tv_nsec)) {
-    RCL_SET_ERROR_MSG("unexpected negative time");
+    RCL_SET_ERROR_MSG("unexpected negative time", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   *now = RCL_S_TO_NS((uint64_t)timespec_now.tv_sec) + timespec_now.tv_nsec;

rcl/src/rcl/time_win32.c

@@ -27,12 +27,13 @@ extern "C"
 
 #include "./common.h"
 #include "./stdatomic_helper.h"
+#include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 
 rcl_ret_t
 rcl_system_time_now(rcl_time_point_value_t * now)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   FILETIME ft;
   GetSystemTimeAsFileTime(&ft);
   ULARGE_INTEGER uli;
@@ -49,7 +50,7 @@ rcl_system_time_now(rcl_time_point_value_t * now)
 rcl_ret_t
 rcl_steady_time_now(rcl_time_point_value_t * now)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(now, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   LARGE_INTEGER cpu_frequency, performance_count;
   // These should not ever fail since XP is already end of life:
   // From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and

rcl/src/rcl/timer.c

@@ -50,9 +50,15 @@ rcl_timer_init(
   const rcl_timer_callback_t callback,
   rcl_allocator_t allocator)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator);
   if (timer->impl) {
-    RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized");
+    RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator);
     return RCL_RET_ALREADY_INIT;
   }
   rcl_time_point_value_t now_steady;
@@ -66,12 +72,9 @@ rcl_timer_init(
   atomic_init(&impl.last_call_time, now_steady);
   atomic_init(&impl.canceled, false);
   impl.allocator = allocator;
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
   timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, allocator);
   *timer->impl = impl;
   return RCL_RET_OK;
 }
@@ -92,10 +95,13 @@ rcl_timer_fini(rcl_timer_t * timer)
 rcl_ret_t
 rcl_timer_call(rcl_timer_t * timer)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
   if (rcl_atomic_load_bool(&timer->impl->canceled)) {
-    RCL_SET_ERROR_MSG("timer is canceled");
+    RCL_SET_ERROR_MSG("timer is canceled", *allocator);
     return RCL_RET_TIMER_CANCELED;
   }
   rcl_time_point_value_t now_steady;
@@ -118,9 +124,12 @@ rcl_timer_call(rcl_timer_t * timer)
 rcl_ret_t
 rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT, *allocator);
   int64_t time_until_next_call;
   rcl_ret_t ret = rcl_timer_get_time_until_next_call(timer, &time_until_next_call);
   if (ret != RCL_RET_OK) {
@@ -133,9 +142,12 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready)
 rcl_ret_t
 rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT, *allocator);
   rcl_time_point_value_t now;
   rcl_ret_t ret = rcl_steady_time_now(&now);
   if (ret != RCL_RET_OK) {
@@ -152,9 +164,12 @@ rcl_timer_get_time_since_last_call(
   const rcl_timer_t * timer,
   rcl_time_point_value_t * time_since_last_call)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT, *allocator);
   rcl_time_point_value_t now;
   rcl_ret_t ret = rcl_steady_time_now(&now);
   if (ret != RCL_RET_OK) {
@@ -168,9 +183,12 @@ rcl_timer_get_time_since_last_call(
 rcl_ret_t
 rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT, *allocator);
   *period = rcl_atomic_load_uint64_t(&timer->impl->period);
   return RCL_RET_OK;
 }
@@ -178,9 +196,12 @@ rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period)
 rcl_ret_t
 rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT, *allocator);
   *old_period = rcl_atomic_exchange_uint64_t(&timer->impl->period, new_period);
   return RCL_RET_OK;
 }
@@ -188,16 +209,18 @@ rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64
 rcl_timer_callback_t
 rcl_timer_get_callback(const rcl_timer_t * timer)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    timer->impl, "timer is invalid", return NULL, rcl_get_default_allocator());
   return (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
 }
 
 rcl_timer_callback_t
 rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    timer->impl, "timer is invalid", return NULL, rcl_get_default_allocator());
   return (rcl_timer_callback_t)rcl_atomic_exchange_uintptr_t(
     &timer->impl->callback, (uintptr_t)new_callback);
 }
@@ -205,8 +228,9 @@ rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_
 rcl_ret_t
 rcl_timer_cancel(rcl_timer_t * timer)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
   rcl_atomic_store(&timer->impl->canceled, true);
   return RCL_RET_OK;
 }
@@ -214,9 +238,12 @@ rcl_timer_cancel(rcl_timer_t * timer)
 rcl_ret_t
 rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT);
+  const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer);
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  if (!allocator) {
+    return RCL_RET_TIMER_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT, *allocator);
   *is_canceled = rcl_atomic_load_bool(&timer->impl->canceled);
   return RCL_RET_OK;
 }
@@ -224,8 +251,9 @@ rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled)
 rcl_ret_t
 rcl_timer_reset(rcl_timer_t * timer)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
   rcl_time_point_value_t now;
   rcl_ret_t now_ret = rcl_steady_time_now(&now);
   if (now_ret != RCL_RET_OK) {
@@ -236,6 +264,15 @@ rcl_timer_reset(rcl_timer_t * timer)
   return RCL_RET_OK;
 }
 
+const rcl_allocator_t *
+rcl_timer_get_allocator(const rcl_timer_t * timer)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL, rcl_get_default_allocator());
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    timer->impl, "timer is invalid", return NULL, rcl_get_default_allocator());
+  return &timer->impl->allocator;
+}
+
 #if __cplusplus
 }
 #endif

rcl/src/rcl/wait.c

@@ -27,6 +27,7 @@ extern "C"
 #include "./stdatomic_helper.h"
 #include "rcl/error_handling.h"
 #include "rcl/time.h"
+#include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
 typedef struct rcl_wait_set_impl_t
@@ -114,22 +115,27 @@ rcl_wait_set_init(
   rcl_allocator_t allocator)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.allocate, "allocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.deallocate, "deallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator.reallocate, "reallocate not set",
+    return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
+
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, allocator);
   if (__wait_set_is_valid(wait_set)) {
-    RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.");
+    RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.", allocator);
     return RCL_RET_ALREADY_INIT;
   }
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    allocator.reallocate, "reallocate not set", return RCL_RET_INVALID_ARGUMENT);
   // Allocate space for the implementation struct.
   wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate(
     sizeof(rcl_wait_set_impl_t), allocator.state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
-    wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+    wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, allocator);
   memset(wait_set->impl, 0, sizeof(rcl_wait_set_impl_t));
   wait_set->impl->rmw_subscriptions.subscribers = NULL;
   wait_set->impl->rmw_subscriptions.subscriber_count = 0;
@@ -215,12 +221,12 @@ rcl_ret_t
 rcl_wait_set_fini(rcl_wait_set_t * wait_set)
 {
   rcl_ret_t result = RCL_RET_OK;
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
 
   if (__wait_set_is_valid(wait_set)) {
     rmw_ret_t ret = rmw_destroy_waitset(wait_set->impl->rmw_waitset);
     if (ret != RMW_RET_OK) {
-      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), wait_set->impl->allocator);
       result = RCL_RET_WAIT_SET_INVALID;
     }
     __wait_set_clean_up(wait_set, wait_set->impl->allocator);
@@ -231,25 +237,25 @@ rcl_wait_set_fini(rcl_wait_set_t * wait_set)
 rcl_ret_t
 rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT);
   if (!__wait_set_is_valid(wait_set)) {
-    RCL_SET_ERROR_MSG("wait set is invalid");
+    RCL_SET_ERROR_MSG("wait set is invalid", rcl_get_default_allocator());
     return RCL_RET_WAIT_SET_INVALID;
   }
+  RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT, wait_set->impl->allocator);
   *allocator = wait_set->impl->allocator;
   return RCL_RET_OK;
 }
 
 #define SET_ADD(Type) \
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); \
-  RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \
   if (!__wait_set_is_valid(wait_set)) { \
-    RCL_SET_ERROR_MSG("wait set is invalid"); \
+    RCL_SET_ERROR_MSG("wait set is invalid", rcl_get_default_allocator()); \
     return RCL_RET_WAIT_SET_INVALID; \
   } \
+  RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT, wait_set->impl->allocator); \
   if (!(wait_set->impl->Type ## _index < wait_set->size_of_ ## Type ## s)) { \
-    RCL_SET_ERROR_MSG(#Type "s set is full"); \
+    RCL_SET_ERROR_MSG(#Type "s set is full", wait_set->impl->allocator); \
     return RCL_RET_WAIT_SET_FULL; \
   } \
   size_t current_index = wait_set->impl->Type ## _index++; \
@@ -259,14 +265,14 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
   /* Also place into rmw storage. */ \
   rmw_ ## Type ## _t * rmw_handle = rcl_ ## Type ## _get_rmw_handle(Type); \
   RCL_CHECK_FOR_NULL_WITH_MSG( \
-    rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR); \
+    rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR, wait_set->impl->allocator); \
   wait_set->impl->RMWStorage[current_index] = rmw_handle->data; \
   wait_set->impl->RMWCount++;
 
 #define SET_CLEAR(Type) \
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); \
   if (!__wait_set_is_valid(wait_set)) { \
-    RCL_SET_ERROR_MSG("wait set is invalid"); \
+    RCL_SET_ERROR_MSG("wait set is invalid", rcl_get_default_allocator()); \
     return RCL_RET_WAIT_SET_INVALID; \
   } \
   memset( \
@@ -284,9 +290,10 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
   wait_set->impl->RMWCount = 0;
 
 #define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); \
   RCL_CHECK_FOR_NULL_WITH_MSG( \
-    wait_set->impl, "wait set is invalid", return RCL_RET_WAIT_SET_INVALID); \
+    wait_set->impl, "wait set is invalid", \
+    return RCL_RET_WAIT_SET_INVALID, rcl_get_default_allocator()); \
   if (size == wait_set->size_of_ ## Type ## s) { \
     return RCL_RET_OK; \
   } \
@@ -302,7 +309,8 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
     wait_set->Type ## s = (const rcl_ ## Type ## _t * *)allocator.reallocate( \
       (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \
     RCL_CHECK_FOR_NULL_WITH_MSG( \
-      wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \
+      wait_set->Type ## s, "allocating memory failed", \
+      return RCL_RET_BAD_ALLOC, wait_set->impl->allocator); \
     wait_set->size_of_ ## Type ## s = size; \
     ExtraRealloc \
   } \
@@ -323,7 +331,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
   if (!wait_set->impl->RMWStorage) { \
     allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \
     wait_set->size_of_ ## Type ## s = 0; \
-    RCL_SET_ERROR_MSG("allocating memory failed"); \
+    RCL_SET_ERROR_MSG("allocating memory failed", wait_set->impl->allocator); \
     return RCL_RET_BAD_ALLOC; \
   }
 
@@ -500,9 +508,9 @@ rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size)
 rcl_ret_t
 rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
 {
-  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
   if (!__wait_set_is_valid(wait_set)) {
-    RCL_SET_ERROR_MSG("wait set is invalid");
+    RCL_SET_ERROR_MSG("wait set is invalid", rcl_get_default_allocator());
     return RCL_RET_WAIT_SET_INVALID;
   }
   if (
@@ -512,7 +520,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
     wait_set->size_of_clients == 0 &&
     wait_set->size_of_services == 0)
   {
-    RCL_SET_ERROR_MSG("wait set is empty");
+    RCL_SET_ERROR_MSG("wait set is empty", wait_set->impl->allocator);
     return RCL_RET_WAIT_SET_EMPTY;
   }
   // Calculate the timeout argument.
@@ -587,7 +595,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
   }
   // Check for error.
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), wait_set->impl->allocator);
     return RCL_RET_ERROR;
   }
   // Check for ready timers next, and set not ready timers to NULL.

rcl/test/CMakeLists.txt

@@ -28,14 +28,6 @@ function(test_target_function)
 
   # Gtests
 
-  rcl_add_custom_gtest(test_allocator${target_suffix}
-    SRCS rcl/test_allocator.cpp
-    ENV ${extra_test_env}
-    APPEND_LIBRARY_DIRS ${extra_lib_dirs}
-    LIBRARIES ${PROJECT_NAME} ${extra_test_libraries}
-    AMENT_DEPENDENCIES ${rmw_implementation}
-  )
-
   rcl_add_custom_gtest(test_client${target_suffix}
     SRCS rcl/test_client.cpp
     ENV ${extra_test_env}
@@ -105,6 +97,14 @@ function(test_target_function)
     AMENT_DEPENDENCIES ${rmw_implementation}
   )
 
+  rcl_add_custom_gtest(test_guard_condition${target_suffix}
+    SRCS rcl/test_guard_condition.cpp
+    ENV ${extra_test_env}
+    APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+    LIBRARIES ${PROJECT_NAME} ${extra_test_libraries}
+    AMENT_DEPENDENCIES ${rmw_implementation}
+  )
+
   rcl_add_custom_gtest(test_publisher${target_suffix}
     SRCS rcl/test_publisher.cpp
     ENV ${extra_test_env}

rcl/test/rcl/test_allocator.cpp

@@ -1,90 +0,0 @@
-// 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.
-
-#include <gtest/gtest.h>
-
-#include "rcl/allocator.h"
-
-#include "../memory_tools/memory_tools.hpp"
-
-#ifdef RMW_IMPLEMENTATION
-# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
-# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
-#else
-# define CLASSNAME(NAME, SUFFIX) NAME
-#endif
-
-class CLASSNAME (TestAllocatorFixture, RMW_IMPLEMENTATION) : public ::testing::Test
-{
-public:
-  CLASSNAME(TestAllocatorFixture, RMW_IMPLEMENTATION)() {
-    start_memory_checking();
-    stop_memory_checking();
-  }
-  void SetUp()
-  {
-    set_on_unexpected_malloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED MALLOC";});
-    set_on_unexpected_realloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED REALLOC";});
-    set_on_unexpected_free_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED FREE";});
-    start_memory_checking();
-  }
-
-  void TearDown()
-  {
-    assert_no_malloc_end();
-    assert_no_realloc_end();
-    assert_no_free_end();
-    stop_memory_checking();
-    set_on_unexpected_malloc_callback(nullptr);
-    set_on_unexpected_realloc_callback(nullptr);
-    set_on_unexpected_free_callback(nullptr);
-  }
-};
-
-/* Tests the default allocator.
- */
-TEST_F(CLASSNAME(TestAllocatorFixture, RMW_IMPLEMENTATION), test_default_allocator_normal) {
-#if defined(WIN32)
-  printf("Allocator tests disabled on Windows.\n");
-  return;
-#endif  // defined(WIN32)
-  ASSERT_NO_MALLOC(
-    rcl_allocator_t allocator = rcl_get_default_allocator();
-  )
-  size_t mallocs = 0;
-  size_t reallocs = 0;
-  size_t frees = 0;
-  set_on_unexpected_malloc_callback([&mallocs]() {
-    mallocs++;
-  });
-  set_on_unexpected_realloc_callback([&reallocs]() {
-    reallocs++;
-  });
-  set_on_unexpected_free_callback([&frees]() {
-    frees++;
-  });
-  assert_no_malloc_begin();
-  assert_no_realloc_begin();
-  assert_no_free_begin();
-  void * allocated_memory = allocator.allocate(1024, allocator.state);
-  EXPECT_EQ(mallocs, 1u);
-  EXPECT_NE(allocated_memory, nullptr);
-  allocated_memory = allocator.reallocate(allocated_memory, 2048, allocator.state);
-  EXPECT_EQ(reallocs, 1u);
-  EXPECT_NE(allocated_memory, nullptr);
-  allocator.deallocate(allocated_memory, allocator.state);
-  EXPECT_EQ(mallocs, 1u);
-  EXPECT_EQ(reallocs, 1u);
-  EXPECT_EQ(frees, 1u);
-}

rcl/test/rcl/test_get_node_names.cpp

@@ -78,7 +78,7 @@ TEST_F(CLASSNAME(TestGetNodeNames, RMW_IMPLEMENTATION), test_rcl_get_node_names)
   std::this_thread::sleep_for(1s);
 
   utilities_string_array_t node_names = utilities_get_zero_initialized_string_array();
-  ret = rcl_get_node_names(node1_ptr, &node_names);
+  ret = rcl_get_node_names(node1_ptr, node1_options.allocator, &node_names);
   ASSERT_EQ(UTILITIES_RET_OK, ret) << rcl_get_error_string_safe();
 
   EXPECT_EQ(size_t(2), node_names.size);

rcl/test/rcl/test_graph.cpp

@@ -126,17 +126,17 @@ TEST_F(
   rcl_topic_names_and_types_t tnat {};
   rcl_node_t zero_node = rcl_get_zero_initialized_node();
   // invalid node
-  ret = rcl_get_topic_names_and_types(nullptr, &tnat);
+  ret = rcl_get_topic_names_and_types(nullptr, rcl_get_default_allocator(), &tnat);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
   rcl_reset_error();
-  ret = rcl_get_topic_names_and_types(&zero_node, &tnat);
+  ret = rcl_get_topic_names_and_types(&zero_node, rcl_get_default_allocator(), &tnat);
   EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
   rcl_reset_error();
-  ret = rcl_get_topic_names_and_types(this->old_node_ptr, &tnat);
+  ret = rcl_get_topic_names_and_types(this->old_node_ptr, rcl_get_default_allocator(), &tnat);
   EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string_safe();
   rcl_reset_error();
   // invalid topic_names_and_types
-  ret = rcl_get_topic_names_and_types(this->node_ptr, nullptr);
+  ret = rcl_get_topic_names_and_types(this->node_ptr, rcl_get_default_allocator(), nullptr);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
   rcl_reset_error();
   // invalid argument to rcl_destroy_topic_names_and_types
@@ -144,7 +144,7 @@ TEST_F(
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string_safe();
   rcl_reset_error();
   // valid calls
-  ret = rcl_get_topic_names_and_types(this->node_ptr, &tnat);
+  ret = rcl_get_topic_names_and_types(this->node_ptr, rcl_get_default_allocator(), &tnat);
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   ret = rcl_destroy_topic_names_and_types(&tnat);
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
@@ -258,7 +258,7 @@ check_graph_state(
     rcl_reset_error();
 
     tnat = rcl_get_zero_initialized_topic_names_and_types();
-    ret = rcl_get_topic_names_and_types(node_ptr, &tnat);
+    ret = rcl_get_topic_names_and_types(node_ptr, rcl_get_default_allocator(), &tnat);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
     rcl_reset_error();
     is_in_tnat = false;

rcl/test/rcl/test_guard_condition.cpp

@@ -0,0 +1,201 @@
+// Copyright 2017 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.
+
+#include <gtest/gtest.h>
+
+#include <string>
+
+#include "rcl/rcl.h"
+#include "rcl/guard_condition.h"
+
+#include "../memory_tools/memory_tools.hpp"
+#include "../scope_exit.hpp"
+#include "rcl/error_handling.h"
+
+#ifdef RMW_IMPLEMENTATION
+# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
+# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
+#else
+# define CLASSNAME(NAME, SUFFIX) NAME
+#endif
+
+class CLASSNAME (TestGuardConditionFixture, RMW_IMPLEMENTATION) : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
+    set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
+    set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
+    start_memory_checking();
+  }
+
+  void TearDown()
+  {
+    assert_no_malloc_end();
+    assert_no_realloc_end();
+    assert_no_free_end();
+    stop_memory_checking();
+    set_on_unexpected_malloc_callback(nullptr);
+    set_on_unexpected_realloc_callback(nullptr);
+    set_on_unexpected_free_callback(nullptr);
+  }
+};
+
+/* Tests the guard condition accessors, i.e. rcl_guard_condition_get_* functions.
+ */
+TEST_F(
+  CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_accessors) {
+  stop_memory_checking();
+  rcl_ret_t ret;
+
+  // Initialize rcl with rcl_init().
+  ret = rcl_init(0, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret);
+  // Setup automatic rcl_shutdown()
+  auto rcl_shutdown_exit = make_scope_exit([]() {
+    stop_memory_checking();
+    rcl_ret_t ret = rcl_shutdown();
+    ASSERT_EQ(RCL_RET_OK, ret);
+  });
+
+  // Create a zero initialized guard_condition (but not initialized).
+  rcl_guard_condition_t zero_guard_condition = rcl_get_zero_initialized_guard_condition();
+
+  // Create a normal guard_condition.
+  rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options();
+  rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
+  ret = rcl_guard_condition_init(&guard_condition, default_options);
+  ASSERT_EQ(RCL_RET_OK, ret);
+  // Setup automatic finalization of guard condition.
+  auto rcl_guard_condition_exit = make_scope_exit([&guard_condition]() {
+    stop_memory_checking();
+    rcl_ret_t ret = rcl_guard_condition_fini(&guard_condition);
+    EXPECT_EQ(RCL_RET_OK, ret);
+  });
+
+  // Test rcl_guard_condition_get_options().
+  const rcl_guard_condition_options_t * actual_options;
+  actual_options = rcl_guard_condition_get_options(nullptr);
+  EXPECT_EQ(nullptr, actual_options);
+  rcl_reset_error();
+  actual_options = rcl_guard_condition_get_options(&zero_guard_condition);
+  EXPECT_EQ(nullptr, actual_options);
+  rcl_reset_error();
+  start_memory_checking();
+  assert_no_malloc_begin();
+  assert_no_realloc_begin();
+  assert_no_free_begin();
+  actual_options = rcl_guard_condition_get_options(&guard_condition);
+  assert_no_malloc_end();
+  assert_no_realloc_end();
+  assert_no_free_end();
+  stop_memory_checking();
+  EXPECT_NE(nullptr, actual_options);
+  if (actual_options) {
+    EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate);
+  }
+  // Test rcl_guard_condition_get_rmw_handle().
+  rmw_guard_condition_t * gc_handle;
+  gc_handle = rcl_guard_condition_get_rmw_handle(nullptr);
+  EXPECT_EQ(nullptr, gc_handle);
+  rcl_reset_error();
+  gc_handle = rcl_guard_condition_get_rmw_handle(&zero_guard_condition);
+  EXPECT_EQ(nullptr, gc_handle);
+  rcl_reset_error();
+  start_memory_checking();
+  assert_no_malloc_begin();
+  assert_no_realloc_begin();
+  assert_no_free_begin();
+  gc_handle = rcl_guard_condition_get_rmw_handle(&guard_condition);
+  assert_no_malloc_end();
+  assert_no_realloc_end();
+  assert_no_free_end();
+  stop_memory_checking();
+  EXPECT_NE(nullptr, gc_handle);
+}
+
+/* Tests the guard condition life cycle, including rcl_guard_condition_init/fini().
+ */
+TEST_F(
+  CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_life_cycle) {
+  stop_memory_checking();
+  rcl_ret_t ret;
+  rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
+  rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options();
+  // Trying to init before rcl_init() should fail.
+  ret = rcl_guard_condition_init(&guard_condition, default_options);
+  ASSERT_EQ(RCL_RET_NOT_INIT, ret) << "Expected RCL_RET_NOT_INIT";
+  ASSERT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  // Initialize rcl with rcl_init().
+  ret = rcl_init(0, nullptr, rcl_get_default_allocator());
+  ASSERT_EQ(RCL_RET_OK, ret);
+  auto rcl_shutdown_exit = make_scope_exit([]() {
+    rcl_ret_t ret = rcl_shutdown();
+    ASSERT_EQ(RCL_RET_OK, ret);
+  });
+  // Try invalid arguments.
+  ret = rcl_guard_condition_init(nullptr, default_options);
+  ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
+  ASSERT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  // Try with invalid allocator.
+  rcl_guard_condition_options_t options_with_invalid_allocator =
+    rcl_guard_condition_get_default_options();
+  options_with_invalid_allocator.allocator.allocate = nullptr;
+  options_with_invalid_allocator.allocator.deallocate = nullptr;
+  options_with_invalid_allocator.allocator.reallocate = nullptr;
+  ret = rcl_guard_condition_init(&guard_condition, options_with_invalid_allocator);
+  ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
+  ASSERT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  // Try with failing allocator.
+  rcl_guard_condition_options_t options_with_failing_allocator =
+    rcl_guard_condition_get_default_options();
+  options_with_failing_allocator.allocator.allocate = failing_malloc;
+  options_with_failing_allocator.allocator.deallocate = failing_free;
+  options_with_failing_allocator.allocator.reallocate = failing_realloc;
+  ret = rcl_guard_condition_init(&guard_condition, options_with_failing_allocator);
+  ASSERT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC";
+  // The error will not be set because the allocator will not work.
+  // It should, however, print a message to the screen and get the bad alloc ret code.
+  // ASSERT_TRUE(rcl_error_is_set());
+  // rcl_reset_error();
+
+  // Try fini with invalid arguments.
+  ret = rcl_guard_condition_fini(nullptr);
+  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
+  ASSERT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  // Try fini with an uninitialized guard_condition.
+  ret = rcl_guard_condition_fini(&guard_condition);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  // Try a normal init and fini.
+  ret = rcl_guard_condition_init(&guard_condition, default_options);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  ret = rcl_guard_condition_fini(&guard_condition);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  // Try repeated init and fini calls.
+  ret = rcl_guard_condition_init(&guard_condition, default_options);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  ret = rcl_guard_condition_init(&guard_condition, default_options);
+  EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << "Expected RCL_RET_ALREADY_INIT";
+  ASSERT_TRUE(rcl_error_is_set());
+  rcl_reset_error();
+  ret = rcl_guard_condition_fini(&guard_condition);
+  EXPECT_EQ(RCL_RET_OK, ret);
+  ret = rcl_guard_condition_fini(&guard_condition);
+  EXPECT_EQ(RCL_RET_OK, ret);
+}

rcl/test/rcl/test_node.cpp

@@ -353,8 +353,11 @@ TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_life_cycle)
   options_with_failing_allocator.allocator.reallocate = failing_realloc;
   ret = rcl_node_init(&node, name, namespace_, &options_with_failing_allocator);
   EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC";
-  ASSERT_TRUE(rcl_error_is_set());
+  // The error will not be set because the allocator will not work.
-  rcl_reset_error();
+  // It should, however, print a message to the screen and get the bad alloc ret code.
+  // ASSERT_TRUE(rcl_error_is_set());
+  // rcl_reset_error();
+
   // Try fini with invalid arguments.
   ret = rcl_node_fini(nullptr);
   EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";

rcl/test/rcl/test_wait.cpp

@@ -34,10 +34,27 @@
 # define CLASSNAME(NAME, SUFFIX) NAME
 #endif
 
-
 #define TOLERANCE RCL_MS_TO_NS(6)
 
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) {
+class CLASSNAME (WaitSetTestFixture, RMW_IMPLEMENTATION) : public ::testing::Test
+{
+public:
+  void SetUp()
+  {
+    rcl_ret_t ret;
+    ret = rcl_init(0, nullptr, rcl_get_default_allocator());
+    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  }
+
+  void TearDown()
+  {
+    rcl_ret_t ret;
+    ret = rcl_shutdown();
+    EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+  }
+};
+
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) {
   // Initialize a waitset with a subscription and then resize it to zero.
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, rcl_get_default_allocator());
@@ -53,7 +70,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) {
 }
 
 // Test rcl_wait with a positive finite timeout value (1ms)
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), finite_timeout) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), finite_timeout) {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
@@ -72,7 +89,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), finite_timeout) {
 }
 
 // Check that a timer overrides a negative timeout value (blocking forever)
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 1, 1, 0, 0, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
@@ -111,7 +128,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
 }
 
 // Test rcl_wait with a timeout value of 0 (non-blocking)
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout) {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 1, 1, 0, 0, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
@@ -142,7 +159,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout) {
 }
 
 // Check that a canceled timer doesn't wake up rcl_wait
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 1, 1, 0, 0, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
@@ -183,7 +200,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) {
 }
 
 // Test rcl_wait_set_t with excess capacity works.
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), excess_capacity) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), excess_capacity) {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 42, 42, 42, 42, 42, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
@@ -194,7 +211,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), excess_capacity) {
 }
 
 // Check rcl_wait can be called in many threads, each with unique wait sets and resources.
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threaded) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threaded) {
   rcl_ret_t ret;
   const size_t number_of_threads = 20;  // concurrent waits
   const size_t count_target = 10;  // number of times each wait should wake up before being "done"
@@ -303,7 +320,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threaded)
     loop_count++;
     for (auto & test_set : test_sets) {
       ret = rcl_trigger_guard_condition(&test_set.guard_condition);
-      EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
+      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
     }
     std::this_thread::sleep_for(trigger_period);
   }
@@ -319,7 +336,7 @@ TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threaded)
 
 // Check the interaction of a guard condition and a negative timeout by
 // triggering a guard condition in a separate thread
-TEST(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), guard_condition) {
+TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), guard_condition) {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
   rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 1, 0, 0, 0, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();

rcl_lifecycle/include/rcl_lifecycle/data_types.h

@@ -84,6 +84,8 @@ typedef struct rcl_lifecycle_state_machine_t
   rcl_lifecycle_transition_map_t transition_map;
   // Communication interface into a ROS world
   rcl_lifecycle_com_interface_t com_interface;
+  // Allocator used.
+  rcl_allocator_t allocator;
 } rcl_lifecycle_state_machine_t;
 
 #if __cplusplus

rcl_lifecycle/src/com_interface.c

@@ -67,24 +67,32 @@ rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
   const rosidl_service_type_support_t * ts_srv_get_available_states,
   const rosidl_service_type_support_t * ts_srv_get_available_transitions)
 {
+  if (!node_handle) {
+    RCL_SET_ERROR_MSG("node_handle is null", rcl_get_default_allocator());
+    return RCL_RET_INVALID_ARGUMENT;
+  }
+  const rcl_node_options_t * node_options = rcl_node_get_options(node_handle);
+  if (!node_options) {
+    return RCL_RET_NODE_INVALID;
+  }
   if (!ts_pub_notify) {
-    RCL_SET_ERROR_MSG("ts_pub_notify is NULL");
+    RCL_SET_ERROR_MSG("ts_pub_notify is NULL", node_options->allocator);
     return RCL_RET_ERROR;
   }
   if (!ts_srv_change_state) {
-    RCL_SET_ERROR_MSG("ts_srv_change_state is NULL");
+    RCL_SET_ERROR_MSG("ts_srv_change_state is NULL", node_options->allocator);
     return RCL_RET_ERROR;
   }
   if (!ts_srv_get_state) {
-    RCL_SET_ERROR_MSG("ts_srv_get_state is NULL");
+    RCL_SET_ERROR_MSG("ts_srv_get_state is NULL", node_options->allocator);
     return RCL_RET_ERROR;
   }
   if (!ts_srv_get_available_states) {
-    RCL_SET_ERROR_MSG("ts_srv_get_available_states is NULL");
+    RCL_SET_ERROR_MSG("ts_srv_get_available_states is NULL", node_options->allocator);
     return RCL_RET_ERROR;
   }
   if (!ts_srv_get_available_states) {
-    RCL_SET_ERROR_MSG("ts_srv_get_available_transitions is NULL");
+    RCL_SET_ERROR_MSG("ts_srv_get_available_transitions is NULL", node_options->allocator);
     return RCL_RET_ERROR;
   }
 
@@ -97,7 +105,7 @@ rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
     const char * topic_prefix = "__transition_event";
     char * topic_name;
     if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
-      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
+      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255", node_options->allocator);
       goto fail;
     }
 
@@ -122,7 +130,7 @@ rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
     const char * topic_prefix = "__change_state";
     char * topic_name;
     if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
-      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
+      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255", node_options->allocator);
       goto fail;
     }
 
@@ -144,7 +152,7 @@ rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
     const char * topic_prefix = "__get_state";
     char * topic_name;
     if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
-      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
+      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255", node_options->allocator);
       goto fail;
     }
 
@@ -166,7 +174,7 @@ rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
     const char * topic_prefix = "__get_available_states";
     char * topic_name;
     if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
-      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
+      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255", node_options->allocator);
       goto fail;
     }
 
@@ -188,7 +196,7 @@ rcl_lifecycle_com_interface_init(rcl_lifecycle_com_interface_t * com_interface,
     const char * topic_prefix = "__get_available_transitions";
     char * topic_name;
     if (!concatenate(&node_name, &topic_prefix, &topic_name)) {
-      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255");
+      RCL_SET_ERROR_MSG("Topic name exceeds maximum size of 255", node_options->allocator);
       goto fail;
     }
 

rcl_lifecycle/src/rcl_lifecycle.c

@@ -41,6 +41,10 @@ rcl_lifecycle_get_zero_initialized_state_machine()
   state_machine.transition_map.transitions = NULL;
   state_machine.transition_map.transitions_size = 0;
   state_machine.com_interface = rcl_lifecycle_get_zero_initialized_com_interface();
+  state_machine.allocator.allocate = NULL;
+  state_machine.allocator.deallocate = NULL;
+  state_machine.allocator.reallocate = NULL;
+  state_machine.allocator.state = NULL;
   return state_machine;
 }
 
@@ -65,8 +69,19 @@ rcl_lifecycle_state_machine_init(
   }
 
   if (default_states) {
-    rcl_lifecycle_init_default_state_machine(state_machine);
+    rcl_ret_t ret = rcl_lifecycle_init_default_state_machine(state_machine);
+    if (ret != RCL_RET_OK) {
+      // error should already be set
+      return ret;
+    }
   }
+
+  const rcl_node_options_t * node_options = rcl_node_get_options(node_handle);
+  if (!node_options) {
+    RCL_SET_ERROR_MSG("node does not have valid options", rcl_get_default_allocator());
+    return RCL_RET_NODE_INVALID;
+  }
+  state_machine->allocator = node_options->allocator;
   return RCL_RET_OK;
 }
 
@@ -100,11 +115,11 @@ rcl_ret_t
 rcl_lifecycle_state_machine_is_initialized(const rcl_lifecycle_state_machine_t * state_machine)
 {
   if (!state_machine->com_interface.srv_get_state.impl) {
-    RCL_SET_ERROR_MSG("get_state service is null");
+    RCL_SET_ERROR_MSG("get_state service is null", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   if (!state_machine->com_interface.srv_change_state.impl) {
-    RCL_SET_ERROR_MSG("change_state service is null");
+    RCL_SET_ERROR_MSG("change_state service is null", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }
   return RCL_RET_OK;
@@ -141,7 +156,7 @@ rcl_lifecycle_trigger_transition(
   if (!transition) {
     fprintf(stderr, "No transition found for node %s with key %d\n",
       state_machine->current_state->label, key);
-    RCL_SET_ERROR_MSG("Transition is not registered.");
+    RCL_SET_ERROR_MSG("Transition is not registered.", rcl_get_default_allocator());
     return RCL_RET_ERROR;
   }