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

Timer tests (#163)

Browse source 
* dont return timeout errcode if we didnt hit user specified timeout

* update axisting test accordingly

* add timer test

* Revert "dont return timeout errcode if we didnt hit user specified timeout"

This reverts commit f44a93b8528e87ce46594af0d1184c116ade0cb4.

* alternative fix to not return timeout when timer is ready

this also fixes a bug where the wait would always return when all the
timers were canceled

fixes rclcpp#319

* fixups

* more tests

* another one just for fun

* remove new tests

* clearer variable name

* update comment
This commit is contained in:
Mikael Arguedas 2017-09-08 10:22:14 -07:00 • committed by GitHub
parent cd8cff0024
commit 1fa4acac8c
2 changed files with 50 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -528,18 +528,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
rmw_time_t * timeout_argument = NULL;
rmw_time_t temporary_timeout_storage;
if (timeout == 0) {
// Then it is non-blocking, so set the temporary storage to 0, 0 and pass it.
temporary_timeout_storage.sec = 0;
temporary_timeout_storage.nsec = 0;
timeout_argument = &temporary_timeout_storage;
} else if (timeout > 0 || wait_set->size_of_timers > 0) {
int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
// Compare the timeout to the time until next callback for each timer.
// Take the lowest and use that for the wait timeout.
// calculate the number of valid (non-NULL and non-canceled) timers
size_t number_of_valid_timers = wait_set->size_of_timers;
{ // scope to prevent i from colliding below
uint64_t i = 0;
for (i = 0; i < wait_set->impl->timer_index; ++i) {
if (!wait_set->timers[i]) {
number_of_valid_timers--;
continue; // Skip NULL timers.
}
bool is_canceled = false;
@ -548,18 +543,40 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
return ret; // The rcl error state should already be set.
}
if (is_canceled) {
continue; // Skip canceled timers.
number_of_valid_timers--;
wait_set->timers[i] = NULL;
}
}
}
bool is_timer_timeout = false;
if (timeout == 0) {
// Then it is non-blocking, so set the temporary storage to 0, 0 and pass it.
temporary_timeout_storage.sec = 0;
temporary_timeout_storage.nsec = 0;
timeout_argument = &temporary_timeout_storage;
} else if (timeout > 0 || number_of_valid_timers > 0) {
int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
// Compare the timeout to the time until next callback for each timer.
// Take the lowest and use that for the wait timeout.
uint64_t i = 0;
for (i = 0; i < wait_set->impl->timer_index; ++i) {
if (!wait_set->timers[i]) {
continue; // Skip NULL timers.
}
// at this point we know any non-NULL timers are also not canceled
int64_t timer_timeout = INT64_MAX;
ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if (timer_timeout < min_timeout) {
is_timer_timeout = true;
min_timeout = timer_timeout;
}
}
// If min_timeout was negative, we need to wake up immediately.
if (min_timeout < 0) {
min_timeout = 0;
@ -577,7 +594,23 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
&wait_set->impl->rmw_clients,
wait_set->impl->rmw_waitset,
timeout_argument);
// Check for timeout.
// Check for ready timers next
// and set not ready timers (which includes canceled timers) to NULL.
size_t i;
for (i = 0; i < wait_set->impl->timer_index; ++i) {
if (!wait_set->timers[i]) {
continue;
}
bool is_ready = false;
rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if (!is_ready) {
wait_set->timers[i] = NULL;
}
}
// 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;
@ -591,25 +624,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
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.
return RCL_RET_TIMEOUT;
}
// Check for error.
if (ret != RMW_RET_OK) {
if (!is_timer_timeout) {
return RCL_RET_TIMEOUT;
}
} else if (ret != RMW_RET_OK) {
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.
size_t i;
for (i = 0; i < wait_set->impl->timer_index; ++i) {
bool is_ready = false;
rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if (!is_ready) {
wait_set->timers[i] = NULL;
}
}
// Set corresponding rcl subscription handles NULL.
for (i = 0; i < wait_set->size_of_subscriptions; ++i) {
if (!wait_set->impl->rmw_subscriptions.subscribers[i]) {

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

@ -123,7 +123,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
ret = rcl_wait(&wait_set, timeout);
std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now();
// We expect a timeout here (timer value reached)
ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string_safe();
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
// Check time
int64_t diff = std::chrono::duration_cast<std::chrono::nanoseconds>(after_sc - before_sc).count();
EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE);