niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

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

Browse source 
* moved allocator to c_utilities

* moved error_handling to c_utilities

* refactor uses of RCL_SET_ERROR_MSG with allocator

* add missing guard condition functions and tests

* add missing timer functions

* refactor rcl_lifecycyle

* missed an instance of RCL_SET_ERROR_MSG

* fix segfaults in error cases for rcl_lifecycle

* remove extra header

* check return code of rcl_lifecycle_init_default_state_machine
This commit is contained in:
William Woodall 2017-04-19 12:37:48 -07:00 • committed by GitHub
parent 415612f8af
commit 90176d9f1a
33 changed files with 872 additions and 618 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rcl/CMakeLists.txt
View file

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

74
rcl/include/rcl/allocator.h
View file

@ -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`
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | Yes
* Uses Atomics | No
* Lock-Free | Yes
*/
RCL_PUBLIC
RCL_WARN_UNUSED
rcl_allocator_t
rcl_get_default_allocator(void);
#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
}

30
rcl/include/rcl/error_handling.h
View file

@ -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_

12
rcl/include/rcl/graph.h
View file

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

27
rcl/include/rcl/guard_condition.h
View file

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

22
rcl/include/rcl/timer.h
View file

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

3
rcl/package.xml
View file

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

65
rcl/src/rcl/allocator.c
View file

@ -1,65 +0,0 @@
// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stdlib.h>
#include "rcl/allocator.h"
#include "rcl/error_handling.h"
static void *
__default_allocate(size_t size, void * state)
{
(void)state; // unused
return malloc(size);
}
static void
__default_deallocate(void * pointer, void * state)
{
(void)state; // unused
free(pointer);
}
static void *
__default_reallocate(void * pointer, size_t size, void * state)
{
(void)state; // unused
return realloc(pointer, size);
}
rcl_allocator_t
rcl_get_default_allocator()
{
static rcl_allocator_t default_allocator = {
__default_allocate,
__default_deallocate,
__default_reallocate,
NULL
};
return default_allocator;
}
void *
rcl_reallocf(void * pointer, size_t size, rcl_allocator_t * allocator)
{
if (!allocator || !allocator->reallocate || !allocator->deallocate) {
RCL_SET_ERROR_MSG("invalid allocator or allocator function pointers");
return NULL;
}
void * new_pointer = allocator->reallocate(pointer, size, allocator->state);
if (!new_pointer) {
allocator->deallocate(pointer, allocator->state);
}
return new_pointer;
}

92
rcl/src/rcl/client.c
View file

@ -21,6 +21,7 @@ extern "C"
#include <string.h>
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
#include "./common.h"
@ -49,29 +50,34 @@ rcl_client_init(
const rcl_client_options_t * options)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
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;

8
rcl/src/rcl/common.c
View file

@ -21,6 +21,8 @@ extern "C"
#include <stdlib.h>
#include "rcl/allocator.h"
#if defined(WIN32)
# define WINDOWS_ENV_BUFFER_SIZE 2048
static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE];
@ -29,8 +31,8 @@ static char __env_buffer[WINDOWS_ENV_BUFFER_SIZE];
rcl_ret_t
rcl_impl_getenv(const char * env_name, const char ** env_value)
{
RCL_CHECK_ARGUMENT_FOR_NULL(env_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(env_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';

9
rcl/src/rcl/common.h
View file

@ -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; \
}

53
rcl/src/rcl/graph.c
View file

@ -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),

69
rcl/src/rcl/guard_condition.c
View file

@ -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;
}

82
rcl/src/rcl/node.c
View file

@ -25,6 +25,7 @@ extern "C"
#include <string.h>
#include "rcl/rcl.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
#include "rmw/validate_namespace.h"
#include "rmw/validate_node_name.h"
@ -69,29 +70,34 @@ rcl_node_init(
rcl_guard_condition_get_default_options();
rcl_ret_t ret;
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT);
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;
}

78
rcl/src/rcl/publisher.c
View file

@ -22,6 +22,7 @@ extern "C"
#include <string.h>
#include "./common.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
typedef struct rcl_publisher_impl_t
@ -46,29 +47,35 @@ rcl_publisher_init(
const rcl_publisher_options_t * options)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
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;
}

29
rcl/src/rcl/rcl.c
View file

@ -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();

93
rcl/src/rcl/service.c
View file

@ -23,6 +23,7 @@ extern "C"
#include <string.h>
#include "./common.h"
#include "rmw/error_handling.h"
#include "rmw/rmw.h"
typedef struct rcl_service_impl_t
@ -47,29 +48,34 @@ rcl_service_init(
const rcl_service_options_t * options)
{
rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
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;
}

72
rcl/src/rcl/subscription.c
View file

@ -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;
}

63
rcl/src/rcl/time.c
View file

@ -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;
}

9
rcl/src/rcl/time_unix.c
View file

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

5
rcl/src/rcl/time_win32.c
View file

@ -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

109
rcl/src/rcl/timer.c
View file

@ -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

66
rcl/src/rcl/wait.c
View file

@ -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.

16
rcl/test/CMakeLists.txt
View file

@ -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}

90
rcl/test/rcl/test_allocator.cpp
View file

@ -1,90 +0,0 @@
// Copyright 2015 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include "rcl/allocator.h"
#include "../memory_tools/memory_tools.hpp"
#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
#else
# define CLASSNAME(NAME, SUFFIX) NAME
#endif
class CLASSNAME (TestAllocatorFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
CLASSNAME(TestAllocatorFixture, RMW_IMPLEMENTATION)() {
start_memory_checking();
stop_memory_checking();
}
void SetUp()
{
set_on_unexpected_malloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {EXPECT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
}
};
/* Tests the default allocator.
*/
TEST_F(CLASSNAME(TestAllocatorFixture, RMW_IMPLEMENTATION), test_default_allocator_normal) {
#if defined(WIN32)
printf("Allocator tests disabled on Windows.\n");
return;
#endif // defined(WIN32)
ASSERT_NO_MALLOC(
rcl_allocator_t allocator = rcl_get_default_allocator();
)
size_t mallocs = 0;
size_t reallocs = 0;
size_t frees = 0;
set_on_unexpected_malloc_callback([&mallocs]() {
mallocs++;
});
set_on_unexpected_realloc_callback([&reallocs]() {
reallocs++;
});
set_on_unexpected_free_callback([&frees]() {
frees++;
});
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
void * allocated_memory = allocator.allocate(1024, allocator.state);
EXPECT_EQ(mallocs, 1u);
EXPECT_NE(allocated_memory, nullptr);
allocated_memory = allocator.reallocate(allocated_memory, 2048, allocator.state);
EXPECT_EQ(reallocs, 1u);
EXPECT_NE(allocated_memory, nullptr);
allocator.deallocate(allocated_memory, allocator.state);
EXPECT_EQ(mallocs, 1u);
EXPECT_EQ(reallocs, 1u);
EXPECT_EQ(frees, 1u);
}

2
rcl/test/rcl/test_get_node_names.cpp
View file

@ -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);

12
rcl/test/rcl/test_graph.cpp
View file

@ -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;

201
rcl/test/rcl/test_guard_condition.cpp Normal file
View file

@ -0,0 +1,201 @@
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <string>
#include "rcl/rcl.h"
#include "rcl/guard_condition.h"
#include "../memory_tools/memory_tools.hpp"
#include "../scope_exit.hpp"
#include "rcl/error_handling.h"
#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
#else
# define CLASSNAME(NAME, SUFFIX) NAME
#endif
class CLASSNAME (TestGuardConditionFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
void SetUp()
{
set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
start_memory_checking();
}
void TearDown()
{
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
set_on_unexpected_malloc_callback(nullptr);
set_on_unexpected_realloc_callback(nullptr);
set_on_unexpected_free_callback(nullptr);
}
};
/* Tests the guard condition accessors, i.e. rcl_guard_condition_get_* functions.
*/
TEST_F(
CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_accessors) {
stop_memory_checking();
rcl_ret_t ret;
// Initialize rcl with rcl_init().
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret);
// Setup automatic rcl_shutdown()
auto rcl_shutdown_exit = make_scope_exit([]() {
stop_memory_checking();
rcl_ret_t ret = rcl_shutdown();
ASSERT_EQ(RCL_RET_OK, ret);
});
// Create a zero initialized guard_condition (but not initialized).
rcl_guard_condition_t zero_guard_condition = rcl_get_zero_initialized_guard_condition();
// Create a normal guard_condition.
rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options();
rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
ret = rcl_guard_condition_init(&guard_condition, default_options);
ASSERT_EQ(RCL_RET_OK, ret);
// Setup automatic finalization of guard condition.
auto rcl_guard_condition_exit = make_scope_exit([&guard_condition]() {
stop_memory_checking();
rcl_ret_t ret = rcl_guard_condition_fini(&guard_condition);
EXPECT_EQ(RCL_RET_OK, ret);
});
// Test rcl_guard_condition_get_options().
const rcl_guard_condition_options_t * actual_options;
actual_options = rcl_guard_condition_get_options(nullptr);
EXPECT_EQ(nullptr, actual_options);
rcl_reset_error();
actual_options = rcl_guard_condition_get_options(&zero_guard_condition);
EXPECT_EQ(nullptr, actual_options);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
actual_options = rcl_guard_condition_get_options(&guard_condition);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_NE(nullptr, actual_options);
if (actual_options) {
EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate);
}
// Test rcl_guard_condition_get_rmw_handle().
rmw_guard_condition_t * gc_handle;
gc_handle = rcl_guard_condition_get_rmw_handle(nullptr);
EXPECT_EQ(nullptr, gc_handle);
rcl_reset_error();
gc_handle = rcl_guard_condition_get_rmw_handle(&zero_guard_condition);
EXPECT_EQ(nullptr, gc_handle);
rcl_reset_error();
start_memory_checking();
assert_no_malloc_begin();
assert_no_realloc_begin();
assert_no_free_begin();
gc_handle = rcl_guard_condition_get_rmw_handle(&guard_condition);
assert_no_malloc_end();
assert_no_realloc_end();
assert_no_free_end();
stop_memory_checking();
EXPECT_NE(nullptr, gc_handle);
}
/* Tests the guard condition life cycle, including rcl_guard_condition_init/fini().
*/
TEST_F(
CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_life_cycle) {
stop_memory_checking();
rcl_ret_t ret;
rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition();
rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options();
// Trying to init before rcl_init() should fail.
ret = rcl_guard_condition_init(&guard_condition, default_options);
ASSERT_EQ(RCL_RET_NOT_INIT, ret) << "Expected RCL_RET_NOT_INIT";
ASSERT_TRUE(rcl_error_is_set());
rcl_reset_error();
// Initialize rcl with rcl_init().
ret = rcl_init(0, nullptr, rcl_get_default_allocator());
ASSERT_EQ(RCL_RET_OK, ret);
auto rcl_shutdown_exit = make_scope_exit([]() {
rcl_ret_t ret = rcl_shutdown();
ASSERT_EQ(RCL_RET_OK, ret);
});
// Try invalid arguments.
ret = rcl_guard_condition_init(nullptr, default_options);
ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
ASSERT_TRUE(rcl_error_is_set());
rcl_reset_error();
// Try with invalid allocator.
rcl_guard_condition_options_t options_with_invalid_allocator =
rcl_guard_condition_get_default_options();
options_with_invalid_allocator.allocator.allocate = nullptr;
options_with_invalid_allocator.allocator.deallocate = nullptr;
options_with_invalid_allocator.allocator.reallocate = nullptr;
ret = rcl_guard_condition_init(&guard_condition, options_with_invalid_allocator);
ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
ASSERT_TRUE(rcl_error_is_set());
rcl_reset_error();
// Try with failing allocator.
rcl_guard_condition_options_t options_with_failing_allocator =
rcl_guard_condition_get_default_options();
options_with_failing_allocator.allocator.allocate = failing_malloc;
options_with_failing_allocator.allocator.deallocate = failing_free;
options_with_failing_allocator.allocator.reallocate = failing_realloc;
ret = rcl_guard_condition_init(&guard_condition, options_with_failing_allocator);
ASSERT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC";
// The error will not be set because the allocator will not work.
// It should, however, print a message to the screen and get the bad alloc ret code.
// ASSERT_TRUE(rcl_error_is_set());
// rcl_reset_error();
// Try fini with invalid arguments.
ret = rcl_guard_condition_fini(nullptr);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT";
ASSERT_TRUE(rcl_error_is_set());
rcl_reset_error();
// Try fini with an uninitialized guard_condition.
ret = rcl_guard_condition_fini(&guard_condition);
EXPECT_EQ(RCL_RET_OK, ret);
// Try a normal init and fini.
ret = rcl_guard_condition_init(&guard_condition, default_options);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_guard_condition_fini(&guard_condition);
EXPECT_EQ(RCL_RET_OK, ret);
// Try repeated init and fini calls.
ret = rcl_guard_condition_init(&guard_condition, default_options);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_guard_condition_init(&guard_condition, default_options);
EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << "Expected RCL_RET_ALREADY_INIT";
ASSERT_TRUE(rcl_error_is_set());
rcl_reset_error();
ret = rcl_guard_condition_fini(&guard_condition);
EXPECT_EQ(RCL_RET_OK, ret);
ret = rcl_guard_condition_fini(&guard_condition);
EXPECT_EQ(RCL_RET_OK, ret);
}

7
rcl/test/rcl/test_node.cpp
View file

@ -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";

37
rcl/test/rcl/test_wait.cpp
View file

@ -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();

2
rcl_lifecycle/include/rcl_lifecycle/data_types.h
View file

@ -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

28
rcl_lifecycle/src/com_interface.c
View file

@ -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;
}

23
rcl_lifecycle/src/rcl_lifecycle.c
View file

@ -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;
}