rclrs/
time.rs

1use crate::{rcl_bindings::*, vendor::builtin_interfaces};
2use std::{
3    num::TryFromIntError,
4    ops::{Add, Sub},
5    sync::{Mutex, Weak},
6    time::Duration,
7};
8
9/// Struct that represents time.
10#[derive(Clone, Debug)]
11pub struct Time {
12    /// Timestamp in nanoseconds.
13    pub nsec: i64,
14    /// Weak reference to the clock that generated this time
15    pub clock: Weak<Mutex<rcl_clock_t>>,
16}
17
18impl Time {
19    /// Compares self to rhs, if they can be compared (originated from the same clock) calls f with
20    /// the values of the timestamps.
21    pub fn compare_with<U, F>(&self, rhs: &Time, f: F) -> Option<U>
22    where
23        F: FnOnce(i64, i64) -> U,
24    {
25        self.clock
26            .ptr_eq(&rhs.clock)
27            .then(|| f(self.nsec, rhs.nsec))
28    }
29
30    /// Convenience function for converting time to ROS message
31    pub fn to_ros_msg(&self) -> Result<builtin_interfaces::msg::Time, TryFromIntError> {
32        let (sec, nanosec) = self.to_sec_nanosec()?;
33        Ok(builtin_interfaces::msg::Time { nanosec, sec })
34    }
35
36    /// Convenience function for converting time into an rcl message.
37    pub fn to_rcl(&self) -> Result<builtin_interfaces__msg__Time, TryFromIntError> {
38        let (sec, nanosec) = self.to_sec_nanosec()?;
39        Ok(builtin_interfaces__msg__Time { sec, nanosec })
40    }
41
42    /// Convenience function for converting time into a (sec, nanosec) representation
43    /// which is commonly used in the ROS ecosystem.
44    pub fn to_sec_nanosec(&self) -> Result<(i32, u32), TryFromIntError> {
45        let sec = self.nsec / 1_000_000_000;
46        let nanosec = self.nsec % 1_000_000_000;
47
48        Ok((sec.try_into()?, nanosec.try_into()?))
49    }
50}
51
52impl Add<Duration> for Time {
53    type Output = Self;
54
55    fn add(self, other: Duration) -> Self {
56        let dur_ns = i64::try_from(other.as_nanos()).unwrap();
57        Time {
58            nsec: self.nsec.checked_add(dur_ns).unwrap(),
59            clock: self.clock.clone(),
60        }
61    }
62}
63
64impl Sub<Duration> for Time {
65    type Output = Self;
66
67    fn sub(self, other: Duration) -> Self {
68        let dur_ns = i64::try_from(other.as_nanos()).unwrap();
69        Time {
70            nsec: self.nsec.checked_sub(dur_ns).unwrap(),
71            clock: self.clock.clone(),
72        }
73    }
74}
75
76#[cfg(test)]
77mod tests {
78    use super::*;
79    use crate::Clock;
80
81    #[test]
82    fn compare_times_from_same_clock() {
83        let clock = Clock::system();
84        let t1 = clock.now();
85        std::thread::sleep(Duration::from_micros(1));
86        let t2 = clock.now();
87        assert_eq!(t1.compare_with(&t2, |t1, t2| t1 > t2), Some(false));
88        assert_eq!(t1.compare_with(&t2, |t1, t2| t2 > t1), Some(true));
89    }
90
91    #[test]
92    fn compare_times_from_different_clocks() {
93        // Times from different clocks, even if of the same type, can't be compared
94        let c1 = Clock::system();
95        let c2 = Clock::system();
96        let t1 = c1.now();
97        let t2 = c2.now();
98        assert!(t2.compare_with(&t1, |_, _| ()).is_none());
99        assert!(t1.compare_with(&t2, |_, _| ()).is_none());
100    }
101
102    #[test]
103    fn add_duration_to_time() {
104        let (clock, _) = Clock::with_source();
105        let t = clock.now();
106        let t2 = t.clone() + Duration::from_secs(1);
107        assert_eq!(t2.nsec - t.nsec, 1_000_000_000i64);
108        let t3 = t2 - Duration::from_secs(1);
109        assert_eq!(t3.nsec, t.nsec);
110    }
111
112    #[test]
113    fn test_conversion() {
114        let clock = Clock::system();
115        let t1 = clock.now();
116        let time = Time {
117            nsec: 1_000_000_100,
118            clock: t1.clock.clone(),
119        };
120        let msg = time.to_ros_msg().unwrap();
121        assert_eq!(msg.nanosec, 100);
122        assert_eq!(msg.sec, 1);
123    }
124}
rclrs/time.rs

rclrs/
time.rs
