diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 09fc23a..1a4fd58 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -28,6 +28,12 @@ typedef rcutils_allocator_t rcl_allocator_t; #define rcl_reallocf rcutils_reallocf +#define RCL_CHECK_ALLOCATOR(allocator, fail_statement) \ + RCUTILS_CHECK_ALLOCATOR(allocator, fail_statement) + +#define RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, msg, fail_statement) \ + RCUTILS_CHECK_ALLOCATOR_WITH_MSG(allocator, msg, fail_statement) + #if __cplusplus } #endif diff --git a/rcl/include/rcl/error_handling.h b/rcl/include/rcl/error_handling.h index 18f0e66..f8b09d0 100644 --- a/rcl/include/rcl/error_handling.h +++ b/rcl/include/rcl/error_handling.h @@ -37,6 +37,12 @@ typedef rcutils_error_state_t rcl_error_state_t; #define rcl_set_error_state rcutils_set_error_state +#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type, allocator) \ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type, allocator) + +#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement, allocator) \ + RCUTILS_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement, allocator) + #define RCL_SET_ERROR_MSG(msg, allocator) RCUTILS_SET_ERROR_MSG(msg, allocator) #define rcl_error_is_set rcutils_error_is_set diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index 9e30acc..eb51e36 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -58,13 +58,7 @@ rcl_client_init( // check the options and allocator first, so the allocator can be passed to errors RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); 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_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); 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) { diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index ba2f90b..3a05b34 100644 --- a/rcl/src/rcl/common.h +++ b/rcl/src/rcl/common.h @@ -23,16 +23,6 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/types.h" -#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, allocator) - -#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement, allocator) \ - if (!(value)) { \ - RCL_SET_ERROR_MSG(msg, allocator); \ - error_statement; \ - } - /// Retrieve the value of the given environment variable if it exists, or "". /* The returned cstring is only valid until the next time this function is * called, because it is a direct pointer to the static storage. diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index 07418ad..2c54b13 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/rcl/src/rcl/guard_condition.c @@ -48,12 +48,7 @@ __rcl_guard_condition_init_from_rmw_impl( // Perform argument validation. 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_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT, *allocator); // Ensure the guard_condition handle is zero initialized. if (guard_condition->impl) { diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index bc53a23..5bd0639 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -102,12 +102,7 @@ rcl_node_init( // Check options and allocator first, so allocator can be used for errors. RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); 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_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT, *allocator); diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 13d0c51..4220884 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -23,6 +23,7 @@ extern "C" #include #include "./common.h" +#include "rcl/allocator.h" #include "rcl/expand_topic_name.h" #include "rcutils/logging_macros.h" #include "rmw/error_handling.h" @@ -55,12 +56,7 @@ rcl_publisher_init( // Check options and allocator first, so allocator can be used with errors. RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); 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_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT, *allocator); if (publisher->impl) { diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 82e4d0d..3b25456 100644 --- a/rcl/src/rcl/rcl.c +++ b/rcl/src/rcl/rcl.c @@ -59,14 +59,7 @@ rcl_init(int argc, char ** argv, rcl_allocator_t allocator) rcl_ret_t fail_ret = RCL_RET_ERROR; // 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, rcl_get_default_allocator()); - RCL_CHECK_FOR_NULL_WITH_MSG( - allocator.deallocate, - "invalid allocator, deallocate not set", - return RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); if (argc > 0) { RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT, allocator); diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index a6c52d5..e25daa2 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -55,12 +55,7 @@ rcl_service_init( // Check options and allocator first, so the allocator can be used in errors. RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); 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_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator); diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 491cd13..d1a9a9e 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -54,12 +54,7 @@ rcl_subscription_init( // Check options and allocator first, so the allocator can be used in errors. RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); 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_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, *allocator); RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, *allocator); diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 72f13ed..9689866 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -51,12 +51,7 @@ rcl_timer_init( const rcl_timer_callback_t callback, rcl_allocator_t 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_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); 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", allocator); diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index f398190..65fc94c 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -116,16 +116,7 @@ rcl_wait_set_init( { rcl_ret_t fail_ret = RCL_RET_ERROR; - 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_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); 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.", allocator);