1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
use std::convert::TryInto;
use std::time::Duration;
use rclrust_msg::builtin_interfaces;
use crate::clock::ClockType;
const S_TO_NS: i64 = 1_000_000_000;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Time {
pub nanosecs: i64,
pub clock_type: ClockType,
}
impl Time {
/// # Examples
///
/// ```
/// use rclrust::{ClockType, Time};
///
/// let time = Time::new(10, 100000, ClockType::RosTime);
/// ```
pub fn new(secs: i32, nanosecs: u32, clock_type: ClockType) -> Self {
Self {
nanosecs: i64::from(secs) * S_TO_NS + i64::from(nanosecs),
clock_type,
}
}
/// # Examples
///
/// ```
/// use rclrust::{ClockType, Time};
///
/// let time = Time::from_nanosecs(100000, ClockType::RosTime);
/// ```
pub const fn from_nanosecs(nanosecs: i64, clock_type: ClockType) -> Self {
Self {
nanosecs,
clock_type,
}
}
/// # Examples
///
/// ```
/// use rclrust::{ClockType, Time};
/// use rclrust_msg::builtin_interfaces;
///
/// let time_msg = builtin_interfaces::msg::Time {
/// sec: 5,
/// nanosec: 10,
/// };
/// let time = Time::from_ros_msg(&time_msg, ClockType::RosTime);
/// assert_eq!(time, Time::new(5, 10, ClockType::RosTime));
/// ```
pub fn from_ros_msg(time_msg: &builtin_interfaces::msg::Time, clock_type: ClockType) -> Self {
Self::new(time_msg.sec, time_msg.nanosec, clock_type)
}
/// # Examples
///
/// ```
/// use rclrust::{ClockType, Time};
/// use rclrust_msg::builtin_interfaces;
///
/// let time = Time::new(5, 10, ClockType::RosTime);
/// let time_msg = builtin_interfaces::msg::Time {
/// sec: 5,
/// nanosec: 10,
/// };
/// assert_eq!(time.to_ros_msg(), time_msg);
/// ```
pub fn to_ros_msg(self) -> builtin_interfaces::msg::Time {
let sec = self.nanosecs / S_TO_NS;
let nanosec = self.nanosecs % S_TO_NS;
let (sec, nanosec) = if self.nanosecs >= 0 {
(sec, nanosec)
} else {
(sec - 1, nanosec + S_TO_NS)
};
builtin_interfaces::msg::Time {
sec: sec.try_into().unwrap(),
nanosec: nanosec.try_into().unwrap(),
}
}
}
pub(crate) trait RclDurationT {
fn to_rmw_time_t(&self) -> rcl_sys::rmw_time_t;
fn from_rmw_time_t(duration: &rcl_sys::rmw_time_t) -> Self;
}
impl RclDurationT for Duration {
fn to_rmw_time_t(&self) -> rcl_sys::rmw_time_t {
rcl_sys::rmw_time_t {
sec: self.as_secs(),
nsec: self.subsec_nanos().into(),
}
}
fn from_rmw_time_t(duration: &rcl_sys::rmw_time_t) -> Self {
Self::new(duration.sec, duration.nsec.try_into().unwrap())
}
}