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

fixes for publisher and wait found during testing

Browse source
This commit is contained in:
William Woodall 2016-02-12 00:41:47 -08:00
parent a48ba5e1db
commit 135836e1de
2 changed files with 22 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -48,6 +48,10 @@ rcl_publisher_init(
rcl_ret_t fail_ret = RCL_RET_ERROR; rcl_ret_t fail_ret = RCL_RET_ERROR;
RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); 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(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(type_support, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, 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); RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);

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

@ -110,6 +110,7 @@ rcl_wait_set_init(
sizeof(rcl_wait_set_impl_t), allocator.state); sizeof(rcl_wait_set_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG( 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);
memset(wait_set->impl, 0, sizeof(rcl_wait_set_impl_t));
wait_set->impl->rmw_subscriptions.subscribers = NULL; wait_set->impl->rmw_subscriptions.subscribers = NULL;
wait_set->impl->rmw_subscriptions.subscriber_count = 0; wait_set->impl->rmw_subscriptions.subscriber_count = 0;
wait_set->impl->rmw_guard_conditions.guard_conditions = NULL; wait_set->impl->rmw_guard_conditions.guard_conditions = NULL;
@ -123,6 +124,8 @@ rcl_wait_set_init(
goto fail; goto fail;
} }
// Set allocator.
wait_set->impl->allocator = allocator;
// Initialize subscription space. // Initialize subscription space.
rcl_ret_t ret; rcl_ret_t ret;
if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) { if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) {
@ -153,8 +156,6 @@ rcl_wait_set_init(
fail_ret = ret; fail_ret = ret;
goto fail; goto fail;
} }
// Set allocator.
wait_set->impl->allocator = allocator;
return RCL_RET_OK; return RCL_RET_OK;
fail: fail:
if (__wait_set_is_valid(wait_set)) { if (__wait_set_is_valid(wait_set)) {
@ -398,7 +399,9 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
if (timeout > 0) { if (timeout > 0) {
// Determine the nearest timeout (given or a timer). // Determine the nearest timeout (given or a timer).
uint64_t min_timeout = timeout; uint64_t min_timeout = timeout;
if (min_timeout > 0) { // Do not consider timer timeouts if non-blocking. // If min_timeout is > 0, then compare it to the time until each timer.
// Take the lowest and use that for the wait timeout.
if (min_timeout > 0) {
size_t i; size_t i;
for (i = 0; i < wait_set->size_of_timers; ++i) { for (i = 0; i < wait_set->size_of_timers; ++i) {
if (!wait_set->timers[i]) { if (!wait_set->timers[i]) {
@ -433,6 +436,18 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
&dummy_clients, &dummy_clients,
wait_set->impl->rmw_waitset, wait_set->impl->rmw_waitset,
timeout_argument); timeout_argument);
// Check for timeout.
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.
return RCL_RET_TIMEOUT;
}
// Check for error. // Check for error.
if (ret != RMW_RET_OK) { if (ret != RMW_RET_OK) {
RCL_SET_ERROR_MSG(rmw_get_error_string_safe()); RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
@ -450,17 +465,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
wait_set->timers[i] = NULL; wait_set->timers[i] = NULL;
} }
} }
// Check for timeout.
if (ret == RMW_RET_TIMEOUT) {
// Assume none were set (because timeout was reached first), and clear all.
rcl_ret_t rcl_ret;
(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.
return RCL_RET_TIMEOUT;
}
// Set corresponding rcl subscription handles NULL. // Set corresponding rcl subscription handles NULL.
for (i = 0; i < wait_set->size_of_subscriptions; ++i) { for (i = 0; i < wait_set->size_of_subscriptions; ++i) {
assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive. assert(i < wait_set->impl->rmw_subscriptions.subscriber_count); // Defensive.