|
| 1 | +use crate::rcl_bindings::*; |
| 2 | +use crate::{error::ToResult, time::Time, to_rclrs_result}; |
| 3 | +use std::sync::{Arc, Mutex}; |
| 4 | + |
| 5 | +/// Enum to describe clock type. Redefined for readability and to eliminate the uninitialized case |
| 6 | +/// from the `rcl_clock_type_t` enum in the binding. |
| 7 | +#[derive(Clone, Debug, Copy)] |
| 8 | +pub enum ClockType { |
| 9 | + /// Time with behavior dependent on the `set_ros_time(bool)` function. If called with `true` |
| 10 | + /// it will be driven by a manual value override, otherwise it will be System Time |
| 11 | + RosTime = 1, |
| 12 | + /// Wall time depending on the current system |
| 13 | + SystemTime = 2, |
| 14 | + /// Steady time, monotonically increasing but not necessarily equal to wall time. |
| 15 | + SteadyTime = 3, |
| 16 | +} |
| 17 | + |
| 18 | +impl From<ClockType> for rcl_clock_type_t { |
| 19 | + fn from(clock_type: ClockType) -> Self { |
| 20 | + match clock_type { |
| 21 | + ClockType::RosTime => rcl_clock_type_t::RCL_ROS_TIME, |
| 22 | + ClockType::SystemTime => rcl_clock_type_t::RCL_SYSTEM_TIME, |
| 23 | + ClockType::SteadyTime => rcl_clock_type_t::RCL_STEADY_TIME, |
| 24 | + } |
| 25 | + } |
| 26 | +} |
| 27 | + |
| 28 | +/// Struct that implements a Clock and wraps `rcl_clock_t`. |
| 29 | +#[derive(Clone, Debug)] |
| 30 | +pub struct Clock { |
| 31 | + _type: ClockType, |
| 32 | + _rcl_clock: Arc<Mutex<rcl_clock_t>>, |
| 33 | + // TODO(luca) Implement jump callbacks |
| 34 | +} |
| 35 | + |
| 36 | +/// A clock source that can be used to drive the contained clock. Created when a clock of type |
| 37 | +/// `ClockType::RosTime` is constructed |
| 38 | +pub struct ClockSource { |
| 39 | + _rcl_clock: Arc<Mutex<rcl_clock_t>>, |
| 40 | +} |
| 41 | + |
| 42 | +impl Clock { |
| 43 | + /// Creates a new Clock with `ClockType::SystemTime` |
| 44 | + pub fn system() -> Self { |
| 45 | + Self::make(ClockType::SystemTime) |
| 46 | + } |
| 47 | + |
| 48 | + /// Creates a new Clock with `ClockType::SteadyTime` |
| 49 | + pub fn steady() -> Self { |
| 50 | + Self::make(ClockType::SteadyTime) |
| 51 | + } |
| 52 | + |
| 53 | + /// Creates a new Clock with `ClockType::RosTime` and a matching `ClockSource` that can be used |
| 54 | + /// to update it |
| 55 | + pub fn with_source() -> (Self, ClockSource) { |
| 56 | + let clock = Self::make(ClockType::RosTime); |
| 57 | + let clock_source = ClockSource::new(clock._rcl_clock.clone()); |
| 58 | + (clock, clock_source) |
| 59 | + } |
| 60 | + |
| 61 | + /// Creates a new clock of the given `ClockType`. |
| 62 | + pub fn new(type_: ClockType) -> (Self, Option<ClockSource>) { |
| 63 | + let clock = Self::make(type_); |
| 64 | + let clock_source = |
| 65 | + matches!(type_, ClockType::RosTime).then(|| ClockSource::new(clock._rcl_clock.clone())); |
| 66 | + (clock, clock_source) |
| 67 | + } |
| 68 | + |
| 69 | + fn make(type_: ClockType) -> Self { |
| 70 | + let mut rcl_clock; |
| 71 | + unsafe { |
| 72 | + // SAFETY: Getting a default value is always safe. |
| 73 | + rcl_clock = Self::init_generic_clock(); |
| 74 | + let mut allocator = rcutils_get_default_allocator(); |
| 75 | + // Function will return Err(_) only if there isn't enough memory to allocate a clock |
| 76 | + // object. |
| 77 | + rcl_clock_init(type_.into(), &mut rcl_clock, &mut allocator) |
| 78 | + .ok() |
| 79 | + .unwrap(); |
| 80 | + } |
| 81 | + Self { |
| 82 | + _type: type_, |
| 83 | + _rcl_clock: Arc::new(Mutex::new(rcl_clock)), |
| 84 | + } |
| 85 | + } |
| 86 | + |
| 87 | + /// Returns the clock's `ClockType`. |
| 88 | + pub fn clock_type(&self) -> ClockType { |
| 89 | + self._type |
| 90 | + } |
| 91 | + |
| 92 | + /// Returns the current clock's timestamp. |
| 93 | + pub fn now(&self) -> Time { |
| 94 | + let mut clock = self._rcl_clock.lock().unwrap(); |
| 95 | + let mut time_point: i64 = 0; |
| 96 | + unsafe { |
| 97 | + // SAFETY: No preconditions for this function |
| 98 | + rcl_clock_get_now(&mut *clock, &mut time_point); |
| 99 | + } |
| 100 | + Time { |
| 101 | + nsec: time_point, |
| 102 | + clock: Arc::downgrade(&self._rcl_clock), |
| 103 | + } |
| 104 | + } |
| 105 | + |
| 106 | + /// Helper function to privately initialize a default clock, with the same behavior as |
| 107 | + /// `rcl_init_generic_clock`. By defining a private function instead of implementing |
| 108 | + /// `Default`, we avoid exposing a public API to create an invalid clock. |
| 109 | + // SAFETY: Getting a default value is always safe. |
| 110 | + unsafe fn init_generic_clock() -> rcl_clock_t { |
| 111 | + let allocator = rcutils_get_default_allocator(); |
| 112 | + rcl_clock_t { |
| 113 | + type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED, |
| 114 | + jump_callbacks: std::ptr::null_mut::<rcl_jump_callback_info_t>(), |
| 115 | + num_jump_callbacks: 0, |
| 116 | + get_now: None, |
| 117 | + data: std::ptr::null_mut::<std::os::raw::c_void>(), |
| 118 | + allocator, |
| 119 | + } |
| 120 | + } |
| 121 | +} |
| 122 | + |
| 123 | +impl Drop for ClockSource { |
| 124 | + fn drop(&mut self) { |
| 125 | + self.set_ros_time_enable(false); |
| 126 | + } |
| 127 | +} |
| 128 | + |
| 129 | +impl PartialEq for ClockSource { |
| 130 | + fn eq(&self, other: &Self) -> bool { |
| 131 | + Arc::ptr_eq(&self._rcl_clock, &other._rcl_clock) |
| 132 | + } |
| 133 | +} |
| 134 | + |
| 135 | +impl ClockSource { |
| 136 | + /// Sets the value of the current ROS time. |
| 137 | + pub fn set_ros_time_override(&self, nanoseconds: i64) { |
| 138 | + let mut clock = self._rcl_clock.lock().unwrap(); |
| 139 | + // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed |
| 140 | + // by the mutex |
| 141 | + unsafe { |
| 142 | + // Function will only fail if timer was uninitialized or not RosTime, which should |
| 143 | + // not happen |
| 144 | + rcl_set_ros_time_override(&mut *clock, nanoseconds) |
| 145 | + .ok() |
| 146 | + .unwrap(); |
| 147 | + } |
| 148 | + } |
| 149 | + |
| 150 | + fn new(clock: Arc<Mutex<rcl_clock_t>>) -> Self { |
| 151 | + let source = Self { _rcl_clock: clock }; |
| 152 | + source.set_ros_time_enable(true); |
| 153 | + source |
| 154 | + } |
| 155 | + |
| 156 | + /// Sets the clock to use ROS Time, if enabled the clock will report the last value set through |
| 157 | + /// `Clock::set_ros_time_override(nanoseconds: i64)`. |
| 158 | + fn set_ros_time_enable(&self, enable: bool) { |
| 159 | + let mut clock = self._rcl_clock.lock().unwrap(); |
| 160 | + if enable { |
| 161 | + // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed |
| 162 | + // by the mutex |
| 163 | + unsafe { |
| 164 | + // Function will only fail if timer was uninitialized or not RosTime, which should |
| 165 | + // not happen |
| 166 | + rcl_enable_ros_time_override(&mut *clock).ok().unwrap(); |
| 167 | + } |
| 168 | + } else { |
| 169 | + // SAFETY: Safe if clock jump callbacks are not edited, which is guaranteed |
| 170 | + // by the mutex |
| 171 | + unsafe { |
| 172 | + // Function will only fail if timer was uninitialized or not RosTime, which should |
| 173 | + // not happen |
| 174 | + rcl_disable_ros_time_override(&mut *clock).ok().unwrap(); |
| 175 | + } |
| 176 | + } |
| 177 | + } |
| 178 | +} |
| 179 | + |
| 180 | +impl Drop for rcl_clock_t { |
| 181 | + fn drop(&mut self) { |
| 182 | + // SAFETY: No preconditions for this function |
| 183 | + let rc = unsafe { rcl_clock_fini(&mut *self) }; |
| 184 | + if let Err(e) = to_rclrs_result(rc) { |
| 185 | + panic!("Unable to release Clock. {:?}", e) |
| 186 | + } |
| 187 | + } |
| 188 | +} |
| 189 | + |
| 190 | +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread |
| 191 | +// they are running in. Therefore, this type can be safely sent to another thread. |
| 192 | +unsafe impl Send for rcl_clock_t {} |
| 193 | + |
| 194 | +#[cfg(test)] |
| 195 | +mod tests { |
| 196 | + use super::*; |
| 197 | + |
| 198 | + fn assert_send<T: Send>() {} |
| 199 | + fn assert_sync<T: Sync>() {} |
| 200 | + |
| 201 | + #[test] |
| 202 | + fn clock_is_send_and_sync() { |
| 203 | + assert_send::<Clock>(); |
| 204 | + assert_sync::<Clock>(); |
| 205 | + } |
| 206 | + |
| 207 | + #[test] |
| 208 | + fn clock_system_time_now() { |
| 209 | + let clock = Clock::system(); |
| 210 | + assert!(clock.now().nsec > 0); |
| 211 | + } |
| 212 | + |
| 213 | + #[test] |
| 214 | + fn clock_ros_time_with_override() { |
| 215 | + let (clock, source) = Clock::with_source(); |
| 216 | + // No manual time set, it should default to 0 |
| 217 | + assert!(clock.now().nsec == 0); |
| 218 | + let set_time = 1234i64; |
| 219 | + source.set_ros_time_override(set_time); |
| 220 | + // Ros time is set, should return the value that was set |
| 221 | + assert_eq!(clock.now().nsec, set_time); |
| 222 | + } |
| 223 | +} |
0 commit comments