diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index d4b2e42cae..e5ff744fe0 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -275,6 +275,11 @@ class ClockWaiter public: RCLCPP_SMART_PTR_DEFINITIONS(ClockWaiter) + /** + * @param clock The clock to wait on. May be null, so that every wait names the clock + * to wait on, for example because the clocks involved may be destroyed + * between waits. + */ RCLCPP_PUBLIC explicit ClockWaiter(const rclcpp::Clock::SharedPtr & clock); @@ -290,6 +295,7 @@ class ClockWaiter * @param pred may be called in cased of spurious wakeups, but must be called every time * notify_one() was called. During the call to pred, the given lock will be locked. * This method will return, if pred returns true. + * @throws std::runtime_error if this waiter was constructed without a clock. */ RCLCPP_PUBLIC bool @@ -297,6 +303,24 @@ class ClockWaiter std::unique_lock & lock, const rclcpp::Time & abs_time, const std::function & pred); + /** + * Like wait_until() above, but waiting on the given clock instead of the one this + * waiter was constructed with. + * @param lock A locked lock. The lock must be locked at call time, or this method will throw. + * The lock will be atomically released and this thread will blocked. + * @param clock The clock to wait on. Only used for the duration of this call. + * @param abs_time The time until which this thread shall be blocked. + * @param pred may be called in cased of spurious wakeups, but must be called every time + * notify_one() was called. During the call to pred, the given lock will be locked. + * This method will return, if pred returns true. + * @throws std::runtime_error if clock is null. + */ + RCLCPP_PUBLIC + bool + wait_until( + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, + const rclcpp::Time & abs_time, const std::function & pred); + /** * Notify the blocked thread, that it should reevaluate the wakeup condition. * The given pred function in wait_until will be reevaluated and wait_until @@ -324,10 +348,18 @@ class ClockConditionalVariable public: RCLCPP_SMART_PTR_DEFINITIONS(ClockConditionalVariable) + /** + * @param clock The clock to wait on. May be null, so that every wait names the clock + * to wait on, for example because the clocks involved may be destroyed + * between waits. + * @param context The context whose shutdown wakes the waiting thread. + * @throws std::runtime_error if the context is null or invalid. + */ RCLCPP_PUBLIC ClockConditionalVariable( const rclcpp::Clock::SharedPtr & clock, const rclcpp::Context::SharedPtr & context = rclcpp::contexts::get_global_default_context()); + RCLCPP_PUBLIC ~ClockConditionalVariable(); @@ -337,12 +369,13 @@ class ClockConditionalVariable * @param lock A locked lock. The lock must be locked at call time, or this method will throw. * The lock will be atomically released and this thread will blocked. * The given lock must be created using the mutex returned my mutex(). - * @param abs_time The time until which this thread shall be blocked. + * @param until The time until which this thread shall be blocked. * @param pred may be called in cased of spurious wakeups, but must be called every time * notify_one() was called. During the call to pred, the given lock will be locked. * This method will return, if pred returns true. * - * @return true if until was reached. + * @return false if the context was already shut down, true otherwise. + * @throws std::runtime_error if this conditional variable was constructed without a clock. */ RCLCPP_PUBLIC bool @@ -350,6 +383,27 @@ class ClockConditionalVariable std::unique_lock & lock, const rclcpp::Time & until, const std::function & pred); + /** + * Like wait_until() above, but waiting on the given clock instead of the one this + * conditional variable was constructed with. + * @param lock A locked lock. The lock must be locked at call time, or this method will throw. + * The lock will be atomically released and this thread will blocked. + * The given lock must be created using the mutex returned my mutex(). + * @param clock The clock to wait on. Only used for the duration of this call. + * @param until The time until which this thread shall be blocked. + * @param pred may be called in cased of spurious wakeups, but must be called every time + * notify_one() was called. During the call to pred, the given lock will be locked. + * This method will return, if pred returns true. + * + * @return false if the context was already shut down, true otherwise. + * @throws std::runtime_error if clock is null. + */ + RCLCPP_PUBLIC + bool + wait_until( + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, + const rclcpp::Time & until, const std::function & pred); + /** * Notify the blocked thread, that is should reevaluate the wakeup condition. * E.g. the given pred function in wait_until shall be reevaluated. diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 126daddd16..2a6b26f9ad 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -356,6 +356,7 @@ class ClockWaiter::ClockWaiterImpl private: std::condition_variable cv_; + /// The clock given at construction, null if the clock is named by each wait rclcpp::Clock::SharedPtr clock_; bool time_source_changed_ = false; std::function post_time_jump_callback; @@ -375,11 +376,11 @@ class ClockWaiter::ClockWaiterImpl bool wait_until_steady_time( - std::unique_lock & lock, + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, const rclcpp::Time & abs_time, const std::function & pred) { // Synchronize because RCL steady clock epoch might differ from chrono::steady_clock epoch - const rclcpp::Time rcl_entry = clock_->now(); + const rclcpp::Time rcl_entry = clock->now(); const std::chrono::steady_clock::time_point chrono_entry = std::chrono::steady_clock::now(); const rclcpp::Duration delta_t = abs_time - rcl_entry; const std::chrono::steady_clock::time_point chrono_until = @@ -391,7 +392,7 @@ class ClockWaiter::ClockWaiterImpl bool wait_until_ros_time( - std::unique_lock & lock, + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, const rclcpp::Time & abs_time, const std::function & pred) { // Install jump handler for any amount of time change, for two purposes: @@ -419,12 +420,12 @@ class ClockWaiter::ClockWaiterImpl // its more overhead to have the callback being called all the time. // As we expect the use case to be low frequency calls to wait_until // with relative big pauses between the calls, we install it on demand. - auto clock_handler = clock_->create_jump_callback( + auto clock_handler = clock->create_jump_callback( nullptr, post_time_jump_callback, threshold); - if (!clock_->ros_time_is_active()) { + if (!clock->ros_time_is_active()) { auto system_time = std::chrono::system_clock::time_point( // Cast because system clock resolution is too big for nanoseconds on some systems std::chrono::duration_cast( @@ -438,11 +439,11 @@ class ClockWaiter::ClockWaiterImpl // RCL_ROS_TIME with ros_time_is_active. // Just wait without "until" because installed // jump callbacks wake the cv on every new sample. - cv_.wait(lock, [this, &pred, &abs_time] () { - return clock_->now() >= abs_time || time_source_changed_ || pred(); + cv_.wait(lock, [this, &clock, &pred, &abs_time] () { + return clock->now() >= abs_time || time_source_changed_ || pred(); }); - return clock_->now() < abs_time; + return clock->now() < abs_time; } public: @@ -453,21 +454,18 @@ class ClockWaiter::ClockWaiterImpl bool wait_until( - std::unique_lock & lock, + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, const rclcpp::Time & abs_time, const std::function & pred) { - switch(clock_->get_clock_type()) { + switch(clock->get_clock_type()) { case RCL_CLOCK_UNINITIALIZED: throw std::runtime_error("Error, wait on uninitialized clock called"); case RCL_ROS_TIME: - return wait_until_ros_time(lock, abs_time, pred); - break; + return wait_until_ros_time(lock, clock, abs_time, pred); case RCL_STEADY_TIME: - return wait_until_steady_time(lock, abs_time, pred); - break; + return wait_until_steady_time(lock, clock, abs_time, pred); case RCL_SYSTEM_TIME: return wait_until_system_time(lock, abs_time, pred); - break; } return false; @@ -478,6 +476,12 @@ class ClockWaiter::ClockWaiterImpl { cv_.notify_one(); } + + const rclcpp::Clock::SharedPtr & + clock() const + { + return clock_; + } }; ClockWaiter::ClockWaiter(const rclcpp::Clock::SharedPtr & clock) @@ -492,7 +496,22 @@ ClockWaiter::wait_until( std::unique_lock & lock, const rclcpp::Time & abs_time, const std::function & pred) { - return impl_->wait_until(lock, abs_time, pred); + if (!impl_->clock()) { + throw std::runtime_error( + "ClockWaiter::wait_until: constructed without a clock, the wait must name one"); + } + return impl_->wait_until(lock, impl_->clock(), abs_time, pred); +} + +bool +ClockWaiter::wait_until( + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, + const rclcpp::Time & abs_time, const std::function & pred) +{ + if (!clock) { + throw std::runtime_error("ClockWaiter::wait_until: clock must not be null"); + } + return impl_->wait_until(lock, clock, abs_time, pred); } void @@ -507,12 +526,12 @@ class ClockConditionalVariable::Impl bool shutdown_ = false; rclcpp::Context::SharedPtr context_; rclcpp::OnShutdownCallbackHandle shutdown_cb_handle_; - ClockWaiter::UniquePtr clock_; + ClockWaiter waiter_; public: Impl(const rclcpp::Clock::SharedPtr & clock, const rclcpp::Context::SharedPtr & context) :context_(context), - clock_(std::make_unique(clock)) + waiter_(clock) { if (!context_ || !context_->is_valid()) { throw std::runtime_error("context cannot be slept with because it's invalid"); @@ -524,7 +543,7 @@ class ClockConditionalVariable::Impl std::unique_lock lock(pred_mutex_); shutdown_ = true; } - clock_->notify_one(); + waiter_.notify_one(); }); } @@ -533,10 +552,11 @@ class ClockConditionalVariable::Impl context_->remove_on_shutdown_callback(shutdown_cb_handle_); } + /// Wait on `clock`, or on the clock given at construction if `clock` is null bool wait_until( - std::unique_lock & lock, const rclcpp::Time & until, - const std::function & pred) + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, + const rclcpp::Time & until, const std::function & pred) { if(lock.mutex() != &pred_mutex_) { throw std::runtime_error( @@ -544,16 +564,26 @@ class ClockConditionalVariable::Impl " mutex returned by this->mutex()"); } - clock_->wait_until(lock, until, [this, &pred] () -> bool { + if(shutdown_) { + return false; + } + + const auto wake = [this, &pred] () -> bool { return shutdown_ || pred(); - }); + }; + if(clock) { + waiter_.wait_until(lock, clock, until, wake); + } else { + waiter_.wait_until(lock, until, wake); + } + return true; } void notify_one() { - clock_->notify_one(); + waiter_.notify_one(); } std::mutex & @@ -583,7 +613,18 @@ ClockConditionalVariable::wait_until( std::unique_lock & lock, const rclcpp::Time & until, const std::function & pred) { - return impl_->wait_until(lock, until, pred); + return impl_->wait_until(lock, nullptr, until, pred); +} + +bool +ClockConditionalVariable::wait_until( + std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, + const rclcpp::Time & until, const std::function & pred) +{ + if (!clock) { + throw std::runtime_error("ClockConditionalVariable::wait_until: clock must not be null"); + } + return impl_->wait_until(lock, clock, until, pred); } std::mutex & diff --git a/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp b/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp index 514cc63a1b..6561b0ca3d 100644 --- a/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp +++ b/rclcpp/src/rclcpp/executors/events_cbg_executor/timer_manager.hpp @@ -23,94 +23,14 @@ #include #include +#include +#include #include #include namespace rclcpp::executors::cbg_executor { -/** - * Specialized version of rclcpp::ClockConditionalVariable - * - * This version accepts the clock on waits instead of on construction. - * This is needed, as clocks may be deleted during normal operation, - * and be don't have a way to create a permanent ros time clock. - */ -class ClockConditionalVariable -{ - std::mutex pred_mutex_; - bool shutdown_ = false; - rclcpp::Context::SharedPtr context_; - rclcpp::OnShutdownCallbackHandle shutdown_cb_handle_; - ClockWaiter::UniquePtr clock_; - -public: - explicit ClockConditionalVariable(rclcpp::Context::SharedPtr context) - :context_(std::move(context)) - { - if (!context_ || !context_->is_valid()) { - throw std::runtime_error("context cannot be slept with because it's invalid"); - } - // Wake this thread if the context is shutdown - shutdown_cb_handle_ = context_->add_on_shutdown_callback( - [this]() { - std::unique_lock lock(pred_mutex_); - shutdown_ = true; - if(clock_) { - clock_->notify_one(); - } - }); - } - - ~ClockConditionalVariable() - { - context_->remove_on_shutdown_callback(shutdown_cb_handle_); - } - - bool - wait_until( - std::unique_lock & lock, const rclcpp::Clock::SharedPtr & clock, - const rclcpp::Time & until, - const std::function & pred) - { - if(lock.mutex() != &pred_mutex_) { - throw std::runtime_error( - "ClockConditionalVariable::wait_until: Internal error, given lock does not use" - " mutex returned by this->mutex()"); - } - - if(shutdown_) { - return false; - } - - clock_ = std::make_unique(clock); - - clock_->wait_until(lock, until, [this, &pred] () -> bool { - return shutdown_ || pred(); - }); - - clock_.reset(); - - return true; - } - - void - notify_one() - { - std::unique_lock lock(pred_mutex_); - - if(clock_) { - clock_->notify_one(); - } - } - - std::mutex & - mutex() - { - return pred_mutex_; - } -}; - /** * @brief A class for managing a queue of timers * @@ -140,7 +60,7 @@ class TimerQueue public: TimerQueue(rcl_clock_type_t timer_type, const rclcpp::Context::SharedPtr & context) - : timer_type(timer_type), clock_waiter(context) + : timer_type(timer_type), context(context), clock_waiter(nullptr, context) { // must be initialized here so that all class members // are initialized @@ -470,9 +390,14 @@ class TimerQueue return wake_up || !running || !rclcpp::ok(); }); wake_up = false; - } catch (const std::runtime_error &) { + } catch (const std::runtime_error & e) { // there is a race on shutdown, were the context may // become invalid, while we call sleep_until + if (rclcpp::ok(context)) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "timer thread failed to wait, stopping all timers of this type: %s", e.what()); + } running = false; } } else { @@ -486,9 +411,11 @@ class TimerQueue rcl_clock_type_t timer_type; + rclcpp::Context::SharedPtr context; + rclcpp::Clock::SharedPtr used_clock_for_timers; - ClockConditionalVariable clock_waiter; + rclcpp::ClockConditionalVariable clock_waiter; bool wake_up = false; std::mutex mutex; diff --git a/rclcpp/test/rclcpp/test_clock_conditional.cpp b/rclcpp/test/rclcpp/test_clock_conditional.cpp index 4b021bfa9e..114dfa024f 100644 --- a/rclcpp/test/rclcpp/test_clock_conditional.cpp +++ b/rclcpp/test/rclcpp/test_clock_conditional.cpp @@ -16,8 +16,10 @@ #include #include +#include #include #include +#include #include #include "rcl/error_handling.h" @@ -34,20 +36,44 @@ using namespace std::chrono_literals; class TestClockWakeup : public ::testing::TestWithParam { public: + /// Create a conditional variable bound to the clock, or one that is given the clock per wait + rclcpp::ClockConditionalVariable::UniquePtr + make_cond(const rclcpp::Clock::SharedPtr & clock) + { + if (clock_on_wait) { + return std::make_unique(nullptr); + } + return std::make_unique(clock); + } + + void + wait_until( + rclcpp::ClockConditionalVariable & cond, std::unique_lock & lk, + const rclcpp::Clock::SharedPtr & clock, const rclcpp::Time & until, + const std::function & pred) + { + if (clock_on_wait) { + cond.wait_until(lk, clock, until, pred); + } else { + cond.wait_until(lk, until, pred); + } + } + void test_wakeup_before_sleep(const rclcpp::Clock::SharedPtr & clock) { std::atomic_bool thread_finished = false; - rclcpp::ClockConditionalVariable cond(clock); + auto cond_ptr = make_cond(clock); + rclcpp::ClockConditionalVariable & cond = *cond_ptr; bool stopSleeping = false; std::thread wait_thread = std::thread( - [&cond, &clock, &stopSleeping, &thread_finished]() + [this, &cond, &clock, &stopSleeping, &thread_finished]() { // make sure the thread starts sleeping late std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::unique_lock lk(cond.mutex()); - cond.wait_until(lk, clock->now() + std::chrono::seconds(3), + wait_until(cond, lk, clock, clock->now() + std::chrono::seconds(3), [&stopSleeping] () {return stopSleeping;}); thread_finished = true; }); @@ -76,15 +102,16 @@ class TestClockWakeup : public ::testing::TestWithParam void test_wakeup_after_sleep(const rclcpp::Clock::SharedPtr & clock) { std::atomic_bool thread_finished = false; - rclcpp::ClockConditionalVariable cond(clock); + auto cond_ptr = make_cond(clock); + rclcpp::ClockConditionalVariable & cond = *cond_ptr; bool stopSleeping = false; std::thread wait_thread = std::thread( - [&cond, &clock, &stopSleeping, &thread_finished]() + [this, &cond, &clock, &stopSleeping, &thread_finished]() { std::unique_lock lk(cond.mutex()); - cond.wait_until(lk, clock->now() + std::chrono::seconds(3), + wait_until(cond, lk, clock, clock->now() + std::chrono::seconds(3), [&stopSleeping] () {return stopSleeping;}); thread_finished = true; }); @@ -113,6 +140,70 @@ class TestClockWakeup : public ::testing::TestWithParam EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); } + void test_wakeup_sleep_ros_time_active() + { + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + + EXPECT_TRUE(clock->ros_time_is_active()); + + test_wakeup_after_sleep(clock); + test_wakeup_before_sleep(clock); + } + + void test_no_wakeup_on_sim_time() + { + node->set_parameter({"use_sim_time", true}); + auto clock = std::make_shared(RCL_ROS_TIME); + EXPECT_FALSE(clock->ros_time_is_active()); + + rclcpp::TimeSource time_source(node); + time_source.attachClock(clock); + EXPECT_TRUE(clock->ros_time_is_active()); + + std::atomic_bool thread_finished = false; + auto cond_ptr = make_cond(clock); + rclcpp::ClockConditionalVariable & cond = *cond_ptr; + + bool stopSleeping = false; + + std::thread wait_thread = std::thread( + [this, &cond, &clock, &stopSleeping, &thread_finished]() + { + std::unique_lock lk(cond.mutex()); + // only sleep for an short period + wait_until(cond, lk, clock, clock->now() + std::chrono::milliseconds(10), + [&stopSleeping] () {return stopSleeping;}); + thread_finished = true; + }); + + // make sure, that the sim time clock does not wakeup, as no clock is provided + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + EXPECT_FALSE(thread_finished); + + { + std::lock_guard lk(cond.mutex()); + // stop sleeping after next notification + stopSleeping = true; + } + // notify the conditional, to recheck it pred + cond.notify_one(); + + auto start_time = std::chrono::steady_clock::now(); + auto cur_time = start_time; + while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + cur_time = std::chrono::steady_clock::now(); + } + + wait_thread.join(); + + EXPECT_TRUE(thread_finished); + EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); + } + protected: static void SetUpTestCase() { @@ -135,6 +226,9 @@ class TestClockWakeup : public ::testing::TestWithParam } rclcpp::Node::SharedPtr node; + + /// Name the clock on every wait, instead of binding it at construction + bool clock_on_wait = false; }; INSTANTIATE_TEST_SUITE_P( @@ -150,65 +244,118 @@ TEST_P(TestClockWakeup, wakeup_sleep) { test_wakeup_before_sleep(clock); } -TEST_F(TestClockWakeup, wakeup_sleep_ros_time_active) { - node->set_parameter({"use_sim_time", true}); - auto clock = std::make_shared(RCL_ROS_TIME); - rclcpp::TimeSource time_source(node); - time_source.attachClock(clock); - - EXPECT_TRUE(clock->ros_time_is_active()); - +TEST_P(TestClockWakeup, wakeup_sleep_clock_given_on_wait) { + clock_on_wait = true; + auto clock = std::make_shared(GetParam()); test_wakeup_after_sleep(clock); test_wakeup_before_sleep(clock); } +TEST_F(TestClockWakeup, wakeup_sleep_ros_time_active) { + test_wakeup_sleep_ros_time_active(); +} + +TEST_F(TestClockWakeup, wakeup_sleep_ros_time_active_clock_given_on_wait) { + clock_on_wait = true; + test_wakeup_sleep_ros_time_active(); +} + TEST_F(TestClockWakeup, no_wakeup_on_sim_time) { - node->set_parameter({"use_sim_time", true}); - auto clock = std::make_shared(RCL_ROS_TIME); - EXPECT_FALSE(clock->ros_time_is_active()); + test_no_wakeup_on_sim_time(); +} - rclcpp::TimeSource time_source(node); - time_source.attachClock(clock); - EXPECT_TRUE(clock->ros_time_is_active()); +TEST_F(TestClockWakeup, no_wakeup_on_sim_time_clock_given_on_wait) { + clock_on_wait = true; + test_no_wakeup_on_sim_time(); +} - std::atomic_bool thread_finished = false; +TEST_P(TestClockWakeup, clock_given_on_wait_is_not_retained) { + auto clock = std::make_shared(GetParam()); + rclcpp::ClockConditionalVariable cond(nullptr); + const auto use_count = clock.use_count(); + + std::unique_lock lk(cond.mutex()); + EXPECT_TRUE(cond.wait_until(lk, clock, clock->now() + 10ms, [] () {return false;})); + + EXPECT_EQ(use_count, clock.use_count()); +} + +TEST_F(TestClockWakeup, wait_without_clock_throws) { + rclcpp::ClockConditionalVariable cond(nullptr); + std::unique_lock lk(cond.mutex()); + + EXPECT_THROW( + cond.wait_until(lk, rclcpp::Time(0, 0), [] () {return true;}), + std::runtime_error); +} + +TEST_F(TestClockWakeup, wait_with_null_clock_throws) { + auto clock = std::make_shared(RCL_SYSTEM_TIME); rclcpp::ClockConditionalVariable cond(clock); + std::unique_lock lk(cond.mutex()); - bool stopSleeping = false; + // A null clock is an error, it does not fall back to the clock given at construction + EXPECT_THROW( + cond.wait_until(lk, nullptr, clock->now(), [] () {return true;}), + std::runtime_error); +} - std::thread wait_thread = std::thread( - [&cond, &clock, &stopSleeping, &thread_finished]() - { - std::unique_lock lk(cond.mutex()); - // only sleep for an short period - cond.wait_until(lk, clock->now() + std::chrono::milliseconds(10), - [&stopSleeping] () {return stopSleeping;}); - thread_finished = true; - }); +TEST_F(TestClockWakeup, wait_on_system_time_ignores_until_clock_type) { + // These paths only read until's nanoseconds, so a time of another clock type is accepted + auto ros_clock = std::make_shared(RCL_ROS_TIME); + ASSERT_FALSE(ros_clock->ros_time_is_active()); + auto system_clock = std::make_shared(RCL_SYSTEM_TIME); + const auto soon = [] (const rclcpp::Clock::SharedPtr & clock, rcl_clock_type_t type) { + return rclcpp::Time((clock->now() + 10ms).nanoseconds(), type); + }; + + rclcpp::ClockConditionalVariable ros_cond(ros_clock); + std::unique_lock ros_lk(ros_cond.mutex()); + EXPECT_NO_THROW( + ros_cond.wait_until(ros_lk, soon(ros_clock, RCL_SYSTEM_TIME), [] () {return false;})); + + rclcpp::ClockConditionalVariable system_cond(system_clock); + std::unique_lock system_lk(system_cond.mutex()); + EXPECT_NO_THROW( + system_cond.wait_until(system_lk, soon(system_clock, RCL_ROS_TIME), [] () {return false;})); +} - // make sure, that the sim time clock does not wakeup, as no clock is provided - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - EXPECT_FALSE(thread_finished); +TEST_P(TestClockWakeup, clock_waiter_clock_given_on_wait_times_out) { + auto clock = std::make_shared(GetParam()); + rclcpp::ClockWaiter waiter(nullptr); + std::mutex mutex; + std::unique_lock lk(mutex); - { - std::lock_guard lk(cond.mutex()); - // stop sleeping after next notification - stopSleeping = true; - } - // notify the conditional, to recheck it pred - cond.notify_one(); + const auto start_time = std::chrono::steady_clock::now(); + EXPECT_FALSE(waiter.wait_until(lk, clock, clock->now() + 10ms, [] () {return false;})); + EXPECT_LT(std::chrono::steady_clock::now(), start_time + 1s); +} - auto start_time = std::chrono::steady_clock::now(); - auto cur_time = start_time; - while (!thread_finished && start_time + std::chrono::seconds(1) > cur_time) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - cur_time = std::chrono::steady_clock::now(); - } +TEST_F(TestClockWakeup, clock_waiter_without_clock_throws) { + rclcpp::ClockWaiter waiter(nullptr); + std::mutex mutex; + std::unique_lock lk(mutex); + + EXPECT_THROW( + waiter.wait_until(lk, rclcpp::Time(0, 0), [] () {return true;}), + std::runtime_error); + EXPECT_THROW( + waiter.wait_until(lk, nullptr, rclcpp::Time(0, 0), [] () {return true;}), + std::runtime_error); +} - wait_thread.join(); +TEST_F(TestClockWakeup, wait_after_shutdown_returns_false) { + auto context = std::make_shared(); + context->init(0, nullptr); - EXPECT_TRUE(thread_finished); - EXPECT_LT(cur_time, start_time + std::chrono::seconds(1)); + auto clock = std::make_shared(RCL_STEADY_TIME); + rclcpp::ClockConditionalVariable cond(clock, context); + + context->shutdown("test done"); + + std::unique_lock lk(cond.mutex()); + EXPECT_FALSE( + cond.wait_until(lk, clock->now() + std::chrono::seconds(10), [] () {return false;})); } TEST_F(TestClockWakeup, wakeup_on_ros_shutdown) {