From 8e9393d7f5eede131b1483309bc1e206d927889e Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Tue, 14 Jun 2022 15:30:35 +0530 Subject: [PATCH 01/25] add time.rs to src as well as to lib.rs (#121) --- rclrs/src/lib.rs | 2 ++ rclrs/src/time.rs | 54 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 56 insertions(+) create mode 100644 rclrs/src/time.rs diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 523a1e91c..4745d2c71 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,6 +11,7 @@ mod node; mod parameter; mod qos; mod wait; +mod time; mod rcl_bindings; @@ -20,6 +21,7 @@ pub use node::*; pub use parameter::*; pub use qos::*; pub use wait::*; +pub use time::*; use rcl_bindings::rcl_context_is_valid; use std::time::Duration; diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs new file mode 100644 index 000000000..1122462e3 --- /dev/null +++ b/rclrs/src/time.rs @@ -0,0 +1,54 @@ +use crate::rcl_bindings::*; +use std::os::raw::{c_void, c_uint}; + +/// Function to get the current steady time +pub fn rcl_get_steady_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { + // SAFETY: No preconditions for this function + unsafe { + rcutils_steady_time_now(current_time) + } +} + +/// Function to return the current system time +pub fn rcl_get_system_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { + // SAFETY: No preconditions for this function + unsafe { + rcutils_system_time_now(current_time) + } +} + +impl rcl_clock_t { + /* + /// Function to create a new rcl_clock_t instance + pub fn new(type_: rcl_clock_type_t, allocator: rcl_allocator_t) -> Self { + unsafe { + let empty_data = c_void::null; + Self { + type_: type_, + jump_callbacks: None, + num_jump_callbacks: 0 as usize, + get_now: None, + data: empty_data, + allocator: allocator, + } + } + }*/ + /// Function to check validity of the clock + pub fn rcl_clock_valid(&self) -> bool { + let mut _flag: bool = false; + match self.type_ { + rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED => _flag = false, + _ => _flag = true, + } + + if !_flag { + match self.get_now { + Option::None => false, + _ => true, + } + } else { + _flag + } + } +} + From efe7010370203438b899d24204e5dd966434018e Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Tue, 14 Jun 2022 17:39:31 +0530 Subject: [PATCH 02/25] added the Time struct --- rclrs/src/time.rs | 35 ++++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 1122462e3..64e141415 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -1,8 +1,12 @@ +use crate::error::RclrsError; +use crate::RclReturnCode; use crate::rcl_bindings::*; use std::os::raw::{c_void, c_uint}; +use std::time::Duration; + /// Function to get the current steady time -pub fn rcl_get_steady_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { +fn rcl_get_steady_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { // SAFETY: No preconditions for this function unsafe { rcutils_steady_time_now(current_time) @@ -10,7 +14,7 @@ pub fn rcl_get_steady_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret } /// Function to return the current system time -pub fn rcl_get_system_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { +fn rcl_get_system_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { // SAFETY: No preconditions for this function unsafe { rcutils_system_time_now(current_time) @@ -34,7 +38,7 @@ impl rcl_clock_t { } }*/ /// Function to check validity of the clock - pub fn rcl_clock_valid(&self) -> bool { + fn rcl_clock_valid(&self) -> bool { let mut _flag: bool = false; match self.type_ { rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED => _flag = false, @@ -52,3 +56,28 @@ impl rcl_clock_t { } } +pub struct Time { + seconds: i32, + nanoseconds: u32, + clock_type: rcl_clock_type_t, +} + +impl Time { + /// Function to create a new instance of Time + pub fn new(seconds: i32, + nanoseconds: u32, + clock_type: rcl_clock_type_t) -> Result { + if seconds < 0 { + return Err(RclrsError::RclError{ + code: RclReturnCode::InvalidArgument, + msg: None + }); + } + + Ok(Self { + seconds, + nanoseconds, + clock_type, + }) + } +} From f024ad621cd0b51dc3f7d927d1e66a9ace0c8dc4 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Tue, 14 Jun 2022 17:40:27 +0530 Subject: [PATCH 03/25] add duration.rs to lib and src --- rclrs/src/duration.rs | 0 rclrs/src/lib.rs | 2 ++ 2 files changed, 2 insertions(+) create mode 100644 rclrs/src/duration.rs diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs new file mode 100644 index 000000000..e69de29bb diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4745d2c71..017d01207 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -12,6 +12,7 @@ mod parameter; mod qos; mod wait; mod time; +mod duration; mod rcl_bindings; @@ -22,6 +23,7 @@ pub use parameter::*; pub use qos::*; pub use wait::*; pub use time::*; +pub use duration::*; use rcl_bindings::rcl_context_is_valid; use std::time::Duration; From 92e55a1cc8811fd5e6724b1cdde68ef43199ecef Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Tue, 14 Jun 2022 22:22:18 +0530 Subject: [PATCH 04/25] added the Duration struct --- rclrs/src/duration.rs | 48 ++++++++++++++++++++++++++++++ rclrs/src/lib.rs | 8 ++--- rclrs/src/time.rs | 68 +++++++++++++++++++++---------------------- 3 files changed, 85 insertions(+), 39 deletions(-) diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs index e69de29bb..27da0971b 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -0,0 +1,48 @@ +use crate::rcl_bindings::*; +use crate::RclReturnCode; +use crate::RclrsError; +use std::os::raw::{c_uint, c_void}; +use std::time; + +/// Enum for Duration constructor arguments +pub enum DurationFrom { + // Create Duration instance using seconds + SECONDS { s: u32 }, + + // Create Duration instance using seconds and ns + SECONDS_AND_NANOS { s: u32, ns: u64 }, + + // Create Duration instance using std::time::Duration + DURATION { duration: time::Duration }, +} + +/// The Duration struct +pub struct Duration { + _duration_handle: rcl_duration_t, +} + +impl Duration { + /// Function to instantiate a Duration object + pub fn new(duration: DurationFrom) -> Result { + match duration { + DurationFrom::SECONDS { s } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: time::Duration::from_secs(s.into()).as_nanos() + as rcl_duration_value_t, + }, + }), + DurationFrom::SECONDS_AND_NANOS { s, ns } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: (time::Duration::from_secs(s.into()).as_nanos() + as rcl_duration_value_t) + + (ns as rcl_duration_value_t), + }, + }), + DurationFrom::DURATION { duration } => Ok(Self { + _duration_handle: rcl_duration_t { + nanoseconds: duration.as_nanos() as rcl_duration_value_t, + }, + }), + } + } +} diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 017d01207..afc7bc912 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -6,24 +6,24 @@ //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md mod context; +mod duration; mod error; mod node; mod parameter; mod qos; -mod wait; mod time; -mod duration; +mod wait; mod rcl_bindings; pub use context::*; +pub use duration::*; pub use error::*; pub use node::*; pub use parameter::*; pub use qos::*; -pub use wait::*; pub use time::*; -pub use duration::*; +pub use wait::*; use rcl_bindings::rcl_context_is_valid; use std::time::Duration; diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 64e141415..71c76ba66 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -1,24 +1,48 @@ use crate::error::RclrsError; -use crate::RclReturnCode; use crate::rcl_bindings::*; -use std::os::raw::{c_void, c_uint}; +use crate::RclReturnCode; +use std::os::raw::{c_uint, c_void}; use std::time::Duration; +/// The Time struct +pub struct Time { + seconds: i32, + nanoseconds: u32, + clock_type: rcl_clock_type_t, +} + +impl Time { + /// Function to create a new instance of Time + pub fn new( + seconds: i32, + nanoseconds: u32, + clock_type: rcl_clock_type_t, + ) -> Result { + if seconds < 0 { + return Err(RclrsError::RclError { + code: RclReturnCode::InvalidArgument, + msg: None, + }); + } + + Ok(Self { + seconds, + nanoseconds, + clock_type, + }) + } +} /// Function to get the current steady time fn rcl_get_steady_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { // SAFETY: No preconditions for this function - unsafe { - rcutils_steady_time_now(current_time) - } + unsafe { rcutils_steady_time_now(current_time) } } /// Function to return the current system time fn rcl_get_system_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { // SAFETY: No preconditions for this function - unsafe { - rcutils_system_time_now(current_time) - } + unsafe { rcutils_system_time_now(current_time) } } impl rcl_clock_t { @@ -26,7 +50,7 @@ impl rcl_clock_t { /// Function to create a new rcl_clock_t instance pub fn new(type_: rcl_clock_type_t, allocator: rcl_allocator_t) -> Self { unsafe { - let empty_data = c_void::null; + let empty_data = c_void::null; Self { type_: type_, jump_callbacks: None, @@ -55,29 +79,3 @@ impl rcl_clock_t { } } } - -pub struct Time { - seconds: i32, - nanoseconds: u32, - clock_type: rcl_clock_type_t, -} - -impl Time { - /// Function to create a new instance of Time - pub fn new(seconds: i32, - nanoseconds: u32, - clock_type: rcl_clock_type_t) -> Result { - if seconds < 0 { - return Err(RclrsError::RclError{ - code: RclReturnCode::InvalidArgument, - msg: None - }); - } - - Ok(Self { - seconds, - nanoseconds, - clock_type, - }) - } -} From c332adb4a4b3e42f74b3d2cb3dba97ad4e5e9e2b Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 12:20:29 +0530 Subject: [PATCH 05/25] complete duration.rs --- rclrs/src/duration.rs | 149 ++++++++++++++++++++++++++++++++++++++---- rclrs/src/lib.rs | 4 +- 2 files changed, 139 insertions(+), 14 deletions(-) diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs index 27da0971b..eed392361 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -1,19 +1,28 @@ use crate::rcl_bindings::*; -use crate::RclReturnCode; +//use crate::RclReturnCode; use crate::RclrsError; -use std::os::raw::{c_uint, c_void}; +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 - SECONDS { s: u32 }, + /// Create Duration instance using seconds + Secs { s: u32 }, - // Create Duration instance using seconds and ns - SECONDS_AND_NANOS { s: u32, ns: u64 }, + /// Create Duration instance using nanoseconds + NanoSecs { ns: rcl_duration_value_t }, - // Create Duration instance using std::time::Duration - DURATION { duration: time::Duration }, + /// Create Duration instance using seconds and ns + SecsAndNanoSecs { s: u32, 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 @@ -21,28 +30,144 @@ 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 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 < 0f32 { + panic!("Can not scale Duration by a negative number"); + } + + if (prod as u128) > (rcl_duration_value_t::MAX as u128) { + panic!( + "Scaling leads to {} overflow", + 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._duration_handle + .nanoseconds + .cmp(&rhs._duration_handle.nanoseconds) + } +} + +impl PartialEq for Duration { + fn eq(&self, rhs: &Self) -> bool { + self._duration_handle.nanoseconds == rhs._duration_handle.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 < 0 { + panic!("Subtraction results in negative duration"); + } + + 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::SECONDS { s } => Ok(Self { + DurationFrom::Secs { s } => Ok(Self { _duration_handle: rcl_duration_t { nanoseconds: time::Duration::from_secs(s.into()).as_nanos() as rcl_duration_value_t, }, }), - DurationFrom::SECONDS_AND_NANOS { s, ns } => Ok(Self { + DurationFrom::NanoSecs { ns } => Ok(Self { + _duration_handle: rcl_duration_t { nanoseconds: ns }, + }), + DurationFrom::SecsAndNanoSecs { s, ns } => Ok(Self { _duration_handle: rcl_duration_t { nanoseconds: (time::Duration::from_secs(s.into()).as_nanos() as rcl_duration_value_t) - + (ns as rcl_duration_value_t), + + ns, }, }), - DurationFrom::DURATION { duration } => Ok(Self { + 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) -> u32 { + time::Duration::from_nanos(self._duration_handle.nanoseconds as u64).as_secs() as u32 + } } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index afc7bc912..f82cb44c1 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,7 +11,7 @@ mod error; mod node; mod parameter; mod qos; -mod time; +//mod time; mod wait; mod rcl_bindings; @@ -22,7 +22,7 @@ pub use error::*; pub use node::*; pub use parameter::*; pub use qos::*; -pub use time::*; +//pub use time::*; pub use wait::*; use rcl_bindings::rcl_context_is_valid; From 68d6df6c6e0bcb3a7359d15e29220787136b1e43 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 12:53:58 +0530 Subject: [PATCH 06/25] add function to return Duration object --- rclrs/src/duration.rs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs index eed392361..c274e085b 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -170,4 +170,9 @@ impl Duration { pub fn seconds(&self) -> u32 { time::Duration::from_nanos(self._duration_handle.nanoseconds as u64).as_secs() as u32 } + + /// 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) + } } From 7eceafec2cc9649df73c58fd26983410f964b93b Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 15:14:18 +0530 Subject: [PATCH 07/25] implemented clone for Duration --- rclrs/src/duration.rs | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs index c274e085b..67814980a 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -49,6 +49,15 @@ impl Add for Duration { } } +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 { From 9c29dee4dbf63d2e65035cab6794b1bbbd993b3e Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 16:12:13 +0530 Subject: [PATCH 08/25] changes made to time.rs and lib.rs --- rclrs/src/lib.rs | 4 +- rclrs/src/time.rs | 125 ++++++++++++++++++++++------------------------ 2 files changed, 62 insertions(+), 67 deletions(-) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index f82cb44c1..afc7bc912 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,7 +11,7 @@ mod error; mod node; mod parameter; mod qos; -//mod time; +mod time; mod wait; mod rcl_bindings; @@ -22,7 +22,7 @@ pub use error::*; pub use node::*; pub use parameter::*; pub use qos::*; -//pub use time::*; +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 index 71c76ba66..71dc24fd0 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -1,81 +1,76 @@ use crate::error::RclrsError; use crate::rcl_bindings::*; -use crate::RclReturnCode; -use std::os::raw::{c_uint, c_void}; -use std::time::Duration; +//use crate::RclReturnCode; +//use crate::duration; +use parking_lot::Mutex; +use std::time; -/// The Time struct -pub struct Time { - seconds: i32, - nanoseconds: u32, - clock_type: rcl_clock_type_t, -} -impl Time { - /// Function to create a new instance of Time - pub fn new( - seconds: i32, - nanoseconds: u32, - clock_type: rcl_clock_type_t, - ) -> Result { - if seconds < 0 { - return Err(RclrsError::RclError { - code: RclReturnCode::InvalidArgument, - msg: None, - }); - } +/// 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}, - Ok(Self { - seconds, - nanoseconds, - clock_type, - }) - } -} + /// Create a Time object using nanoseconds + NanoSecs{ns: u64}, -/// Function to get the current steady time -fn rcl_get_steady_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { - // SAFETY: No preconditions for this function - unsafe { rcutils_steady_time_now(current_time) } + /// Create a Time object using `std::time::Duration` + Duration{d: time::Duration}, } -/// Function to return the current system time -fn rcl_get_system_time(current_time: *mut rcl_time_point_value_t) -> rcl_ret_t { - // SAFETY: No preconditions for this function - unsafe { rcutils_system_time_now(current_time) } +/// The Time struct +pub struct Time { + rcl_time_: Mutex, + // wrapped in mutex to allow the Clock struct to access it } -impl rcl_clock_t { - /* - /// Function to create a new rcl_clock_t instance - pub fn new(type_: rcl_clock_type_t, allocator: rcl_allocator_t) -> Self { - unsafe { - let empty_data = c_void::null; +impl Clone for Time { + fn clone(&self) -> Self { + let lock = self.rcl_time_.lock(); Self { - type_: type_, - jump_callbacks: None, - num_jump_callbacks: 0 as usize, - get_now: None, - data: empty_data, - allocator: allocator, - } - } - }*/ - /// Function to check validity of the clock - fn rcl_clock_valid(&self) -> bool { - let mut _flag: bool = false; - match self.type_ { - rcl_clock_type_t::RCL_CLOCK_UNINITIALIZED => _flag = false, - _ => _flag = true, + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: (*lock).nanoseconds, + clock_type: (*lock).clock_type, + }) } + } +} - if !_flag { - match self.get_now { - Option::None => false, - _ => true, - } - } else { - _flag +#[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, + }), + }), } } } From 7f9a6a78ef5ff4b4ef4a20df4227473a1f72b288 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 17:01:05 +0530 Subject: [PATCH 09/25] implement Add for time.rs --- rclrs/src/time.rs | 53 +++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 49 insertions(+), 4 deletions(-) diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 71dc24fd0..1dcc8f0a5 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -1,10 +1,10 @@ use crate::error::RclrsError; use crate::rcl_bindings::*; //use crate::RclReturnCode; -//use crate::duration; +use crate::duration::Duration; use parking_lot::Mutex; use std::time; - +use std::ops::{Add, Sub}; /// Enum to provide different ways to construct the Time struct #[allow(missing_docs)] @@ -25,18 +25,63 @@ pub struct Time { // 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.rcl_time_.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 an overlflow for {}", std::any::type_name::()) + } + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: sum, + clock_type: (*lock).clock_type, + }) + } + } +} + +impl Sub for Time { + type Output = Self; + + fn sub(self, rhs: Time) -> Self { + let lock = self.rcl_time_.lock(); + let rhs_lock = rhs.rcl_time_.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"); + } else { + Self{ + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: diff, + clock_type: lock.clock_type, + }) + } + } + + + } +} + +//impl Sub + impl Clone for Time { fn clone(&self) -> Self { let lock = self.rcl_time_.lock(); Self { rcl_time_: Mutex::new(rcl_time_point_t { - nanoseconds: (*lock).nanoseconds, - clock_type: (*lock).clock_type, + nanoseconds: lock.nanoseconds, + clock_type: lock.clock_type, }) } } } + #[allow(dead_code)] impl Time { /// Function to create a new instance of Time From 15c9914269d25e87159cf5383c376951e67ad25a Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 17:24:33 +0530 Subject: [PATCH 10/25] add max implementation for Duration --- rclrs/src/duration.rs | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs index 67814980a..fd3a96d4e 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -184,4 +184,13 @@ impl Duration { 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, + }, + } + } } From 49fe3ed7ac413b6fe5fb869b3863680648c2e868 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 17:25:17 +0530 Subject: [PATCH 11/25] complete time.rs --- rclrs/src/time.rs | 83 +++++++++++++++++++++++++++++++++++------------ 1 file changed, 62 insertions(+), 21 deletions(-) diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 1dcc8f0a5..94dab9cda 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -3,20 +3,20 @@ use crate::rcl_bindings::*; //use crate::RclReturnCode; use crate::duration::Duration; use parking_lot::Mutex; -use std::time; 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}, + Secs { s: u64 }, /// Create a Time object using nanoseconds - NanoSecs{ns: u64}, + NanoSecs { ns: u64 }, /// Create a Time object using `std::time::Duration` - Duration{d: time::Duration}, + Duration { d: time::Duration }, } /// The Time struct @@ -32,13 +32,16 @@ impl Add for Time { let lock = self.rcl_time_.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 an overlflow for {}", std::any::type_name::()) + panic!( + "Addition causes an overlflow for {}", + std::any::type_name::() + ) } Self { rcl_time_: Mutex::new(rcl_time_point_t { nanoseconds: sum, clock_type: (*lock).clock_type, - }) + }), } } } @@ -54,21 +57,35 @@ impl Sub for Time { panic!("Can not subtract times with different time sources"); } else if diff < 0 { panic!("Time subtraction leads to negative time"); - } else { - Self{ - rcl_time_: Mutex::new(rcl_time_point_t { - nanoseconds: diff, - clock_type: lock.clock_type, - }) - } } + 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.rcl_time_.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, + }), + } } } -//impl Sub - impl Clone for Time { fn clone(&self) -> Self { let lock = self.rcl_time_.lock(); @@ -76,12 +93,11 @@ impl Clone for Time { rcl_time_: Mutex::new(rcl_time_point_t { nanoseconds: lock.nanoseconds, clock_type: lock.clock_type, - }) + }), } } } - #[allow(dead_code)] impl Time { /// Function to create a new instance of Time @@ -98,19 +114,19 @@ impl Time { /// `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 { + 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 { + 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 { + 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, @@ -118,4 +134,29 @@ impl Time { }), } } + + /// Function to return time in nanoseconds + fn nanoseconds(&self) -> rcl_time_point_value_t { + (*self.rcl_time_.lock()).nanoseconds + } + + /// Function to return time in seconds + fn seconds(&self) -> u64 { + time::Duration::from_nanos((*self.rcl_time_.lock()).nanoseconds.try_into().unwrap()) + .as_secs() + } + + /// Function to return clock type + fn get_clock_type(&self) -> rcl_clock_type_t { + self.rcl_time_.lock().clock_type + } + + fn max(&self) -> Self { + Self { + rcl_time_: Mutex::new(rcl_time_point_t { + nanoseconds: rcl_time_point_value_t::MAX, + clock_type: self.rcl_time_.lock().clock_type, + }), + } + } } From 607a2014009eb73d33ea0373c8d6d755fa3315ec Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 17:31:05 +0530 Subject: [PATCH 12/25] add clock.rs to src and lib.rs --- rclrs/src/clock.rs | 0 rclrs/src/lib.rs | 2 ++ 2 files changed, 2 insertions(+) create mode 100644 rclrs/src/clock.rs diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs new file mode 100644 index 000000000..e69de29bb diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index c4f714e0d..2d8976d8b 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -5,6 +5,7 @@ //! //! [1]: https://github.com/ros2-rust/ros2_rust/blob/main/README.md +mod clock; mod context; mod duration; mod error; @@ -16,6 +17,7 @@ mod wait; mod rcl_bindings; +pub use clock::*; pub use context::*; pub use duration::*; pub use error::*; From a0d288b7bbd7546441dac53e555b08cbce3e6a8c Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Wed, 15 Jun 2022 22:20:34 +0530 Subject: [PATCH 13/25] adjusted duration.rs to accomodate negative values as well --- rclrs/Cargo.toml | 2 ++ rclrs/src/duration.rs | 37 ++++++++++++++++++++++++------------- 2 files changed, 26 insertions(+), 13 deletions(-) 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/duration.rs b/rclrs/src/duration.rs index fd3a96d4e..5f544368f 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -1,6 +1,7 @@ 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}; @@ -10,13 +11,13 @@ use std::time; #[allow(missing_docs)] pub enum DurationFrom { /// Create Duration instance using seconds - Secs { s: u32 }, + Secs { s: i32 }, /// Create Duration instance using nanoseconds NanoSecs { ns: rcl_duration_value_t }, /// Create Duration instance using seconds and ns - SecsAndNanoSecs { s: u32, ns: rcl_duration_value_t }, + SecsAndNanoSecs { s: i32, ns: rcl_duration_value_t }, /// Create Duration instance using std::time::Duration Duration { duration: time::Duration }, @@ -65,17 +66,21 @@ impl Mul for Duration { fn mul(self, rhs: f32) -> Self { let prod = (self._duration_handle.nanoseconds as f32) * rhs; - if prod < 0f32 { - panic!("Can not scale Duration by a negative number"); - } - if (prod as u128) > (rcl_duration_value_t::MAX as u128) { + 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, }) @@ -108,8 +113,11 @@ impl Sub for Duration { fn sub(self, rhs: Self) -> Self { let diff = self._duration_handle.nanoseconds - rhs._duration_handle.nanoseconds; - if diff < 0 { - panic!("Subtraction results in negative duration"); + if (diff as i128) < (rcl_duration_value_t::MIN as i128) { + panic!( + "Subtraction leads to {} underflow", + std::any::type_name::() + ); } Self::new(DurationFrom::NanoSecs { @@ -142,7 +150,8 @@ impl Duration { match duration { DurationFrom::Secs { s } => Ok(Self { _duration_handle: rcl_duration_t { - nanoseconds: time::Duration::from_secs(s.into()).as_nanos() + nanoseconds: ((signum(s) as i64) + * (time::Duration::from_secs(abs(s) as u64).as_nanos()) as i64) as rcl_duration_value_t, }, }), @@ -151,8 +160,9 @@ impl Duration { }), DurationFrom::SecsAndNanoSecs { s, ns } => Ok(Self { _duration_handle: rcl_duration_t { - nanoseconds: (time::Duration::from_secs(s.into()).as_nanos() - as rcl_duration_value_t) + nanoseconds: ((signum(s) as i64) + * (time::Duration::from_secs(abs(s) as u64).as_nanos() as i64) + as rcl_duration_value_t) + ns, }, }), @@ -176,8 +186,9 @@ impl Duration { } /// Function to get the count of seconds in the Duration object - pub fn seconds(&self) -> u32 { - time::Duration::from_nanos(self._duration_handle.nanoseconds as u64).as_secs() as u32 + 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 From 42601244e1a99935cadd94b131cbf16f648f6610 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Thu, 16 Jun 2022 09:33:45 +0530 Subject: [PATCH 14/25] refactor time.rs --- rclrs/src/time.rs | 34 ++++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 94dab9cda..1bb97a4a7 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -2,7 +2,7 @@ use crate::error::RclrsError; use crate::rcl_bindings::*; //use crate::RclReturnCode; use crate::duration::Duration; -use parking_lot::Mutex; +use parking_lot::{Mutex, MutexGuard}; use std::ops::{Add, Sub}; use std::time; @@ -29,11 +29,11 @@ impl Add for Time { type Output = Self; fn add(self, rhs: Duration) -> Self { - let lock = self.rcl_time_.lock(); + 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 an overlflow for {}", + "Addition causes a {} overlflow", std::any::type_name::() ) } @@ -50,8 +50,8 @@ impl Sub for Time { type Output = Self; fn sub(self, rhs: Time) -> Self { - let lock = self.rcl_time_.lock(); - let rhs_lock = rhs.rcl_time_.lock(); + 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"); @@ -71,7 +71,7 @@ impl Sub for Time { type Output = Self; fn sub(self, rhs: Duration) -> Self { - let lock = self.rcl_time_.lock(); + 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"); @@ -88,7 +88,7 @@ impl Sub for Time { impl Clone for Time { fn clone(&self) -> Self { - let lock = self.rcl_time_.lock(); + let lock = self.get_lock(); Self { rcl_time_: Mutex::new(rcl_time_point_t { nanoseconds: lock.nanoseconds, @@ -136,26 +136,32 @@ impl Time { } /// Function to return time in nanoseconds - fn nanoseconds(&self) -> rcl_time_point_value_t { - (*self.rcl_time_.lock()).nanoseconds + pub fn nanoseconds(&self) -> rcl_time_point_value_t { + (*self.get_lock()).nanoseconds } /// Function to return time in seconds - fn seconds(&self) -> u64 { + 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_` + fn get_lock(&self) -> MutexGuard<'_, rcl_time_point_t> { + self.rcl_time_.lock() + } + /// Function to return clock type - fn get_clock_type(&self) -> rcl_clock_type_t { - self.rcl_time_.lock().clock_type + pub fn get_clock_type(&self) -> rcl_clock_type_t { + self.get_lock().clock_type } - fn max(&self) -> Self { + /// 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.rcl_time_.lock().clock_type, + clock_type: self.get_lock().clock_type, }), } } From bd356cba2a83e1089efd37532ef1e6610ecbe391 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Thu, 16 Jun 2022 13:16:54 +0530 Subject: [PATCH 15/25] add initial functionality to clock.rs --- rclrs/src/clock.rs | 100 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index e69de29bb..76a87ae02 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -0,0 +1,100 @@ +use crate::error::RclrsError; +use crate::rcl_bindings::*; +use crate::RclReturnCode; +use crate::duration; +use parking_lot::Mutex; +use std::os::raw::{c_void, c_int}; + +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: &'static dyn Fn() -> (), + post_callback: &'static dyn Fn() -> Mutex, + threshold: rcl_jump_threshold_t, +} + +impl JumpHandler { + fn new(pre_callback: &dyn Fn() -> (), + post_callback: &dyn Fn() -> Mutex, + threshold: rcl_jump_threshold_t) -> Self { + todo!("implement it"); + } +} + +struct Impl { + rcl_clock_: Mutex, + allocator_: Mutex, + //clock_mutex_: Mutex<_>, +} + +/// 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()) + }; + // 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_, + }) + } +} From 0fce763e98b148bc1b7f533d91a90542d22f816f Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Thu, 16 Jun 2022 13:22:32 +0530 Subject: [PATCH 16/25] add todos to finish after review --- rclrs/src/clock.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index 76a87ae02..e329232bc 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -97,4 +97,14 @@ impl Clock { impl_, }) } + + todo!("add function now"); + todo!("add function sleep_until"); + todo!("add function sleep_for"); + todo!("add function ros_time_is_active"); + todo!("add function get_clock_handle"); + todo!("add function get_clock_type"); + todo!("add function get_clock_mutex"); + todo!("add function on_time_jump"); + todo!("add function create_jump_callback"); } From 48a55431695bc268fdd30ebec671b5ab5cbc4e91 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Thu, 16 Jun 2022 16:31:33 +0530 Subject: [PATCH 17/25] made get_lock publich --- rclrs/src/clock.rs | 19 +++++++++++++++++-- rclrs/src/time.rs | 2 +- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index e329232bc..c178d390f 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -2,7 +2,8 @@ use crate::error::RclrsError; use crate::rcl_bindings::*; use crate::RclReturnCode; use crate::duration; -use parking_lot::Mutex; +use crate::time; +use parking_lot::{Mutex, MutexGuard}; use std::os::raw::{c_void, c_int}; impl std::default::Default for rcl_allocator_t { @@ -98,7 +99,21 @@ impl Clock { }) } - todo!("add function now"); + pub fn get_clock_type(&self) -> rcl_clock_type_t { + (*self.impl_.rcl_clock_.lock()).type_ + } + + pub fn now(&self) -> time::Time { + + let now = time::Time::new( + time::TimeFrom::NanoSecs{ns: 0u64}, + self.get_clock_type(), + ).unwrap(); + + let ret = rcl_clock_get_now(&mut *self.impl_.rcl_clock_.lock(), &mut now.get_lock().nanoseconds) + + } + todo!("add function sleep_until"); todo!("add function sleep_for"); todo!("add function ros_time_is_active"); diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index 1bb97a4a7..ea3eec243 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -147,7 +147,7 @@ impl Time { } /// Function to get a lock on `rcl_time_` - fn get_lock(&self) -> MutexGuard<'_, rcl_time_point_t> { + pub fn get_lock(&self) -> MutexGuard<'_, rcl_time_point_t> { self.rcl_time_.lock() } From 7853af4abf6a27e5025728f7a513ecab6adf4225 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Thu, 16 Jun 2022 16:40:50 +0530 Subject: [PATCH 18/25] added now to clock.rs --- rclrs/src/clock.rs | 65 +++++++++++++++++++++++++++++----------------- 1 file changed, 41 insertions(+), 24 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index c178d390f..e8cc8ce04 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -1,10 +1,10 @@ use crate::error::RclrsError; use crate::rcl_bindings::*; use crate::RclReturnCode; -use crate::duration; +//use crate::duration; use crate::time; -use parking_lot::{Mutex, MutexGuard}; -use std::os::raw::{c_void, c_int}; +use parking_lot::Mutex; +use std::os::raw::{c_int, c_void}; impl std::default::Default for rcl_allocator_t { fn default() -> Self { @@ -26,12 +26,12 @@ impl std::default::Default for rcl_clock_t { threshold: rcl_jump_threshold_t { on_clock_change: false, min_forward: rcl_duration_t { - nanoseconds: 1 as rcl_duration_value_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, }; @@ -53,9 +53,11 @@ struct JumpHandler { } impl JumpHandler { - fn new(pre_callback: &dyn Fn() -> (), - post_callback: &dyn Fn() -> Mutex, - threshold: rcl_jump_threshold_t) -> Self { + fn new( + pre_callback: &dyn Fn() -> (), + post_callback: &dyn Fn() -> Mutex, + threshold: rcl_jump_threshold_t, + ) -> Self { todo!("implement it"); } } @@ -77,14 +79,16 @@ impl Clock { 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()) + allocator_: Mutex::new(rcl_allocator_t::default()), }; // 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) + 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 { @@ -94,26 +98,39 @@ impl Clock { }); } - Ok(Self{ - impl_, - }) + 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_ } - pub fn now(&self) -> time::Time { - - let now = time::Time::new( - time::TimeFrom::NanoSecs{ns: 0u64}, - self.get_clock_type(), - ).unwrap(); + /// 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, + ) + }; - let ret = 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) } - +} +/* todo!("add function sleep_until"); todo!("add function sleep_for"); todo!("add function ros_time_is_active"); @@ -122,4 +139,4 @@ impl Clock { todo!("add function get_clock_mutex"); todo!("add function on_time_jump"); todo!("add function create_jump_callback"); -} +*/ From c4327469cb78df7fab0204ecfee9615c94174f03 Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Fri, 17 Jun 2022 15:54:49 +0530 Subject: [PATCH 19/25] refined duration.rs --- rclrs/src/duration.rs | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs index 5f544368f..70185a538 100644 --- a/rclrs/src/duration.rs +++ b/rclrs/src/duration.rs @@ -90,15 +90,14 @@ impl Mul for Duration { impl Ord for Duration { fn cmp(&self, rhs: &Self) -> Ordering { - self._duration_handle - .nanoseconds - .cmp(&rhs._duration_handle.nanoseconds) + self.nanoseconds() + .cmp(&rhs.nanoseconds()) } } impl PartialEq for Duration { fn eq(&self, rhs: &Self) -> bool { - self._duration_handle.nanoseconds == rhs._duration_handle.nanoseconds + self.nanoseconds() == rhs.nanoseconds() } } From 3a9d90c20f4351d9732ae9fbf28ec247aa66d12e Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Fri, 17 Jun 2022 15:55:17 +0530 Subject: [PATCH 20/25] implement ord and eq for Tie --- rclrs/src/time.rs | 44 +++++++++++++++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 11 deletions(-) diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs index ea3eec243..0515f5708 100644 --- a/rclrs/src/time.rs +++ b/rclrs/src/time.rs @@ -3,6 +3,7 @@ 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; @@ -46,6 +47,38 @@ impl Add for Time { } } +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; @@ -86,17 +119,6 @@ impl Sub for Time { } } -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, - }), - } - } -} #[allow(dead_code)] impl Time { From e5c194d56777d59bf4752b56e193ff3e34e7d6fb Mon Sep 17 00:00:00 2001 From: Deepesh Padala Date: Fri, 17 Jun 2022 16:18:05 +0530 Subject: [PATCH 21/25] added some more functions --- rclrs/src/clock.rs | 76 +++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 69 insertions(+), 7 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index e8cc8ce04..6301cabde 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -1,10 +1,12 @@ +use crate::context; use crate::error::RclrsError; use crate::rcl_bindings::*; use crate::RclReturnCode; -//use crate::duration; +use crate::duration; use crate::time; -use parking_lot::Mutex; +use parking_lot::{Mutex, Condvar}; use std::os::raw::{c_int, c_void}; +use std::sync::Arc; impl std::default::Default for rcl_allocator_t { fn default() -> Self { @@ -65,7 +67,7 @@ impl JumpHandler { struct Impl { rcl_clock_: Mutex, allocator_: Mutex, - //clock_mutex_: Mutex<_>, + thread_handler_: Arc<(Mutex, Condvar)>, } /// The Clock struct @@ -80,6 +82,7 @@ impl Clock { 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 @@ -129,13 +132,72 @@ impl Clock { 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_)); + // Safety: No preconditions for this function + while (self.now().unwrap() < until) && unsafe { rcl_context_is_valid(&mut *(*context.rcl_context_mtx).lock()) } { + cvar.wait(&mut lock.lock()); + } + }, + rcl_clock_type_t::RCL_STEADY_TIME => { + todo!("implement it for RCL_STEADY_TIME"); + }, + } + } + 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 sleep_for"); - todo!("add function ros_time_is_active"); - todo!("add function get_clock_handle"); - todo!("add function get_clock_type"); todo!("add function get_clock_mutex"); todo!("add function on_time_jump"); todo!("add function create_jump_callback"); From 4dc0d006e82979faad22459eed01d186ac9929b8 Mon Sep 17 00:00:00 2001 From: Deepesh Padala <33351122+DS3a@users.noreply.github.com> Date: Sun, 26 Jun 2022 08:44:04 +0530 Subject: [PATCH 22/25] adding ContextBuildr --- rclrs/src/builder.rs | 3 +++ rclrs/src/context.rs | 3 +++ rclrs/src/context/builder.rs | 3 +++ 3 files changed, 9 insertions(+) create mode 100644 rclrs/src/builder.rs create mode 100644 rclrs/src/context/builder.rs diff --git a/rclrs/src/builder.rs b/rclrs/src/builder.rs new file mode 100644 index 000000000..6d0a8fa60 --- /dev/null +++ b/rclrs/src/builder.rs @@ -0,0 +1,3 @@ +pub struct ContextBuilder { + +} diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index d9fe67f86..249568369 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -1,6 +1,9 @@ +mod builder; + use crate::rcl_bindings::*; use crate::{Node, NodeBuilder, RclrsError, ToResult}; +use self::builder::*; use std::ffi::CString; use std::os::raw::c_char; use std::string::String; diff --git a/rclrs/src/context/builder.rs b/rclrs/src/context/builder.rs new file mode 100644 index 000000000..a517d2a9c --- /dev/null +++ b/rclrs/src/context/builder.rs @@ -0,0 +1,3 @@ +pub struct NodeBuilder { + +} From 7adb4f65cc4f726ba25105a6d46cb171c7a1c54c Mon Sep 17 00:00:00 2001 From: Deepesh Padala <33351122+DS3a@users.noreply.github.com> Date: Sun, 10 Jul 2022 13:57:17 +0530 Subject: [PATCH 23/25] implemented new in context/builder.rs --- rclrs/src/context/builder.rs | 37 +++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/rclrs/src/context/builder.rs b/rclrs/src/context/builder.rs index a517d2a9c..1e16d7383 100644 --- a/rclrs/src/context/builder.rs +++ b/rclrs/src/context/builder.rs @@ -1,3 +1,38 @@ -pub struct NodeBuilder { +use crate::rcl_bindings::*; +use crate::{Context, RclrsError}; +use std::ffi::CString; +use std::os::raw::c_char; +use parking_lot::Mutex; +pub struct ContextBuilder { + cstring_args: Vec, + c_args: Option>, + context_mtx: Mutex, + allocator: rcl_allocator_t, + init_options_mtx: Mutex, +} + +impl ContextBuilder { + + /// Build a new ContextBuilder instance + pub fn new(args: impl IntoIterator) -> Result { + unsafe { + Ok(ContextBuilder { + cstring_args: args.into_iter().map(|arg| { + CString::new(arg.as_str()).map_err(|err| RclrsError::StringContainsNul{ + err, + s: arg.clone(), + }) + }).collect::>()?, + c_args: None, // to be built in the build function + context_mtx: Mutex::new(rcl_get_zero_initialized_context()), + allocator: rcutils_get_default_allocator(), + init_options_mtx: Mutex::new(rcl_get_zero_initialized_init_options()) + }) + } + } + + pub fn build(&self) -> Result { + todo!("call build here"); + } } From 3f0360ff0926d81828ed6244ac85f235a61d0fd9 Mon Sep 17 00:00:00 2001 From: Deepesh Padala <33351122+DS3a@users.noreply.github.com> Date: Sun, 10 Jul 2022 21:21:49 +0530 Subject: [PATCH 24/25] add context builder (#206) --- rclrs/src/builder.rs | 3 - rclrs/src/clock.rs | 204 ---------------------------------- rclrs/src/context.rs | 50 +-------- rclrs/src/context/builder.rs | 67 +++++++++--- rclrs/src/duration.rs | 206 ----------------------------------- rclrs/src/lib.rs | 6 - rclrs/src/time.rs | 190 -------------------------------- 7 files changed, 53 insertions(+), 673 deletions(-) delete mode 100644 rclrs/src/builder.rs delete mode 100644 rclrs/src/clock.rs delete mode 100644 rclrs/src/duration.rs delete mode 100644 rclrs/src/time.rs diff --git a/rclrs/src/builder.rs b/rclrs/src/builder.rs deleted file mode 100644 index 6d0a8fa60..000000000 --- a/rclrs/src/builder.rs +++ /dev/null @@ -1,3 +0,0 @@ -pub struct ContextBuilder { - -} diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs deleted file mode 100644 index 6301cabde..000000000 --- a/rclrs/src/clock.rs +++ /dev/null @@ -1,204 +0,0 @@ -use crate::context; -use crate::error::RclrsError; -use crate::rcl_bindings::*; -use crate::RclReturnCode; -use crate::duration; -use crate::time; -use parking_lot::{Mutex, Condvar}; -use std::os::raw::{c_int, c_void}; -use std::sync::Arc; - -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: &'static dyn Fn() -> (), - post_callback: &'static dyn Fn() -> Mutex, - threshold: rcl_jump_threshold_t, -} - -impl JumpHandler { - fn new( - pre_callback: &dyn Fn() -> (), - post_callback: &dyn Fn() -> Mutex, - threshold: rcl_jump_threshold_t, - ) -> Self { - todo!("implement it"); - } -} - -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_)); - // Safety: No preconditions for this function - while (self.now().unwrap() < until) && unsafe { rcl_context_is_valid(&mut *(*context.rcl_context_mtx).lock()) } { - cvar.wait(&mut lock.lock()); - } - }, - rcl_clock_type_t::RCL_STEADY_TIME => { - todo!("implement it for RCL_STEADY_TIME"); - }, - } - } - 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/context.rs b/rclrs/src/context.rs index 378e659c8..2e6c9a103 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -1,14 +1,11 @@ mod builder; use crate::rcl_bindings::*; -use crate::{RclrsError, ToResult}; +use crate::RclrsError; use self::builder::*; -use std::ffi::CString; -use std::os::raw::c_char; use std::string::String; use std::sync::Arc; -use std::vec::Vec; use parking_lot::Mutex; @@ -63,49 +60,8 @@ impl Context { /// assert!(Context::new(invalid_remapping).is_err()); /// ``` pub fn new(args: impl IntoIterator) -> Result { - // SAFETY: Getting a zero-initialized value is always safe - let mut rcl_context = unsafe { rcl_get_zero_initialized_context() }; - let cstring_args: Vec = args - .into_iter() - .map(|arg| { - CString::new(arg.as_str()).map_err(|err| RclrsError::StringContainsNul { - err, - s: arg.clone(), - }) - }) - .collect::>()?; - // Vector of pointers into cstring_args - let c_args: Vec<*const c_char> = cstring_args.iter().map(|arg| arg.as_ptr()).collect(); - unsafe { - // SAFETY: No preconditions for this function. - let allocator = rcutils_get_default_allocator(); - // SAFETY: Getting a zero-initialized value is always safe. - let mut rcl_init_options = rcl_get_zero_initialized_init_options(); - // SAFETY: Passing in a zero-initialized value is expected. - // In the case where this returns not ok, there's nothing to clean up. - rcl_init_options_init(&mut rcl_init_options, allocator).ok()?; - // SAFETY: This function does not store the ephemeral init_options and c_args - // pointers. Passing in a zero-initialized rcl_context is expected. - let ret = rcl_init( - c_args.len() as i32, - if c_args.is_empty() { - std::ptr::null() - } else { - c_args.as_ptr() - }, - &rcl_init_options, - &mut rcl_context, - ) - .ok(); - // SAFETY: It's safe to pass in an initialized object. - // Early return will not leak memory, because this is the last fini function. - rcl_init_options_fini(&mut rcl_init_options).ok()?; - // Move the check after the last fini() - ret?; - } - Ok(Self { - rcl_context_mtx: Arc::new(Mutex::new(rcl_context)), - }) + let builder = ContextBuilder::new(args)?; + builder.build() } /// Checks if the context is still valid. diff --git a/rclrs/src/context/builder.rs b/rclrs/src/context/builder.rs index 1e16d7383..2f7e80b01 100644 --- a/rclrs/src/context/builder.rs +++ b/rclrs/src/context/builder.rs @@ -1,38 +1,71 @@ +use crate::error::ToResult; use crate::rcl_bindings::*; use crate::{Context, RclrsError}; use std::ffi::CString; -use std::os::raw::c_char; +use std::sync::Arc; + use parking_lot::Mutex; +use std::os::raw::c_char; pub struct ContextBuilder { cstring_args: Vec, - c_args: Option>, - context_mtx: Mutex, - allocator: rcl_allocator_t, init_options_mtx: Mutex, } impl ContextBuilder { - /// Build a new ContextBuilder instance pub fn new(args: impl IntoIterator) -> Result { - unsafe { - Ok(ContextBuilder { - cstring_args: args.into_iter().map(|arg| { - CString::new(arg.as_str()).map_err(|err| RclrsError::StringContainsNul{ + Ok(ContextBuilder { + cstring_args: args + .into_iter() + .map(|arg| { + CString::new(arg.as_str()).map_err(|err| RclrsError::StringContainsNul { err, s: arg.clone(), }) - }).collect::>()?, - c_args: None, // to be built in the build function - context_mtx: Mutex::new(rcl_get_zero_initialized_context()), - allocator: rcutils_get_default_allocator(), - init_options_mtx: Mutex::new(rcl_get_zero_initialized_init_options()) - }) - } + }) + .collect::>()?, + // SAFETY: Getting a zero-initialized value is always safe. + init_options_mtx: unsafe { Mutex::new(rcl_get_zero_initialized_init_options()) }, + }) } + /// Function to build the Context instance pub fn build(&self) -> Result { - todo!("call build here"); + let mut rcl_init_options = self.init_options_mtx.lock(); + + let c_args: Vec<*const c_char> = self.cstring_args.iter().map(|arg| arg.as_ptr()).collect(); + unsafe { + // SAFETY: Getting a zero-initialized value is always safe + let mut rcl_context: rcl_context_t = rcl_get_zero_initialized_context(); + // SAFETY: No preconditions for this function. + let allocator: rcutils_allocator_t = rcutils_get_default_allocator(); + + // SAFETY: Passing in a zero-initialized value is expected. + // In the case where this returns not ok, there's nothing to clean up. + rcl_init_options_init(&mut *rcl_init_options, allocator).ok()?; + // SAFETY: This function does not store the ephemeral init_options and c_args + // pointers. Passing in a zero-initialized rcl_context is expected. + + let ret = rcl_init( + c_args.len() as i32, + if c_args.is_empty() { + std::ptr::null() + } else { + c_args.as_ptr() + }, + &*rcl_init_options, + &mut rcl_context, + ) + .ok(); + // SAFETY: It's safe to pass in an initialized object. + // Early return will not leak memory, because this is the last fini function. + rcl_init_options_fini(&mut *rcl_init_options).ok()?; + // Move the check after the last fini() + ret?; + Ok(Context { + rcl_context_mtx: Arc::new(Mutex::new(rcl_context)), + }) + } } } diff --git a/rclrs/src/duration.rs b/rclrs/src/duration.rs deleted file mode 100644 index 70185a538..000000000 --- a/rclrs/src/duration.rs +++ /dev/null @@ -1,206 +0,0 @@ -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 2dc58ea71..af0429247 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -5,26 +5,20 @@ //! //! [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 deleted file mode 100644 index 0515f5708..000000000 --- a/rclrs/src/time.rs +++ /dev/null @@ -1,190 +0,0 @@ -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, - }), - } - } -} From 2f69b60ac62f05ee204f47f27de6118e27045555 Mon Sep 17 00:00:00 2001 From: Deepesh Padala <33351122+DS3a@users.noreply.github.com> Date: Sun, 10 Jul 2022 21:24:57 +0530 Subject: [PATCH 25/25] remove unwanted dependencies --- rclrs/Cargo.toml | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index d4a750036..93b277b5e 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -13,8 +13,6 @@ 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