diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 157c86c..8219966 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -521,17 +521,16 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) temporary_timeout_storage.sec = 0; temporary_timeout_storage.nsec = 0; timeout_argument = &temporary_timeout_storage; - } else if (timeout > 0) { - int64_t min_timeout = INT64_MAX; + } 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. - min_timeout = timeout; // Take the lowest and use that for the wait timeout. uint64_t i = 0; for (i = 0; i < wait_set->size_of_timers; ++i) { if (!wait_set->timers[i]) { continue; // Skip NULL timers. } - int64_t timer_timeout; + int64_t timer_timeout = INT64_MAX; 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. @@ -540,17 +539,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) min_timeout = timer_timeout; } } - if (min_timeout == INT64_MAX) { - timeout_argument = NULL; - } else { - // If min_timeout was negative, we need to wake up immediately. - if (min_timeout < 0) { - min_timeout = 0; - } - temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout); - temporary_timeout_storage.nsec = min_timeout % 1000000000; - timeout_argument = &temporary_timeout_storage; + // If min_timeout was negative, we need to wake up immediately. + if (min_timeout < 0) { + min_timeout = 0; } + temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout); + temporary_timeout_storage.nsec = min_timeout % 1000000000; + timeout_argument = &temporary_timeout_storage; } // Wait. diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 28e96db..0434ac2 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -12,9 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include -#include +#include "gtest/gtest.h" +#include "../scope_exit.hpp" #include "rcl/rcl.h" #include "rcl/error_handling.h" #include "rcl/wait.h" @@ -27,17 +31,156 @@ #endif +#define TOLERANCE RCL_MS_TO_NS(6) + TEST(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()); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_resize_subscriptions(&wait_set, 0); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); EXPECT_EQ(wait_set.size_of_subscriptions, 0); ret = rcl_wait_set_fini(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); } + +// Test rcl_wait with a positive finite timeout value (1ms) +TEST(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(); + + int64_t timeout = RCL_MS_TO_NS(10); // nanoseconds + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // Check time + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, timeout + TOLERANCE); + + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); +} + +// Check that a timer overrides a negative timeout value (blocking forever) +TEST(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(); + + // Add a dummy guard condition to avoid an error + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init(&timer, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_add_timer(&wait_set, &timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + auto scope_exit = make_scope_exit([&guard_cond, &wait_set, &timer]() { + rcl_ret_t ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + }); + + int64_t timeout = -1; + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + 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(); + // Check time + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE); +} + +// Test rcl_wait with a timeout value of 0 (non-blocking) +TEST(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(); + + // Add a dummy guard condition to avoid an error + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + auto scope_exit = make_scope_exit([&guard_cond, &wait_set]() { + rcl_ret_t ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + ); + + + // Time spent during wait should be negligible. + int64_t timeout = 0; + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + 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(); + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, TOLERANCE); +} + +// 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) { + 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(); + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + auto scope_exit = make_scope_exit([&wait_set, &guard_cond]() { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + } + ); + + std::promise p; + + int64_t timeout = -1; + + std::chrono::nanoseconds trigger_diff; + std::thread trigger_thread([&p, &guard_cond, &trigger_diff]() { + std::chrono::steady_clock::time_point before_trigger = std::chrono::steady_clock::now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + rcl_ret_t ret = rcl_trigger_guard_condition(&guard_cond); + std::chrono::steady_clock::time_point after_trigger = std::chrono::steady_clock::now(); + trigger_diff = std::chrono::duration_cast( + after_trigger - before_trigger); + p.set_value(ret); + } + ); + auto f = p.get_future(); + + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + trigger_thread.join(); + EXPECT_EQ(RCL_RET_OK, f.get()); + EXPECT_LE(std::abs(diff - trigger_diff.count()), TOLERANCE); +}