From 6f9543c48ecef58404f3c1b74dcf630115fd8ac4 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Thu, 28 Nov 2024 19:52:57 +0100 Subject: [PATCH 01/26] Basic Timer type compiling :). Signed-off-by: Agustin Alba Chicar --- rclrs/src/clock.rs | 2 +- rclrs/src/lib.rs | 2 + rclrs/src/rcl_bindings.rs | 4 ++ rclrs/src/timer.rs | 92 +++++++++++++++++++++++++++++++++++++++ 4 files changed, 99 insertions(+), 1 deletion(-) create mode 100644 rclrs/src/timer.rs diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index f7c085e14..2e8612742 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -28,7 +28,7 @@ impl From for rcl_clock_type_t { #[derive(Clone, Debug)] pub struct Clock { kind: ClockType, - rcl_clock: Arc>, + pub rcl_clock: Arc>, // TODO(luca) Implement jump callbacks } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 3a22c6da8..0b8f4c7f1 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -19,6 +19,7 @@ mod qos; mod service; mod subscription; mod time; +mod timer; mod time_source; mod vendor; mod wait; @@ -48,6 +49,7 @@ pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; pub use time::*; +pub use timer::*; use time_source::*; pub use wait::*; diff --git a/rclrs/src/rcl_bindings.rs b/rclrs/src/rcl_bindings.rs index 90f434009..dbfb5d5b0 100644 --- a/rclrs/src/rcl_bindings.rs +++ b/rclrs/src/rcl_bindings.rs @@ -89,6 +89,10 @@ cfg_if::cfg_if! { #[derive(Debug)] pub struct rcl_wait_set_t; + #[repr(C)] + #[derive(Debug)] + pub struct rcl_timer_t; + #[repr(C)] #[derive(Debug)] pub struct rcutils_string_array_t; diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs new file mode 100644 index 000000000..1d5877cf5 --- /dev/null +++ b/rclrs/src/timer.rs @@ -0,0 +1,92 @@ +use crate::{ + clock::Clock, context::Context, error::RclrsError, rcl_bindings::*, to_rclrs_result +}; +use std::sync::{Arc, Mutex}; + + +#[derive(Debug)] +pub struct Timer { + rcl_timer: Arc>, +} + +unsafe extern "C" fn timer_callback(_: *mut rcl_timer_t, time_since_last_callback_ns: i64) { + println!("timer_callback, time_since_last_callback_ns {0}", time_since_last_callback_ns); +} + +impl Timer { + pub fn new(clock: &Clock, context: &Context, period: i64) -> Result { + let mut rcl_timer; + let timer_init_result; + unsafe { + // SAFETY: Getting a default value is always safe. + rcl_timer = rcl_get_zero_initialized_timer(); + let allocator = rcutils_get_default_allocator(); + let mut rcl_clock = clock.rcl_clock.lock().unwrap(); + let mut rcl_context = context.handle.rcl_context.lock().unwrap(); + let callback: rcl_timer_callback_t = Some(timer_callback); + // Function will return Err(_) only if there isn't enough memory to allocate a clock + // object. + timer_init_result = rcl_timer_init( + &mut rcl_timer, + &mut *rcl_clock, + &mut *rcl_context, + period, + callback, + allocator, + ); + } + to_rclrs_result(timer_init_result).map(|_| { + Timer { + rcl_timer: Arc::new(Mutex::new(rcl_timer)) + } + }) + } + + // handle() -> RCLC Timer Type + + // destroy() -> None + + // clock() -> Clock ? + + // timer_period_ns -> i64 ? + + // is_ready() -> bool + + // is_cancelled() -> bool + + // cancel() -> None + + // reset() -> None + + // time_since_last_call() -> i64 + + // time_until_next_call() -> Option + +} + +impl Drop for rcl_timer_t { + fn drop(&mut self) { + // SAFETY: No preconditions for this function + let rc = unsafe { rcl_timer_fini(&mut *self) }; + if let Err(e) = to_rclrs_result(rc) { + panic!("Unable to release Timer. {:?}", e) + } + } +} + +// SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread +// they are running in. Therefore, this type can be safely sent to another thread. +unsafe impl Send for rcl_timer_t {} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn traits() { + use crate::test_helpers::*; + + assert_send::(); + assert_sync::(); + } +} From 7b42c023d1fc46da0322eb60d84f87069adcb4b8 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 10:19:45 +0100 Subject: [PATCH 02/26] Evaluates the Timer::new() againts different clock types. Signed-off-by: Agustin Alba Chicar --- rclrs/src/clock.rs | 1 + rclrs/src/timer.rs | 62 ++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 58 insertions(+), 5 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index 2e8612742..30558ea1a 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -28,6 +28,7 @@ impl From for rcl_clock_type_t { #[derive(Clone, Debug)] pub struct Clock { kind: ClockType, + // TODO(ekumen): Fix the extra pub here. pub rcl_clock: Arc>, // TODO(luca) Implement jump callbacks } diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 1d5877cf5..5bfc49aad 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -16,8 +16,7 @@ unsafe extern "C" fn timer_callback(_: *mut rcl_timer_t, time_since_last_callbac impl Timer { pub fn new(clock: &Clock, context: &Context, period: i64) -> Result { let mut rcl_timer; - let timer_init_result; - unsafe { + let timer_init_result = unsafe { // SAFETY: Getting a default value is always safe. rcl_timer = rcl_get_zero_initialized_timer(); let allocator = rcutils_get_default_allocator(); @@ -26,15 +25,15 @@ impl Timer { let callback: rcl_timer_callback_t = Some(timer_callback); // Function will return Err(_) only if there isn't enough memory to allocate a clock // object. - timer_init_result = rcl_timer_init( + rcl_timer_init( &mut rcl_timer, &mut *rcl_clock, &mut *rcl_context, period, callback, allocator, - ); - } + ) + }; to_rclrs_result(timer_init_result).map(|_| { Timer { rcl_timer: Arc::new(Mutex::new(rcl_timer)) @@ -42,6 +41,20 @@ impl Timer { }) } + pub fn time_since_last_call(&self) -> Result { + let mut time_value_ns: i64 = 0; + let time_since_last_call_result = unsafe { + let rcl_timer = self.rcl_timer.lock().unwrap(); + rcl_timer_get_time_since_last_call( + &* rcl_timer, + &mut time_value_ns + ) + }; + to_rclrs_result(time_since_last_call_result).map(|_| { + time_value_ns + }) + } + // handle() -> RCLC Timer Type // destroy() -> None @@ -89,4 +102,43 @@ mod tests { assert_send::(); assert_sync::(); } + + #[test] + fn test_new_with_system_clock() { + let clock = Clock::system(); + let context = Context::new(vec![]).unwrap(); + let period: i64 = 1000000000; // 1000 milliseconds. + + let dut = Timer::new(&clock, &context, period); + assert!(dut.is_ok()); + } + + #[test] + fn test_new_with_steady_clock() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period: i64 = 1000000000; // 1000 milliseconds. + + let dut = Timer::new(&clock, &context, period); + assert!(dut.is_ok()); + } + + #[ignore = "SIGSEGV when creating the timer with Clock::with_source()."] + #[test] + fn test_new_with_source_clock() { + let (clock, source) = Clock::with_source(); + // No manual time set, it should default to 0 + assert!(clock.now().nsec == 0); + let set_time = 1234i64; + source.set_ros_time_override(set_time); + // Ros time is set, should return the value that was set + assert_eq!(clock.now().nsec, set_time); + + + let context = Context::new(vec![]).unwrap(); + let period: i64 = 1000000000; // 1000 milliseconds. + + let dut = Timer::new(&clock, &context, period); + assert!(dut.is_ok()); + } } From a60d9f327ffbba6bbd6ad26c54bc716e4c705982 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 10:58:18 +0100 Subject: [PATCH 03/26] Implement time_until_next_call Signed-off-by: Agustin Alba Chicar --- rclrs/src/timer.rs | 55 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 51 insertions(+), 4 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 5bfc49aad..a8966b0b1 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -55,6 +55,20 @@ impl Timer { }) } + pub fn time_until_next_call(&self) -> Result { + let mut time_value_ns: i64 = 0; + let time_until_next_call_result = unsafe { + let rcl_timer = self.rcl_timer.lock().unwrap(); + rcl_timer_get_time_until_next_call( + &* rcl_timer, + &mut time_value_ns + ) + }; + to_rclrs_result(time_until_next_call_result).map(|_| { + time_value_ns + }) + } + // handle() -> RCLC Timer Type // destroy() -> None @@ -94,6 +108,7 @@ unsafe impl Send for rcl_timer_t {} #[cfg(test)] mod tests { use super::*; + use std::{thread, time}; #[test] fn traits() { @@ -107,7 +122,7 @@ mod tests { fn test_new_with_system_clock() { let clock = Clock::system(); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1000000000; // 1000 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -117,7 +132,7 @@ mod tests { fn test_new_with_steady_clock() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1000000000; // 1000 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -134,11 +149,43 @@ mod tests { // Ros time is set, should return the value that was set assert_eq!(clock.now().nsec, set_time); - let context = Context::new(vec![]).unwrap(); - let period: i64 = 1000000000; // 1000 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds.. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); } + + #[test] + fn test_time_since_last_call_before_first_event() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 2e6 as i64; // 2 milliseconds. + let sleep_period_ms = time::Duration::from_millis(1); + + let dut = Timer::new(&clock, &context, period_ns); + assert!(dut.is_ok()); + let dut = dut.unwrap(); + thread::sleep(sleep_period_ms); + let time_since_last_call = dut.time_since_last_call(); + assert!(time_since_last_call.is_ok()); + let time_since_last_call = time_since_last_call.unwrap(); + assert!(time_since_last_call > 9e5 as i64, "time_since_last_call: {}", time_since_last_call); + } + + #[test] + fn test_time_until_next_call_before_first_event() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 2e6 as i64; // 2 milliseconds. + let sleep_period_ms = time::Duration::from_millis(1); + + let dut = Timer::new(&clock, &context, period_ns); + assert!(dut.is_ok()); + let dut = dut.unwrap(); + let time_until_next_call = dut.time_until_next_call(); + assert!(time_until_next_call.is_ok()); + let time_until_next_call = time_until_next_call.unwrap(); + assert!(time_until_next_call < period_ns, "time_until_next_call: {}", time_until_next_call); + } } From d233b473a8bcf03260bbe277670a016f75c03480 Mon Sep 17 00:00:00 2001 From: JesusSilvaUtrera Date: Fri, 29 Nov 2024 11:49:59 +0100 Subject: [PATCH 04/26] Added cancel behavior --- rclrs/src/timer.rs | 47 ++++++++++++++++++++++++++++++++++++---------- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index a8966b0b1..f779e432b 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -41,6 +41,26 @@ impl Timer { }) } + pub fn cancel(&self) -> Result<(), RclrsError> { + let mut rcl_timer = self.rcl_timer.lock().unwrap(); + let cancel_result = unsafe { rcl_timer_cancel(&mut *rcl_timer) }; + to_rclrs_result(cancel_result) + } + + pub fn is_canceled(&self) -> Result { + let mut is_canceled = false; + let is_canceled_result = unsafe { + let rcl_timer = self.rcl_timer.lock().unwrap(); + rcl_timer_is_canceled( + &* rcl_timer, + &mut is_canceled + ) + }; + to_rclrs_result(is_canceled_result).map(|_| { + is_canceled + }) + } + pub fn time_since_last_call(&self) -> Result { let mut time_value_ns: i64 = 0; let time_since_last_call_result = unsafe { @@ -71,24 +91,14 @@ impl Timer { // handle() -> RCLC Timer Type - // destroy() -> None - // clock() -> Clock ? // timer_period_ns -> i64 ? // is_ready() -> bool - // is_cancelled() -> bool - - // cancel() -> None - // reset() -> None - // time_since_last_call() -> i64 - - // time_until_next_call() -> Option - } impl Drop for rcl_timer_t { @@ -156,6 +166,23 @@ mod tests { assert!(dut.is_ok()); } + #[test] + fn test_cancel() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period: i64 = 1e6 as i64; // 1 milliseconds. + + let dut = Timer::new(&clock, &context, period); + assert!(dut.is_ok()); + let dut = dut.unwrap(); + assert!(dut.is_canceled().is_ok()); + assert!(!dut.is_canceled().unwrap()); + let cancel_result = dut.cancel(); + assert!(cancel_result.is_ok()); + assert!(dut.is_canceled().is_ok()); + assert!(dut.is_canceled().unwrap()); + } + #[test] fn test_time_since_last_call_before_first_event() { let clock = Clock::steady(); From 189606f5367ee8a5e7e6dde36a3f86aeaae30e39 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 11:28:52 +0100 Subject: [PATCH 05/26] Implement rcl_timer_reset --- rclrs/src/timer.rs | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index f779e432b..effc46d08 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -89,6 +89,12 @@ impl Timer { }) } + pub fn reset(&mut self) -> Result<(), RclrsError> + { + let mut rcl_timer = self.rcl_timer.lock().unwrap(); + to_rclrs_result(unsafe {rcl_timer_reset(&mut *rcl_timer)}) + } + // handle() -> RCLC Timer Type // clock() -> Clock ? @@ -205,8 +211,6 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 2e6 as i64; // 2 milliseconds. - let sleep_period_ms = time::Duration::from_millis(1); - let dut = Timer::new(&clock, &context, period_ns); assert!(dut.is_ok()); let dut = dut.unwrap(); @@ -215,4 +219,20 @@ mod tests { let time_until_next_call = time_until_next_call.unwrap(); assert!(time_until_next_call < period_ns, "time_until_next_call: {}", time_until_next_call); } + + #[test] + fn test_reset() { + let tolerance = 1e4 as i64; + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 2e6 as i64; // 2 milliseconds. + let mut dut = Timer::new(&clock, &context, period_ns).unwrap(); + let elapsed = period_ns - dut.time_until_next_call().unwrap(); + assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); + thread::sleep(time::Duration::from_millis(1)); + assert!(dut.reset().is_ok()); + let elapsed = period_ns - dut.time_until_next_call().unwrap(); + assert!(elapsed < tolerance , "elapsed after reset: {}", elapsed); + } + } From 59ed7e21c9ee9a59e03357917be1350ccdfc3d88 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 11:58:59 +0100 Subject: [PATCH 06/26] Adds Timer::call(). Signed-off-by: Agustin Alba Chicar --- rclrs/src/timer.rs | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index effc46d08..fb306da41 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -95,6 +95,12 @@ impl Timer { to_rclrs_result(unsafe {rcl_timer_reset(&mut *rcl_timer)}) } + pub fn call(&mut self) -> Result<(), RclrsError> + { + let mut rcl_timer = self.rcl_timer.lock().unwrap(); + to_rclrs_result(unsafe {rcl_timer_call(&mut *rcl_timer)}) + } + // handle() -> RCLC Timer Type // clock() -> Clock ? @@ -222,7 +228,7 @@ mod tests { #[test] fn test_reset() { - let tolerance = 1e4 as i64; + let tolerance = 20e4 as i64; let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 2e6 as i64; // 2 milliseconds. @@ -235,4 +241,24 @@ mod tests { assert!(elapsed < tolerance , "elapsed after reset: {}", elapsed); } + #[test] + fn test_call() { + let tolerance = 20e4 as i64; + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let mut dut = Timer::new(&clock, &context, period_ns).unwrap(); + let elapsed = period_ns - dut.time_until_next_call().unwrap(); + assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); + + thread::sleep(time::Duration::from_micros(1500)); + + let elapsed = period_ns - dut.time_until_next_call().unwrap(); + assert!(elapsed > 1500000i64, "time_until_next_call before call: {}", elapsed); + + assert!(dut.call().is_ok()); + + let elapsed = dut.time_until_next_call().unwrap(); + assert!(elapsed < 500000i64, "time_until_next_call after call: {}", elapsed); + } } From 9af9dd95f4d6ecfc92ad3970e6088f547bf00cc7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jes=C3=BAs=20Silva?= <79662866+JesusSilvaUtrera@users.noreply.github.com> Date: Fri, 29 Nov 2024 12:15:13 +0100 Subject: [PATCH 07/26] Added timer_period_ns (#2) --- rclrs/src/timer.rs | 31 +++++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index fb306da41..0f40bbd63 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -41,6 +41,20 @@ impl Timer { }) } + pub fn timer_period_ns(&self) -> Result { + let mut timer_period_ns = 0; + let get_period_result = unsafe { + let rcl_timer = self.rcl_timer.lock().unwrap(); + rcl_timer_get_period( + &* rcl_timer, + &mut timer_period_ns + ) + }; + to_rclrs_result(get_period_result).map(|_| { + timer_period_ns + }) + } + pub fn cancel(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); let cancel_result = unsafe { rcl_timer_cancel(&mut *rcl_timer) }; @@ -105,8 +119,6 @@ impl Timer { // clock() -> Clock ? - // timer_period_ns -> i64 ? - // is_ready() -> bool // reset() -> None @@ -178,6 +190,21 @@ mod tests { assert!(dut.is_ok()); } + #[test] + fn test_get_period() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period: i64 = 1e6 as i64; // 1 milliseconds. + + let dut = Timer::new(&clock, &context, period); + assert!(dut.is_ok()); + let dut = dut.unwrap(); + let period_result = dut.timer_period_ns(); + assert!(period_result.is_ok()); + let period_result = period_result.unwrap(); + assert_eq!(period_result, 1e6 as i64); + } + #[test] fn test_cancel() { let clock = Clock::steady(); From e46224f767589937da1d321cf9ccd090edd2579d Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 12:16:01 +0100 Subject: [PATCH 08/26] Adds Timer::is_ready(). Signed-off-by: Agustin Alba Chicar --- rclrs/src/timer.rs | 38 +++++++++++++++++++++++++++++++++----- 1 file changed, 33 insertions(+), 5 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 0f40bbd63..5e0e72eba 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -115,14 +115,24 @@ impl Timer { to_rclrs_result(unsafe {rcl_timer_call(&mut *rcl_timer)}) } + pub fn is_ready(&self) -> Result + { + let (is_ready, is_ready_result) = unsafe { + let mut is_ready: bool = false; + let rcl_timer = self.rcl_timer.lock().unwrap(); + let is_ready_result = rcl_timer_is_ready( + &* rcl_timer, + &mut is_ready + ); + (is_ready, is_ready_result) + }; + to_rclrs_result(is_ready_result).map(|_| { + is_ready + }) + } // handle() -> RCLC Timer Type // clock() -> Clock ? - - // is_ready() -> bool - - // reset() -> None - } impl Drop for rcl_timer_t { @@ -288,4 +298,22 @@ mod tests { let elapsed = dut.time_until_next_call().unwrap(); assert!(elapsed < 500000i64, "time_until_next_call after call: {}", elapsed); } + + #[test] + fn test_is_ready() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let dut = Timer::new(&clock, &context, period_ns).unwrap(); + + let is_ready = dut.is_ready(); + assert!(is_ready.is_ok()); + assert!(!is_ready.unwrap()); + + thread::sleep(time::Duration::from_micros(1100)); + + let is_ready = dut.is_ready(); + assert!(is_ready.is_ok()); + assert!(is_ready.unwrap()); + } } From 501439db8f43a36cd8df30a890cd6d0c87df71dd Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 12:57:56 +0100 Subject: [PATCH 09/26] WIP Timer callback implementation. Signed-off-by: Agustin Alba Chicar --- rclrs/src/timer.rs | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 5e0e72eba..b9dd146bd 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -4,13 +4,15 @@ use crate::{ use std::sync::{Arc, Mutex}; + +pub trait TimerCallback { + fn call(time_since_last_callback_ns: i64); +} + #[derive(Debug)] pub struct Timer { rcl_timer: Arc>, -} - -unsafe extern "C" fn timer_callback(_: *mut rcl_timer_t, time_since_last_callback_ns: i64) { - println!("timer_callback, time_since_last_callback_ns {0}", time_since_last_callback_ns); + // callback: Option, } impl Timer { @@ -22,7 +24,7 @@ impl Timer { let allocator = rcutils_get_default_allocator(); let mut rcl_clock = clock.rcl_clock.lock().unwrap(); let mut rcl_context = context.handle.rcl_context.lock().unwrap(); - let callback: rcl_timer_callback_t = Some(timer_callback); + let callback: rcl_timer_callback_t = None; // Function will return Err(_) only if there isn't enough memory to allocate a clock // object. rcl_timer_init( @@ -36,7 +38,8 @@ impl Timer { }; to_rclrs_result(timer_init_result).map(|_| { Timer { - rcl_timer: Arc::new(Mutex::new(rcl_timer)) + rcl_timer: Arc::new(Mutex::new(rcl_timer)), + // callback: None } }) } From 132c9dbba6009895d6139fdb4316ba8562c37230 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 15:25:20 +0100 Subject: [PATCH 10/26] Preliminary callback. Signed-off-by: Agustin Alba Chicar --- rclrs/src/timer.rs | 35 +++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index b9dd146bd..c1ef79f96 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -1,22 +1,24 @@ use crate::{ clock::Clock, context::Context, error::RclrsError, rcl_bindings::*, to_rclrs_result }; +// use std::fmt::Debug; use std::sync::{Arc, Mutex}; +pub type TimerCallback = Box; - -pub trait TimerCallback { - fn call(time_since_last_callback_ns: i64); -} - -#[derive(Debug)] +// #[derive(Debug)] pub struct Timer { rcl_timer: Arc>, - // callback: Option, + callback: Option, } impl Timer { + pub fn new(clock: &Clock, context: &Context, period: i64) -> Result { + Self::with_callback(clock, context, period, None) + } + + pub fn with_callback(clock: &Clock, context: &Context, period: i64, callback: Option) -> Result { let mut rcl_timer; let timer_init_result = unsafe { // SAFETY: Getting a default value is always safe. @@ -24,7 +26,8 @@ impl Timer { let allocator = rcutils_get_default_allocator(); let mut rcl_clock = clock.rcl_clock.lock().unwrap(); let mut rcl_context = context.handle.rcl_context.lock().unwrap(); - let callback: rcl_timer_callback_t = None; + // Callbacks will be handled in the WaitSet. + let rcl_timer_callback: rcl_timer_callback_t = None; // Function will return Err(_) only if there isn't enough memory to allocate a clock // object. rcl_timer_init( @@ -32,14 +35,14 @@ impl Timer { &mut *rcl_clock, &mut *rcl_context, period, - callback, + rcl_timer_callback, allocator, ) }; to_rclrs_result(timer_init_result).map(|_| { Timer { rcl_timer: Arc::new(Mutex::new(rcl_timer)), - // callback: None + callback, } }) } @@ -319,4 +322,16 @@ mod tests { assert!(is_ready.is_ok()); assert!(is_ready.unwrap()); } + + #[test] + fn test_callback_wip() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let foo = Arc::new(Mutex::new(0i64)); + let foo_callback = foo.clone(); + let dut = Timer::with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + dut.callback.unwrap()(123); + assert_eq!(*foo.lock().unwrap(), 123); + } } From 1095351c55af53106e60374d82aaa0885be93f1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jes=C3=BAs=20Silva?= <79662866+JesusSilvaUtrera@users.noreply.github.com> Date: Fri, 29 Nov 2024 15:31:15 +0100 Subject: [PATCH 11/26] Added comments to avoid warnings (#3) * Added timer_period_ns * Adds Timer::is_ready(). Signed-off-by: Agustin Alba Chicar * Added comments to avoid warnings * Added the getter for rcl_clock_t --------- Signed-off-by: Agustin Alba Chicar Co-authored-by: Agustin Alba Chicar --- rclrs/src/clock.rs | 8 ++++++-- rclrs/src/timer.rs | 27 +++++++++++++++++---------- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index 30558ea1a..85aa736ab 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -28,8 +28,7 @@ impl From for rcl_clock_type_t { #[derive(Clone, Debug)] pub struct Clock { kind: ClockType, - // TODO(ekumen): Fix the extra pub here. - pub rcl_clock: Arc>, + rcl_clock: Arc>, // TODO(luca) Implement jump callbacks } @@ -84,6 +83,11 @@ impl Clock { } } + /// Return the 'rcl_clock_t' of the Clock + pub(crate) fn get_rcl_clock(&self) -> &Arc> { + &self.rcl_clock + } + /// Returns the clock's `ClockType`. pub fn clock_type(&self) -> ClockType { self.kind diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index c1ef79f96..39d6d247d 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -13,7 +13,7 @@ pub struct Timer { } impl Timer { - + /// Creates a new timer (constructor) pub fn new(clock: &Clock, context: &Context, period: i64) -> Result { Self::with_callback(clock, context, period, None) } @@ -23,8 +23,8 @@ impl Timer { let timer_init_result = unsafe { // SAFETY: Getting a default value is always safe. rcl_timer = rcl_get_zero_initialized_timer(); + let mut rcl_clock = clock.get_rcl_clock().lock().unwrap(); let allocator = rcutils_get_default_allocator(); - let mut rcl_clock = clock.rcl_clock.lock().unwrap(); let mut rcl_context = context.handle.rcl_context.lock().unwrap(); // Callbacks will be handled in the WaitSet. let rcl_timer_callback: rcl_timer_callback_t = None; @@ -47,7 +47,8 @@ impl Timer { }) } - pub fn timer_period_ns(&self) -> Result { + /// Gets the period of the timer in nanoseconds + pub fn get_timer_period_ns(&self) -> Result { let mut timer_period_ns = 0; let get_period_result = unsafe { let rcl_timer = self.rcl_timer.lock().unwrap(); @@ -61,12 +62,14 @@ impl Timer { }) } + /// Cancels the timer, stopping the execution of the callback pub fn cancel(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); let cancel_result = unsafe { rcl_timer_cancel(&mut *rcl_timer) }; to_rclrs_result(cancel_result) } + /// Checks whether the timer is canceled or not pub fn is_canceled(&self) -> Result { let mut is_canceled = false; let is_canceled_result = unsafe { @@ -81,6 +84,7 @@ impl Timer { }) } + /// Retrieves the time since the last call to the callback pub fn time_since_last_call(&self) -> Result { let mut time_value_ns: i64 = 0; let time_since_last_call_result = unsafe { @@ -95,6 +99,7 @@ impl Timer { }) } + /// Retrieves the time until the next call of the callback pub fn time_until_next_call(&self) -> Result { let mut time_value_ns: i64 = 0; let time_until_next_call_result = unsafe { @@ -109,18 +114,21 @@ impl Timer { }) } + /// Resets the timer, setting the last call time to now pub fn reset(&mut self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); to_rclrs_result(unsafe {rcl_timer_reset(&mut *rcl_timer)}) } + /// Executes the callback of the timer (this is triggered by the executor or the node directly) pub fn call(&mut self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); to_rclrs_result(unsafe {rcl_timer_call(&mut *rcl_timer)}) } + /// Checks if the timer is ready (not canceled) pub fn is_ready(&self) -> Result { let (is_ready, is_ready_result) = unsafe { @@ -137,10 +145,9 @@ impl Timer { }) } // handle() -> RCLC Timer Type - - // clock() -> Clock ? } +/// 'Drop' trait implementation to be able to release the resources impl Drop for rcl_timer_t { fn drop(&mut self) { // SAFETY: No preconditions for this function @@ -215,7 +222,7 @@ mod tests { let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); let dut = dut.unwrap(); - let period_result = dut.timer_period_ns(); + let period_result = dut.get_timer_period_ns(); assert!(period_result.is_ok()); let period_result = period_result.unwrap(); assert_eq!(period_result, 1e6 as i64); @@ -293,14 +300,14 @@ mod tests { let mut dut = Timer::new(&clock, &context, period_ns).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); - + thread::sleep(time::Duration::from_micros(1500)); let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!(elapsed > 1500000i64, "time_until_next_call before call: {}", elapsed); - + assert!(dut.call().is_ok()); - + let elapsed = dut.time_until_next_call().unwrap(); assert!(elapsed < 500000i64, "time_until_next_call after call: {}", elapsed); } @@ -311,7 +318,7 @@ mod tests { let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 1e6 as i64; // 1 millisecond. let dut = Timer::new(&clock, &context, period_ns).unwrap(); - + let is_ready = dut.is_ready(); assert!(is_ready.is_ok()); assert!(!is_ready.unwrap()); From 965ca22eba8b0980783c60cd66f5ed191c843bf5 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 17:09:42 +0100 Subject: [PATCH 12/26] Integrated the Timer into the WaitSet. Signed-off-by: Agustin Alba Chicar --- rclrs/src/executor.rs | 6 +++- rclrs/src/timer.rs | 69 ++++++++++++++++++++++++++++++++++------- rclrs/src/wait.rs | 72 ++++++++++++++++++++++++++++++++++++++++++- 3 files changed, 134 insertions(+), 13 deletions(-) diff --git a/rclrs/src/executor.rs b/rclrs/src/executor.rs index 37c43a68e..bb795a96e 100644 --- a/rclrs/src/executor.rs +++ b/rclrs/src/executor.rs @@ -48,7 +48,11 @@ impl SingleThreadedExecutor { }) { let wait_set = WaitSet::new_for_node(&node)?; - let ready_entities = wait_set.wait(timeout)?; + let mut ready_entities = wait_set.wait(timeout)?; + + for ready_timer in ready_entities.timers.iter_mut() { + ready_timer.execute()?; + } for ready_subscription in ready_entities.subscriptions { ready_subscription.execute()?; diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 39d6d247d..47a53635a 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -2,23 +2,24 @@ use crate::{ clock::Clock, context::Context, error::RclrsError, rcl_bindings::*, to_rclrs_result }; // use std::fmt::Debug; -use std::sync::{Arc, Mutex}; +use std::sync::{atomic::AtomicBool, Arc, Mutex}; -pub type TimerCallback = Box; +pub type TimerCallback = Box; // #[derive(Debug)] pub struct Timer { - rcl_timer: Arc>, + pub(crate) rcl_timer: Arc>, callback: Option, + pub(crate) in_use_by_wait_set: Arc, } impl Timer { /// Creates a new timer (constructor) pub fn new(clock: &Clock, context: &Context, period: i64) -> Result { - Self::with_callback(clock, context, period, None) + Self::new_with_callback(clock, context, period, None) } - pub fn with_callback(clock: &Clock, context: &Context, period: i64, callback: Option) -> Result { + pub fn new_with_callback(clock: &Clock, context: &Context, period: i64, callback: Option) -> Result { let mut rcl_timer; let timer_init_result = unsafe { // SAFETY: Getting a default value is always safe. @@ -43,6 +44,7 @@ impl Timer { Timer { rcl_timer: Arc::new(Mutex::new(rcl_timer)), callback, + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), } }) } @@ -114,8 +116,7 @@ impl Timer { }) } - /// Resets the timer, setting the last call time to now - pub fn reset(&mut self) -> Result<(), RclrsError> + pub fn reset(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); to_rclrs_result(unsafe {rcl_timer_reset(&mut *rcl_timer)}) @@ -144,7 +145,20 @@ impl Timer { is_ready }) } - // handle() -> RCLC Timer Type + + pub(crate) fn execute(&self) -> Result<(), RclrsError> + { + if self.is_ready()? + { + let time_since_last_call = self.time_since_last_call()?; + self.call()?; + if let Some(ref callback) = self.callback + { + callback(time_since_last_call); + } + } + Ok(()) + } } /// 'Drop' trait implementation to be able to release the resources @@ -158,6 +172,12 @@ impl Drop for rcl_timer_t { } } +impl PartialEq for Timer { + fn eq(&self, other: &Self) -> bool { + Arc::ptr_eq(&self.rcl_timer, &other.rcl_timer) + } +} + // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. unsafe impl Send for rcl_timer_t {} @@ -282,7 +302,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 2e6 as i64; // 2 milliseconds. - let mut dut = Timer::new(&clock, &context, period_ns).unwrap(); + let dut = Timer::new(&clock, &context, period_ns).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); thread::sleep(time::Duration::from_millis(1)); @@ -297,7 +317,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 1e6 as i64; // 1 millisecond. - let mut dut = Timer::new(&clock, &context, period_ns).unwrap(); + let dut = Timer::new(&clock, &context, period_ns).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); @@ -337,8 +357,35 @@ mod tests { let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + let dut = Timer::new_with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); dut.callback.unwrap()(123); assert_eq!(*foo.lock().unwrap(), 123); } + + #[test] + fn test_execute_when_is_not_ready() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let foo = Arc::new(Mutex::new(0i64)); + let foo_callback = foo.clone(); + let dut = Timer::new_with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + assert!(dut.execute().is_ok()); + assert_eq!(*foo.lock().unwrap(), 0i64); + } + + #[test] + fn test_execute_when_is_ready() { + let clock = Clock::steady(); + let context = Context::new(vec![]).unwrap(); + let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let foo = Arc::new(Mutex::new(0i64)); + let foo_callback = foo.clone(); + let dut = Timer::new_with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + thread::sleep(time::Duration::from_micros(1500)); + assert!(dut.execute().is_ok()); + let x = *foo.lock().unwrap(); + assert!(x > 1500000i64); + assert!(x < 1600000i64); + } } diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 243c9d857..65ea3b134 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -20,7 +20,7 @@ use std::{sync::Arc, time::Duration, vec::Vec}; use crate::{ error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, + ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, Timer }; mod exclusivity_guard; @@ -51,6 +51,7 @@ pub struct WaitSet { guard_conditions: Vec>>, services: Vec>>, handle: WaitSetHandle, + timers: Vec>>, } /// A list of entities that are ready, returned by [`WaitSet::wait`]. @@ -63,6 +64,8 @@ pub struct ReadyEntities { pub guard_conditions: Vec>, /// A list of services that have potentially received requests. pub services: Vec>, + /// TODO + pub timers: Vec>, } impl Drop for rcl_wait_set_t { @@ -127,6 +130,7 @@ impl WaitSet { rcl_wait_set, context_handle: Arc::clone(&context.handle), }, + timers: Vec::new(), }) } @@ -178,6 +182,7 @@ impl WaitSet { self.guard_conditions.clear(); self.clients.clear(); self.services.clear(); + self.timers.clear(); // This cannot fail – the rcl_wait_set_clear function only checks that the input handle is // valid, which it always is in our case. Hence, only debug_assert instead of returning // Result. @@ -311,6 +316,27 @@ impl WaitSet { Ok(()) } + /// TBD + pub fn add_timer(&mut self, timer: Arc) -> Result<(), RclrsError> { + let exclusive_timer = ExclusivityGuard::new( + Arc::clone(&timer), + Arc::clone(&timer.in_use_by_wait_set), + )?; + unsafe { + // SAFETY: I'm not sure if it's required, but the timer pointer will remain valid + // for as long as the wait set exists, because it's stored in self.timers. + // Passing in a null pointer for the third argument is explicitly allowed. + rcl_wait_set_add_timer( + &mut self.handle.rcl_wait_set, + &* (*(*timer).rcl_timer).lock().unwrap() as *const _, // TODO :) + core::ptr::null_mut(), + ) + } + .ok()?; + self.timers.push(exclusive_timer); + Ok(()) + } + /// Blocks until the wait set is ready, or until the timeout has been exceeded. /// /// If the timeout is `None` then this function will block indefinitely until @@ -365,6 +391,7 @@ impl WaitSet { clients: Vec::new(), guard_conditions: Vec::new(), services: Vec::new(), + timers: Vec::new(), }; for (i, subscription) in self.subscriptions.iter().enumerate() { // SAFETY: The `subscriptions` entry is an array of pointers, and this dereferencing is @@ -409,6 +436,16 @@ impl WaitSet { ready_entities.services.push(Arc::clone(&service.waitable)); } } + + for (i, timer) in self.timers.iter().enumerate() { + // SAFETY: The `timers` entry is an array of pointers, and this dereferencing is + // equivalent to + // https://github.com/ros2/rcl/blob/35a31b00a12f259d492bf53c0701003bd7f1745c/rcl/include/rcl/wait.h#L419 + let wait_set_entry = unsafe { *self.handle.rcl_wait_set.timers.add(i) }; + if !wait_set_entry.is_null() { + ready_entities.timers.push(Arc::clone(&timer.waitable)); + } + } Ok(ready_entities) } } @@ -416,6 +453,7 @@ impl WaitSet { #[cfg(test)] mod tests { use super::*; + use crate::{clock::Clock}; #[test] fn traits() { @@ -440,4 +478,36 @@ mod tests { Ok(()) } + + #[test] + fn timer_in_wait_not_set_readies() -> Result<(), RclrsError> { + let context = Context::new([])?; + let clock = Clock::steady(); + let period: i64 = 1e6 as i64; // 1 milliseconds. + let timer = Arc::new(Timer::new(&clock, &context, period)?); + + let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; + wait_set.add_timer(timer.clone())?; + + let readies = wait_set.wait(Some(std::time::Duration::from_micros(0)))?; + assert!(!readies.timers.contains(&timer)); + + Ok(()) + } + + #[test] + fn timer_in_wait_set_readies() -> Result<(), RclrsError> { + let context = Context::new([])?; + let clock = Clock::steady(); + let period: i64 = 1e6 as i64; // 1 milliseconds. + let timer = Arc::new(Timer::new(&clock, &context, period)?); + + let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; + wait_set.add_timer(timer.clone())?; + + let readies = wait_set.wait(Some(std::time::Duration::from_micros(1500)))?; + assert!(readies.timers.contains(&timer)); + + Ok(()) + } } From f503c84bcd94ec4a86aead692dae4d0222d38a80 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jes=C3=BAs=20Silva?= <79662866+JesusSilvaUtrera@users.noreply.github.com> Date: Fri, 29 Nov 2024 18:58:56 +0100 Subject: [PATCH 13/26] Add create_timer to node (WIP) (#4) * Added timer_period_ns * Adds Timer::is_ready(). Signed-off-by: Agustin Alba Chicar * Added comments to avoid warnings * Added the getter for rcl_clock_t * WIP add create_timer to node * Cleaning some stuff --------- Signed-off-by: Agustin Alba Chicar Co-authored-by: Agustin Alba Chicar --- rclrs/src/node.rs | 31 ++++++++++++++++++++++++++++++- rclrs/src/node/builder.rs | 1 + 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b51b59817..848eb9914 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -16,9 +16,12 @@ use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, - SubscriptionCallback, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + SubscriptionCallback, Timer, TimerCallback, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, }; +/// Constant conversion from seconds to nanoseconds +const S_TO_NS: f64 = 1e9; + // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. unsafe impl Send for rcl_node_t {} @@ -63,6 +66,7 @@ pub struct Node { pub(crate) guard_conditions_mtx: Mutex>>, pub(crate) services_mtx: Mutex>>, pub(crate) subscriptions_mtx: Mutex>>, + pub(crate) timers_mtx: Mutex>>, time_source: TimeSource, parameter: ParameterInterface, pub(crate) handle: Arc, @@ -340,6 +344,31 @@ impl Node { Ok(subscription) } + /// Creates a [`Timer`] + pub(crate) fn create_timer( + &self, + timer_period_s: i64, + context: Context, + callback: Option, + clock: Option + ) -> Arc { + let timer_period_ns = (timer_period_s as f64 * S_TO_NS) as i64; + let clock_used; + match clock { + Some(value) => { + clock_used = value; + } + None => { + clock_used = self.get_clock(); + } + } + let context = Context::new(vec![]).unwrap(); + let timer = Timer::new(&clock_used, &context, timer_period_ns); + let timer = Arc::new(timer.unwrap()); + self.timers_mtx.lock().unwrap().push(Arc::downgrade(&timer) as Weak); + timer + } + /// Returns the subscriptions that have not been dropped yet. pub(crate) fn live_subscriptions(&self) -> Vec> { { self.subscriptions_mtx.lock().unwrap() } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 1e7a9fc63..4aec2686d 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -340,6 +340,7 @@ impl NodeBuilder { guard_conditions_mtx: Mutex::new(vec![]), services_mtx: Mutex::new(vec![]), subscriptions_mtx: Mutex::new(vec![]), + timers_mtx: Mutex::new(vec![]), time_source: TimeSource::builder(self.clock_type) .clock_qos(self.clock_qos) .build(), From ed78b350e738d5ab38de53816aa2a0b8ce3c192c Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 18:35:46 +0100 Subject: [PATCH 14/26] Makes it work with the integration demo. Signed-off-by: Agustin Alba Chicar --- rclrs/src/timer.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 47a53635a..8afdd0b57 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -123,7 +123,7 @@ impl Timer { } /// Executes the callback of the timer (this is triggered by the executor or the node directly) - pub fn call(&mut self) -> Result<(), RclrsError> + pub fn call(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); to_rclrs_result(unsafe {rcl_timer_call(&mut *rcl_timer)}) @@ -146,7 +146,7 @@ impl Timer { }) } - pub(crate) fn execute(&self) -> Result<(), RclrsError> + pub/* (crate)*/ fn execute(&self) -> Result<(), RclrsError> { if self.is_ready()? { From 214a991c55daeba43d27ee7520c31c362b0de4ea Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 19:55:48 +0100 Subject: [PATCH 15/26] Working E2E timer with node. Signed-off-by: Agustin Alba Chicar --- rclrs/src/node.rs | 36 ++++++++++++++++++------------------ rclrs/src/timer.rs | 2 +- rclrs/src/wait.rs | 7 ++++++- 3 files changed, 25 insertions(+), 20 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 848eb9914..34f575211 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -345,28 +345,21 @@ impl Node { } /// Creates a [`Timer`] - pub(crate) fn create_timer( + pub fn create_timer( &self, - timer_period_s: i64, - context: Context, + period_ns: i64, + context: &Context, callback: Option, clock: Option - ) -> Arc { - let timer_period_ns = (timer_period_s as f64 * S_TO_NS) as i64; - let clock_used; - match clock { - Some(value) => { - clock_used = value; - } - None => { - clock_used = self.get_clock(); - } - } - let context = Context::new(vec![]).unwrap(); - let timer = Timer::new(&clock_used, &context, timer_period_ns); - let timer = Arc::new(timer.unwrap()); + ) -> Result, RclrsError> { + let clock_used = match clock { + Some(value) => value, + None => self.get_clock(), + }; + let timer = Timer::new_with_callback(&clock_used, &context, period_ns, callback)?; + let timer = Arc::new(timer); self.timers_mtx.lock().unwrap().push(Arc::downgrade(&timer) as Weak); - timer + Ok(timer) } /// Returns the subscriptions that have not been dropped yet. @@ -398,6 +391,13 @@ impl Node { .collect() } + pub(crate) fn live_timers(&self) -> Vec> { + { self.timers_mtx.lock().unwrap() } + .iter() + .filter_map(Weak::upgrade) + .collect() + } + /// Returns the ROS domain ID that the node is using. /// /// The domain ID controls which nodes can send messages to each other, see the [ROS 2 concept article][1]. diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index 8afdd0b57..ebd16b33d 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -146,7 +146,7 @@ impl Timer { }) } - pub/* (crate)*/ fn execute(&self) -> Result<(), RclrsError> + pub fn execute(&self) -> Result<(), RclrsError> { if self.is_ready()? { diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 65ea3b134..4b195e879 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -142,13 +142,14 @@ impl WaitSet { let live_clients = node.live_clients(); let live_guard_conditions = node.live_guard_conditions(); let live_services = node.live_services(); + let live_timers = node.live_timers(); let ctx = Context { handle: Arc::clone(&node.handle.context_handle), }; let mut wait_set = WaitSet::new( live_subscriptions.len(), live_guard_conditions.len(), - 0, + live_timers.len(), live_clients.len(), live_services.len(), 0, @@ -170,6 +171,10 @@ impl WaitSet { for live_service in &live_services { wait_set.add_service(live_service.clone())?; } + + for live_timer in &live_timers { + wait_set.add_timer(live_timer.clone())?; + } Ok(wait_set) } From 1eb1acc52c2fed8d9f7a060105df148a021f68a0 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 20:03:06 +0100 Subject: [PATCH 16/26] Format fix. Signed-off-by: Agustin Alba Chicar --- rclrs/src/lib.rs | 4 +- rclrs/src/node.rs | 9 ++- rclrs/src/timer.rs | 174 +++++++++++++++++++++++---------------------- rclrs/src/wait.rs | 16 ++--- 4 files changed, 104 insertions(+), 99 deletions(-) diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 0b8f4c7f1..ddb3e9c3f 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -19,8 +19,8 @@ mod qos; mod service; mod subscription; mod time; -mod timer; mod time_source; +mod timer; mod vendor; mod wait; @@ -49,8 +49,8 @@ pub use rcl_bindings::rmw_request_id_t; pub use service::*; pub use subscription::*; pub use time::*; -pub use timer::*; use time_source::*; +pub use timer::*; pub use wait::*; /// Polls the node for new messages and executes the corresponding callbacks. diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 34f575211..66473b6e1 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -16,7 +16,7 @@ use crate::{ rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, LogParams, Logger, ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, - SubscriptionCallback, Timer, TimerCallback, TimeSource, ToLogParams, ENTITY_LIFECYCLE_MUTEX, + SubscriptionCallback, TimeSource, Timer, TimerCallback, ToLogParams, ENTITY_LIFECYCLE_MUTEX, }; /// Constant conversion from seconds to nanoseconds @@ -350,7 +350,7 @@ impl Node { period_ns: i64, context: &Context, callback: Option, - clock: Option + clock: Option, ) -> Result, RclrsError> { let clock_used = match clock { Some(value) => value, @@ -358,7 +358,10 @@ impl Node { }; let timer = Timer::new_with_callback(&clock_used, &context, period_ns, callback)?; let timer = Arc::new(timer); - self.timers_mtx.lock().unwrap().push(Arc::downgrade(&timer) as Weak); + self.timers_mtx + .lock() + .unwrap() + .push(Arc::downgrade(&timer) as Weak); Ok(timer) } diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index ebd16b33d..c7e4dfa95 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -1,8 +1,6 @@ -use crate::{ - clock::Clock, context::Context, error::RclrsError, rcl_bindings::*, to_rclrs_result -}; +use crate::{clock::Clock, context::Context, error::RclrsError, rcl_bindings::*, to_rclrs_result}; // use std::fmt::Debug; -use std::sync::{atomic::AtomicBool, Arc, Mutex}; +use std::sync::{atomic::AtomicBool, Arc, Mutex}; pub type TimerCallback = Box; @@ -19,7 +17,12 @@ impl Timer { Self::new_with_callback(clock, context, period, None) } - pub fn new_with_callback(clock: &Clock, context: &Context, period: i64, callback: Option) -> Result { + pub fn new_with_callback( + clock: &Clock, + context: &Context, + period: i64, + callback: Option, + ) -> Result { let mut rcl_timer; let timer_init_result = unsafe { // SAFETY: Getting a default value is always safe. @@ -40,12 +43,10 @@ impl Timer { allocator, ) }; - to_rclrs_result(timer_init_result).map(|_| { - Timer { - rcl_timer: Arc::new(Mutex::new(rcl_timer)), - callback, - in_use_by_wait_set: Arc::new(AtomicBool::new(false)), - } + to_rclrs_result(timer_init_result).map(|_| Timer { + rcl_timer: Arc::new(Mutex::new(rcl_timer)), + callback, + in_use_by_wait_set: Arc::new(AtomicBool::new(false)), }) } @@ -54,14 +55,9 @@ impl Timer { let mut timer_period_ns = 0; let get_period_result = unsafe { let rcl_timer = self.rcl_timer.lock().unwrap(); - rcl_timer_get_period( - &* rcl_timer, - &mut timer_period_ns - ) + rcl_timer_get_period(&*rcl_timer, &mut timer_period_ns) }; - to_rclrs_result(get_period_result).map(|_| { - timer_period_ns - }) + to_rclrs_result(get_period_result).map(|_| timer_period_ns) } /// Cancels the timer, stopping the execution of the callback @@ -76,14 +72,9 @@ impl Timer { let mut is_canceled = false; let is_canceled_result = unsafe { let rcl_timer = self.rcl_timer.lock().unwrap(); - rcl_timer_is_canceled( - &* rcl_timer, - &mut is_canceled - ) + rcl_timer_is_canceled(&*rcl_timer, &mut is_canceled) }; - to_rclrs_result(is_canceled_result).map(|_| { - is_canceled - }) + to_rclrs_result(is_canceled_result).map(|_| is_canceled) } /// Retrieves the time since the last call to the callback @@ -91,14 +82,9 @@ impl Timer { let mut time_value_ns: i64 = 0; let time_since_last_call_result = unsafe { let rcl_timer = self.rcl_timer.lock().unwrap(); - rcl_timer_get_time_since_last_call( - &* rcl_timer, - &mut time_value_ns - ) + rcl_timer_get_time_since_last_call(&*rcl_timer, &mut time_value_ns) }; - to_rclrs_result(time_since_last_call_result).map(|_| { - time_value_ns - }) + to_rclrs_result(time_since_last_call_result).map(|_| time_value_ns) } /// Retrieves the time until the next call of the callback @@ -106,59 +92,43 @@ impl Timer { let mut time_value_ns: i64 = 0; let time_until_next_call_result = unsafe { let rcl_timer = self.rcl_timer.lock().unwrap(); - rcl_timer_get_time_until_next_call( - &* rcl_timer, - &mut time_value_ns - ) + rcl_timer_get_time_until_next_call(&*rcl_timer, &mut time_value_ns) }; - to_rclrs_result(time_until_next_call_result).map(|_| { - time_value_ns - }) + to_rclrs_result(time_until_next_call_result).map(|_| time_value_ns) } - pub fn reset(&self) -> Result<(), RclrsError> - { + pub fn reset(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); - to_rclrs_result(unsafe {rcl_timer_reset(&mut *rcl_timer)}) + to_rclrs_result(unsafe { rcl_timer_reset(&mut *rcl_timer) }) } /// Executes the callback of the timer (this is triggered by the executor or the node directly) - pub fn call(&self) -> Result<(), RclrsError> - { + pub fn call(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); - to_rclrs_result(unsafe {rcl_timer_call(&mut *rcl_timer)}) + to_rclrs_result(unsafe { rcl_timer_call(&mut *rcl_timer) }) } /// Checks if the timer is ready (not canceled) - pub fn is_ready(&self) -> Result - { + pub fn is_ready(&self) -> Result { let (is_ready, is_ready_result) = unsafe { let mut is_ready: bool = false; let rcl_timer = self.rcl_timer.lock().unwrap(); - let is_ready_result = rcl_timer_is_ready( - &* rcl_timer, - &mut is_ready - ); + let is_ready_result = rcl_timer_is_ready(&*rcl_timer, &mut is_ready); (is_ready, is_ready_result) }; - to_rclrs_result(is_ready_result).map(|_| { - is_ready - }) + to_rclrs_result(is_ready_result).map(|_| is_ready) } - - pub fn execute(&self) -> Result<(), RclrsError> - { - if self.is_ready()? - { + + pub fn execute(&self) -> Result<(), RclrsError> { + if self.is_ready()? { let time_since_last_call = self.time_since_last_call()?; self.call()?; - if let Some(ref callback) = self.callback - { + if let Some(ref callback) = self.callback { callback(time_since_last_call); } } Ok(()) - } + } } /// 'Drop' trait implementation to be able to release the resources @@ -199,7 +169,7 @@ mod tests { fn test_new_with_system_clock() { let clock = Clock::system(); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -209,7 +179,7 @@ mod tests { fn test_new_with_steady_clock() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -227,7 +197,7 @@ mod tests { assert_eq!(clock.now().nsec, set_time); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1e6 as i64; // 1 milliseconds.. + let period: i64 = 1e6 as i64; // 1 milliseconds.. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -237,7 +207,7 @@ mod tests { fn test_get_period() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -252,7 +222,7 @@ mod tests { fn test_cancel() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let dut = Timer::new(&clock, &context, period); assert!(dut.is_ok()); @@ -269,7 +239,7 @@ mod tests { fn test_time_since_last_call_before_first_event() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 2e6 as i64; // 2 milliseconds. + let period_ns: i64 = 2e6 as i64; // 2 milliseconds. let sleep_period_ms = time::Duration::from_millis(1); let dut = Timer::new(&clock, &context, period_ns); @@ -279,21 +249,29 @@ mod tests { let time_since_last_call = dut.time_since_last_call(); assert!(time_since_last_call.is_ok()); let time_since_last_call = time_since_last_call.unwrap(); - assert!(time_since_last_call > 9e5 as i64, "time_since_last_call: {}", time_since_last_call); + assert!( + time_since_last_call > 9e5 as i64, + "time_since_last_call: {}", + time_since_last_call + ); } #[test] fn test_time_until_next_call_before_first_event() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 2e6 as i64; // 2 milliseconds. + let period_ns: i64 = 2e6 as i64; // 2 milliseconds. let dut = Timer::new(&clock, &context, period_ns); assert!(dut.is_ok()); let dut = dut.unwrap(); let time_until_next_call = dut.time_until_next_call(); assert!(time_until_next_call.is_ok()); let time_until_next_call = time_until_next_call.unwrap(); - assert!(time_until_next_call < period_ns, "time_until_next_call: {}", time_until_next_call); + assert!( + time_until_next_call < period_ns, + "time_until_next_call: {}", + time_until_next_call + ); } #[test] @@ -301,14 +279,14 @@ mod tests { let tolerance = 20e4 as i64; let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 2e6 as i64; // 2 milliseconds. + let period_ns: i64 = 2e6 as i64; // 2 milliseconds. let dut = Timer::new(&clock, &context, period_ns).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); - assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); + assert!(elapsed < tolerance, "elapsed before reset: {}", elapsed); thread::sleep(time::Duration::from_millis(1)); assert!(dut.reset().is_ok()); let elapsed = period_ns - dut.time_until_next_call().unwrap(); - assert!(elapsed < tolerance , "elapsed after reset: {}", elapsed); + assert!(elapsed < tolerance, "elapsed after reset: {}", elapsed); } #[test] @@ -316,27 +294,35 @@ mod tests { let tolerance = 20e4 as i64; let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let period_ns: i64 = 1e6 as i64; // 1 millisecond. let dut = Timer::new(&clock, &context, period_ns).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); - assert!(elapsed < tolerance , "elapsed before reset: {}", elapsed); + assert!(elapsed < tolerance, "elapsed before reset: {}", elapsed); thread::sleep(time::Duration::from_micros(1500)); let elapsed = period_ns - dut.time_until_next_call().unwrap(); - assert!(elapsed > 1500000i64, "time_until_next_call before call: {}", elapsed); + assert!( + elapsed > 1500000i64, + "time_until_next_call before call: {}", + elapsed + ); assert!(dut.call().is_ok()); let elapsed = dut.time_until_next_call().unwrap(); - assert!(elapsed < 500000i64, "time_until_next_call after call: {}", elapsed); + assert!( + elapsed < 500000i64, + "time_until_next_call after call: {}", + elapsed + ); } #[test] fn test_is_ready() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let period_ns: i64 = 1e6 as i64; // 1 millisecond. let dut = Timer::new(&clock, &context, period_ns).unwrap(); let is_ready = dut.is_ready(); @@ -354,10 +340,16 @@ mod tests { fn test_callback_wip() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::new_with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + let dut = Timer::new_with_callback( + &clock, + &context, + period_ns, + Some(Box::new(move |x| *foo_callback.lock().unwrap() = x)), + ) + .unwrap(); dut.callback.unwrap()(123); assert_eq!(*foo.lock().unwrap(), 123); } @@ -366,10 +358,16 @@ mod tests { fn test_execute_when_is_not_ready() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::new_with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + let dut = Timer::new_with_callback( + &clock, + &context, + period_ns, + Some(Box::new(move |x| *foo_callback.lock().unwrap() = x)), + ) + .unwrap(); assert!(dut.execute().is_ok()); assert_eq!(*foo.lock().unwrap(), 0i64); } @@ -378,10 +376,16 @@ mod tests { fn test_execute_when_is_ready() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); - let period_ns: i64 = 1e6 as i64; // 1 millisecond. + let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::new_with_callback(&clock, &context, period_ns, Some(Box::new(move |x| *foo_callback.lock().unwrap() = x ))).unwrap(); + let dut = Timer::new_with_callback( + &clock, + &context, + period_ns, + Some(Box::new(move |x| *foo_callback.lock().unwrap() = x)), + ) + .unwrap(); thread::sleep(time::Duration::from_micros(1500)); assert!(dut.execute().is_ok()); let x = *foo.lock().unwrap(); diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 4b195e879..cac3f6900 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -20,7 +20,7 @@ use std::{sync::Arc, time::Duration, vec::Vec}; use crate::{ error::{to_rclrs_result, RclReturnCode, RclrsError, ToResult}, rcl_bindings::*, - ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, Timer + ClientBase, Context, ContextHandle, Node, ServiceBase, SubscriptionBase, Timer, }; mod exclusivity_guard; @@ -323,17 +323,15 @@ impl WaitSet { /// TBD pub fn add_timer(&mut self, timer: Arc) -> Result<(), RclrsError> { - let exclusive_timer = ExclusivityGuard::new( - Arc::clone(&timer), - Arc::clone(&timer.in_use_by_wait_set), - )?; + let exclusive_timer = + ExclusivityGuard::new(Arc::clone(&timer), Arc::clone(&timer.in_use_by_wait_set))?; unsafe { // SAFETY: I'm not sure if it's required, but the timer pointer will remain valid // for as long as the wait set exists, because it's stored in self.timers. // Passing in a null pointer for the third argument is explicitly allowed. rcl_wait_set_add_timer( &mut self.handle.rcl_wait_set, - &* (*(*timer).rcl_timer).lock().unwrap() as *const _, // TODO :) + &*(*(*timer).rcl_timer).lock().unwrap() as *const _, // TODO :) core::ptr::null_mut(), ) } @@ -458,7 +456,7 @@ impl WaitSet { #[cfg(test)] mod tests { use super::*; - use crate::{clock::Clock}; + use crate::clock::Clock; #[test] fn traits() { @@ -488,7 +486,7 @@ mod tests { fn timer_in_wait_not_set_readies() -> Result<(), RclrsError> { let context = Context::new([])?; let clock = Clock::steady(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let timer = Arc::new(Timer::new(&clock, &context, period)?); let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; @@ -504,7 +502,7 @@ mod tests { fn timer_in_wait_set_readies() -> Result<(), RclrsError> { let context = Context::new([])?; let clock = Clock::steady(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 milliseconds. let timer = Arc::new(Timer::new(&clock, &context, period)?); let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; From 91756ca909b190f779a6b9ad1183d2cc75b65279 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Fri, 29 Nov 2024 20:09:02 +0100 Subject: [PATCH 17/26] Fix a TODO for peace of mind. Signed-off-by: Agustin Alba Chicar --- rclrs/src/wait.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index cac3f6900..205012014 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -331,7 +331,7 @@ impl WaitSet { // Passing in a null pointer for the third argument is explicitly allowed. rcl_wait_set_add_timer( &mut self.handle.rcl_wait_set, - &*(*(*timer).rcl_timer).lock().unwrap() as *const _, // TODO :) + &* timer.rcl_timer.lock().unwrap() as *const _, core::ptr::null_mut(), ) } From fbb862903dcf88210626c062324619cbe53f2bab Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 12:47:55 +0100 Subject: [PATCH 18/26] Adds an example. Signed-off-by: Agustin Alba Chicar --- examples/rclrs_timer_demo/Cargo.toml | 12 +++++ examples/rclrs_timer_demo/package.xml | 13 ++++++ .../rclrs_timer_demo/src/rclrs_timer_demo.rs | 44 +++++++++++++++++++ 3 files changed, 69 insertions(+) create mode 100644 examples/rclrs_timer_demo/Cargo.toml create mode 100644 examples/rclrs_timer_demo/package.xml create mode 100644 examples/rclrs_timer_demo/src/rclrs_timer_demo.rs diff --git a/examples/rclrs_timer_demo/Cargo.toml b/examples/rclrs_timer_demo/Cargo.toml new file mode 100644 index 000000000..772d635be --- /dev/null +++ b/examples/rclrs_timer_demo/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "rclrs_timer_demo" +version = "0.1.0" +edition = "2021" + +[[bin]] +name="rclrs_timer_demo" +path="src/rclrs_timer_demo.rs" + + +[dependencies] +rclrs = "*" diff --git a/examples/rclrs_timer_demo/package.xml b/examples/rclrs_timer_demo/package.xml new file mode 100644 index 000000000..64e673704 --- /dev/null +++ b/examples/rclrs_timer_demo/package.xml @@ -0,0 +1,13 @@ + + rclrs_timer_demo + 0.1.0 + Shows how to implement a timer within a Node using rclrs. + user + TODO: License declaration. + + rclrs + + + ament_cargo + + diff --git a/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs new file mode 100644 index 000000000..931e50b87 --- /dev/null +++ b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs @@ -0,0 +1,44 @@ +/// Creates a SimpleTimerNode, initializes a node and the timer with a callback +/// that prints the timer callback execution iteration. The callback is executed +/// thanks to the spin, which is in charge of executing the timer's events among +/// other entities' events. +use rclrs::{create_node, Context, Node, Timer, RclrsError}; +use std::{env, sync::{Arc, Mutex}}; + +/// Contains both the node and timer. +struct SimpleTimerNode { + node: Arc, + timer: Arc, +} + +impl SimpleTimerNode { + + /// Creates a node and a timer with a callback. + /// + /// The callback will simply print to stdout: + /// "Drinking 🧉 for the th time every

nanoseconds." + /// where is the iteration callback counter and

is the period of the timer. + fn new(context: &Context, timer_period_ns: i64) -> Result { + let node = create_node(context, "simple_timer_node")?; + let count: Arc> = Arc::new(Mutex::new(0)); + let timer = node.create_timer( + timer_period_ns, + context, + Some(Box::new(move |_| { + let x = *count.lock().unwrap(); + println!("Drinking 🧉 for the {}th time every {} nanoseconds.", x, timer_period_ns); + *count.lock().unwrap() = x + 1; + })), + None, + )?; + Ok(Self { node, timer }) + } +} + + +fn main() -> Result<(), RclrsError> { + let timer_period: i64 = 1e9 as i64; // 1 seconds. + let context = Context::new(env::args()).unwrap(); + let simple_timer_node = Arc::new(SimpleTimerNode::new(&context, timer_period).unwrap()); + rclrs::spin(simple_timer_node.node.clone()) +} \ No newline at end of file From ab3d63a2521ded2436a4a59114316cdc4a027e48 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 12:49:46 +0100 Subject: [PATCH 19/26] Fix format for the example. Signed-off-by: Agustin Alba Chicar --- .../rclrs_timer_demo/src/rclrs_timer_demo.rs | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs index 931e50b87..4889534eb 100644 --- a/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs +++ b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs @@ -2,8 +2,11 @@ /// that prints the timer callback execution iteration. The callback is executed /// thanks to the spin, which is in charge of executing the timer's events among /// other entities' events. -use rclrs::{create_node, Context, Node, Timer, RclrsError}; -use std::{env, sync::{Arc, Mutex}}; +use rclrs::{create_node, Context, Node, RclrsError, Timer}; +use std::{ + env, + sync::{Arc, Mutex}, +}; /// Contains both the node and timer. struct SimpleTimerNode { @@ -12,7 +15,6 @@ struct SimpleTimerNode { } impl SimpleTimerNode { - /// Creates a node and a timer with a callback. /// /// The callback will simply print to stdout: @@ -26,7 +28,10 @@ impl SimpleTimerNode { context, Some(Box::new(move |_| { let x = *count.lock().unwrap(); - println!("Drinking 🧉 for the {}th time every {} nanoseconds.", x, timer_period_ns); + println!( + "Drinking 🧉 for the {}th time every {} nanoseconds.", + x, timer_period_ns + ); *count.lock().unwrap() = x + 1; })), None, @@ -35,10 +40,9 @@ impl SimpleTimerNode { } } - fn main() -> Result<(), RclrsError> { let timer_period: i64 = 1e9 as i64; // 1 seconds. let context = Context::new(env::args()).unwrap(); let simple_timer_node = Arc::new(SimpleTimerNode::new(&context, timer_period).unwrap()); rclrs::spin(simple_timer_node.node.clone()) -} \ No newline at end of file +} From 4515e9a13c0d0103a944b1644ed3fa3b3743cbdb Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 13:24:04 +0100 Subject: [PATCH 20/26] Adds tests, documentation and removes dead code in node.rs. Signed-off-by: Agustin Alba Chicar --- rclrs/src/node.rs | 54 +++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 50 insertions(+), 4 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 66473b6e1..3b169096b 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -19,9 +19,6 @@ use crate::{ SubscriptionCallback, TimeSource, Timer, TimerCallback, ToLogParams, ENTITY_LIFECYCLE_MUTEX, }; -/// Constant conversion from seconds to nanoseconds -const S_TO_NS: f64 = 1e9; - // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread // they are running in. Therefore, this type can be safely sent to another thread. unsafe impl Send for rcl_node_t {} @@ -344,7 +341,10 @@ impl Node { Ok(subscription) } - /// Creates a [`Timer`] + /// Creates a [`Timer`][1]. + /// + /// [1]: crate::Timer + /// TODO: make timer's lifetime depend on node's lifetime. pub fn create_timer( &self, period_ns: i64, @@ -583,6 +583,52 @@ mod tests { Ok(()) } + #[test] + fn test_create_timer_without_clock_source() -> Result<(), RclrsError> { + let timer_period_ns: i64 = 1e6 as i64; // 1 millisecond. + let flag: Arc> = Arc::new(Mutex::new(false)); + let callback_flag: Arc> = Arc::new(Mutex::new(false)); + let context = Context::new([])?; + let dut = NodeBuilder::new(&context, "node_with_timer") + .namespace("test_create_timer") + .build()?; + + let timer = dut.create_timer( + timer_period_ns, + &context, + Some(Box::new(move |_| { + *callback_flag.lock().unwrap() = true; + })), + None, + )?; + assert_eq!(dut.live_timers().len(), 1); + + Ok(()) + } + + #[test] + fn test_create_timer_with_clock_source() -> Result<(), RclrsError> { + let timer_period_ns: i64 = 1e6 as i64; // 1 millisecond. + let flag: Arc> = Arc::new(Mutex::new(false)); + let callback_flag: Arc> = Arc::new(Mutex::new(false)); + let context = Context::new([])?; + let dut = NodeBuilder::new(&context, "node_with_timer") + .namespace("test_create_timer") + .build()?; + + let timer = dut.create_timer( + timer_period_ns, + &context, + Some(Box::new(move |_| { + *callback_flag.lock().unwrap() = true; + })), + None, + )?; + assert_eq!(dut.live_timers().len(), 1); + + Ok(()) + } + #[test] fn test_logger_name() -> Result<(), RclrsError> { // Use helper to create 2 nodes for us From 85930a34058c93ebac4359347152c247405c376c Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 13:34:04 +0100 Subject: [PATCH 21/26] Fix documentation style in clock.rs. Signed-off-by: Agustin Alba Chicar --- rclrs/src/clock.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs index 85aa736ab..8182d37ae 100644 --- a/rclrs/src/clock.rs +++ b/rclrs/src/clock.rs @@ -83,7 +83,7 @@ impl Clock { } } - /// Return the 'rcl_clock_t' of the Clock + /// Returns the clock's `rcl_clock_t`. pub(crate) fn get_rcl_clock(&self) -> &Arc> { &self.rcl_clock } From 1563895f7cda2530c1e84a541da019f7b05c7a9f Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 13:36:11 +0100 Subject: [PATCH 22/26] Removes duplicated test in node.rs Signed-off-by: Agustin Alba Chicar --- rclrs/src/node.rs | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 3b169096b..11ad4d36b 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -606,29 +606,6 @@ mod tests { Ok(()) } - #[test] - fn test_create_timer_with_clock_source() -> Result<(), RclrsError> { - let timer_period_ns: i64 = 1e6 as i64; // 1 millisecond. - let flag: Arc> = Arc::new(Mutex::new(false)); - let callback_flag: Arc> = Arc::new(Mutex::new(false)); - let context = Context::new([])?; - let dut = NodeBuilder::new(&context, "node_with_timer") - .namespace("test_create_timer") - .build()?; - - let timer = dut.create_timer( - timer_period_ns, - &context, - Some(Box::new(move |_| { - *callback_flag.lock().unwrap() = true; - })), - None, - )?; - assert_eq!(dut.live_timers().len(), 1); - - Ok(()) - } - #[test] fn test_logger_name() -> Result<(), RclrsError> { // Use helper to create 2 nodes for us From 08acef56ebd8182aa43a7b387d8cbc0703b668da Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 13:47:36 +0100 Subject: [PATCH 23/26] Fix warnings while running tests in node.rs. Signed-off-by: Agustin Alba Chicar --- rclrs/src/node.rs | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 11ad4d36b..459b4445f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -586,19 +586,15 @@ mod tests { #[test] fn test_create_timer_without_clock_source() -> Result<(), RclrsError> { let timer_period_ns: i64 = 1e6 as i64; // 1 millisecond. - let flag: Arc> = Arc::new(Mutex::new(false)); - let callback_flag: Arc> = Arc::new(Mutex::new(false)); let context = Context::new([])?; let dut = NodeBuilder::new(&context, "node_with_timer") .namespace("test_create_timer") .build()?; - let timer = dut.create_timer( + let _timer = dut.create_timer( timer_period_ns, &context, - Some(Box::new(move |_| { - *callback_flag.lock().unwrap() = true; - })), + Some(Box::new(move |_| { })), None, )?; assert_eq!(dut.live_timers().len(), 1); From a4c1a97cefc727bd3b722aa1c7631a64fb1e85f2 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 13:48:44 +0100 Subject: [PATCH 24/26] Fix missing documentation in wait.rs. Signed-off-by: Agustin Alba Chicar --- rclrs/src/wait.rs | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 205012014..108a17561 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -64,7 +64,7 @@ pub struct ReadyEntities { pub guard_conditions: Vec>, /// A list of services that have potentially received requests. pub services: Vec>, - /// TODO + /// A list of timers that are potentially due. pub timers: Vec>, } @@ -321,7 +321,16 @@ impl WaitSet { Ok(()) } - /// TBD + /// Adds a timer to the wait set. + /// + /// # Errors + /// - If the timer was already added to this wait set or another one, + /// [`AlreadyAddedToWaitSet`][1] will be returned + /// - If the number of timer in the wait set is larger than the + /// capacity set in [`WaitSet::new`], [`WaitSetFull`][2] will be returned + /// + /// [1]: crate::RclrsError + /// [2]: crate::RclReturnCode pub fn add_timer(&mut self, timer: Arc) -> Result<(), RclrsError> { let exclusive_timer = ExclusivityGuard::new(Arc::clone(&timer), Arc::clone(&timer.in_use_by_wait_set))?; @@ -331,7 +340,7 @@ impl WaitSet { // Passing in a null pointer for the third argument is explicitly allowed. rcl_wait_set_add_timer( &mut self.handle.rcl_wait_set, - &* timer.rcl_timer.lock().unwrap() as *const _, + &*timer.rcl_timer.lock().unwrap() as *const _, core::ptr::null_mut(), ) } @@ -486,7 +495,7 @@ mod tests { fn timer_in_wait_not_set_readies() -> Result<(), RclrsError> { let context = Context::new([])?; let clock = Clock::steady(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 millisecond. let timer = Arc::new(Timer::new(&clock, &context, period)?); let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; @@ -502,7 +511,7 @@ mod tests { fn timer_in_wait_set_readies() -> Result<(), RclrsError> { let context = Context::new([])?; let clock = Clock::steady(); - let period: i64 = 1e6 as i64; // 1 milliseconds. + let period: i64 = 1e6 as i64; // 1 millisecond. let timer = Arc::new(Timer::new(&clock, &context, period)?); let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; From 655185d8b04ec0ef5ffef32600798e423cd7a447 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 14:16:24 +0100 Subject: [PATCH 25/26] Improvements to timer. - Adds documentation. - Removes the no-callback constructor. - Removes vertical space. Signed-off-by: Agustin Alba Chicar --- rclrs/src/node.rs | 10 ++----- rclrs/src/timer.rs | 73 ++++++++++++++++++++++++---------------------- rclrs/src/wait.rs | 7 +++-- 3 files changed, 46 insertions(+), 44 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 459b4445f..5df9c057d 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -356,7 +356,7 @@ impl Node { Some(value) => value, None => self.get_clock(), }; - let timer = Timer::new_with_callback(&clock_used, &context, period_ns, callback)?; + let timer = Timer::new(&clock_used, &context, period_ns, callback)?; let timer = Arc::new(timer); self.timers_mtx .lock() @@ -591,12 +591,8 @@ mod tests { .namespace("test_create_timer") .build()?; - let _timer = dut.create_timer( - timer_period_ns, - &context, - Some(Box::new(move |_| { })), - None, - )?; + let _timer = + dut.create_timer(timer_period_ns, &context, Some(Box::new(move |_| {})), None)?; assert_eq!(dut.live_timers().len(), 1); Ok(()) diff --git a/rclrs/src/timer.rs b/rclrs/src/timer.rs index c7e4dfa95..fb5f761a7 100644 --- a/rclrs/src/timer.rs +++ b/rclrs/src/timer.rs @@ -1,23 +1,35 @@ use crate::{clock::Clock, context::Context, error::RclrsError, rcl_bindings::*, to_rclrs_result}; +// TODO: fix me when the callback type is properly defined. // use std::fmt::Debug; use std::sync::{atomic::AtomicBool, Arc, Mutex}; +/// Type alias for the `Timer` callback. pub type TimerCallback = Box; +/// Struct for executing periodic events. +/// +/// The execution of the callbacks is tied to [`spin_once`][1] or [`spin`][2] on the timers's node. +/// +/// Timer can be created via [`Node::create_timer()`][3], this is to ensure that [`Node`][4]s can +/// track all the timers that have been created. However, a user of a `Timer` can also +/// use it standalone. +/// +/// [1]: crate::spin_once +/// [2]: crate::spin +/// [3]: crate::Node::create_timer +/// [4]: crate::Node +// TODO: callback type prevents us from making the Timer implement the Debug trait. // #[derive(Debug)] pub struct Timer { pub(crate) rcl_timer: Arc>, + /// The callback function that runs when the timer is due. callback: Option, pub(crate) in_use_by_wait_set: Arc, } impl Timer { - /// Creates a new timer (constructor) - pub fn new(clock: &Clock, context: &Context, period: i64) -> Result { - Self::new_with_callback(clock, context, period, None) - } - - pub fn new_with_callback( + /// Creates a new timer. + pub fn new( clock: &Clock, context: &Context, period: i64, @@ -97,6 +109,7 @@ impl Timer { to_rclrs_result(time_until_next_call_result).map(|_| time_value_ns) } + /// Resets the timer. pub fn reset(&self) -> Result<(), RclrsError> { let mut rcl_timer = self.rcl_timer.lock().unwrap(); to_rclrs_result(unsafe { rcl_timer_reset(&mut *rcl_timer) }) @@ -119,7 +132,7 @@ impl Timer { to_rclrs_result(is_ready_result).map(|_| is_ready) } - pub fn execute(&self) -> Result<(), RclrsError> { + pub(crate) fn execute(&self) -> Result<(), RclrsError> { if self.is_ready()? { let time_since_last_call = self.time_since_last_call()?; self.call()?; @@ -157,6 +170,10 @@ mod tests { use super::*; use std::{thread, time}; + fn create_dummy_callback() -> Option { + Some(Box::new(move |_| {})) + } + #[test] fn traits() { use crate::test_helpers::*; @@ -170,8 +187,7 @@ mod tests { let clock = Clock::system(); let context = Context::new(vec![]).unwrap(); let period: i64 = 1e6 as i64; // 1 milliseconds. - - let dut = Timer::new(&clock, &context, period); + let dut = Timer::new(&clock, &context, period, create_dummy_callback()); assert!(dut.is_ok()); } @@ -180,8 +196,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period: i64 = 1e6 as i64; // 1 milliseconds. - - let dut = Timer::new(&clock, &context, period); + let dut = Timer::new(&clock, &context, period, create_dummy_callback()); assert!(dut.is_ok()); } @@ -195,11 +210,9 @@ mod tests { source.set_ros_time_override(set_time); // Ros time is set, should return the value that was set assert_eq!(clock.now().nsec, set_time); - let context = Context::new(vec![]).unwrap(); let period: i64 = 1e6 as i64; // 1 milliseconds.. - - let dut = Timer::new(&clock, &context, period); + let dut = Timer::new(&clock, &context, period, create_dummy_callback()); assert!(dut.is_ok()); } @@ -208,8 +221,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period: i64 = 1e6 as i64; // 1 milliseconds. - - let dut = Timer::new(&clock, &context, period); + let dut = Timer::new(&clock, &context, period, create_dummy_callback()); assert!(dut.is_ok()); let dut = dut.unwrap(); let period_result = dut.get_timer_period_ns(); @@ -223,8 +235,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period: i64 = 1e6 as i64; // 1 milliseconds. - - let dut = Timer::new(&clock, &context, period); + let dut = Timer::new(&clock, &context, period, create_dummy_callback()); assert!(dut.is_ok()); let dut = dut.unwrap(); assert!(dut.is_canceled().is_ok()); @@ -241,8 +252,7 @@ mod tests { let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 2e6 as i64; // 2 milliseconds. let sleep_period_ms = time::Duration::from_millis(1); - - let dut = Timer::new(&clock, &context, period_ns); + let dut = Timer::new(&clock, &context, period_ns, create_dummy_callback()); assert!(dut.is_ok()); let dut = dut.unwrap(); thread::sleep(sleep_period_ms); @@ -261,7 +271,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 2e6 as i64; // 2 milliseconds. - let dut = Timer::new(&clock, &context, period_ns); + let dut = Timer::new(&clock, &context, period_ns, create_dummy_callback()); assert!(dut.is_ok()); let dut = dut.unwrap(); let time_until_next_call = dut.time_until_next_call(); @@ -280,7 +290,7 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 2e6 as i64; // 2 milliseconds. - let dut = Timer::new(&clock, &context, period_ns).unwrap(); + let dut = Timer::new(&clock, &context, period_ns, create_dummy_callback()).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!(elapsed < tolerance, "elapsed before reset: {}", elapsed); thread::sleep(time::Duration::from_millis(1)); @@ -295,21 +305,17 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 1e6 as i64; // 1 millisecond. - let dut = Timer::new(&clock, &context, period_ns).unwrap(); + let dut = Timer::new(&clock, &context, period_ns, create_dummy_callback()).unwrap(); let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!(elapsed < tolerance, "elapsed before reset: {}", elapsed); - thread::sleep(time::Duration::from_micros(1500)); - let elapsed = period_ns - dut.time_until_next_call().unwrap(); assert!( elapsed > 1500000i64, "time_until_next_call before call: {}", elapsed ); - assert!(dut.call().is_ok()); - let elapsed = dut.time_until_next_call().unwrap(); assert!( elapsed < 500000i64, @@ -323,27 +329,24 @@ mod tests { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 1e6 as i64; // 1 millisecond. - let dut = Timer::new(&clock, &context, period_ns).unwrap(); - + let dut = Timer::new(&clock, &context, period_ns, create_dummy_callback()).unwrap(); let is_ready = dut.is_ready(); assert!(is_ready.is_ok()); assert!(!is_ready.unwrap()); - thread::sleep(time::Duration::from_micros(1100)); - let is_ready = dut.is_ready(); assert!(is_ready.is_ok()); assert!(is_ready.unwrap()); } #[test] - fn test_callback_wip() { + fn test_callback() { let clock = Clock::steady(); let context = Context::new(vec![]).unwrap(); let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::new_with_callback( + let dut = Timer::new( &clock, &context, period_ns, @@ -361,7 +364,7 @@ mod tests { let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::new_with_callback( + let dut = Timer::new( &clock, &context, period_ns, @@ -379,7 +382,7 @@ mod tests { let period_ns: i64 = 1e6 as i64; // 1 millisecond. let foo = Arc::new(Mutex::new(0i64)); let foo_callback = foo.clone(); - let dut = Timer::new_with_callback( + let dut = Timer::new( &clock, &context, period_ns, diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 108a17561..f19ebb8cb 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -466,6 +466,7 @@ impl WaitSet { mod tests { use super::*; use crate::clock::Clock; + use crate::timer::TimerCallback; #[test] fn traits() { @@ -496,7 +497,8 @@ mod tests { let context = Context::new([])?; let clock = Clock::steady(); let period: i64 = 1e6 as i64; // 1 millisecond. - let timer = Arc::new(Timer::new(&clock, &context, period)?); + let callback: Option = Some(Box::new(move |_| {})); + let timer = Arc::new(Timer::new(&clock, &context, period, callback)?); let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; wait_set.add_timer(timer.clone())?; @@ -512,7 +514,8 @@ mod tests { let context = Context::new([])?; let clock = Clock::steady(); let period: i64 = 1e6 as i64; // 1 millisecond. - let timer = Arc::new(Timer::new(&clock, &context, period)?); + let callback: Option = Some(Box::new(move |_| {})); + let timer = Arc::new(Timer::new(&clock, &context, period, callback)?); let mut wait_set = WaitSet::new(0, 0, 1, 0, 0, 0, &context)?; wait_set.add_timer(timer.clone())?; From 30a6717ce9374907087203aa51231ceb4c6cf2eb Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Sun, 1 Dec 2024 14:20:34 +0100 Subject: [PATCH 26/26] Makes rustdoc pass in the examples. Signed-off-by: Agustin Alba Chicar --- examples/rclrs_timer_demo/src/rclrs_timer_demo.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs index 4889534eb..279858be3 100644 --- a/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs +++ b/examples/rclrs_timer_demo/src/rclrs_timer_demo.rs @@ -18,8 +18,8 @@ impl SimpleTimerNode { /// Creates a node and a timer with a callback. /// /// The callback will simply print to stdout: - /// "Drinking 🧉 for the th time every

nanoseconds." - /// where is the iteration callback counter and

is the period of the timer. + /// "Drinking 🧉 for the xth time every p nanoseconds." + /// where x is the iteration callback counter and p is the period of the timer. fn new(context: &Context, timer_period_ns: i64) -> Result { let node = create_node(context, "simple_timer_node")?; let count: Arc> = Arc::new(Mutex::new(0));