diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index f70654b..d46eade 100644 --- a/rcl/CMakeLists.txt +++ b/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 "$") ament_export_dependencies(ament_cmake) +ament_export_dependencies(c_utilities) ament_export_dependencies(rcl_interfaces) ament_export_dependencies(rmw) ament_export_dependencies(rmw_implementation) diff --git a/rcl/include/rcl/allocator.h b/rcl/include/rcl/allocator.h index 6ace7d8..cd0ae0c 100644 --- a/rcl/include/rcl/allocator.h +++ b/rcl/include/rcl/allocator.h @@ -20,79 +20,13 @@ extern "C" { #endif -#include "rcl/macros.h" -#include "rcl/types.h" -#include "rcl/visibility_control.h" +#include "c_utilities/allocator.h" -/// Encapsulation of an allocator. -/** - * 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; +typedef utilities_allocator_t rcl_allocator_t; -/// Return a properly initialized rcl_allocator_t with default values. -/** - * This defaults to: - * - * - allocate = wraps std::malloc() - * - deallocate = wraps std::free() - * - reallocate = wrapps std::realloc() - * - state = `NULL` - * - *
- * 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); +#define rcl_get_default_allocator utilities_get_default_allocator -/// Emulate the behavior of [reallocf](https://linux.die.net/man/3/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); +#define rcl_reallocf utilities_reallocf #if __cplusplus } diff --git a/rcl/include/rcl/error_handling.h b/rcl/include/rcl/error_handling.h index 1e81e95..8c0efc2 100644 --- a/rcl/include/rcl/error_handling.h +++ b/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_ diff --git a/rcl/include/rcl/graph.h b/rcl/include/rcl/graph.h index 1238e1f..7c16a96 100644 --- a/rcl/include/rcl/graph.h +++ b/rcl/include/rcl/graph.h @@ -63,6 +63,7 @@ rcl_get_zero_initialized_topic_names_and_types(void); * [1] implementation may need to protect the data structure with a lock * * \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. + * *
* 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. + * *
* 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. + * *
* Attribute | Adherence * ------------------ | ------------- diff --git a/rcl/include/rcl/guard_condition.h b/rcl/include/rcl/guard_condition.h index 33f0f41..f0149ec 100644 --- a/rcl/include/rcl/guard_condition.h +++ b/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. + * + *
+ * 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); *
* Attribute | Adherence * ------------------ | ------------- - * Allocates Memory | Yes + * Allocates Memory | No * Thread-Safe | No * Uses Atomics | No * Lock-Free | Yes diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index ffca244..7505106 100644 --- a/rcl/include/rcl/timer.h +++ b/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. + * + *
+ * 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 diff --git a/rcl/package.xml b/rcl/package.xml index dc6c829..ae55ff6 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -13,12 +13,15 @@ rmw + c_utilities rcl_interfaces rosidl_generator_c + c_utilities rcl_interfaces rosidl_generator_c ament_cmake + c_utilities rosidl_default_runtime rmw_implementation diff --git a/rcl/src/rcl/allocator.c b/rcl/src/rcl/allocator.c deleted file mode 100644 index 32c6406..0000000 --- a/rcl/src/rcl/allocator.c +++ /dev/null @@ -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 - -#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; -} diff --git a/rcl/src/rcl/client.c b/rcl/src/rcl/client.c index a982d4e..f7776ce 100644 --- a/rcl/src/rcl/client.c +++ b/rcl/src/rcl/client.c @@ -21,6 +21,7 @@ extern "C" #include +#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); - if (!node->impl) { - 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; - } + + // 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); + 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(node, 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_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); - RCL_CHECK_FOR_NULL_WITH_MSG( - client->impl->rmw_handle, "client is invalid", return NULL); + client->impl->rmw_handle, "client is invalid", return NULL, options->allocator); 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); - RCL_CHECK_FOR_NULL_WITH_MSG( - client->impl, "client is invalid", return NULL); + const rcl_client_options_t * options = rcl_client_get_options(client); + if (!options) { + 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(ros_request, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, 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_get_default_allocator()); + 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); - RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, 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_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; diff --git a/rcl/src/rcl/common.c b/rcl/src/rcl/common.c index fe7f5e5..51f1271 100644 --- a/rcl/src/rcl/common.c +++ b/rcl/src/rcl/common.c @@ -21,6 +21,8 @@ extern "C" #include +#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_value, 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_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'; diff --git a/rcl/src/rcl/common.h b/rcl/src/rcl/common.h index e5c8027..84c13f6 100644 --- a/rcl/src/rcl/common.h +++ b/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) \ - RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return 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, 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; \ } diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index 6873bc8..52c71d2 100644 --- a/rcl/src/rcl/graph.c +++ b/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); - RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT); + const rcl_node_options_t * node_options = rcl_node_get_options(node); + 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); - RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT); + const rcl_node_options_t * node_options = rcl_node_get_options(node); + 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); - RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT); + const rcl_node_options_t * node_options = rcl_node_get_options(node); + 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), diff --git a/rcl/src/rcl/guard_condition.c b/rcl/src/rcl/guard_condition.c index de9f0cd..381f2e0 100644 --- a/rcl/src/rcl/guard_condition.c +++ b/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; - if (allocator.deallocate) { - allocator.deallocate(guard_condition->impl, allocator.state); - } else { - RCL_SET_ERROR_MSG("deallocate not set"); - result = RCL_RET_ERROR; - } + allocator.deallocate(guard_condition->impl, allocator.state); + guard_condition->impl = NULL; } 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. - RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG( - guard_condition->impl, - "guard condition implementation is invalid", - return RCL_RET_INVALID_ARGUMENT); + const rcl_guard_condition_options_t * options = rcl_guard_condition_get_options(guard_condition); + if (!options) { + return RCL_RET_INVALID_ARGUMENT; // error already set + } // 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); - RCL_CHECK_FOR_NULL_WITH_MSG( - guard_condition->impl, "guard condition implementation is invalid", return NULL); + const rcl_guard_condition_options_t * options = rcl_guard_condition_get_options(guard_condition); + if (!options) { + return NULL; // error already set + } return guard_condition->impl->rmw_handle; } diff --git a/rcl/src/rcl/node.c b/rcl/src/rcl/node.c index aa3d5e2..60dc46d 100644 --- a/rcl/src/rcl/node.c +++ b/rcl/src/rcl/node.c @@ -25,6 +25,7 @@ extern "C" #include #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); - RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(node, 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_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_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; - RCL_CHECK_FOR_NULL_WITH_MSG( - allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + // assuming that allocate and deallocate are ok since they are checked in init 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_FOR_NULL_WITH_MSG(node->impl, "rcl node implementation is invalid", return 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_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(domain_id, RCL_RET_INVALID_ARGUMENT); - if (!rcl_node_is_valid(node)) { + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_node_options_t * node_options = rcl_node_get_options(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_FOR_NULL_WITH_MSG(node->impl, "node implementation is invalid", return 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_get_default_allocator()); return node->impl->rcl_instance_id; } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 6959623..9f7fa5b 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -22,6 +22,7 @@ extern "C" #include #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); - if (!node->impl) { - 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; - } + + // 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); + 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(node, 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_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(ros_message, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + 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; } diff --git a/rcl/src/rcl/rcl.c b/rcl/src/rcl/rcl.c index 7c460cb..82e4d0d 100644 --- a/rcl/src/rcl/rcl.c +++ b/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(); diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c index c1fd2ad..5faf8fe 100644 --- a/rcl/src/rcl/service.c +++ b/rcl/src/rcl/service.c @@ -23,6 +23,7 @@ extern "C" #include #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); - if (!node->impl) { - 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; - } + + // 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); + 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(node, 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_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); - RCL_CHECK_FOR_NULL_WITH_MSG( - service->impl->rmw_handle, "service is invalid", return NULL); + service->impl->rmw_handle, "service is invalid", return NULL, options->allocator); 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_FOR_NULL_WITH_MSG( - service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_service_options_t * options = rcl_service_get_options(service); + if (!options) { + return RCL_RET_SERVICE_INVALID; + } + 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_FOR_NULL_WITH_MSG( - service->impl, "service is invalid", return RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_service_options_t * options = rcl_service_get_options(service); + if (!options) { + return RCL_RET_SERVICE_INVALID; + } + 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; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index d394afd..88b4790 100644 --- a/rcl/src/rcl/subscription.c +++ b/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); - 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 (subscription->impl) { - RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized"); - return RCL_RET_ALREADY_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); + 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(node, 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_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(ros_message, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + 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); - RCL_CHECK_FOR_NULL_WITH_MSG( - subscription->impl, "subscription is invalid", return NULL); + const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription); + if (!options) { + 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; } diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 0bba26d..2a4841e 100644 --- a/rcl/src/rcl/time.c +++ b/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(is_enabled, 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_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; } diff --git a/rcl/src/rcl/time_unix.c b/rcl/src/rcl/time_unix.c index 7c2e339..2d8a2ac 100644 --- a/rcl/src/rcl/time_unix.c +++ b/rcl/src/rcl/time_unix.c @@ -32,6 +32,7 @@ extern "C" #include #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, ×pec_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; diff --git a/rcl/src/rcl/time_win32.c b/rcl/src/rcl/time_win32.c index 2fcd293..2dc3584 100644 --- a/rcl/src/rcl/time_win32.c +++ b/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 diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index d3dde96..89795b8 100644 --- a/rcl/src/rcl/timer.c +++ b/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_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + 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(is_ready, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); + 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(time_until_next_call, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); + 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(time_since_last_call, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); + 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(period, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); + 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(old_period, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); + 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_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return 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_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_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return 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_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_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + 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_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(is_canceled, RCL_RET_INVALID_ARGUMENT); - RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + const rcl_allocator_t * allocator = rcl_timer_get_allocator(timer); + 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_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + 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_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 diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 3f5a6f2..a549017 100644 --- a/rcl/src/rcl/wait.c +++ b/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(allocator, 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; } + 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(Type, 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; \ } \ + 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. diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 250e9ea..c9b1884 100644 --- a/rcl/test/CMakeLists.txt +++ b/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} diff --git a/rcl/test/rcl/test_allocator.cpp b/rcl/test/rcl/test_allocator.cpp deleted file mode 100644 index b486a32..0000000 --- a/rcl/test/rcl/test_allocator.cpp +++ /dev/null @@ -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 - -#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); -} diff --git a/rcl/test/rcl/test_get_node_names.cpp b/rcl/test/rcl/test_get_node_names.cpp index 08117e4..a4d14c6 100644 --- a/rcl/test/rcl/test_get_node_names.cpp +++ b/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); diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index b54426a..c769f6e 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/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; diff --git a/rcl/test/rcl/test_guard_condition.cpp b/rcl/test/rcl/test_guard_condition.cpp new file mode 100644 index 0000000..35c0d7a --- /dev/null +++ b/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 + +#include + +#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); +} diff --git a/rcl/test/rcl/test_node.cpp b/rcl/test/rcl/test_node.cpp index 27ed3cd..bbfe1c7 100644 --- a/rcl/test/rcl/test_node.cpp +++ b/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()); - rcl_reset_error(); + // 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_node_fini(nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 97680bd..75afe14 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/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(); diff --git a/rcl_lifecycle/include/rcl_lifecycle/data_types.h b/rcl_lifecycle/include/rcl_lifecycle/data_types.h index 226003c..5501ffd 100644 --- a/rcl_lifecycle/include/rcl_lifecycle/data_types.h +++ b/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 diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index a420cb9..d2e539f 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/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; } diff --git a/rcl_lifecycle/src/rcl_lifecycle.c b/rcl_lifecycle/src/rcl_lifecycle.c index 32ca640..b81b993 100644 --- a/rcl_lifecycle/src/rcl_lifecycle.c +++ b/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; }