consolidate wait set functions (#285)
* Consolidate functions to clear wait set Add rcl_wait_set_clear() Remove rcl_wait_set_clear_subscriptions() rcl_wait_set_clear_guard_conditions() rcl_wait_set_clear_clients() rcl_wait_set_clear_services() rcl_wait_set_clear_timers() * Consolidate functions to resize wait set Add rcl_wait_set_resize() Removed rcl_wait_set_resize_subscriptions() rcl_wait_set_resize_guard_conditions() rcl_wait_set_resize_timers() rcl_wait_set_resize_clients() rcl_wait_set_resize_services()
This commit is contained in:
Shane Loretz
GitHub
parent
3aebe9fbf9
commit
ba9f24b852
8 changed files with 151 additions and 323 deletions
|
|
@ -209,10 +209,10 @@ rcl_wait_set_add_subscription(
|
|||
rcl_wait_set_t * wait_set,
|
||||
const rcl_subscription_t * subscription);
|
||||
|
||||
/// Remove (sets to `NULL`) the subscriptions in the wait set.
|
||||
/// Remove (sets to `NULL`) all entities in the wait set.
|
||||
/**
|
||||
* This function should be used after passing using rcl_wait, but before
|
||||
* adding new subscriptions to the set.
|
||||
* adding new entities to the set.
|
||||
* Sets all of the entries in the underlying rmw array to `NULL`, and sets the
|
||||
* count in the rmw array to `0`.
|
||||
*
|
||||
|
|
@ -226,7 +226,7 @@ rcl_wait_set_add_subscription(
|
|||
* Uses Atomics | No
|
||||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[inout] wait_set struct to have its subscriptions cleared
|
||||
* \param[inout] wait_set struct to have its entities cleared
|
||||
* \return `RCL_RET_OK` if cleared successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized, or
|
||||
|
|
@ -235,12 +235,11 @@ rcl_wait_set_add_subscription(
|
|||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set);
|
||||
rcl_wait_set_clear(rcl_wait_set_t * wait_set);
|
||||
|
||||
/// Reallocate space for the subscriptions in the wait set.
|
||||
/// Reallocate space for entities in the wait set.
|
||||
/**
|
||||
* This function will deallocate and reallocate the memory for the
|
||||
* subscriptions set.
|
||||
* This function will deallocate and reallocate the memory for all entity sets.
|
||||
*
|
||||
* A size of 0 will just deallocate the memory and assign `NULL` to the array.
|
||||
*
|
||||
|
|
@ -248,7 +247,7 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set);
|
|||
* wait set's initialization.
|
||||
*
|
||||
* After calling this function all values in the set will be set to `NULL`,
|
||||
* effectively the same as calling rcl_wait_set_clear_subscriptions().
|
||||
* effectively the same as calling rcl_wait_set_clear().
|
||||
* Similarly, the underlying rmw representation is reallocated and reset:
|
||||
* all entries are set to `NULL` and the count is set to zero.
|
||||
*
|
||||
|
|
@ -264,8 +263,12 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set);
|
|||
* Uses Atomics | No
|
||||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[inout] wait_set struct to have its subscriptions cleared
|
||||
* \param[in] size a size for the new set
|
||||
* \param[inout] wait_set struct to be resized
|
||||
* \param[in] subscriptions_size a size for the new subscriptions set
|
||||
* \param[in] guard_conditions_size a size for the new subscriptions set
|
||||
* \param[in] timers_size a size for the new subscriptions set
|
||||
* \param[in] clients_size a size for the new subscriptions set
|
||||
* \param[in] services_size a size for the new subscriptions set
|
||||
* \return `RCL_RET_OK` if resized successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
|
||||
|
|
@ -274,7 +277,13 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set);
|
|||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size);
|
||||
rcl_wait_set_resize(
|
||||
rcl_wait_set_t * wait_set,
|
||||
size_t subscriptions_size,
|
||||
size_t guard_conditions_size,
|
||||
size_t timers_size,
|
||||
size_t clients_size,
|
||||
size_t services_size);
|
||||
|
||||
/// Store a pointer to the guard condition in the next empty spot in the set.
|
||||
/**
|
||||
|
|
@ -288,26 +297,6 @@ rcl_wait_set_add_guard_condition(
|
|||
rcl_wait_set_t * wait_set,
|
||||
const rcl_guard_condition_t * guard_condition);
|
||||
|
||||
/// Remove (sets to `NULL`) the guard conditions in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_clear_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set);
|
||||
|
||||
/// Reallocate space for the guard conditions in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_resize_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size);
|
||||
|
||||
/// Store a pointer to the timer in the next empty spot in the set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
|
|
@ -320,26 +309,6 @@ rcl_wait_set_add_timer(
|
|||
rcl_wait_set_t * wait_set,
|
||||
const rcl_timer_t * timer);
|
||||
|
||||
/// Remove (sets to `NULL`) the timers in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_clear_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set);
|
||||
|
||||
/// Reallocate space for the timers in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_resize_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size);
|
||||
|
||||
/// Store a pointer to the client in the next empty spot in the set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
|
|
@ -352,26 +321,6 @@ rcl_wait_set_add_client(
|
|||
rcl_wait_set_t * wait_set,
|
||||
const rcl_client_t * client);
|
||||
|
||||
/// Remove (sets to `NULL`) the clients in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_clear_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_clients(rcl_wait_set_t * wait_set);
|
||||
|
||||
/// Reallocate space for the clients in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_resize_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size);
|
||||
|
||||
/// Store a pointer to the client in the next empty spot in the set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
|
|
@ -384,26 +333,6 @@ rcl_wait_set_add_service(
|
|||
rcl_wait_set_t * wait_set,
|
||||
const rcl_service_t * service);
|
||||
|
||||
/// Remove (sets to `NULL`) the services in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_clear_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_services(rcl_wait_set_t * wait_set);
|
||||
|
||||
/// Reallocate space for the services in the wait set.
|
||||
/**
|
||||
* This function behaves exactly the same as for subscriptions.
|
||||
* \see rcl_wait_set_resize_subscriptions
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size);
|
||||
|
||||
/// Block until the wait set is ready or until the timeout has been exceeded.
|
||||
/**
|
||||
* This function will collect the items in the rcl_wait_set_t and pass them
|
||||
|
|
@ -430,9 +359,7 @@ rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size);
|
|||
* rcl_ret_t ret = rcl_wait_set_init(&wait_set, 2, 1, 0, 0, 0, rcl_get_default_allocator());
|
||||
* // ... error handling
|
||||
* do {
|
||||
* ret = rcl_wait_set_clear_subscriptions(&wait_set);
|
||||
* // ... error handling
|
||||
* ret = rcl_wait_set_clear_guard_conditions(&wait_set);
|
||||
* ret = rcl_wait_set_clear(&wait_set);
|
||||
* // ... error handling
|
||||
* ret = rcl_wait_set_add_subscription(&wait_set, &sub1);
|
||||
* // ... error handling
|
||||
|
|
|
|||
|
|
@ -33,15 +33,20 @@ extern "C"
|
|||
|
||||
typedef struct rcl_wait_set_impl_t
|
||||
{
|
||||
// number of subscriptions that have been added to the wait set
|
||||
size_t subscription_index;
|
||||
rmw_subscriptions_t rmw_subscriptions;
|
||||
// number of guard_conditions that have been added to the wait set
|
||||
size_t guard_condition_index;
|
||||
rmw_guard_conditions_t rmw_guard_conditions;
|
||||
// number of clients that have been added to the wait set
|
||||
size_t client_index;
|
||||
rmw_clients_t rmw_clients;
|
||||
// number of services that have been added to the wait set
|
||||
size_t service_index;
|
||||
rmw_services_t rmw_services;
|
||||
rmw_wait_set_t * rmw_wait_set;
|
||||
// number of timers that have been added to the wait set
|
||||
size_t timer_index;
|
||||
rcl_allocator_t allocator;
|
||||
} rcl_wait_set_impl_t;
|
||||
|
|
@ -75,29 +80,9 @@ static void
|
|||
__wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator)
|
||||
{
|
||||
if (wait_set->subscriptions) {
|
||||
rcl_ret_t ret = rcl_wait_set_resize_subscriptions(wait_set, 0);
|
||||
rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0, 0, 0, 0, 0);
|
||||
(void)ret; // NO LINT
|
||||
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
|
||||
}
|
||||
if (wait_set->guard_conditions) {
|
||||
rcl_ret_t ret = rcl_wait_set_resize_guard_conditions(wait_set, 0);
|
||||
(void)ret; // NO LINT
|
||||
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
|
||||
}
|
||||
if (wait_set->timers) {
|
||||
rcl_ret_t ret = rcl_wait_set_resize_timers(wait_set, 0);
|
||||
(void)ret; // NO LINT
|
||||
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
|
||||
}
|
||||
if (wait_set->clients) {
|
||||
rcl_ret_t ret = rcl_wait_set_resize_clients(wait_set, 0);
|
||||
(void)ret; // NO LINT
|
||||
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
|
||||
}
|
||||
if (wait_set->services) {
|
||||
rcl_ret_t ret = rcl_wait_set_resize_services(wait_set, 0);
|
||||
(void)ret; // NO LINT
|
||||
assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0.
|
||||
assert(RCL_RET_OK == ret); // Defensive, shouldn't fail with size 0.
|
||||
}
|
||||
if (wait_set->impl) {
|
||||
allocator.deallocate(wait_set->impl, allocator.state);
|
||||
|
|
@ -153,52 +138,10 @@ rcl_wait_set_init(
|
|||
// Set allocator.
|
||||
wait_set->impl->allocator = allocator;
|
||||
// Initialize subscription space.
|
||||
rcl_ret_t ret;
|
||||
if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
// Initialize guard condition space.
|
||||
ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
// Initialize timer space.
|
||||
ret = rcl_wait_set_resize_timers(wait_set, number_of_timers);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
if ((ret = rcl_wait_set_clear_timers(wait_set)) != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
// Initialize client space.
|
||||
ret = rcl_wait_set_resize_clients(wait_set, number_of_clients);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
if ((ret = rcl_wait_set_clear_clients(wait_set)) != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
// Initialize service space.
|
||||
ret = rcl_wait_set_resize_services(wait_set, number_of_services);
|
||||
if (ret != RCL_RET_OK) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
if ((ret = rcl_wait_set_clear_services(wait_set)) != RCL_RET_OK) {
|
||||
rcl_ret_t ret = rcl_wait_set_resize(
|
||||
wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers,
|
||||
number_of_clients, number_of_services);
|
||||
if (RCL_RET_OK != ret) {
|
||||
fail_ret = ret;
|
||||
goto fail;
|
||||
}
|
||||
|
|
@ -267,37 +210,30 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
|
|||
wait_set->impl->RMWCount++;
|
||||
|
||||
#define SET_CLEAR(Type) \
|
||||
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_get_default_allocator()); \
|
||||
return RCL_RET_WAIT_SET_INVALID; \
|
||||
} \
|
||||
do { \
|
||||
memset( \
|
||||
(void *)wait_set->Type ## s, \
|
||||
0, \
|
||||
sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \
|
||||
wait_set->impl->Type ## _index = 0;
|
||||
wait_set->impl->Type ## _index = 0; \
|
||||
} while (false)
|
||||
|
||||
#define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \
|
||||
do { \
|
||||
/* Also clear the rmw storage. */ \
|
||||
memset( \
|
||||
wait_set->impl->RMWStorage, \
|
||||
0, \
|
||||
sizeof(void *) * wait_set->impl->RMWCount); \
|
||||
wait_set->impl->RMWCount = 0;
|
||||
wait_set->impl->RMWCount = 0; \
|
||||
} while (false)
|
||||
|
||||
#define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \
|
||||
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, rcl_get_default_allocator()); \
|
||||
if (size == wait_set->size_of_ ## Type ## s) { \
|
||||
return RCL_RET_OK; \
|
||||
} \
|
||||
do { \
|
||||
rcl_allocator_t allocator = wait_set->impl->allocator; \
|
||||
wait_set->size_of_ ## Type ## s = 0; \
|
||||
wait_set->impl->Type ## _index = 0; \
|
||||
if (size == 0) { \
|
||||
if (0 == Type ## s_size) { \
|
||||
if (wait_set->Type ## s) { \
|
||||
allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \
|
||||
wait_set->Type ## s = NULL; \
|
||||
|
|
@ -305,15 +241,16 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
|
|||
ExtraDealloc \
|
||||
} else { \
|
||||
wait_set->Type ## s = (const rcl_ ## Type ## _t **)allocator.reallocate( \
|
||||
(void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \
|
||||
(void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * Type ## s_size, \
|
||||
allocator.state); \
|
||||
RCL_CHECK_FOR_NULL_WITH_MSG( \
|
||||
wait_set->Type ## s, "allocating memory failed", \
|
||||
return RCL_RET_BAD_ALLOC, wait_set->impl->allocator); \
|
||||
memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * size); \
|
||||
wait_set->size_of_ ## Type ## s = size; \
|
||||
memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \
|
||||
wait_set->size_of_ ## Type ## s = Type ## s_size; \
|
||||
ExtraRealloc \
|
||||
} \
|
||||
return RCL_RET_OK;
|
||||
} while (false)
|
||||
|
||||
#define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \
|
||||
/* Also deallocate the rmw storage. */ \
|
||||
|
|
@ -327,14 +264,14 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al
|
|||
/* Also resize the rmw storage. */ \
|
||||
wait_set->impl->RMWCount = 0; \
|
||||
wait_set->impl->RMWStorage = (void **)allocator.reallocate( \
|
||||
wait_set->impl->RMWStorage, sizeof(void *) * size, allocator.state); \
|
||||
wait_set->impl->RMWStorage, sizeof(void *) * Type ## s_size, allocator.state); \
|
||||
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", wait_set->impl->allocator); \
|
||||
return RCL_RET_BAD_ALLOC; \
|
||||
} \
|
||||
memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * size);
|
||||
memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size);
|
||||
|
||||
/* Implementation-specific notes:
|
||||
*
|
||||
|
|
@ -357,13 +294,35 @@ rcl_wait_set_add_subscription(
|
|||
* count in the rmw array to 0.
|
||||
*/
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set)
|
||||
rcl_wait_set_clear(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
SET_CLEAR(subscription)
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(
|
||||
wait_set->impl, RCL_RET_WAIT_SET_INVALID, rcl_get_default_allocator());
|
||||
|
||||
SET_CLEAR(subscription);
|
||||
SET_CLEAR(guard_condition);
|
||||
SET_CLEAR(client);
|
||||
SET_CLEAR(service);
|
||||
SET_CLEAR(timer);
|
||||
|
||||
SET_CLEAR_RMW(
|
||||
subscription,
|
||||
rmw_subscriptions.subscribers,
|
||||
rmw_subscriptions.subscriber_count)
|
||||
rmw_subscriptions.subscriber_count);
|
||||
SET_CLEAR_RMW(
|
||||
guard_condition,
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count);
|
||||
SET_CLEAR_RMW(
|
||||
clients,
|
||||
rmw_clients.clients,
|
||||
rmw_clients.client_count);
|
||||
SET_CLEAR_RMW(
|
||||
services,
|
||||
rmw_services.services,
|
||||
rmw_services.service_count);
|
||||
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
@ -373,15 +332,48 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set)
|
|||
* all entries are set to null and the count is set to zero.
|
||||
*/
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size)
|
||||
rcl_wait_set_resize(
|
||||
rcl_wait_set_t * wait_set,
|
||||
size_t subscriptions_size,
|
||||
size_t guard_conditions_size,
|
||||
size_t timers_size,
|
||||
size_t clients_size,
|
||||
size_t services_size)
|
||||
{
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(
|
||||
wait_set->impl, RCL_RET_WAIT_SET_INVALID, rcl_get_default_allocator());
|
||||
SET_RESIZE(
|
||||
subscription,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count)
|
||||
)
|
||||
);
|
||||
SET_RESIZE(
|
||||
guard_condition,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
guard_condition,
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count)
|
||||
);
|
||||
SET_RESIZE(timer,;,;); // NOLINT
|
||||
SET_RESIZE(client,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_clients.clients, rmw_clients.client_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
client, rmw_clients.clients, rmw_clients.client_count)
|
||||
);
|
||||
SET_RESIZE(service,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_services.services, rmw_services.service_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
service, rmw_services.services, rmw_services.service_count)
|
||||
);
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
|
|
@ -395,32 +387,6 @@ rcl_wait_set_add_guard_condition(
|
|||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
SET_CLEAR(guard_condition)
|
||||
SET_CLEAR_RMW(
|
||||
guard_condition,
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count)
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size)
|
||||
{
|
||||
SET_RESIZE(
|
||||
guard_condition,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
guard_condition,
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count)
|
||||
)
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_add_timer(
|
||||
rcl_wait_set_t * wait_set,
|
||||
|
|
@ -430,19 +396,6 @@ rcl_wait_set_add_timer(
|
|||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
SET_CLEAR(timer)
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size)
|
||||
{
|
||||
SET_RESIZE(timer,;,;) // NOLINT
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_add_client(
|
||||
rcl_wait_set_t * wait_set,
|
||||
|
|
@ -453,28 +406,6 @@ rcl_wait_set_add_client(
|
|||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_clients(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
SET_CLEAR(client)
|
||||
SET_CLEAR_RMW(
|
||||
clients,
|
||||
rmw_clients.clients,
|
||||
rmw_clients.client_count)
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size)
|
||||
{
|
||||
SET_RESIZE(client,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_clients.clients, rmw_clients.client_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
client, rmw_clients.clients, rmw_clients.client_count)
|
||||
)
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_add_service(
|
||||
rcl_wait_set_t * wait_set,
|
||||
|
|
@ -485,28 +416,6 @@ rcl_wait_set_add_service(
|
|||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_clear_services(rcl_wait_set_t * wait_set)
|
||||
{
|
||||
SET_CLEAR(service)
|
||||
SET_CLEAR_RMW(
|
||||
services,
|
||||
rmw_services.services,
|
||||
rmw_services.service_count)
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size)
|
||||
{
|
||||
SET_RESIZE(service,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_services.services, rmw_services.service_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
service, rmw_services.services, rmw_services.service_count)
|
||||
)
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
|
||||
{
|
||||
|
|
@ -627,23 +536,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
|
|||
}
|
||||
}
|
||||
// Check for timeout, return RCL_RET_TIMEOUT only if it wasn't a timer.
|
||||
if (ret == RMW_RET_TIMEOUT) {
|
||||
// Assume none were set (because timeout was reached first), and clear all.
|
||||
rcl_ret_t rcl_ret;
|
||||
// This next line prevents "assigned but never used" warnings in Release mode.
|
||||
(void)rcl_ret; // NO LINT
|
||||
rcl_ret = rcl_wait_set_clear_subscriptions(wait_set);
|
||||
assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set.
|
||||
rcl_ret = rcl_wait_set_clear_guard_conditions(wait_set);
|
||||
assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set.
|
||||
rcl_ret = rcl_wait_set_clear_services(wait_set);
|
||||
assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set.
|
||||
rcl_ret = rcl_wait_set_clear_clients(wait_set);
|
||||
assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set.
|
||||
if (!is_timer_timeout) {
|
||||
return RCL_RET_TIMEOUT;
|
||||
}
|
||||
} else if (ret != RMW_RET_OK) {
|
||||
if (ret != RMW_RET_OK && ret != RMW_RET_TIMEOUT) {
|
||||
RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), wait_set->impl->allocator);
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
|
@ -681,6 +574,10 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
|
|||
wait_set->services[i] = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
if (RMW_RET_TIMEOUT == ret && !is_timer_timeout) {
|
||||
return RCL_RET_TIMEOUT;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -77,9 +77,9 @@ wait_for_client_to_be_ready(
|
|||
size_t iteration = 0;
|
||||
do {
|
||||
++iteration;
|
||||
if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) {
|
||||
if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
ROS_PACKAGE_NAME, "Error in wait_set_clear_clients: %s", rcl_get_error_string_safe())
|
||||
ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe())
|
||||
return false;
|
||||
}
|
||||
if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) {
|
||||
|
|
|
|||
|
|
@ -50,9 +50,9 @@ wait_for_service_to_be_ready(
|
|||
size_t iteration = 0;
|
||||
do {
|
||||
++iteration;
|
||||
if (rcl_wait_set_clear_services(&wait_set) != RCL_RET_OK) {
|
||||
if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) {
|
||||
RCUTILS_LOG_ERROR_NAMED(
|
||||
ROS_PACKAGE_NAME, "Error in wait_set_clear_services: %s", rcl_get_error_string_safe())
|
||||
ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe())
|
||||
return false;
|
||||
}
|
||||
if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) {
|
||||
|
|
|
|||
|
|
@ -285,7 +285,7 @@ check_graph_state(
|
|||
// Don't wait for the graph to change on the last loop because we won't check again.
|
||||
continue;
|
||||
}
|
||||
ret = rcl_wait_set_clear_guard_conditions(wait_set_ptr);
|
||||
ret = rcl_wait_set_clear(wait_set_ptr);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
@ -444,7 +444,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi
|
|||
size_t graph_changes_count = 0;
|
||||
// while the topic thread is not done, wait and count the graph changes
|
||||
while (future.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
|
||||
ret = rcl_wait_set_clear_guard_conditions(this->wait_set_ptr);
|
||||
ret = rcl_wait_set_clear(this->wait_set_ptr);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
@ -502,7 +502,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_
|
|||
if (time_to_sleep > min_sleep) {
|
||||
time_to_sleep = min_sleep;
|
||||
}
|
||||
rcl_ret_t ret = rcl_wait_set_clear_guard_conditions(this->wait_set_ptr);
|
||||
rcl_ret_t ret = rcl_wait_set_clear(this->wait_set_ptr);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ wait_for_service_to_be_ready(
|
|||
size_t iteration = 0;
|
||||
do {
|
||||
++iteration;
|
||||
ret = rcl_wait_set_clear_services(&wait_set);
|
||||
ret = rcl_wait_set_clear(&wait_set);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
ret = rcl_wait_set_add_service(&wait_set, service);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ wait_for_subscription_to_be_ready(
|
|||
size_t iteration = 0;
|
||||
do {
|
||||
++iteration;
|
||||
ret = rcl_wait_set_clear_subscriptions(&wait_set);
|
||||
ret = rcl_wait_set_clear(&wait_set);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
ret = rcl_wait_set_add_subscription(&wait_set, subscription);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
|
|||
|
|
@ -59,13 +59,17 @@ public:
|
|||
TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) {
|
||||
// Initialize a wait set 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());
|
||||
rcl_ret_t ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, rcl_get_default_allocator());
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
||||
ret = rcl_wait_set_resize_subscriptions(&wait_set, 0);
|
||||
ret = rcl_wait_set_resize(&wait_set, 0u, 0u, 0u, 0u, 0u);
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
||||
EXPECT_EQ(wait_set.size_of_subscriptions, 0ull);
|
||||
EXPECT_EQ(wait_set.size_of_guard_conditions, 0ull);
|
||||
EXPECT_EQ(wait_set.size_of_clients, 0ull);
|
||||
EXPECT_EQ(wait_set.size_of_services, 0ull);
|
||||
EXPECT_EQ(wait_set.size_of_timers, 0ull);
|
||||
|
||||
ret = rcl_wait_set_fini(&wait_set);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
@ -287,7 +291,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade
|
|||
while (wake_try_count < retry_limit) {
|
||||
wake_try_count++;
|
||||
rcl_ret_t ret;
|
||||
ret = rcl_wait_set_clear_guard_conditions(&test_set.wait_set);
|
||||
ret = rcl_wait_set_clear(&test_set.wait_set);
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
ret = rcl_wait_set_add_guard_condition(&test_set.wait_set, &test_set.guard_condition);
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue