diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index d43d9883c..db49d9201 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -13,6 +13,8 @@ path = "src/lib.rs" [dependencies] # Needed for FFI libc = "0.2.43" +# Needed for numeric traits +num = "0.1.4" # Provides better concurrency primitives than std parking_lot = "0.11.2" # Needed for the Message trait, among others diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs new file mode 100644 index 000000000..69e39e434 --- /dev/null +++ b/rclrs/src/clock.rs @@ -0,0 +1,242 @@ +use crate::context; +use crate::duration; +use crate::error::RclrsError; +use crate::rcl_bindings::*; +use crate::time; +use crate::RclReturnCode; +use parking_lot::{Condvar, Mutex}; +use std::os::raw::{c_int, c_void}; +use std::sync::Arc; +use std::time::Instant; + +impl std::default::Default for rcl_allocator_t { + fn default() -> Self { + let empty: c_int = 0; + rcl_allocator_t { + allocate: None, + deallocate: None, + reallocate: None, + zero_allocate: None, + state: empty as *mut c_void, + } + } +} + +impl std::default::Default for rcl_clock_t { + fn default() -> Self { + let mut threshold_ = rcl_jump_callback_info_t { + callback: None, + threshold: rcl_jump_threshold_t { + on_clock_change: false, + min_forward: rcl_duration_t { + nanoseconds: 1 as rcl_duration_value_t, + }, + min_backward: rcl_duration_t { + nanoseconds: -1 as rcl_duration_value_t, + }, + }, + user_data: 0 as *mut c_void, + }; + + rcl_clock_t { + type_: rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED, + jump_callbacks: &mut threshold_, + num_jump_callbacks: usize::default(), + get_now: None, + data: 0 as *mut c_void, + allocator: rcl_allocator_t::default(), + } + } +} + +struct JumpHandler { + pre_callback: fn(), + post_callback: fn() -> Mutex, + threshold: rcl_jump_threshold_t, +} + +#[allow(dead_code)] +impl JumpHandler { + fn new( + pre_callback: fn(), + post_callback: fn() -> Mutex, + threshold: rcl_jump_threshold_t, + ) -> Self { + /* + static pre_cb: &dyn Fn() = & || { + pre_callback() + }; + static post_cb: &dyn Fn() -> Mutex = & || -> Mutex { + post_callback() + };*/ + Self { + pre_callback, + post_callback, + threshold, + } + } +} + +struct Impl { + rcl_clock_: Mutex, + allocator_: Mutex, + thread_handler_: Arc<(Mutex, Condvar)>, +} + +/// The Clock struct +pub struct Clock { + impl_: Impl, +} + +#[allow(dead_code)] +impl Clock { + /// Function to create a new Clock instance + pub fn new(clock_type: rcl_clock_type_t) -> Result { + let mut impl_ = Impl { + rcl_clock_: Mutex::new(rcl_clock_t::default()), + allocator_: Mutex::new(rcl_allocator_t::default()), + thread_handler_: Arc::new((Mutex::new(bool::default()), Condvar::new())), + }; + // Safety: variables are wrapped in Mutex + // raw pointer get converted back to safe types once `get_mut` goes out of scope + let ret: rcl_ret_t = unsafe { + rcl_clock_init( + clock_type, + impl_.rcl_clock_.get_mut() as *mut rcl_clock_t, + impl_.allocator_.get_mut() as *mut rcl_allocator_t, + ) + }; + + if ret != 0 { + return Err(RclrsError::RclError { + code: RclReturnCode::Error, + msg: None, + }); + } + + Ok(Self { impl_ }) + } + + /// Function to get clock type of Clock object + pub fn get_clock_type(&self) -> rcl_clock_type_t { + (*self.impl_.rcl_clock_.lock()).type_ + } + + /// Function to get the time from the source at a given instant + pub fn now(&self) -> Result { + let now = + time::Time::new(time::TimeFrom::NanoSecs { ns: 0u64 }, self.get_clock_type()).unwrap(); + + // Safety: Variables are wrapped in mutex, to ensure type safety + // Unsafe variables are converted back to safe types + let ret = unsafe { + rcl_clock_get_now( + &mut *self.impl_.rcl_clock_.lock(), + &mut (now.get_lock()).nanoseconds, + ) + }; + + if ret != 0 { + return Err(RclrsError::RclError { + code: RclReturnCode::Error, + msg: None, + }); + } + + Ok(now) + } + + /// Function to check if ros clock is valid or not + fn ros_time_is_active(&self) -> bool { + // Safety: No preconditions for this function + if unsafe { !rcl_clock_valid(&mut *self.impl_.rcl_clock_.lock()) } { + return false; + } + + let mut is_enabled: bool = bool::default(); + + // Safety: No preconditions for this function + let ret = unsafe { + rcl_is_enabled_ros_time_override(&mut *self.impl_.rcl_clock_.lock(), &mut is_enabled) + }; + if ret != 0 { + panic!("Failed to check ros time status") + } + is_enabled + } + + /// Function to return clock handle + pub fn get_clock_handle(&mut self) -> &mut rcl_clock_t { + self.impl_.rcl_clock_.get_mut() + } + + /// Function to sleep until a given time stamp + pub fn sleep_until( + &self, + until: time::Time, + context: &context::Context, + ) -> Result { + let context_mtx = Arc::clone(&context.rcl_context_mtx); + // Safety: No preconditions for this function + if unsafe { !rcl_context_is_valid(&mut *context_mtx.lock()) } { + return Err(RclrsError::RclError { + code: RclReturnCode::Error, + msg: None, + }); + } else { + let mut time_source_changed: bool = false; + match self.get_clock_type() { + rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED => { + return Err(RclrsError::RclError { + code: RclReturnCode::Error, + msg: None, + }); + } + rcl_clock_type_t::RCL_ROS_TIME => { + todo!("implement it for RCL_ROS_TIME"); + } + rcl_clock_type_t::RCL_SYSTEM_TIME => { + let &(ref lock, ref cvar) = &*(Arc::clone(&self.impl_.thread_handler_)); + + let delta = (until.clone() - self.now().unwrap()).to_duration(); + let sys_time = Instant::now() + delta; + // Safety: No preconditions for this function + while (self.now().unwrap() < until) + && unsafe { rcl_context_is_valid(&mut *(*context.rcl_context_mtx).lock()) } + { + cvar.wait_until(&mut lock.lock(), sys_time); + } + } + rcl_clock_type_t::RCL_STEADY_TIME => { + let rcl_entry = self.now().unwrap(); + let std_time_entry = Instant::now(); + let delta = (until.clone() - rcl_entry).to_duration(); + let std_time_until = std_time_entry + delta; + + let &(ref lock, ref cvar) = &*(Arc::clone(&self.impl_.thread_handler_)); + while (self.now().unwrap() < until) + && unsafe { rcl_context_is_valid(&mut *(*context.rcl_context_mtx).lock()) } + { + cvar.wait_until(&mut lock.lock(), std_time_until); + } + } + } + } + Ok(true) + } + + /// Function to sleep for a given duration + pub fn sleep_for( + &self, + duration: duration::Duration, + context: &context::Context, + ) -> Result { + self.sleep_until(self.now().unwrap() + duration, context) + } +} +/* + todo!("add function sleep_until"); + todo!("add function get_clock_mutex"); + todo!("add function on_time_jump"); + todo!("add function create_jump_callback"); +*/ diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs new file mode 100644 index 000000000..c1cefeabe --- /dev/null +++ b/rclrs/src/duration.rs @@ -0,0 +1,205 @@ +use crate::rcl_bindings::*; +//use crate::RclReturnCode; +use crate::RclrsError; +use num::{abs, signum}; +use std::cmp::Ordering; +use std::ops::{Add, Mul, Sub}; +//use std::os::raw::{c_uint, c_void}; +use std::time; + +/// Enum for Duration constructor arguments +#[allow(missing_docs)] +pub enum DurationFrom { + /// Create Duration instance using seconds + Secs { s: i32 }, + + /// Create Duration instance using nanoseconds + NanoSecs { ns: rcl_duration_value_t }, + + /// Create Duration instance using seconds and ns + SecsAndNanoSecs { s: i32, ns: rcl_duration_value_t }, + + /// Create Duration instance using std::time::Duration + Duration { duration: time::Duration }, + + /// Create Duration from rmw_time_t + RMWTime { time: rmw_time_t }, +} + +/// The Duration struct +pub struct Duration { + _duration_handle: rcl_duration_t, +} + +impl Add for Duration { + type Output = Self; + + fn add(self, rhs: Self) -> Self { + let sum = self._duration_handle.nanoseconds + rhs._duration_handle.nanoseconds; + if (sum as u128) > (rcl_duration_value_t::MAX as u128) { + panic!( + "Addition leads to {} overflow", + std::any::type_name::() + ); + } + + Self::new(DurationFrom::NanoSecs { + ns: sum as rcl_duration_value_t, + }) + .unwrap() + } +} + +impl Clone for Duration { + fn clone(&self) -> Self { + Self::new(DurationFrom::NanoSecs { + ns: self._duration_handle.nanoseconds, + }) + .unwrap() + } +} + +impl Eq for Duration {} + +impl Mul for Duration { + type Output = Self; + + fn mul(self, rhs: f32) -> Self { + let prod = (self._duration_handle.nanoseconds as f32) * rhs; + + if (prod as i128) > (rcl_duration_value_t::MAX as i128) { + panic!( + "Scaling leads to {} overflow", + std::any::type_name::() + ); + } + + if (prod as i128) < (rcl_duration_value_t::MIN as i128) { + panic!( + "Scaling leads to {} underflow", + std::any::type_name::() + ); + } + + Self::new(DurationFrom::NanoSecs { + ns: prod as rcl_duration_value_t, + }) + .unwrap() + } +} + +impl Ord for Duration { + fn cmp(&self, rhs: &Self) -> Ordering { + self.nanoseconds().cmp(&rhs.nanoseconds()) + } +} + +impl PartialEq for Duration { + fn eq(&self, rhs: &Self) -> bool { + self.nanoseconds() == rhs.nanoseconds() + } +} + +impl PartialOrd for Duration { + fn partial_cmp(&self, rhs: &Self) -> Option { + Some(self.cmp(rhs)) + } +} + +impl Sub for Duration { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + let diff = self._duration_handle.nanoseconds - rhs._duration_handle.nanoseconds; + if (diff as i128) < (rcl_duration_value_t::MIN as i128) { + panic!( + "Subtraction leads to {} underflow", + std::any::type_name::() + ); + } + + Self::new(DurationFrom::NanoSecs { + ns: diff as rcl_duration_value_t, + }) + .unwrap() + } +} + +#[allow(dead_code)] +impl Duration { + /// Function to instantiate a Duration object + /// + /// # Example + /// ## initialize using Seconds + /// `Duration::new(DurationFrom::Secs{s: })` + /// + /// ## initialize using Nanoseconds + /// `Duration::new(DurationFrom::NanoSecs{ns: })` + /// + /// ## initialize using Seconds and Nanoseconds + /// `Duration::new(DurationFrom::SecsAndNanoSecs{s: , ns: })` + /// + /// ## initialize using `std::time::Duration` + /// `Duration::new(DurationFrom::Duration{duration: std::time::Duration::from...})` + /// + /// ## initialize using `rmw_time_t` + /// `Duration::new(DurationFrom::Duration{time: rmw_time_t { ... }})` + pub fn new(duration: DurationFrom) -> Result { + match duration { + DurationFrom::Secs { s } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: ((signum(s) as i64) + * (time::Duration::from_secs(abs(s) as u64).as_nanos()) as i64) + as rcl_duration_value_t, + }, + }), + DurationFrom::NanoSecs { ns } => Ok(Self { + _duration_handle: rcl_duration_t { nanoseconds: ns }, + }), + DurationFrom::SecsAndNanoSecs { s, ns } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: ((signum(s) as i64) + * (time::Duration::from_secs(abs(s) as u64).as_nanos() as i64) + as rcl_duration_value_t) + + ns, + }, + }), + DurationFrom::Duration { duration } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: duration.as_nanos() as rcl_duration_value_t, + }, + }), + DurationFrom::RMWTime { time } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: ((time::Duration::from_secs(time.sec).as_nanos() as u64) + + time.nsec) as rcl_duration_value_t, + }, + }), + } + } + + /// Function to get the count of nanoseconds in the Duration object + pub fn nanoseconds(&self) -> rcl_duration_value_t { + self._duration_handle.nanoseconds + } + + /// Function to get the count of seconds in the Duration object + pub fn seconds(&self) -> i32 { + let ns = self._duration_handle.nanoseconds; + (signum(ns) as i32) * (time::Duration::from_nanos(abs(ns) as u64).as_secs() as i32) + } + + /// Function to get a `std::time::Duration` object + pub fn to_duration(&self) -> time::Duration { + time::Duration::from_nanos(self._duration_handle.nanoseconds as u64) + } + + /// Function to get the maximum value that can be held in duration + pub fn max(&self) -> Self { + Self { + _duration_handle: rcl_duration_t { + nanoseconds: rcl_duration_value_t::MAX, + }, + } + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 09aafa512..2d8976d8b 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -5,20 +5,26 @@ //! //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md +mod clock; mod context; +mod duration; mod error; mod node; mod parameter; mod qos; +mod time; mod wait; mod rcl_bindings; +pub use clock::*; pub use context::*; +pub use duration::*; pub use error::*; pub use node::*; pub use parameter::*; pub use qos::*; +pub use time::*; pub use wait::*; use rcl_bindings::rcl_context_is_valid; diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs new file mode 100644 index 000000000..1907e6091 --- /dev/null +++ b/rclrs/src/time.rs @@ -0,0 +1,194 @@ +use crate::error::RclrsError; +use crate::rcl_bindings::*; +//use crate::RclReturnCode; +use crate::duration::Duration; +use parking_lot::{Mutex, MutexGuard}; +use std::cmp::Ordering; +use std::ops::{Add, Sub}; +use std::time; + +/// Enum to provide different ways to construct the Time struct +#[allow(missing_docs)] +pub enum TimeFrom { + /// Create a Time object using seconds + Secs { s: u64 }, + + /// Create a Time object using nanoseconds + NanoSecs { ns: u64 }, + + /// Create a Time object using `std::time::Duration` + Duration { d: time::Duration }, +} + +/// The Time struct +pub struct Time { + rcl_time_: Mutex, + // wrapped in mutex to allow the Clock struct to access it +} + +impl Add for Time { + type Output = Self; + + fn add(self, rhs: Duration) -> Self { + let lock = self.get_lock(); + let sum = (*lock).nanoseconds + (rhs.nanoseconds() as rcl_time_point_value_t); + if (sum as u128) > (rcl_time_point_value_t::MAX as u128) { + panic!( + "Addition causes a {} overlflow", + std::any::type_name::() + ) + } + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: sum, + clock_type: (*lock).clock_type, + }), + } + } +} + +impl Clone for Time { + fn clone(&self) -> Self { + let lock = self.get_lock(); + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: lock.nanoseconds, + clock_type: lock.clock_type, + }), + } + } +} + +impl Eq for Time {} + +impl Ord for Time { + fn cmp(&self, rhs: &Self) -> Ordering { + self.nanoseconds().cmp(&rhs.nanoseconds()) + } +} + +impl PartialEq for Time { + fn eq(&self, rhs: &Self) -> bool { + self.nanoseconds() == rhs.nanoseconds() + } +} + +impl PartialOrd for Time { + fn partial_cmp(&self, rhs: &Self) -> Option { + Some(self.cmp(rhs)) + } +} + +impl Sub for Time { + type Output = Self; + + fn sub(self, rhs: Time) -> Self { + let lock = self.get_lock(); + let rhs_lock = rhs.get_lock(); + let diff = lock.nanoseconds - rhs_lock.nanoseconds; + if lock.clock_type != rhs_lock.clock_type { + panic!("Can not subtract times with different time sources"); + } else if diff < 0 { + panic!("Time subtraction leads to negative time"); + } + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: diff, + clock_type: lock.clock_type, + }), + } + } +} + +impl Sub for Time { + type Output = Self; + + fn sub(self, rhs: Duration) -> Self { + let lock = self.get_lock(); + let diff = lock.nanoseconds - (rhs.nanoseconds() as rcl_time_point_value_t); + if diff < 0 { + panic!("Subtraction leads to negative time"); + } + + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: diff, + clock_type: lock.clock_type, + }), + } + } +} + +#[allow(dead_code)] +impl Time { + /// Function to create a new instance of Time + /// + /// # Example + /// + /// ## Create a Time object using seconds + /// `Time::new(TimeFrom::Secs{s: })` + /// + /// ## Create a Time object using nanoseconds + /// `Time::new(TimeFrom::NanoSecs{ns: })` + /// + /// ## Create a Time object using `std::time::Duration` + /// `Time::new(TimeFrom::Duration{d: std::time::Duration...})` + pub fn new(arg: TimeFrom, clock_type: rcl_clock_type_t) -> Result { + match arg { + TimeFrom::Secs { s } => Ok(Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: time::Duration::from_secs(s).as_nanos() as rcl_time_point_value_t, + clock_type, + }), + }), + TimeFrom::NanoSecs { ns } => Ok(Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: ns as rcl_time_point_value_t, + clock_type, + }), + }), + TimeFrom::Duration { d } => Ok(Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: d.as_nanos() as rcl_time_point_value_t, + clock_type, + }), + }), + } + } + + /// Function to return time in nanoseconds + pub fn nanoseconds(&self) -> rcl_time_point_value_t { + (*self.get_lock()).nanoseconds + } + + /// Function to return time in seconds + pub fn seconds(&self) -> u64 { + time::Duration::from_nanos((*self.rcl_time_.lock()).nanoseconds.try_into().unwrap()) + .as_secs() + } + + /// Function to get a lock on `rcl_time_` + pub fn get_lock(&self) -> MutexGuard<'_, rcl_time_point_t> { + self.rcl_time_.lock() + } + + /// Function to return clock type + pub fn get_clock_type(&self) -> rcl_clock_type_t { + self.get_lock().clock_type + } + + /// Function to return the maximum possible value that can be held in the given instance + pub fn max(&self) -> Self { + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: rcl_time_point_value_t::MAX, + clock_type: self.get_lock().clock_type, + }), + } + } + + /// Function to return the time object as a std::time::Duration object + pub fn to_duration(&self) -> time::Duration { + time::Duration::from_secs(self.seconds()) + } +}