1use std::{fmt::Debug, mem::MaybeUninit, time::Duration};
2
3use crate::{error::*, msg_types::generated_msgs::builtin_interfaces};
4use r2r_rcl::*;
5
6#[derive(Debug, Copy, Clone)]
8pub enum ClockType {
9 RosTime,
10 SystemTime,
11 SteadyTime,
12}
13
14unsafe impl Send for Clock {}
15
16pub struct Clock {
18 pub(crate) clock_handle: Box<rcl_clock_t>,
19 clock_type: ClockType,
20}
21
22pub fn clock_type_to_rcl(ct: &ClockType) -> rcl_clock_type_t {
23 match ct {
24 ClockType::RosTime => rcl_clock_type_t::RCL_ROS_TIME,
25 ClockType::SystemTime => rcl_clock_type_t::RCL_SYSTEM_TIME,
26 ClockType::SteadyTime => rcl_clock_type_t::RCL_STEADY_TIME,
27 }
28}
29
30impl Clock {
31 pub fn create(ct: ClockType) -> Result<Clock> {
33 let mut clock_handle = MaybeUninit::<rcl_clock_t>::uninit();
34
35 let rcl_ct = clock_type_to_rcl(&ct);
36 let ret = unsafe {
37 rcl_clock_init(rcl_ct, clock_handle.as_mut_ptr(), &mut rcutils_get_default_allocator())
38 };
39 if ret != RCL_RET_OK as i32 {
40 log::error!("could not create {:?} clock: {}", ct, ret);
41 return Err(Error::from_rcl_error(ret));
42 }
43
44 let clock_handle = Box::new(unsafe { clock_handle.assume_init() });
45 Ok(Clock {
46 clock_handle,
47 clock_type: ct,
48 })
49 }
50
51 pub fn get_now(&mut self) -> Result<Duration> {
52 let valid = unsafe { rcl_clock_valid(&mut *self.clock_handle) };
53 if !valid {
54 return Err(Error::from_rcl_error(RCL_RET_INVALID_ARGUMENT as i32));
55 }
56 let mut tp: rcutils_time_point_value_t = 0;
57 let ret = unsafe { rcl_clock_get_now(&mut *self.clock_handle, &mut tp) };
58
59 if ret != RCL_RET_OK as i32 {
60 log::error!("could not create steady clock: {}", ret);
61 return Err(Error::from_rcl_error(ret));
62 }
63
64 let dur = Duration::from_nanos(tp as u64);
65
66 Ok(dur)
67 }
68
69 pub fn get_clock_type(&self) -> ClockType {
70 self.clock_type
71 }
72
73 pub fn to_builtin_time(d: &Duration) -> builtin_interfaces::msg::Time {
75 let sec = d.as_secs() as i32;
76 let nanosec = d.subsec_nanos();
77 builtin_interfaces::msg::Time { sec, nanosec }
78 }
79
80 #[cfg(r2r__rosgraph_msgs__msg__Clock)]
86 pub(crate) fn enable_ros_time_override(
87 &mut self, initial_time: rcl_time_point_value_t,
88 ) -> Result<()> {
89 let valid = unsafe { rcl_clock_valid(&mut *self.clock_handle) };
90 if !valid {
91 return Err(Error::from_rcl_error(RCL_RET_INVALID_ARGUMENT as i32));
92 }
93
94 let ret = unsafe { rcl_enable_ros_time_override(&mut *self.clock_handle) };
95 if ret != RCL_RET_OK as i32 {
96 log::error!("could not enable ros time override: {}", ret);
97 return Err(Error::from_rcl_error(ret));
98 }
99
100 self.set_ros_time_override(initial_time)?;
101
102 Ok(())
103 }
104
105 #[cfg(r2r__rosgraph_msgs__msg__Clock)]
111 pub(crate) fn disable_ros_time_override(&mut self) -> Result<()> {
112 let valid = unsafe { rcl_clock_valid(&mut *self.clock_handle) };
113 if !valid {
114 return Err(Error::from_rcl_error(RCL_RET_INVALID_ARGUMENT as i32));
115 }
116
117 let ret = unsafe { rcl_disable_ros_time_override(&mut *self.clock_handle) };
118 if ret != RCL_RET_OK as i32 {
119 log::error!("could not disable ros time override: {}", ret);
120 return Err(Error::from_rcl_error(ret));
121 }
122
123 Ok(())
124 }
125
126 #[cfg(r2r__rosgraph_msgs__msg__Clock)]
134 pub(crate) fn set_ros_time_override(&mut self, time: rcl_time_point_value_t) -> Result<()> {
135 let valid = unsafe { rcl_clock_valid(&mut *self.clock_handle) };
136 if !valid {
137 return Err(Error::from_rcl_error(RCL_RET_INVALID_ARGUMENT as i32));
138 }
139
140 let ret = unsafe { rcl_set_ros_time_override(&mut *self.clock_handle, time) };
141 if ret != RCL_RET_OK as i32 {
142 log::error!("could not set ros time override: {}", ret);
143 return Err(Error::from_rcl_error(ret));
144 }
145
146 Ok(())
147 }
148}
149
150impl From<builtin_interfaces::msg::Time> for rcutils_time_point_value_t {
151 fn from(msg: builtin_interfaces::msg::Time) -> Self {
152 (msg.sec as rcl_time_point_value_t) * 1_000_000_000
153 + (msg.nanosec as rcl_time_point_value_t)
154 }
155}
156
157impl Drop for Clock {
158 fn drop(&mut self) {
159 unsafe {
160 rcl_clock_fini(&mut *self.clock_handle);
161 }
162 }
163}