r2r/
clocks.rs

1use std::{fmt::Debug, mem::MaybeUninit, time::Duration};
2
3use crate::{error::*, msg_types::generated_msgs::builtin_interfaces};
4use r2r_rcl::*;
5
6/// Different ROS clock types.
7#[derive(Debug, Copy, Clone)]
8pub enum ClockType {
9    RosTime,
10    SystemTime,
11    SteadyTime,
12}
13
14unsafe impl Send for Clock {}
15
16/// A ROS clock.
17pub 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    /// Create a new clock with the specified type.
32    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    /// TODO: move to builtin helper methods module.
74    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    /// Enables alternative source of time for this clock
81    ///
82    /// The clock must be [`ClockType::RosTime`].
83    ///
84    /// Wrapper for `rcl_enable_ros_time_override`
85    #[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    /// Disables alternative source of time for this clock
106    ///
107    /// The clock must be [`ClockType::RosTime`].
108    ///
109    /// Wrapper for `rcl_disable_ros_time_override`
110    #[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    /// Sets new time value if the clock has enabled alternative time source
127    ///
128    /// If the clock does not have alternative time source enabled this function will not change the time.
129    ///
130    /// The clock must be [`ClockType::RosTime`].
131    ///
132    /// Wrapper for `rcl_set_ros_time_override`
133    #[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}