From 2a610cad22bd4477b56b6219dab8c5af4e81418a Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Fri, 25 Oct 2024 09:26:24 +0000 Subject: [PATCH 01/19] * Basic init and fini of the timer class --- rclrs/src/clock.rs | 2 +- rclrs/src/lib.rs | 1 + rclrs/src/timer.rs | 138 +++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 140 insertions(+), 1 deletion(-) create mode 100644 rclrs/src/timer.rs diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index f7c085e14..311eed9ee 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -28,7 +28,7 @@ impl From for rcl_clock_type_t { #[derive(Clone, Debug)] pub struct Clock { kind: ClockType, - rcl_clock: Arc>, + pub(crate) rcl_clock: Arc>, // TODO(luca) Implement jump callbacks } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 3a22c6da8..f4881bccd 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -20,6 +20,7 @@ mod service; mod subscription; mod time; mod time_source; +mod timer; mod vendor; mod wait; diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs new file mode 100644 index 000000000..a426f1e8e --- /dev/null +++ b/rclrs/src/timer.rs @@ -0,0 +1,138 @@ +use crate::{ + clock::Clock, + rcl_bindings::{ + rcl_get_zero_initialized_timer, rcl_timer_fini, rcl_timer_init, rcl_timer_t, + rcutils_get_default_allocator, + }, + NodeHandle, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, +}; +use std::{ + sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, + time::Duration, +}; + +// SAFETY: The functions accessing this type, including drop(), shouldn't care +// about the thread they are running in (partly because they're protected by mutex). +// Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_timer_t {} + +/// Manage the lifecycle of an `rcl_timer_t`, including managing its dependencies +/// on `rcl_clock_t` and `rcl_context_t` by ensuring that these dependencies are +/// [dropped after][1] the `rcl_timer_t`. +/// +/// [1]: +pub(crate) struct TimerHandle { + rcl_timer: Mutex, + clock: Clock, + node_handle: Arc, + pub(crate) in_use_by_wait_set: Arc, +} + +impl TimerHandle { + pub(crate) fn lock(&self) -> MutexGuard { + self.rcl_timer.lock().unwrap() + } +} + +impl Drop for TimerHandle { + fn drop(&mut self) { + let rcl_timer = self.rcl_timer.get_mut().unwrap(); + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of + // global variables in the rmw implementation being unsafely modified during cleanup. + unsafe { + rcl_timer_fini(rcl_timer); + } + } +} + +/// Trait to be implemented by concrete [`Timer`]s. +pub(crate) trait TimerBase: Send + Sync { + /// Internal function to get a reference to the `rcl` handle. + fn handle(&self) -> &TimerHandle; + /// Tries to call the timer and run the associated callback. + fn execute(&self) -> Result<(), RclrsError>; +} + +struct Timer { + callback: Arc, + handle: TimerHandle, +} + +impl Timer { + pub(crate) fn new( + node_handle: Arc, + clock: Clock, + period: &Duration, + callback: F, + ) -> Result + where + F: Fn(&mut Timer) + 'static + Send + Sync, + { + // Move the callback to our reference counted container so rcl_callback can use it + let callback = Arc::new(callback); + + // SAFETY: Getting a zero-initialized value is always safe. + let mut rcl_timer = unsafe { rcl_get_zero_initialized_timer() }; + + let clock_clone = clock.rcl_clock.clone(); + let mut rcl_clock = clock_clone.lock().unwrap(); + + let node_handle_clone = node_handle.clone(); + let mut rcl_context = node_handle_clone.context_handle.rcl_context.lock().unwrap(); + + // core::time::Duration will always be >= 0, so no need to check for negatives. + let period_nanos = period.as_nanos() as i64; + + // SAFETY: No preconditions for this function. + let allocator = unsafe { rcutils_get_default_allocator() }; + { + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: + // * The rcl_timer is zero-initialized as mandated by this function. + // * The rcl_clock is kept alive by the Clock within TimerHandle because it is + // a dependency of the timer. + // * The rcl_context is kept alive by the NodeHandle within TimerHandle because + // it is a dependency of the timer. + // * The period is copied into this function so it can be dropped afterwards. + // * The callback is None / nullptr so doesn't need to be kept alive. + // * The entity lifecycle mutex is locked to protect against the risk of global + // variables in the rmw implementation being unsafely modified during cleanup. + rcl_timer_init( + &mut rcl_timer, + &mut *rcl_clock, + &mut *rcl_context, + period_nanos, + None, + allocator, + ) + .ok()?; + } + } + + Ok(Self { + callback, + handle: TimerHandle { + rcl_timer: Mutex::new(rcl_timer), + clock, + node_handle, + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), + }, + }) + } +} + +impl TimerBase for Timer { + fn handle(&self) -> &TimerHandle { + &self.handle + } + + fn execute(&self) -> Result<(), RclrsError> { + // let mut callback = self.callback.clone(); + // callback(&mut self); + Ok(()) + } +} + +// tests: timers with 0 periods should return immediately, could be a good test for the timer From 98ff99d725ac7038c51c6bba47fcfbfcf26faadf Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Fri, 25 Oct 2024 09:27:03 +0000 Subject: [PATCH 02/19] * Fixed typo claiming to protect against cleanup as part of rcl_*_init --- rclrs/src/publisher.rs | 2 +- rclrs/src/subscription.rs | 2 +- rclrs/src/timer.rs | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..adf449fee 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -108,7 +108,7 @@ where // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the publisher. // * The topic name and the options are copied by this function, so they can be dropped afterwards. // * The entity lifecycle mutex is locked to protect against the risk of global - // variables in the rmw implementation being unsafely modified during cleanup. + // variables in the rmw implementation being unsafely modified during initialization. rcl_publisher_init( &mut rcl_publisher, &*rcl_node, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 05b01beb5..3ecd7513a 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -126,7 +126,7 @@ where // * The rcl_node is kept alive by the NodeHandle because it is a dependency of the subscription. // * The topic name and the options are copied by this function, so they can be dropped afterwards. // * The entity lifecycle mutex is locked to protect against the risk of global - // variables in the rmw implementation being unsafely modified during cleanup. + // variables in the rmw implementation being unsafely modified during initialization. rcl_subscription_init( &mut rcl_subscription, &*rcl_node, diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index a426f1e8e..c247bdb42 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -98,7 +98,7 @@ impl Timer { // * The period is copied into this function so it can be dropped afterwards. // * The callback is None / nullptr so doesn't need to be kept alive. // * The entity lifecycle mutex is locked to protect against the risk of global - // variables in the rmw implementation being unsafely modified during cleanup. + // variables in the rmw implementation being unsafely modified during initialization. rcl_timer_init( &mut rcl_timer, &mut *rcl_clock, From 71bc220b17ce3180a2e0ca8fafefab7ccc9646e8 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 10:10:23 +1000 Subject: [PATCH 03/19] * Added remaining rcl_timer functions --- rclrs/src/timer.rs | 137 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 133 insertions(+), 4 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index c247bdb42..19be1c277 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -1,12 +1,16 @@ use crate::{ clock::Clock, rcl_bindings::{ - rcl_get_zero_initialized_timer, rcl_timer_fini, rcl_timer_init, rcl_timer_t, + rcl_get_zero_initialized_timer, rcl_timer_call, rcl_timer_cancel, + rcl_timer_exchange_period, rcl_timer_fini, rcl_timer_get_period, + rcl_timer_get_time_since_last_call, rcl_timer_get_time_until_next_call, rcl_timer_init, + rcl_timer_is_canceled, rcl_timer_is_ready, rcl_timer_reset, rcl_timer_t, rcutils_get_default_allocator, }, NodeHandle, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, }; use std::{ + i64, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, time::Duration, }; @@ -21,7 +25,7 @@ unsafe impl Send for rcl_timer_t {} /// [dropped after][1] the `rcl_timer_t`. /// /// [1]: -pub(crate) struct TimerHandle { +pub struct TimerHandle { rcl_timer: Mutex, clock: Clock, node_handle: Arc, @@ -54,12 +58,14 @@ pub(crate) trait TimerBase: Send + Sync { fn execute(&self) -> Result<(), RclrsError>; } -struct Timer { +pub struct Timer { callback: Arc, handle: TimerHandle, } impl Timer { + /// Creates a new `Timer` with the given period and callback. + /// Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. pub(crate) fn new( node_handle: Arc, clock: Clock, @@ -82,7 +88,7 @@ impl Timer { let mut rcl_context = node_handle_clone.context_handle.rcl_context.lock().unwrap(); // core::time::Duration will always be >= 0, so no need to check for negatives. - let period_nanos = period.as_nanos() as i64; + let period_nanos = i64::try_from(period.as_nanos()).unwrap_or(i64::MAX); // SAFETY: No preconditions for this function. let allocator = unsafe { rcutils_get_default_allocator() }; @@ -121,6 +127,129 @@ impl Timer { }, }) } + + /// Calculates if the timer is ready to be called. + /// Returns true if the timer is due or past due to be called. + pub fn is_ready(&self) -> Result { + let mut timer = self.handle.lock(); + let mut is_ready = false; + // SAFETY: + // * The timer is initialized, which is guaranteed by the constructor. + // * The is_ready pointer is allocated on the stack and is valid for the duration of this function. + unsafe { + rcl_timer_is_ready(&mut *timer, &mut is_ready).ok()?; + } + Ok(is_ready) + } + + /// Get the time until the next call of the timer is due. Saturates to 0 if the timer is ready. + pub fn time_until_next_call(&self) -> Result { + let mut timer = self.handle.lock(); + let mut remaining_time = 0; + // SAFETY: + // * The timer is initialized, which is guaranteed by the constructor. + // * The remaining_time pointer is allocated on the stack and is valid for the duration of this function. + unsafe { + rcl_timer_get_time_until_next_call(&mut *timer, &mut remaining_time).ok()?; + } + Ok(Duration::from_nanos( + u64::try_from(remaining_time).unwrap_or(0), + )) + } + + /// Get the time since the last call of the timer. + /// Calling this function within a callback will not return the time since the + /// previous call but instead the time since the current callback was called. + /// Saturates to 0 if the timer was last called in the future (i.e. the clock jumped). + pub fn time_since_last_call(&self) -> Result { + let mut timer = self.handle.lock(); + let mut elapsed_time = 0; + // SAFETY: + // * The timer is initialized, which is guaranteed by the constructor. + // * The elapsed_time pointer is allocated on the stack and is valid for the duration of this function. + unsafe { + rcl_timer_get_time_since_last_call(&mut *timer, &mut elapsed_time).ok()?; + } + Ok(Duration::from_nanos( + u64::try_from(elapsed_time).unwrap_or(0), + )) + } + + /// Get the period of the timer. + pub fn get_period(&self) -> Result { + let timer = self.handle.lock(); + let mut period = 0; + // SAFETY: + // * The timer is initialized, which is guaranteed by the constructor. + // * The period pointer is allocated on the stack and is valid for the duration of this function. + unsafe { + rcl_timer_get_period(&*timer, &mut period).ok()?; + } + // The period should never be negative as we only expose (unsigned) Duration structs + // for setting, but if it is, saturate to 0. + Ok(Duration::from_nanos(u64::try_from(period).unwrap_or(0))) + } + + /// Set the period of the timer. Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. + pub fn set_period(&self, period: &Duration) -> Result<(), RclrsError> { + let timer = self.handle.lock(); + let new_period = i64::try_from(period.as_nanos()).unwrap_or(i64::MAX); + let mut old_period = 0; + // SAFETY: + // * The timer is initialized, which is guaranteed by the constructor. + // * The new_period is copied into this function so it can be dropped afterwards. + // * The old_period pointer is allocated on the stack and is valid for the duration of this function. + unsafe { + rcl_timer_exchange_period(&*timer, new_period, &mut old_period).ok()?; + } + Ok(()) + } + + /// Cancel the timer so it will no longer be fired by the waitset. Can be restarted with [`reset`][1]. + /// + /// [1]: crate::timer::Timer::reset + pub fn cancel(&self) -> Result<(), RclrsError> { + let mut timer = self.handle.lock(); + // SAFETY: The timer is initialized, which is guaranteed by the constructor. + unsafe { + rcl_timer_cancel(&mut *timer).ok()?; + } + Ok(()) + } + + /// Check if the timer has been cancelled. + pub fn is_canceled(&self) -> Result { + let timer = self.handle.lock(); + let mut cancelled = false; + // SAFETY: + // * The timer is initialized, which is guaranteed by the constructor. + // * The new_period is copied into this function so it can be dropped afterwards. + // * The old_period pointer is allocated on the stack and is valid for the duration of this function. + unsafe { + rcl_timer_is_canceled(&*timer, &mut cancelled).ok()?; + } + Ok(cancelled) + } + + /// Set the timer's last call time to now. Additionally marks cancelled timers as not-cancelled. + pub fn reset(&self) -> Result<(), RclrsError> { + let mut timer = self.handle.lock(); + // SAFETY: The timer is initialized, which is guaranteed by the constructor. + unsafe { + rcl_timer_reset(&mut *timer).ok()?; + } + Ok(()) + } + + /// Internal function to check the timer is still valid and set the last call time in rcl. + fn call_rcl(&self) -> Result<(), RclrsError> { + let mut timer = self.handle.lock(); + // SAFETY: Safe if the timer is initialized, which is guaranteed by the constructor. + unsafe { + rcl_timer_call(&mut *timer).ok()?; + } + Ok(()) + } } impl TimerBase for Timer { From d182dba1bc6f94ec3a14f8a23495f0355a53ff15 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 01:00:46 +0000 Subject: [PATCH 04/19] * basic rcl usage tests for timer --- rclrs/src/timer.rs | 98 ++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 95 insertions(+), 3 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 19be1c277..c696bab7f 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -69,7 +69,7 @@ impl Timer { pub(crate) fn new( node_handle: Arc, clock: Clock, - period: &Duration, + period: Duration, callback: F, ) -> Result where @@ -191,7 +191,7 @@ impl Timer { } /// Set the period of the timer. Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. - pub fn set_period(&self, period: &Duration) -> Result<(), RclrsError> { + pub fn set_period(&self, period: Duration) -> Result<(), RclrsError> { let timer = self.handle.lock(); let new_period = i64::try_from(period.as_nanos()).unwrap_or(i64::MAX); let mut old_period = 0; @@ -264,4 +264,96 @@ impl TimerBase for Timer { } } -// tests: timers with 0 periods should return immediately, could be a good test for the timer +// Timer.rs does very little other than call rcl functions. +// To keep these tests easy to maintain, most of them just check the rcl functions +// can be called without returning errors. +#[cfg(test)] +mod tests { + use std::time::Duration; + + use crate::{create_node, Context}; + + use super::Timer; + + // Pass in a new node name each time to avoid logging conflicts. + fn new_timer(node_name: &str) -> Timer { + let node = create_node(&Context::new([]).unwrap(), node_name).unwrap(); + + let timer = Timer::new( + node.handle.clone(), + node.get_clock(), + Duration::from_secs(0), + |_| {}, + ); + + timer.expect("Timer::new should not return an error") + } + + #[test] + fn creation() { + let _ = new_timer("test_timer_creation"); + } + + #[test] + fn is_ready() { + let timer = new_timer("test_timer_is_ready"); + + // Period is 0, so the timer should be already ready + timer + .is_ready() + .expect("Timer::is_ready should not return an error"); + } + + #[test] + fn time_until_next_call() { + let timer = new_timer("test_timer_next_call"); + + timer + .time_until_next_call() + .expect("Timer::time_until_next_call should not error"); + } + + #[test] + fn time_since_last_call() { + let timer = new_timer("test_timer_last_call"); + + timer + .time_since_last_call() + .expect("Timer::time_since_last_call should not error"); + } + + #[test] + fn update_period() { + let timer = new_timer("test_timer_update_period"); + + let new_period = Duration::from_millis(100); + timer + .set_period(new_period.clone()) + .expect("Timer::set_period should not error"); + + let retrieved_period = timer.get_period().unwrap(); + + assert_eq!(new_period, retrieved_period); + } + + #[test] + fn cancel_timer() { + let timer = new_timer("test_timer_cancel"); + + assert!(!timer.is_canceled().unwrap()); + + timer.cancel().unwrap(); + + assert!(timer.is_canceled().unwrap()); + } + + #[test] + fn reset_cancelled_timer() { + let timer = new_timer("test_timer_reset"); + timer.cancel().unwrap(); + + timer.reset().unwrap(); + + assert!(!timer.is_canceled().unwrap()); + } +} From bb539c35cbe8f0fb99b28f1b68e4bb49ec49641c Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 12:14:05 +1000 Subject: [PATCH 05/19] * Replaced returning Result with debug_asserts for functions that should never error. --- rclrs/src/timer.rs | 140 +++++++++++++++++++++++++-------------------- 1 file changed, 78 insertions(+), 62 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index c696bab7f..480fa39ca 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -7,7 +7,7 @@ use crate::{ rcl_timer_is_canceled, rcl_timer_is_ready, rcl_timer_reset, rcl_timer_t, rcutils_get_default_allocator, }, - NodeHandle, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, + NodeHandle, RclReturnCode, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, }; use std::{ i64, @@ -130,19 +130,24 @@ impl Timer { /// Calculates if the timer is ready to be called. /// Returns true if the timer is due or past due to be called. - pub fn is_ready(&self) -> Result { + /// Returns false if the timer is not yet due or has been canceled. + pub fn is_ready(&self) -> bool { let mut timer = self.handle.lock(); let mut is_ready = false; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. // * The is_ready pointer is allocated on the stack and is valid for the duration of this function. - unsafe { - rcl_timer_is_ready(&mut *timer, &mut is_ready).ok()?; - } - Ok(is_ready) + let ret = unsafe { rcl_timer_is_ready(&mut *timer, &mut is_ready) }; + + // rcl_timer_is_ready should only error if incorrect arguments are given or something isn't initialised, + // both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); + + is_ready } /// Get the time until the next call of the timer is due. Saturates to 0 if the timer is ready. + /// Returns [`RclReturnCode::TimerCanceled`] as an error if the timer has already been canceled. pub fn time_until_next_call(&self) -> Result { let mut timer = self.handle.lock(); let mut remaining_time = 0; @@ -161,37 +166,41 @@ impl Timer { /// Calling this function within a callback will not return the time since the /// previous call but instead the time since the current callback was called. /// Saturates to 0 if the timer was last called in the future (i.e. the clock jumped). - pub fn time_since_last_call(&self) -> Result { + pub fn time_since_last_call(&self) -> Duration { let mut timer = self.handle.lock(); let mut elapsed_time = 0; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. // * The elapsed_time pointer is allocated on the stack and is valid for the duration of this function. - unsafe { - rcl_timer_get_time_since_last_call(&mut *timer, &mut elapsed_time).ok()?; - } - Ok(Duration::from_nanos( - u64::try_from(elapsed_time).unwrap_or(0), - )) + let ret = unsafe { rcl_timer_get_time_since_last_call(&mut *timer, &mut elapsed_time) }; + + // rcl_timer_get_time_since_last_call should only error if incorrect arguments are given + // or something isn't initialised, both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); + + Duration::from_nanos(u64::try_from(elapsed_time).unwrap_or(0)) } /// Get the period of the timer. - pub fn get_period(&self) -> Result { + pub fn get_period(&self) -> Duration { let timer = self.handle.lock(); let mut period = 0; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. // * The period pointer is allocated on the stack and is valid for the duration of this function. - unsafe { - rcl_timer_get_period(&*timer, &mut period).ok()?; - } + let ret = unsafe { rcl_timer_get_period(&*timer, &mut period) }; + + // rcl_timer_get_period should only error if incorrect arguments are given + // or something isn't initialised, both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); + // The period should never be negative as we only expose (unsigned) Duration structs // for setting, but if it is, saturate to 0. - Ok(Duration::from_nanos(u64::try_from(period).unwrap_or(0))) + Duration::from_nanos(u64::try_from(period).unwrap_or(0)) } /// Set the period of the timer. Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. - pub fn set_period(&self, period: Duration) -> Result<(), RclrsError> { + pub fn set_period(&self, period: Duration) { let timer = self.handle.lock(); let new_period = i64::try_from(period.as_nanos()).unwrap_or(i64::MAX); let mut old_period = 0; @@ -199,49 +208,55 @@ impl Timer { // * The timer is initialized, which is guaranteed by the constructor. // * The new_period is copied into this function so it can be dropped afterwards. // * The old_period pointer is allocated on the stack and is valid for the duration of this function. - unsafe { - rcl_timer_exchange_period(&*timer, new_period, &mut old_period).ok()?; - } - Ok(()) + let ret = unsafe { rcl_timer_exchange_period(&*timer, new_period, &mut old_period) }; + + // rcl_timer_exchange_period should only error if incorrect arguments are given + // or something isn't initialised, both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); } - /// Cancel the timer so it will no longer be fired by the waitset. Can be restarted with [`reset`][1]. + /// Cancel the timer so it will no longer return ready. Can be restarted with [`reset`][1]. /// /// [1]: crate::timer::Timer::reset - pub fn cancel(&self) -> Result<(), RclrsError> { + pub fn cancel(&self) { let mut timer = self.handle.lock(); // SAFETY: The timer is initialized, which is guaranteed by the constructor. - unsafe { - rcl_timer_cancel(&mut *timer).ok()?; - } - Ok(()) + let ret = unsafe { rcl_timer_cancel(&mut *timer) }; + + // rcl_timer_cancel should only error if incorrect arguments are given + // or something isn't initialised, both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); } - /// Check if the timer has been cancelled. - pub fn is_canceled(&self) -> Result { + /// Check if the timer has been canceled. + pub fn is_canceled(&self) -> bool { let timer = self.handle.lock(); - let mut cancelled = false; + let mut canceled = false; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. - // * The new_period is copied into this function so it can be dropped afterwards. - // * The old_period pointer is allocated on the stack and is valid for the duration of this function. - unsafe { - rcl_timer_is_canceled(&*timer, &mut cancelled).ok()?; - } - Ok(cancelled) + // * The canceled pointer is allocated on the stack and is valid for the duration of this function. + let ret = unsafe { rcl_timer_is_canceled(&*timer, &mut canceled) }; + + // rcl_timer_is_canceled should only error if incorrect arguments are given + // or something isn't initialised, both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); + + canceled } - /// Set the timer's last call time to now. Additionally marks cancelled timers as not-cancelled. - pub fn reset(&self) -> Result<(), RclrsError> { + /// Set the timer's last call time to now. Additionally marks canceled timers as not-canceled. + pub fn reset(&self) { let mut timer = self.handle.lock(); // SAFETY: The timer is initialized, which is guaranteed by the constructor. - unsafe { - rcl_timer_reset(&mut *timer).ok()?; - } - Ok(()) + let ret = unsafe { rcl_timer_reset(&mut *timer) }; + + // rcl_timer_reset should only error if incorrect arguments are given + // or something isn't initialised, both of which we control in this function. + debug_assert_eq!(RclReturnCode::try_from(ret).unwrap(), RclReturnCode::Ok); } /// Internal function to check the timer is still valid and set the last call time in rcl. + /// Returns [`RclReturnCode::TimerCanceled`] as an error if the timer has already been canceled. fn call_rcl(&self) -> Result<(), RclrsError> { let mut timer = self.handle.lock(); // SAFETY: Safe if the timer is initialized, which is guaranteed by the constructor. @@ -298,10 +313,8 @@ mod tests { fn is_ready() { let timer = new_timer("test_timer_is_ready"); - // Period is 0, so the timer should be already ready - timer - .is_ready() - .expect("Timer::is_ready should not return an error"); + // Calling is_ready will trigger the debug_assert check on the rcl return value. + timer.is_ready(); } #[test] @@ -317,9 +330,8 @@ mod tests { fn time_since_last_call() { let timer = new_timer("test_timer_last_call"); - timer - .time_since_last_call() - .expect("Timer::time_since_last_call should not error"); + // Calling time_since_last_call will trigger the debug_assert check on the rcl return value. + timer.time_since_last_call(); } #[test] @@ -327,11 +339,12 @@ mod tests { let timer = new_timer("test_timer_update_period"); let new_period = Duration::from_millis(100); - timer - .set_period(new_period.clone()) - .expect("Timer::set_period should not error"); - let retrieved_period = timer.get_period().unwrap(); + // Calling set_period will trigger the debug_assert check on the rcl return value. + timer.set_period(new_period.clone()); + + // Calling get_period will trigger the debug_assert check on the rcl return value. + let retrieved_period = timer.get_period(); assert_eq!(new_period, retrieved_period); } @@ -340,20 +353,23 @@ mod tests { fn cancel_timer() { let timer = new_timer("test_timer_cancel"); - assert!(!timer.is_canceled().unwrap()); + // Calling is_canceled will trigger the debug_assert check on the rcl return value. + assert!(!timer.is_canceled()); - timer.cancel().unwrap(); + // Calling cancel will trigger the debug_assert check on the rcl return value. + timer.cancel(); - assert!(timer.is_canceled().unwrap()); + assert!(timer.is_canceled()); } #[test] - fn reset_cancelled_timer() { + fn reset_canceled_timer() { let timer = new_timer("test_timer_reset"); - timer.cancel().unwrap(); + timer.cancel(); - timer.reset().unwrap(); + // Calling reset will trigger the debug_assert check on the rcl return value. + timer.reset(); - assert!(!timer.is_canceled().unwrap()); + assert!(!timer.is_canceled()); } } From a5a3d22b8067cb1f1053b3d7516e8b0834a2f348 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 12:33:20 +1000 Subject: [PATCH 06/19] * Implemented TimerBase::execute --- rclrs/src/timer.rs | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 480fa39ca..116a69139 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -55,7 +55,8 @@ pub(crate) trait TimerBase: Send + Sync { /// Internal function to get a reference to the `rcl` handle. fn handle(&self) -> &TimerHandle; /// Tries to call the timer and run the associated callback. - fn execute(&self) -> Result<(), RclrsError>; + /// Timers are allowed to modify themselves while being executed. + fn execute(&mut self) -> Result<(), RclrsError>; } pub struct Timer { @@ -272,9 +273,13 @@ impl TimerBase for Timer { &self.handle } - fn execute(&self) -> Result<(), RclrsError> { - // let mut callback = self.callback.clone(); - // callback(&mut self); + fn execute(&mut self) -> Result<(), RclrsError> { + // Timer still needs to be called within RCL, even though we handle the callback ourselves. + self.call_rcl()?; + + let callback = self.callback.clone(); + callback(self); + Ok(()) } } @@ -323,7 +328,7 @@ mod tests { timer .time_until_next_call() - .expect("Timer::time_until_next_call should not error"); + .expect("Calling Timer::time_until_next_call on a non-canceled timer should not error"); } #[test] From 8d6007a62cff86fcd07bdd488f7bad89c0f8aa07 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 15:46:33 +1000 Subject: [PATCH 07/19] * Added timers to Node and WaitSet --- rclrs/src/executor.rs | 4 +++ rclrs/src/lib.rs | 1 + rclrs/src/node.rs | 41 ++++++++++++++++++++++-- rclrs/src/node/builder.rs | 1 + rclrs/src/timer.rs | 23 +++++++++++--- rclrs/src/wait.rs | 66 +++++++++++++++++++++++++++++++++++++-- 6 files changed, 126 insertions(+), 10 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..b6e609c9b 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -61,6 +61,10 @@ impl SingleThreadedExecutor { for ready_service in ready_entities.services { ready_service.execute()?; } + + for ready_timer in ready_entities.timers { + ready_timer.lock().unwrap().execute()?; + } } Ok(()) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index f4881bccd..ddb3e9c3f 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -50,6 +50,7 @@ pub use service::*; pub use subscription::*; pub use time::*; use time_source::*; +pub use timer::*; pub use wait::*; /// Polls the node for new messages and executes the corresponding callbacks. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 394c5f740..8f451bbdc 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -6,6 +6,7 @@ use std::{ fmt, os::raw::c_char, sync::{atomic::AtomicBool, Arc, Mutex, Weak}, + time::Duration, vec::Vec, }; @@ -16,7 +17,7 @@ use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + TimeSource, Timer, TimerBase, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -47,7 +48,7 @@ unsafe impl Send for rcl_node_t {} /// The namespace and name given when creating the node can be overridden through the command line. /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. -/// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the +/// See also the [official tutorial][2] on the command line arguments for ROS nodes, and the /// [`Node::namespace()`] and [`Node::name()`] functions for examples. /// /// ## Rules for valid names @@ -63,6 +64,7 @@ pub struct Node { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, + pub(crate) timers_mtx: Mutex>>>, time_source: TimeSource, parameter: ParameterInterface, pub(crate) handle: Arc, @@ -339,6 +341,34 @@ impl Node { Ok(subscription) } + /// Create a [`Timer`][1]. + /// + /// A Timer may be modified via the `Arc` returned by this function or from + /// within its callback. + /// A weak pointer to the `Timer` is stored within this node. + /// + /// [1]: crate::Timer + pub fn create_timer( + &self, + period: Duration, + callback: F, + ) -> Result>, RclrsError> + where + F: Fn(&mut Timer) + 'static + Send + Sync, + { + let timer = Arc::new(Mutex::new(Timer::new( + Arc::clone(&self.handle), + self.get_clock(), + period, + callback, + )?)); + + { self.timers_mtx.lock() } + .unwrap() + .push(Arc::downgrade(&timer) as Weak>); + Ok(timer) + } + /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { { self.subscriptions_mtx.lock().unwrap() } @@ -368,6 +398,13 @@ impl Node { .collect() } + pub(crate) fn live_timers(&self) -> Vec>> { + { self.timers_mtx.lock().unwrap() } + .iter() + .filter_map(Weak::upgrade) + .collect() + } + /// Returns the ROS domain ID that the node is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index f8df82101..a5e76eba6 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -319,6 +319,7 @@ impl NodeBuilder { guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), + timers_mtx: Mutex::new(vec![]), time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 116a69139..6b0a52de2 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -27,8 +27,8 @@ unsafe impl Send for rcl_timer_t {} /// [1]: pub struct TimerHandle { rcl_timer: Mutex, - clock: Clock, - node_handle: Arc, + _clock: Clock, + _node_handle: Arc, pub(crate) in_use_by_wait_set: Arc, } @@ -51,7 +51,7 @@ impl Drop for TimerHandle { } /// Trait to be implemented by concrete [`Timer`]s. -pub(crate) trait TimerBase: Send + Sync { +pub trait TimerBase: Send + Sync { /// Internal function to get a reference to the `rcl` handle. fn handle(&self) -> &TimerHandle; /// Tries to call the timer and run the associated callback. @@ -59,6 +59,19 @@ pub(crate) trait TimerBase: Send + Sync { fn execute(&mut self) -> Result<(), RclrsError>; } +/// A struct for triggering a callback at regular intervals. +/// +/// Calling [`spin_once`][1] or [`spin`][2] on the timer's node will wait until the configured +/// period of time has passed since the timer was last called (or since it was created) before +/// triggering the timer's callback. +/// +/// The only available way to instantiate timers is via [`Node::create_timer()`][3], this +/// is to ensure that [`Node`][4]s can track all the timers that have been created. +/// +/// [1]: crate::spin_once +/// [2]: crate::spin +/// [3]: crate::Node::create_timer +/// [4]: crate::Node pub struct Timer { callback: Arc, handle: TimerHandle, @@ -122,8 +135,8 @@ impl Timer { callback, handle: TimerHandle { rcl_timer: Mutex::new(rcl_timer), - clock, - node_handle, + _clock: clock, + _node_handle: node_handle, in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }, }) diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 2ef99c026..ea775dc84 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -15,12 +15,16 @@ // DISTRIBUTION A. Approved for public release; distribution unlimited. // OPSEC #4584. -use std::{sync::Arc, time::Duration, vec::Vec}; +use std::{ + sync::{Arc, Mutex}, + time::Duration, + vec::Vec, +}; use crate::{ error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, + ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, TimerBase, }; mod exclusivity_guard; @@ -50,6 +54,8 @@ pub struct WaitSet { // The guard conditions that are currently registered in the wait set. guard_conditions: Vec>>, services: Vec>>, + // Timers need interior mutability to modify themselves within their callback. + timers: Vec>>>, handle: WaitSetHandle, } @@ -63,6 +69,9 @@ pub struct ReadyEntities { pub guard_conditions: Vec>, /// A list of services that have potentially received requests. pub services: Vec>, + /// A list of timers that are ready to be called. + // Timers need interior mutability to modify themselves within their callback. + pub timers: Vec>>, } impl Drop for rcl_wait_set_t { @@ -123,6 +132,7 @@ impl WaitSet { guard_conditions: Vec::new(), clients: Vec::new(), services: Vec::new(), + timers: Vec::new(), handle: WaitSetHandle { rcl_wait_set, context_handle: Arc::clone(&context.handle), @@ -138,13 +148,14 @@ impl WaitSet { let live_clients = node.live_clients(); let live_guard_conditions = node.live_guard_conditions(); let live_services = node.live_services(); + let live_timers = node.live_timers(); let ctx = Context { handle: Arc::clone(&node.handle.context_handle), }; let mut wait_set = WaitSet::new( live_subscriptions.len(), live_guard_conditions.len(), - 0, + live_timers.len(), live_clients.len(), live_services.len(), 0, @@ -166,6 +177,10 @@ impl WaitSet { for live_service in &live_services { wait_set.add_service(live_service.clone())?; } + + for live_timer in &live_timers { + wait_set.add_timer(live_timer.clone())?; + } Ok(wait_set) } @@ -178,6 +193,7 @@ impl WaitSet { self.guard_conditions.clear(); self.clients.clear(); self.services.clear(); + self.timers.clear(); // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is // valid, which it always is in our case. Hence, only debug_assert instead of returning // Result. @@ -311,6 +327,39 @@ impl WaitSet { Ok(()) } + /// Adds a timer to the wait set. + /// + /// # Errors + /// - If the timer was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of timers in the wait set is larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode + pub fn add_timer(&mut self, timer: Arc>) -> Result<(), RclrsError> { + let exclusive_timer = ExclusivityGuard::new( + Arc::clone(&timer), + Arc::clone(&timer.lock().unwrap().handle().in_use_by_wait_set), + )?; + unsafe { + // SAFETY: + // * The WaitSet is initialized, which is guaranteed by the constructor. + // * Timer pointer will remain valid for as long as the wait set exists, + // because it's stored in self.timers. + // * Null pointer for `index` is explicitly allowed and doesn't need + // to be kept alive. + rcl_wait_set_add_timer( + &mut self.handle.rcl_wait_set, + &*timer.lock().unwrap().handle().lock(), + core::ptr::null_mut(), + ) + } + .ok()?; + self.timers.push(exclusive_timer); + Ok(()) + } + /// Blocks until the wait set is ready, or until the timeout has been exceeded. /// /// If the timeout is `None` then this function will block indefinitely until @@ -365,6 +414,7 @@ impl WaitSet { clients: Vec::new(), guard_conditions: Vec::new(), services: Vec::new(), + timers: Vec::new(), }; for (i, subscription) in self.subscriptions.iter().enumerate() { // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is @@ -409,6 +459,16 @@ impl WaitSet { ready_entities.services.push(Arc::clone(&service.waitable)); } } + + for (i, timer) in self.timers.iter().enumerate() { + // SAFETY: The `timers` entry is an array of pointers, and this dereferencing is + // equivalent to + // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 + let wait_set_entry = unsafe { *self.handle.rcl_wait_set.timers.add(i) }; + if !wait_set_entry.is_null() { + ready_entities.timers.push(Arc::clone(&timer.waitable)); + } + } Ok(ready_entities) } } From 9378aab78d7a6c713ce560f8ccff1e495997a6df Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 16:00:39 +1000 Subject: [PATCH 08/19] * Replaced NodeHandle within Timer with a ContextHandle --- rclrs/src/node.rs | 2 +- rclrs/src/timer.rs | 40 ++++++++++++++++++++-------------------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 8f451bbdc..0af80a6d8 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -357,7 +357,7 @@ impl Node { F: Fn(&mut Timer) + 'static + Send + Sync, { let timer = Arc::new(Mutex::new(Timer::new( - Arc::clone(&self.handle), + Arc::clone(&self.handle.context_handle), self.get_clock(), period, callback, diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 6b0a52de2..9735f6623 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -7,7 +7,7 @@ use crate::{ rcl_timer_is_canceled, rcl_timer_is_ready, rcl_timer_reset, rcl_timer_t, rcutils_get_default_allocator, }, - NodeHandle, RclReturnCode, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, + ContextHandle, RclReturnCode, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, }; use std::{ i64, @@ -28,7 +28,7 @@ unsafe impl Send for rcl_timer_t {} pub struct TimerHandle { rcl_timer: Mutex, _clock: Clock, - _node_handle: Arc, + _context_handle: Arc, pub(crate) in_use_by_wait_set: Arc, } @@ -81,7 +81,7 @@ impl Timer { /// Creates a new `Timer` with the given period and callback. /// Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. pub(crate) fn new( - node_handle: Arc, + context_handle: Arc, clock: Clock, period: Duration, callback: F, @@ -98,8 +98,8 @@ impl Timer { let clock_clone = clock.rcl_clock.clone(); let mut rcl_clock = clock_clone.lock().unwrap(); - let node_handle_clone = node_handle.clone(); - let mut rcl_context = node_handle_clone.context_handle.rcl_context.lock().unwrap(); + let context_handle_clone = context_handle.clone(); + let mut rcl_context = context_handle_clone.rcl_context.lock().unwrap(); // core::time::Duration will always be >= 0, so no need to check for negatives. let period_nanos = i64::try_from(period.as_nanos()).unwrap_or(i64::MAX); @@ -113,7 +113,7 @@ impl Timer { // * The rcl_timer is zero-initialized as mandated by this function. // * The rcl_clock is kept alive by the Clock within TimerHandle because it is // a dependency of the timer. - // * The rcl_context is kept alive by the NodeHandle within TimerHandle because + // * The rcl_context is kept alive by the ContextHandle within TimerHandle because // it is a dependency of the timer. // * The period is copied into this function so it can be dropped afterwards. // * The callback is None / nullptr so doesn't need to be kept alive. @@ -136,7 +136,7 @@ impl Timer { handle: TimerHandle { rcl_timer: Mutex::new(rcl_timer), _clock: clock, - _node_handle: node_handle, + _context_handle: context_handle, in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }, }) @@ -304,17 +304,17 @@ impl TimerBase for Timer { mod tests { use std::time::Duration; - use crate::{create_node, Context}; + use crate::{Clock, Context}; use super::Timer; - // Pass in a new node name each time to avoid logging conflicts. - fn new_timer(node_name: &str) -> Timer { - let node = create_node(&Context::new([]).unwrap(), node_name).unwrap(); + fn new_timer() -> Timer { + let context = Context::new([]).unwrap(); + let clock = Clock::system(); let timer = Timer::new( - node.handle.clone(), - node.get_clock(), + context.handle.clone(), + clock, Duration::from_secs(0), |_| {}, ); @@ -324,12 +324,12 @@ mod tests { #[test] fn creation() { - let _ = new_timer("test_timer_creation"); + let _ = new_timer(); } #[test] fn is_ready() { - let timer = new_timer("test_timer_is_ready"); + let timer = new_timer(); // Calling is_ready will trigger the debug_assert check on the rcl return value. timer.is_ready(); @@ -337,7 +337,7 @@ mod tests { #[test] fn time_until_next_call() { - let timer = new_timer("test_timer_next_call"); + let timer = new_timer(); timer .time_until_next_call() @@ -346,7 +346,7 @@ mod tests { #[test] fn time_since_last_call() { - let timer = new_timer("test_timer_last_call"); + let timer = new_timer(); // Calling time_since_last_call will trigger the debug_assert check on the rcl return value. timer.time_since_last_call(); @@ -354,7 +354,7 @@ mod tests { #[test] fn update_period() { - let timer = new_timer("test_timer_update_period"); + let timer = new_timer(); let new_period = Duration::from_millis(100); @@ -369,7 +369,7 @@ mod tests { #[test] fn cancel_timer() { - let timer = new_timer("test_timer_cancel"); + let timer = new_timer(); // Calling is_canceled will trigger the debug_assert check on the rcl return value. assert!(!timer.is_canceled()); @@ -382,7 +382,7 @@ mod tests { #[test] fn reset_canceled_timer() { - let timer = new_timer("test_timer_reset"); + let timer = new_timer(); timer.cancel(); // Calling reset will trigger the debug_assert check on the rcl return value. From 3f2946109c3d0cde2e494353928df2af635e51ae Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 16:40:17 +1000 Subject: [PATCH 09/19] * Added unit test to confirm a timer will trigger the waitset. --- rclrs/src/node.rs | 2 +- rclrs/src/timer.rs | 26 ++++++++++++++++++-------- rclrs/src/wait.rs | 26 ++++++++++++++++++++++++++ 3 files changed, 45 insertions(+), 9 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 0af80a6d8..0ad0d9783 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -356,7 +356,7 @@ impl Node { where F: Fn(&mut Timer) + 'static + Send + Sync, { - let timer = Arc::new(Mutex::new(Timer::new( + let timer = Arc::new(Mutex::new(Timer::new_with_context_handle( Arc::clone(&self.handle.context_handle), self.get_clock(), period, diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 9735f6623..7ab6b464f 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -7,7 +7,7 @@ use crate::{ rcl_timer_is_canceled, rcl_timer_is_ready, rcl_timer_reset, rcl_timer_t, rcutils_get_default_allocator, }, - ContextHandle, RclReturnCode, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, + Context, ContextHandle, RclReturnCode, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, }; use std::{ i64, @@ -80,7 +80,20 @@ pub struct Timer { impl Timer { /// Creates a new `Timer` with the given period and callback. /// Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. - pub(crate) fn new( + pub fn new( + context: &Context, + clock: Clock, + period: Duration, + callback: F, + ) -> Result + where + F: Fn(&mut Timer) + 'static + Send + Sync, + { + Timer::new_with_context_handle(Arc::clone(&context.handle), clock, period, callback) + } + + /// Version of [`Timer::new`] that takes a context handle directly. + pub(crate) fn new_with_context_handle( context_handle: Arc, clock: Clock, period: Duration, @@ -310,14 +323,11 @@ mod tests { fn new_timer() -> Timer { let context = Context::new([]).unwrap(); + + // This is technically a wall clock, but we have a period of 0 so it won't slow down unit testing. let clock = Clock::system(); - let timer = Timer::new( - context.handle.clone(), - clock, - Duration::from_secs(0), - |_| {}, - ); + let timer = Timer::new(&context, clock, Duration::from_secs(0), |_| {}); timer.expect("Timer::new should not return an error") } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index ea775dc84..3d4995087 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -475,6 +475,8 @@ impl WaitSet { #[cfg(test)] mod tests { + use crate::{Clock, Timer}; + use super::*; #[test] @@ -500,4 +502,28 @@ mod tests { Ok(()) } + + #[test] + fn timer_in_wait_set_readies() -> Result<(), RclrsError> { + let context = Context::new([])?; + // This is technically a wall clock, but we have a period of 0 so it won't slow down unit testing. + let clock = Clock::system(); + + let timer: Arc> = Arc::new(Mutex::new(Timer::new( + &context, + clock, + Duration::from_secs(0), + |_| {}, + )?)); + let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; + + assert!(wait_set.timers.is_empty()); + wait_set.add_timer(Arc::clone(&timer))?; + + let readies = wait_set.wait(Some(std::time::Duration::from_millis(10)))?; + + assert_eq!(readies.timers.len(), 1); + + Ok(()) + } } From 1f37cfabb527d28aeae0388c6efccd04ddcd04d0 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 17:10:12 +1000 Subject: [PATCH 10/19] * Added unit test to executor to confirm spinning will trigger a timer callback. --- rclrs/src/executor.rs | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index b6e609c9b..0da45b5eb 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -86,3 +86,28 @@ impl SingleThreadedExecutor { Ok(()) } } + +#[cfg(test)] +mod tests { + use crate::{spin_once, Context}; + + use super::*; + + #[test] + fn spin_once_fires_timer() -> Result<(), RclrsError> { + let context = Context::new([])?; + let node = Node::new(&context, "test_spin_timer")?; + + let callback_triggered = Arc::new(Mutex::new(false)); + let callback_flag = Arc::clone(&callback_triggered); + + let _timer = node.create_timer(Duration::from_secs(0), move |_| { + *callback_flag.lock().unwrap() = true; + })?; + + spin_once(node, Some(Duration::ZERO))?; + + assert!(*callback_triggered.lock().unwrap()); + Ok(()) + } +} From fc72b29f58a352f5b74249f7536b45d5d74d7c1f Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 17:58:40 +1000 Subject: [PATCH 11/19] * Changed the timer callback type to an FnMut, added some helper docs --- rclrs/src/executor.rs | 6 +++--- rclrs/src/node.rs | 2 +- rclrs/src/timer.rs | 15 ++++++++++----- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 0da45b5eb..530bb51db 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -98,16 +98,16 @@ mod tests { let context = Context::new([])?; let node = Node::new(&context, "test_spin_timer")?; - let callback_triggered = Arc::new(Mutex::new(false)); + let callback_triggered = Arc::new(Mutex::new(0)); let callback_flag = Arc::clone(&callback_triggered); let _timer = node.create_timer(Duration::from_secs(0), move |_| { - *callback_flag.lock().unwrap() = true; + *callback_flag.lock().unwrap() += 1; })?; spin_once(node, Some(Duration::ZERO))?; - assert!(*callback_triggered.lock().unwrap()); + assert_eq!(*callback_triggered.lock().unwrap(), 1); Ok(()) } } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 0ad0d9783..ed19f61ac 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -354,7 +354,7 @@ impl Node { callback: F, ) -> Result>, RclrsError> where - F: Fn(&mut Timer) + 'static + Send + Sync, + F: FnMut(&mut Timer) + 'static + Send + Sync, { let timer = Arc::new(Mutex::new(Timer::new_with_context_handle( Arc::clone(&self.handle.context_handle), diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 7ab6b464f..1d18998b9 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -73,7 +73,7 @@ pub trait TimerBase: Send + Sync { /// [3]: crate::Node::create_timer /// [4]: crate::Node pub struct Timer { - callback: Arc, + callback: Arc>, handle: TimerHandle, } @@ -87,7 +87,7 @@ impl Timer { callback: F, ) -> Result where - F: Fn(&mut Timer) + 'static + Send + Sync, + F: FnMut(&mut Timer) + 'static + Send + Sync, { Timer::new_with_context_handle(Arc::clone(&context.handle), clock, period, callback) } @@ -100,10 +100,10 @@ impl Timer { callback: F, ) -> Result where - F: Fn(&mut Timer) + 'static + Send + Sync, + F: FnMut(&mut Timer) + 'static + Send + Sync, { // Move the callback to our reference counted container so rcl_callback can use it - let callback = Arc::new(callback); + let callback = Arc::new(Mutex::new(callback)); // SAFETY: Getting a zero-initialized value is always safe. let mut rcl_timer = unsafe { rcl_get_zero_initialized_timer() }; @@ -227,6 +227,11 @@ impl Timer { } /// Set the period of the timer. Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. + /// + /// The updated period will not take affect until either [`reset`][1] is called + /// or the timer next expires, whichever comes first. + /// + /// [1]: crate::Timer::reset pub fn set_period(&self, period: Duration) { let timer = self.handle.lock(); let new_period = i64::try_from(period.as_nanos()).unwrap_or(i64::MAX); @@ -304,7 +309,7 @@ impl TimerBase for Timer { self.call_rcl()?; let callback = self.callback.clone(); - callback(self); + (*callback.lock().unwrap())(self); Ok(()) } From 9ed47e4d10abc89133f298ad116b9a577afc1977 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 23:13:10 +0000 Subject: [PATCH 12/19] * Updated documentation around creating timers --- rclrs/src/node.rs | 4 ++-- rclrs/src/timer.rs | 27 ++++++++++++++++++--------- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index ed19f61ac..dd36623b7 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -341,11 +341,11 @@ impl Node { Ok(subscription) } - /// Create a [`Timer`][1]. + /// Create a [`Timer`][1] that will use the node's clock. /// /// A Timer may be modified via the `Arc` returned by this function or from /// within its callback. - /// A weak pointer to the `Timer` is stored within this node. + /// A weak reference counter to the `Timer` is stored within this node. /// /// [1]: crate::Timer pub fn create_timer( diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 1d18998b9..fefd76ead 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -61,17 +61,20 @@ pub trait TimerBase: Send + Sync { /// A struct for triggering a callback at regular intervals. /// -/// Calling [`spin_once`][1] or [`spin`][2] on the timer's node will wait until the configured -/// period of time has passed since the timer was last called (or since it was created) before -/// triggering the timer's callback. +/// If created via [`Node::create_timer()`][1], calling [`spin_once`][2] or [`spin`][3] +/// on the timer's node will wait until the configured period of time has passed since +/// the timer was last called (or since it was created) before triggering the timer's callback. /// -/// The only available way to instantiate timers is via [`Node::create_timer()`][3], this -/// is to ensure that [`Node`][4]s can track all the timers that have been created. +/// If created via [`Timer::new`], [`is_ready`][4] must be polled until the timer has +/// expired, after which [`execute`][5] must be called to trigger the timer's callback. +/// The timer can also be added to a [`WaitSet`][6] to block until it is ready. /// -/// [1]: crate::spin_once -/// [2]: crate::spin -/// [3]: crate::Node::create_timer -/// [4]: crate::Node +/// [1]: crate::Node::create_timer +/// [2]: crate::spin_once +/// [3]: crate::spin +/// [4]: crate::Timer::is_ready +/// [5]: crate::Timer::execute +/// [6]: crate::WaitSet pub struct Timer { callback: Arc>, handle: TimerHandle, @@ -80,6 +83,12 @@ pub struct Timer { impl Timer { /// Creates a new `Timer` with the given period and callback. /// Periods greater than i64::MAX nanoseconds will saturate to i64::MAX. + /// + /// Note that most of the time [`Node::create_timer`][1] is the better way to make + /// a new timer, as that will allow the timer to be triggered by spinning the node. + /// Timers created with [`Timer::new`] must be checked and executed by the user. + /// + /// [1]: crate::Node::create_timer pub fn new( context: &Context, clock: Clock, From a295956ef38b1cf6e81a7daec33316ee581422bf Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Mon, 28 Oct 2024 23:58:17 +0000 Subject: [PATCH 13/19] * Removed outdated comment --- rclrs/src/timer.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index fefd76ead..f9932f8ef 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -111,7 +111,6 @@ impl Timer { where F: FnMut(&mut Timer) + 'static + Send + Sync, { - // Move the callback to our reference counted container so rcl_callback can use it let callback = Arc::new(Mutex::new(callback)); // SAFETY: Getting a zero-initialized value is always safe. From 34603d3165dcdb95fefbd84e35b9d3ae1067ace5 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Thu, 31 Oct 2024 04:05:43 +0000 Subject: [PATCH 14/19] * Added subtrait for TimerCallback to address clippy error --- rclrs/src/node.rs | 4 ++-- rclrs/src/timer.rs | 12 +++++++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index dd36623b7..c1ddb45fb 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -17,7 +17,7 @@ use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, Timer, TimerBase, ENTITY_LIFECYCLE_MUTEX, + TimeSource, Timer, TimerBase, TimerCallback, ENTITY_LIFECYCLE_MUTEX, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -354,7 +354,7 @@ impl Node { callback: F, ) -> Result>, RclrsError> where - F: FnMut(&mut Timer) + 'static + Send + Sync, + F: TimerCallback + 'static, { let timer = Arc::new(Mutex::new(Timer::new_with_context_handle( Arc::clone(&self.handle.context_handle), diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index f9932f8ef..274ed93ad 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -15,6 +15,12 @@ use std::{ time::Duration, }; +/// A trait for the callback function of a timer. +pub trait TimerCallback: FnMut(&mut Timer) + Send + Sync {} + +// Blanket implementation of TimerCallback for all types that implement the necessary traits. +impl TimerCallback for T {} + // SAFETY: The functions accessing this type, including drop(), shouldn't care // about the thread they are running in (partly because they're protected by mutex). // Therefore, this type can be safely sent to another thread. @@ -76,7 +82,7 @@ pub trait TimerBase: Send + Sync { /// [5]: crate::Timer::execute /// [6]: crate::WaitSet pub struct Timer { - callback: Arc>, + callback: Arc>, handle: TimerHandle, } @@ -96,7 +102,7 @@ impl Timer { callback: F, ) -> Result where - F: FnMut(&mut Timer) + 'static + Send + Sync, + F: TimerCallback + 'static, { Timer::new_with_context_handle(Arc::clone(&context.handle), clock, period, callback) } @@ -109,7 +115,7 @@ impl Timer { callback: F, ) -> Result where - F: FnMut(&mut Timer) + 'static + Send + Sync, + F: TimerCallback + 'static, { let callback = Arc::new(Mutex::new(callback)); From c03c596576e6b5854cd69607b06c328785901aa4 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Thu, 31 Oct 2024 04:10:49 +0000 Subject: [PATCH 15/19] * Fixed clippy errors --- rclrs/src/logging.rs | 6 ++++++ rclrs/src/timer.rs | 15 +++++++-------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index 3943183c7..36e8554ae 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -120,6 +120,12 @@ pub struct LogConditions { pub log_if_true: bool, } +impl Default for LogConditions { + fn default() -> Self { + Self::new() + } +} + impl LogConditions { /// Default construct an instance pub fn new() -> Self { diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 274ed93ad..6ee62d851 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -10,7 +10,6 @@ use crate::{ Context, ContextHandle, RclReturnCode, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, }; use std::{ - i64, sync::{atomic::AtomicBool, Arc, Mutex, MutexGuard}, time::Duration, }; @@ -173,12 +172,12 @@ impl Timer { /// Returns true if the timer is due or past due to be called. /// Returns false if the timer is not yet due or has been canceled. pub fn is_ready(&self) -> bool { - let mut timer = self.handle.lock(); + let timer = self.handle.lock(); let mut is_ready = false; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. // * The is_ready pointer is allocated on the stack and is valid for the duration of this function. - let ret = unsafe { rcl_timer_is_ready(&mut *timer, &mut is_ready) }; + let ret = unsafe { rcl_timer_is_ready(&*timer, &mut is_ready) }; // rcl_timer_is_ready should only error if incorrect arguments are given or something isn't initialised, // both of which we control in this function. @@ -190,13 +189,13 @@ impl Timer { /// Get the time until the next call of the timer is due. Saturates to 0 if the timer is ready. /// Returns [`RclReturnCode::TimerCanceled`] as an error if the timer has already been canceled. pub fn time_until_next_call(&self) -> Result { - let mut timer = self.handle.lock(); + let timer = self.handle.lock(); let mut remaining_time = 0; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. // * The remaining_time pointer is allocated on the stack and is valid for the duration of this function. unsafe { - rcl_timer_get_time_until_next_call(&mut *timer, &mut remaining_time).ok()?; + rcl_timer_get_time_until_next_call(&*timer, &mut remaining_time).ok()?; } Ok(Duration::from_nanos( u64::try_from(remaining_time).unwrap_or(0), @@ -208,12 +207,12 @@ impl Timer { /// previous call but instead the time since the current callback was called. /// Saturates to 0 if the timer was last called in the future (i.e. the clock jumped). pub fn time_since_last_call(&self) -> Duration { - let mut timer = self.handle.lock(); + let timer = self.handle.lock(); let mut elapsed_time = 0; // SAFETY: // * The timer is initialized, which is guaranteed by the constructor. // * The elapsed_time pointer is allocated on the stack and is valid for the duration of this function. - let ret = unsafe { rcl_timer_get_time_since_last_call(&mut *timer, &mut elapsed_time) }; + let ret = unsafe { rcl_timer_get_time_since_last_call(&*timer, &mut elapsed_time) }; // rcl_timer_get_time_since_last_call should only error if incorrect arguments are given // or something isn't initialised, both of which we control in this function. @@ -388,7 +387,7 @@ mod tests { let new_period = Duration::from_millis(100); // Calling set_period will trigger the debug_assert check on the rcl return value. - timer.set_period(new_period.clone()); + timer.set_period(new_period); // Calling get_period will trigger the debug_assert check on the rcl return value. let retrieved_period = timer.get_period(); From 3a3d7ef8bd26dcdd292c488d37759550f063e071 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Thu, 31 Oct 2024 04:14:27 +0000 Subject: [PATCH 16/19] * Corrected grammar within timer handle comment --- rclrs/src/timer.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 6ee62d851..aceeb1daf 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -25,8 +25,8 @@ impl TimerCallback for T {} // Therefore, this type can be safely sent to another thread. unsafe impl Send for rcl_timer_t {} -/// Manage the lifecycle of an `rcl_timer_t`, including managing its dependencies -/// on `rcl_clock_t` and `rcl_context_t` by ensuring that these dependencies are +/// Manage the lifecycle of an `rcl_timer_t` and its dependency on +/// `rcl_clock_t` and `rcl_context_t` by ensuring that these dependencies are /// [dropped after][1] the `rcl_timer_t`. /// /// [1]: From 27a619ec799d45ca282e4ba17ea5914edf6becbc Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Tue, 5 Nov 2024 01:44:17 +0000 Subject: [PATCH 17/19] * Added serde feature to remove build warnings --- rclrs/Cargo.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index f489ea3f0..ecd5ecdcd 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -49,6 +49,7 @@ dyn_msg = ["ament_rs", "libloading"] # This feature is solely for the purpose of being able to generate documetation without a ROS installation # The only intended usage of this feature is for docs.rs builders to work, and is not intended to be used by end users generate_docs = ["rosidl_runtime_rs/generate_docs"] +serde = [] [package.metadata.docs.rs] features = ["generate_docs"] From 28b6eca06ce12e617748737b3ae5acf714365d28 Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Tue, 5 Nov 2024 04:25:25 +0000 Subject: [PATCH 18/19] * Cleaned up weird braces when dealing with timer mutex --- rclrs/src/node.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index c1ddb45fb..1c8236caf 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -363,7 +363,8 @@ impl Node { callback, )?)); - { self.timers_mtx.lock() } + self.timers_mtx + .lock() .unwrap() .push(Arc::downgrade(&timer) as Weak>); Ok(timer) From ed06edff4226270c798e5e20e815fe3c95fb57eb Mon Sep 17 00:00:00 2001 From: Sam Moran Date: Tue, 5 Nov 2024 04:27:56 +0000 Subject: [PATCH 19/19] * More brace weirdness --- rclrs/src/node.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 1c8236caf..5a4aa10d8 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -400,7 +400,9 @@ impl Node { } pub(crate) fn live_timers(&self) -> Vec>> { - { self.timers_mtx.lock().unwrap() } + self.timers_mtx + .lock() + .unwrap() .iter() .filter_map(Weak::upgrade) .collect()