roslibrust_codegen/
integral_types.rs

1use simple_error::{bail, SimpleError};
2
3use roslibrust_common::RosMessageType;
4
5/// Matches the integral ros1 type time, with extensions for ease of use
6/// NOTE: in ROS1 "Time" is not a message in and of itself and std_msgs/Time should be used.
7/// However, in ROS2 "Time" is a message and part of builtin_interfaces/Time.
8// Okay some complexities lurk here that I really don't like
9// In ROS1 time is i32 secs and i32 nsecs
10// In ROS2 time is i32 secs and u32 nsecs
11// How many nsecs are there in a sec? +1e9 which will fit inside of either.
12// But ROS really doesn't declare what is valid for nsecs larger than 1e9, how should that be handled?
13// How should negative nsecs work anyway?
14// https://docs.ros2.org/foxy/api/builtin_interfaces/msg/Time.html
15#[derive(:: serde :: Deserialize, :: serde :: Serialize, Debug, Default, Clone, PartialEq)]
16pub struct Time {
17    // Note: rosbridge appears to accept secs and nsecs in for time without issue?
18    // Not sure we should actually rely on this behavior, but ok for now...
19
20    // This alias is required for ros2 where field has been renamed
21    #[serde(alias = "sec")]
22    pub secs: i32,
23    // This alias is required for ros2 where field has been renamed
24    #[serde(alias = "nanosec")]
25    pub nsecs: i32,
26}
27
28/// Provide a standard conversion between ROS time and std::time::SystemTime
29impl TryFrom<std::time::SystemTime> for Time {
30    type Error = SimpleError;
31    fn try_from(val: std::time::SystemTime) -> Result<Self, Self::Error> {
32        let delta = match val.duration_since(std::time::UNIX_EPOCH) {
33            Ok(delta) => delta,
34            Err(e) => bail!("Failed to convert system time into unix epoch: {}", e),
35        };
36        // TODO our current method doesn't try to handel negative times
37        // It is unclear from ROS documentation how these would be generated or how they should be handled
38        // For now adopting a strict conversion policy of only converting when it makes clear logical sense
39        let downcast_secs = match i32::try_from(delta.as_secs()) {
40            Ok(val) => val,
41            Err(e) => bail!("Failed to convert seconds to i32: {e:?}"),
42        };
43        let downcast_nanos = match i32::try_from(delta.subsec_nanos()) {
44            Ok(val) => val,
45            Err(e) => bail!("Failed to convert nanoseconds to i32: {e:?}"),
46        };
47        Ok(Time {
48            secs: downcast_secs,
49            nsecs: downcast_nanos,
50        })
51    }
52}
53
54/// Provide a standard conversion between ROS time and std::time::SystemTime
55impl TryFrom<Time> for std::time::SystemTime {
56    type Error = SimpleError;
57    fn try_from(val: Time) -> Result<Self, Self::Error> {
58        // TODO our current method doesn't try to handel negative times
59        // It is unclear from ROS documentation how these would be generated or how they should be handled
60        // For now adopting a strict conversion policy of only converting when it makes clear logical sense
61        let secs = match u64::try_from(val.secs){
62            Ok(val) => val,
63            Err(e) => bail!( "Failed to convert ROS time to std::time::SystemTime, secs term overflows u64 likely: {val:?}, {e:?}"),
64        };
65        let nsecs = match u64::try_from(val.nsecs) {
66            Ok(val) => val,
67            Err(e) => bail!("Failed to convert ROS time to std::time::SystemTime, nsecs term overflows u64 likely: {val:?}, {e:?}"),
68        };
69        let duration = std::time::Duration::new(secs, nsecs as u32);
70        Ok(std::time::UNIX_EPOCH + duration)
71    }
72}
73
74impl RosMessageType for Time {
75    const ROS_TYPE_NAME: &'static str = "builtin_interfaces/Time";
76    // TODO: ROS2 support
77    const MD5SUM: &'static str = "";
78    const DEFINITION: &'static str = "";
79}
80
81/// Matches the integral ros1 duration type, with extensions for ease of use
82/// NOTE: Is not a message in and of itself use std_msgs/Duration for that
83#[derive(:: serde :: Deserialize, :: serde :: Serialize, Debug, Default, Clone, PartialEq)]
84pub struct Duration {
85    pub sec: i32,
86    pub nsec: i32,
87}
88
89/// Conversion from [std::time::Duration] to our internal [Duration] type
90/// Note: this provides both [tokio::time::Duration] and [std::time::Duration]
91impl TryFrom<std::time::Duration> for Duration {
92    type Error = SimpleError;
93    fn try_from(val: std::time::Duration) -> Result<Self, Self::Error> {
94        let downcast_sec = match i32::try_from(val.as_secs()) {
95            Ok(val) => val,
96            Err(e) => bail!(
97                "Failed to cast tokio duration to ROS duration, secs could not fit in i32:  {e:?}"
98            ),
99        };
100        let downcast_nsec = match i32::try_from(val.subsec_nanos()) {
101            Ok(val) => val,
102            Err(e) => bail!(
103                "Failed to cast tokio duration ROS duration, nsecs could not fit in i32: {e:?}"
104            ),
105        };
106        Ok(Duration {
107            sec: downcast_sec,
108            nsec: downcast_nsec,
109        })
110    }
111}
112
113/// Conversion from our internal [Duration] type to [std::time::Duration]
114/// Note: this provides both [tokio::time::Duration] and [std::time::Duration]
115impl TryFrom<Duration> for std::time::Duration {
116    type Error = SimpleError;
117    fn try_from(val: Duration) -> Result<Self, Self::Error> {
118        let upcast_sec = match u64::try_from(val.sec) {
119            Ok(val) => val,
120            Err(e) => bail!(
121                "Failed to cast ROS duration to tokio duration, secs could not fit in u64: {e:?}"
122            ),
123        };
124        let upcast_nsec = match u32::try_from(val.nsec) {
125            Ok(val) => val,
126            Err(e) => bail!(
127                "Failed to cast ROS duration to tokio duration, nsecs could not fit in u64: {e:?}"
128            ),
129        };
130        Ok(std::time::Duration::new(upcast_sec, upcast_nsec))
131    }
132}
133
134/// Conversion from chrono::DateTime<chrono::Utc> to our internal Time type
135#[cfg(feature = "chrono")]
136impl TryFrom<chrono::DateTime<chrono::Utc>> for Time {
137    type Error = SimpleError;
138    fn try_from(val: chrono::DateTime<chrono::Utc>) -> Result<Self, Self::Error> {
139        let downcast_secs = match i32::try_from(val.timestamp()) {
140            Ok(val) => val,
141            Err(e) => {
142                bail!("Failed to convert chrono time to ROS time, secs could not fit in i32: {e:?}")
143            }
144        };
145        let downcast_nanos = match i32::try_from(val.timestamp_subsec_nanos()) {
146            Ok(val) => val,
147            Err(e) => bail!(
148                "Failed to convert chrono time to ROS time, nsecs could not fit in i32: {e:?}"
149            ),
150        };
151        Ok(Time {
152            secs: downcast_secs,
153            nsecs: downcast_nanos,
154        })
155    }
156}
157
158/// Conversion from our internal [Time] type to [chrono::DateTime]
159#[cfg(feature = "chrono")]
160impl TryFrom<Time> for chrono::DateTime<chrono::Utc> {
161    type Error = SimpleError;
162    fn try_from(val: Time) -> Result<Self, Self::Error> {
163        let secs = i64::from(val.secs);
164        let nsecs = match u32::try_from(val.nsecs) {
165            Ok(val) => val,
166            Err(e) => bail!(
167                "Failed to convert ROS time to chrono time, nsecs could not fit in u32: {e:?}"
168            ),
169        };
170        match chrono::DateTime::from_timestamp(secs, nsecs) {
171            Some(val) => Ok(val),
172            None => bail!("Failed to convert ROS time to chrono time, secs and nsecs could not fit in chrono::DateTime."),
173        }
174    }
175}
176
177/// Conversion from [chrono::Duration] to our internal [Duration] type
178#[cfg(feature = "chrono")]
179impl TryFrom<chrono::Duration> for Duration {
180    type Error = SimpleError;
181    fn try_from(val: chrono::Duration) -> Result<Self, Self::Error> {
182        // Chrono uses i64 for seconds, ROS uses i32 have to attempt downcast
183        let downcast_sec = match i32::try_from(val.num_seconds()) {
184            Ok(val) => val,
185            Err(e) => bail!(
186                "Failed to cast chrono duration to ROS duration, secs could not fit in i32:  {e:?}"
187            ),
188        };
189        Ok(Duration {
190            sec: downcast_sec,
191            nsec: val.subsec_nanos(),
192        })
193    }
194}
195
196/// Conversion from our internal [Duration] type to [chrono::Duration]
197#[cfg(feature = "chrono")]
198impl TryFrom<Duration> for chrono::Duration {
199    type Error = SimpleError;
200    // Note: this conversion shouldn't be fallible, ROS time should always fit in chrono::Duration
201    // Just being pedantic about error handling, and matching style of other conversions
202    fn try_from(val: Duration) -> Result<Self, Self::Error> {
203        let secs = match chrono::Duration::try_seconds(i64::from(val.sec)) {
204            Some(val) => val,
205            None => bail!("Failed to cast ROS duration to chrono duration, secs could not fit in chrono::Duration."),
206        };
207        // Not fallible because nanoseconds can't overflow TimeDelta
208        let nsecs = chrono::Duration::nanoseconds(i64::from(val.nsec));
209        // Probably unnecessary, but being pedantic about error handling
210        let total = match secs.checked_add(&nsecs) {
211            Some(val) => val,
212            None => bail!("Failed to cast ROS duration to chrono duration, addition overflowed when combining secs and nsecs."),
213        };
214        Ok(total)
215    }
216}
217
218#[cfg(test)]
219mod test {
220    #[test]
221    fn test_time_conversions() {
222        // Basic round trip test of now
223        let time = std::time::SystemTime::now();
224        let ros_time: crate::Time = time.try_into().unwrap();
225        let std_time: std::time::SystemTime = ros_time.try_into().unwrap();
226        assert_eq!(time, std_time);
227
228        // Can "min time" convert
229        let time = std::time::SystemTime::UNIX_EPOCH;
230        let ros_time: crate::Time = time.try_into().unwrap();
231        let std_time: std::time::SystemTime = ros_time.try_into().unwrap();
232        assert_eq!(time, std_time);
233
234        // Can "max time" convert
235        let ros_time = crate::Time {
236            secs: i32::MAX,
237            nsecs: i32::MAX,
238        };
239        let std_time: std::time::SystemTime = ros_time.try_into().unwrap();
240        assert_eq!(
241            std_time,
242            std::time::SystemTime::UNIX_EPOCH
243                + std::time::Duration::new(i32::MAX as u64, i32::MAX as u32)
244        );
245
246        // Can "negative time" convert
247        let ros_time = crate::Time {
248            secs: i32::MIN,
249            nsecs: i32::MIN,
250        };
251        let std_time: Result<std::time::SystemTime, _> = ros_time.try_into();
252        // No it can't, not with how our current implementation is setup
253        assert!(std_time.is_err());
254
255        // How about positive time with negative nsecs?
256        let ros_time = crate::Time { secs: 1, nsecs: -1 };
257        let std_time: Result<std::time::SystemTime, _> = ros_time.try_into();
258        // Nope our current implementation doesn't support negative nsecs at all
259        // Would need to find some ROS code generating these to really confirm how this should be handled
260        assert!(std_time.is_err());
261    }
262
263    #[test]
264    fn test_duration_conversions() {
265        // Basic test
266        let tokio_duration = tokio::time::Duration::from_millis(1000);
267        let ros_duration: crate::Duration = tokio_duration.try_into().unwrap();
268        let roundtrip_duration: tokio::time::Duration = ros_duration.try_into().unwrap();
269        assert_eq!(tokio_duration, roundtrip_duration);
270
271        // Confirm std::time::Duration works as well
272        let std_duration = std::time::Duration::from_millis(1000);
273        let ros_duration: crate::Duration = std_duration.try_into().unwrap();
274        let roundtrip_duration: std::time::Duration = ros_duration.try_into().unwrap();
275        assert_eq!(std_duration, roundtrip_duration);
276
277        // Test 0 duration
278        let tokio_duration = tokio::time::Duration::from_millis(0);
279        let ros_duration: crate::Duration = tokio_duration.try_into().unwrap();
280        let roundtrip_duration: tokio::time::Duration = ros_duration.try_into().unwrap();
281        assert_eq!(tokio_duration, roundtrip_duration);
282
283        // Test negative ros duration
284        let ros_duration = crate::Duration { sec: -1, nsec: -1 };
285        let tokio_duration: Result<tokio::time::Duration, _> = ros_duration.try_into();
286        // Won't work, we currently don't respect negative durations
287        assert!(tokio_duration.is_err());
288    }
289
290    #[test]
291    #[cfg(feature = "chrono")]
292    fn test_chrono_duration_conversions() {
293        // Basic test
294        let chrono_duration = chrono::Duration::seconds(1) + chrono::Duration::nanoseconds(69);
295        let ros_duration: crate::Duration = chrono_duration.try_into().unwrap();
296        let roundtrip_duration: chrono::Duration = ros_duration.try_into().unwrap();
297        assert_eq!(chrono_duration, roundtrip_duration);
298
299        // Test 0 duration
300        let chrono_duration = chrono::Duration::seconds(0);
301        let ros_duration: crate::Duration = chrono_duration.try_into().unwrap();
302        let roundtrip_duration: chrono::Duration = ros_duration.try_into().unwrap();
303        assert_eq!(chrono_duration, roundtrip_duration);
304
305        // Test large chrono time that can't fit into ros
306        let chrono_duration = chrono::Duration::seconds(i64::MAX / 10_000);
307        let ros_duration: Result<crate::Duration, _> = chrono_duration.try_into();
308        assert!(ros_duration.is_err());
309
310        // Test negative chrono time
311        let chrono_duration = chrono::Duration::seconds(-1) + chrono::Duration::nanoseconds(-42);
312        let ros_duration: crate::Duration = chrono_duration.try_into().unwrap();
313        let roundtrip_duration: chrono::Duration = ros_duration.try_into().unwrap();
314        assert_eq!(chrono_duration, roundtrip_duration);
315    }
316
317    #[test]
318    #[cfg(feature = "chrono")]
319    fn test_chrono_time_conversions() {
320        // Basic test
321        let now = chrono::offset::Utc::now();
322        let ros_time: crate::Time = now.try_into().unwrap();
323        let roundtrip_time: chrono::DateTime<chrono::Utc> = ros_time.try_into().unwrap();
324        assert_eq!(now, roundtrip_time);
325
326        // Test EPOCH
327        let epoch = chrono::DateTime::<chrono::Utc>::UNIX_EPOCH;
328        let ros_epoch: crate::Time = epoch.try_into().unwrap();
329        let roundtrip_epoch: chrono::DateTime<chrono::Utc> = ros_epoch.try_into().unwrap();
330        assert_eq!(epoch, roundtrip_epoch);
331
332        // Test time that can't fit into ros
333        let too_large = chrono::DateTime::<chrono::Utc>::UNIX_EPOCH
334            + chrono::Duration::seconds(i32::MAX as i64 + 1000);
335        let ros_time: Result<crate::Time, _> = too_large.try_into();
336        assert!(ros_time.is_err());
337    }
338}
roslibrust_codegen/integral_types.rs

roslibrust_codegen/
integral_types.rs
