ros2_client/
ros_time.rs

1//! ROS timing abstractions [`ROSTime`] and [`SystemTime`]
2//!
3//! See the ROS 2 design article [Clock and Time](https://design.ros2.org/articles/clock_and_time.html).
4//!
5//! The abstaction Steady Time is not implemented here, because it is not
6//! intended to be communicated over interfaces, but kept internal to a
7//! component.
8use std::{
9  convert::TryFrom,
10  ops::{Add, Sub},
11  time::Duration,
12};
13
14use serde::{Deserialize, Serialize};
15use chrono::{DateTime, Utc};
16use log::error;
17use rustdds::Timestamp;
18
19/// ROS Time with nanosecond precision
20///
21/// This is the in-memory representation of builtin_interfaces::Time
22#[derive(Clone, Copy, PartialEq, PartialOrd, Eq, Ord, Debug, Serialize, Deserialize)]
23pub struct ROSTime {
24  nanos_since_epoch: i64,
25}
26
27impl ROSTime {
28  /// Returns the current time for the system clock.
29  ///
30  /// To use simulation-capable time, ask from `Node`.
31  pub(crate) fn now() -> Self {
32    Self::try_from(chrono::Utc::now()).unwrap_or(Self::ZERO)
33  }
34
35  pub const ZERO: Self = Self::from_nanos(0);
36  pub const UNIX_EPOCH: Self = Self::from_nanos(0);
37
38  pub fn to_nanos(&self) -> i64 {
39    self.nanos_since_epoch
40  }
41
42  pub const fn from_nanos(nanos_since_unix_epoch: i64) -> Self {
43    Self {
44      nanos_since_epoch: nanos_since_unix_epoch,
45    }
46  }
47}
48
49/// Overflow/underflow in timestamp conversion
50#[derive(Clone, Debug)]
51pub struct OutOfRangeError {}
52
53// chrono <-> ROSTime
54
55/// Fallible conversion to nanoseconds since non-leap-nanoseconds since
56/// January 1, 1970 UTC
57///
58/// chrono docs:
59///
60/// "An i64 with nanosecond precision can span a range of ~584 years. This
61/// function returns None on an out of range DateTime. The dates that can be
62/// represented as nanoseconds are between 1677-09-21T00:12:43.145224192 and
63/// 2262-04-11T23:47:16.854775807"
64impl TryFrom<chrono::DateTime<Utc>> for ROSTime {
65  type Error = OutOfRangeError;
66
67  fn try_from(chrono_time: chrono::DateTime<Utc>) -> Result<ROSTime, OutOfRangeError> {
68    chrono_time
69      .timestamp_nanos_opt()
70      .ok_or_else(|| {
71        error!("ROSTime: chrono timestamp is out of range: {chrono_time:?}");
72        OutOfRangeError {}
73      })
74      .map(ROSTime::from_nanos)
75  }
76}
77
78impl From<ROSTime> for chrono::DateTime<Utc> {
79  fn from(rt: ROSTime) -> chrono::DateTime<Utc> {
80    DateTime::<Utc>::from_timestamp_nanos(rt.to_nanos())
81  }
82}
83
84// rustDDS::Timestamp <-> ROSTime
85
86impl From<ROSTime> for Timestamp {
87  fn from(rt: ROSTime) -> Timestamp {
88    let chrono_time = chrono::DateTime::<Utc>::from(rt);
89    Timestamp::try_from(chrono_time).unwrap_or_else(|e| {
90      error!("Time conversion ROSTime to Timestamp error: {e} source={rt:?}");
91      rustdds::Timestamp::INVALID
92    })
93  }
94}
95
96/// Failure to convert DDS/RTPS Timestamp (`Time_t`) to `ROSTime`.
97///
98/// See DDS Specification v1.4 Sections "2.3.2 PIM to PSM Mapping Rules"
99/// and "2.3.3 DCPS PSM : IDL" for DDS `Time_t` type definitions and RTPS
100/// Specification v2.5 Section "8.3.2 Type Definitions" for RTPS view of
101/// `Time_t`.
102pub enum TimestampConversionError {
103  /// DDS Timestamp indicates an invalid value
104  Invalid,
105  /// DDS Timestamp indicates "infinity"  
106  Infinite,
107}
108
109impl TryFrom<Timestamp> for ROSTime {
110  type Error = TimestampConversionError;
111  fn try_from(ts: Timestamp) -> Result<ROSTime, TimestampConversionError> {
112    match ts {
113      Timestamp::INVALID => Err(TimestampConversionError::Invalid),
114      Timestamp::INFINITE => Err(TimestampConversionError::Infinite),
115      ts => {
116        let ticks: u64 = ts.to_ticks(); // tick length is (1 / 2^32) seconds
117        let seconds = ticks >> 32;
118        let frac_ticks = ticks - (seconds << 32); // fractional part only in ticks
119        let frac_nanos = (frac_ticks * 1_000_000_000) >> 32;
120        // Both Timestamp and ROSTime are represented as i64, but
121        // units are different. Timestamp can count up to 2^32 = 4Gi seconds
122        // from epoch, or until year 2106.
123        // ROSTime can count up (2^63/10^9) or 9.22*10^9 seconds from epoch,
124        // which is in year 2262.
125        // Timestamp cannot be negative.
126        // Therefore, we cannot overflow the i64 in ROStime.
127        Ok(ROSTime::from_nanos(
128          (seconds * 1_000_000_000 + frac_nanos) as i64,
129        ))
130      }
131    }
132  }
133}
134
135impl Sub for ROSTime {
136  type Output = ROSDuration;
137
138  fn sub(self, other: ROSTime) -> ROSDuration {
139    ROSDuration {
140      diff: self.nanos_since_epoch - other.nanos_since_epoch,
141    }
142  }
143}
144
145impl Sub<ROSDuration> for ROSTime {
146  type Output = ROSTime;
147
148  fn sub(self, other: ROSDuration) -> ROSTime {
149    ROSTime {
150      nanos_since_epoch: self.nanos_since_epoch - other.diff,
151    }
152  }
153}
154
155impl Add<ROSDuration> for ROSTime {
156  type Output = ROSTime;
157
158  fn add(self, other: ROSDuration) -> ROSTime {
159    ROSTime {
160      nanos_since_epoch: self.nanos_since_epoch + other.diff,
161    }
162  }
163}
164
165/// Difference between [`ROSTime`] or [`SystemTime`] instances
166///
167/// Supports conversions to/from
168/// * [`std::time::Duration`]
169/// * [`chrono::Duration`]
170pub struct ROSDuration {
171  diff: i64,
172}
173
174impl ROSDuration {
175  /// Construct from nanosecond count
176  pub const fn from_nanos(nanos: i64) -> Self {
177    ROSDuration { diff: nanos }
178  }
179
180  /// Convert to nanoseconds.
181  /// Returns `None` if `i64` would overflow.
182  pub const fn to_nanos(&self) -> i64 {
183    self.diff
184  }
185}
186
187// std::time::Duration <-> ROSDuration
188
189impl TryFrom<Duration> for ROSDuration {
190  type Error = OutOfRangeError;
191
192  fn try_from(std_duration: Duration) -> Result<Self, Self::Error> {
193    let nanos = std_duration.as_nanos();
194    if nanos <= (i64::MAX as u128) {
195      Ok(ROSDuration { diff: nanos as i64 })
196    } else {
197      Err(OutOfRangeError {})
198    }
199  }
200}
201
202impl TryFrom<ROSDuration> for Duration {
203  type Error = OutOfRangeError;
204
205  fn try_from(ros_duration: ROSDuration) -> Result<Duration, Self::Error> {
206    Ok(Duration::from_nanos(
207      u64::try_from(ros_duration.to_nanos()).map_err(|_e| OutOfRangeError {})?,
208    ))
209  }
210}
211
212// chrono::Duration <-> ROSDuration
213
214impl From<ROSDuration> for chrono::Duration {
215  fn from(d: ROSDuration) -> chrono::Duration {
216    chrono::Duration::nanoseconds(d.to_nanos())
217  }
218}
219
220impl TryFrom<chrono::Duration> for ROSDuration {
221  type Error = OutOfRangeError;
222
223  fn try_from(c_duration: chrono::Duration) -> Result<Self, Self::Error> {
224    c_duration
225      .num_nanoseconds()
226      .map(ROSDuration::from_nanos)
227      .ok_or(OutOfRangeError {})
228  }
229}
230
231// Addition and subtraction
232
233/// Note: panics on overflow/underflow like integer arithmetic
234impl Add for ROSDuration {
235  type Output = ROSDuration;
236  fn add(self, other: ROSDuration) -> ROSDuration {
237    ROSDuration {
238      diff: self.diff + other.diff,
239    }
240  }
241}
242
243/// Note: panics on overflow/underflow like integer arithmetic
244impl Sub for ROSDuration {
245  type Output = ROSDuration;
246  fn sub(self, other: ROSDuration) -> ROSDuration {
247    ROSDuration {
248      diff: self.diff - other.diff,
249    }
250  }
251}
252
253/// Same as ROSTime, except this one cannot be simulated.
254///
255/// *TODO*: This has no methods implemented, so just a placeholder type for now.
256#[derive(Clone, Copy, PartialEq, PartialOrd, Eq, Ord, Debug, Serialize, Deserialize)]
257pub struct SystemTime {
258  ros_time: ROSTime,
259}
260
261//TODO: SystemTime implementation missing
262
263#[cfg(test)]
264mod test {
265  //use rustdds::Timestamp;
266
267  //use super::ROSTime;
268
269  #[test]
270  fn conversion() {
271    //TODO
272  }
273}
ros2_client/ros_time.rs

ros2_client/
ros_time.rs
