From 07eaf14048962be831410deedbd5c894d49f9ca5 Mon Sep 17 00:00:00 2001
From: Janosch Machowinski <J.Machowinski@cellumation.com>
Date: Mon, 25 Mar 2024 16:15:20 +0100
Subject: [PATCH] feat(clock): Allow to pass condition_variable for sleep

This allows us to wait on a external clock, while also have the
ability to wake up, if an operation finishes.

Signed-off-by: Janosch Machowinski <J.Machowinski@cellumation.com>
---
 rclcpp/include/rclcpp/clock.hpp | 24 ++++++++++++++++++++++++
 rclcpp/src/rclcpp/clock.cpp     | 27 ++++++++++++++++++++++++---
 2 files changed, 48 insertions(+), 3 deletions(-)

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)
 {