diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index 702b224d53..cd1682f8c1 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -113,6 +113,30 @@ class Clock Time until, Context::SharedPtr context = contexts::get_global_default_context()); + /** + * Sleep until a specified Time, according to clock type, with the ability to provide + * a condition_variable to interrupt the sleep operation. + * + * For futher information see \rclcpp::Clock::sleep_until + * + * \param until absolute time according to current clock type to sleep until. + * \param context the rclcpp context the clock should use to check that ROS is still initialized. + * \param cv Conditional that is internally used for the sleep operation + * \param ignore_wakeups if set to true, the function will ignore any wakeup until the given sleep time has passend + * \return true immediately if `until` is in the past + * \return true when the time `until` is reached + * \return false if time cannot be reached reliably, for example from shutdown or a change + * of time source. + * \throws std::runtime_error if the context is invalid + * \throws std::runtime_error if `until` has a different clock type from this clock + */ + bool + sleep_until( + Time until, + std::condition_variable & cv, + bool ignore_wakeups, + Context::SharedPtr context = contexts::get_global_default_context()); + /** * Sleep for a specified Duration. * diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index f46649a77d..615d4e7426 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -80,7 +80,11 @@ Clock::now() const } bool -Clock::sleep_until(Time until, Context::SharedPtr context) +Clock::sleep_until( + Time until, + std::condition_variable & cv, + bool ignore_wakeups, + Context::SharedPtr context) { if (!context || !context->is_valid()) { throw std::runtime_error("context cannot be slept with because it's invalid"); @@ -91,8 +95,6 @@ Clock::sleep_until(Time until, Context::SharedPtr context) } bool time_source_changed = false; - std::condition_variable cv; - // Wake this thread if the context is shutdown rclcpp::OnShutdownCallbackHandle shutdown_cb_handle = context->add_on_shutdown_callback( [&cv]() { @@ -116,6 +118,9 @@ Clock::sleep_until(Time until, Context::SharedPtr context) std::unique_lock lock(impl_->clock_mutex_); while (now() < until && context->is_valid()) { cv.wait_until(lock, chrono_until); + if (!ignore_wakeups) { + break; + } } } else if (this_clock_type == RCL_SYSTEM_TIME) { auto system_time = std::chrono::system_clock::time_point( @@ -127,6 +132,9 @@ Clock::sleep_until(Time until, Context::SharedPtr context) std::unique_lock lock(impl_->clock_mutex_); while (now() < until && context->is_valid()) { cv.wait_until(lock, system_time); + if (!ignore_wakeups) { + break; + } } } else if (this_clock_type == RCL_ROS_TIME) { // Install jump handler for any amount of time change, for two purposes: @@ -157,6 +165,9 @@ Clock::sleep_until(Time until, Context::SharedPtr context) std::unique_lock lock(impl_->clock_mutex_); while (now() < until && context->is_valid() && !time_source_changed) { cv.wait_until(lock, system_time); + if (!ignore_wakeups) { + break; + } } } else { // RCL_ROS_TIME with ros_time_is_active. @@ -165,6 +176,9 @@ Clock::sleep_until(Time until, Context::SharedPtr context) std::unique_lock lock(impl_->clock_mutex_); while (now() < until && context->is_valid() && !time_source_changed) { cv.wait(lock); + if (!ignore_wakeups) { + break; + } } } } @@ -176,6 +190,13 @@ Clock::sleep_until(Time until, Context::SharedPtr context) return now() >= until; } +bool +Clock::sleep_until(Time until, Context::SharedPtr context) +{ + std::condition_variable cv; + return sleep_until(until, cv, true, context); +} + bool Clock::sleep_for(Duration rel_time, Context::SharedPtr context) {