diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 14e1704d..4227a0d2 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -1,8 +1,10 @@ use crate::rcl_bindings::*; +use std::ops::{Add, Sub}; use std::sync::{Mutex, Weak}; +use std::time::Duration; /// Struct that represents time. -#[derive(Debug)] +#[derive(Clone, Debug)] pub struct Time { /// Timestamp in nanoseconds. pub nsec: i64, @@ -23,9 +25,34 @@ impl Time { } } +impl Add for Time { + type Output = Self; + + fn add(self, other: Duration) -> Self { + let dur_ns = i64::try_from(other.as_nanos()).unwrap(); + Time { + nsec: self.nsec.checked_add(dur_ns).unwrap(), + clock: self.clock.clone(), + } + } +} + +impl Sub for Time { + type Output = Self; + + fn sub(self, other: Duration) -> Self { + let dur_ns = i64::try_from(other.as_nanos()).unwrap(); + Time { + nsec: self.nsec.checked_sub(dur_ns).unwrap(), + clock: self.clock.clone(), + } + } +} + #[cfg(test)] mod tests { - use crate::clock::Clock; + use super::*; + use crate::Clock; #[test] fn compare_times_from_same_clock() { @@ -47,4 +74,14 @@ mod tests { assert!(t2.compare_with(&t1, |_, _| ()).is_none()); assert!(t1.compare_with(&t2, |_, _| ()).is_none()); } + + #[test] + fn add_duration_to_time() { + let (clock, _) = Clock::with_source().unwrap(); + let t = clock.now(); + let t2 = t.clone() + Duration::from_secs(1); + assert_eq!(t2.nsec - t.nsec, 1_000_000_000i64); + let t3 = t2 - Duration::from_secs(1); + assert_eq!(t3.nsec, t.nsec); + } } diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 5f3e8356..4ea33b7d 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -102,11 +102,16 @@ impl fmt::Display for ClockMismatchError { } impl TimeSource { - /// Creates a new time source with default parameters. + /// Creates a new `TimeSource` with default parameters. pub fn new(node: Arc, clock: ClockSource) -> Self { TimeSourceBuilder::new(node, clock).build().unwrap() } + /// Creates a new `TimeSourceBuilder` with default parameters. + pub fn builder(node: Arc, clock: ClockSource) -> TimeSourceBuilder { + TimeSourceBuilder::new(node, clock) + } + /// Attaches the given clock to the `TimeSource`, enabling the `TimeSource` to control it. pub fn attach_clock(&self, clock: ClockSource) { if let Some(last_msg) = self._last_time_msg.lock().unwrap().as_ref() {