From cf57154c36f6c6b73f1514766622ca64cbf6ed33 Mon Sep 17 00:00:00 2001 From: Pranshu Rao <29.pranshurao@gmail.com> Date: Sat, 13 Jun 2026 22:46:49 -0700 Subject: [PATCH] feat(rclrs): add Clock sleep and async sleep APIs Add sleep_for, sleep_until, async_sleep_for, and async_sleep_until to Clock, aligned with rclcpp semantics. Async variants yield to the runtime so they work under use_sim_time without blocking the executor. Fixes #604 --- rclrs/src/clock.rs | 163 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 161 insertions(+), 2 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index 8dd543455..4fd0aab67 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -1,5 +1,8 @@ -use crate::{error::ToResult, rcl_bindings::*, time::Time, to_rclrs_result}; -use std::sync::{Arc, Mutex}; +use crate::{error::ToResult, rcl_bindings::*, time::Time, to_rclrs_result, Context}; +use std::{ + sync::{Arc, Mutex}, + time::Duration, +}; /// Enum to describe clock type. Redefined for readability and to eliminate the uninitialized case /// from the `rcl_clock_type_t` enum in the binding. @@ -38,6 +41,20 @@ pub struct ClockSource { rcl_clock: Arc>, } +/// How long a sleep loop waits between checks for shutdown and clock progress. +/// This caps the wakeup latency for detecting shutdown and, under sim time, the +/// latency for noticing that the clock has advanced. +const SLEEP_POLL_TICK: Duration = Duration::from_millis(10); + +/// The outcome of a single iteration of a sleep loop. +enum SleepStep { + /// The sleep is finished. The boolean is the value to return: `true` if the + /// target time was reached, `false` if the context became invalid. + Done(bool), + /// The target time has not been reached yet. Wait this long and check again. + Wait(Duration), +} + impl Clock { /// Creates a new Clock with `ClockType::SystemTime` pub fn system() -> Self { @@ -111,6 +128,98 @@ impl Clock { clock: Arc::downgrade(&self.rcl_clock), } } + + /// Returns `true` if this is a [`ClockType::RosTime`] clock that is currently + /// being driven by an external time source, i.e. `use_sim_time` is active. + /// + /// System and steady clocks always return `false`. + pub fn ros_time_is_active(&self) -> bool { + if !matches!(self.kind, ClockType::RosTime) { + return false; + } + let mut clock = self.rcl_clock.lock().unwrap(); + let mut is_enabled = false; + // SAFETY: The clock is valid and the out-pointer is a valid reference. + let ret = unsafe { rcl_is_enabled_ros_time_override(&mut *clock, &mut is_enabled) }; + debug_assert_eq!(ret, 0); + is_enabled + } + + /// Sleeps until this clock reaches the time `until`, according to the clock's type. + /// + /// Returns `true` once the target time is reached. Returns `false` if `context` + /// becomes invalid, for example because ROS was shut down, before the target time + /// is reached. + /// + /// Returns immediately if the target time is already in the past. + /// + /// # Notes for [`ClockType::RosTime`] + /// When ROS time is active, i.e. `use_sim_time:=true`, this can block forever if the + /// external `/clock` source never advances to `until`. Time only advances while the + /// executor that owns the node's `/clock` subscription is spinning, so calling this + /// from a callback on a single-threaded executor will deadlock under sim time. Prefer + /// [`Clock::async_sleep_until`] or driving sim-time sleeps from a separate thread. + pub fn sleep_until(&self, until: Time, context: &Context) -> bool { + // Evaluated once: a time-source change mid-sleep is not detected by this + // implementation. See issue #604 for a jump-callback-based follow-up. + let wall_based = !self.ros_time_is_active(); + loop { + match self.sleep_step(wall_based, until.nsec, context) { + SleepStep::Done(reached) => return reached, + SleepStep::Wait(tick) => std::thread::sleep(tick), + } + } + } + + /// Sleeps for the relative duration `duration`, according to the clock's type. + /// + /// Equivalent to `self.sleep_until(self.now() + duration, context)`. See + /// [`Clock::sleep_until`] for the return value and sim-time caveats. + pub fn sleep_for(&self, duration: Duration, context: &Context) -> bool { + self.sleep_until(self.now() + duration, context) + } + + /// The async counterpart of [`Clock::sleep_until`]. + /// + /// This yields to the async runtime between checks instead of blocking the thread, + /// so it can be awaited inside an async callback without preventing the executor from + /// spinning. That makes it safe to use under sim time, where the `/clock` subscription + /// must keep being processed for the clock to advance. + pub async fn async_sleep_until(&self, until: Time, context: &Context) -> bool { + let wall_based = !self.ros_time_is_active(); + loop { + match self.sleep_step(wall_based, until.nsec, context) { + SleepStep::Done(reached) => return reached, + SleepStep::Wait(tick) => async_std::task::sleep(tick).await, + } + } + } + + /// The async counterpart of [`Clock::sleep_for`]. + /// + /// Equivalent to `self.async_sleep_until(self.now() + duration, context).await`. + pub async fn async_sleep_for(&self, duration: Duration, context: &Context) -> bool { + self.async_sleep_until(self.now() + duration, context).await + } + + /// Computes one iteration of a sleep loop. Shared by the sync and async sleep methods. + fn sleep_step(&self, wall_based: bool, until_nsec: i64, context: &Context) -> SleepStep { + let now = self.now().nsec; + if now >= until_nsec { + return SleepStep::Done(true); + } + if !context.ok() { + return SleepStep::Done(false); + } + let tick = if wall_based { + // For system and steady clocks `until_nsec - now` is a real elapsed + // duration, so we can wait most of it, capped so we still notice shutdown. + Duration::from_nanos((until_nsec - now) as u64).min(SLEEP_POLL_TICK) + } else { + SLEEP_POLL_TICK + }; + SleepStep::Wait(tick) + } } impl Drop for ClockSource { @@ -212,4 +321,54 @@ mod tests { // Ros time is set, should return the value that was set assert_eq!(clock.now().nsec, set_time); } + + #[test] + fn sleep_for_steady_time_waits() { + let context = Context::default(); + let clock = Clock::steady(); + let start = clock.now().nsec; + let reached = clock.sleep_for(Duration::from_millis(50), &context); + let elapsed = clock.now().nsec - start; + assert!(reached); + assert!(elapsed >= 50_000_000); + } + + #[test] + fn sleep_until_past_returns_immediately() { + let context = Context::default(); + let clock = Clock::system(); + let until = clock.now() - Duration::from_secs(1); + assert!(clock.sleep_until(until, &context)); + } + + #[test] + fn sleep_for_ros_time_advances_with_source() { + use std::thread; + let context = Context::default(); + let (clock, source) = Clock::with_source(); + source.set_ros_time_override(0); + + let advancer = thread::spawn(move || { + thread::sleep(Duration::from_millis(50)); + // Push ROS time well past the 5s target. + source.set_ros_time_override(10_000_000_000); + }); + + let reached = clock.sleep_for(Duration::from_secs(5), &context); + advancer.join().unwrap(); + assert!(reached); + } + + #[tokio::test] + async fn async_sleep_for_steady_time_waits() { + let context = Context::default(); + let clock = Clock::steady(); + let start = clock.now().nsec; + let reached = clock + .async_sleep_for(Duration::from_millis(50), &context) + .await; + let elapsed = clock.now().nsec - start; + assert!(reached); + assert!(elapsed >= 50_000_000); + } }