diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index 7bbfb423e..5143ae35c 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -81,7 +81,7 @@ macro_rules! log { // Note: that issue appears to be specific to jetbrains intellisense however, // observed same/similar behaviour with rust-analyzer/rustc use std::sync::{Once, OnceLock, Mutex}; - use std::time::SystemTime; + use std::time::{SystemTime, Instant}; // We wrap the functional body of the macro inside of a closure which // we immediately trigger. This allows us to use `return` to exit the @@ -128,19 +128,64 @@ macro_rules! log { // of that interval. let throttle = params.get_throttle(); if throttle > std::time::Duration::ZERO { - static LAST_LOG_TIME: OnceLock> = OnceLock::new(); - let last_log_time = LAST_LOG_TIME.get_or_init(|| { - Mutex::new(std::time::SystemTime::now()) - }); - - if !first_time { - let now = std::time::SystemTime::now(); - let mut previous = last_log_time.lock().unwrap(); - if now >= *previous + throttle { - *previous = now; - } else { - // We are still inside the throttle interval, so just exit here. - return; + match params.get_throttle_clock() { + $crate::ThrottleClock::SteadyTime => { + static LAST_LOG_STEADY_TIME: OnceLock> = OnceLock::new(); + let last_log_time = LAST_LOG_STEADY_TIME.get_or_init(|| { + Mutex::new(Instant::now()) + }); + + if !first_time { + let now = Instant::now(); + let mut previous = last_log_time.lock().unwrap(); + if now >= *previous + throttle { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit here. + return; + } + } + } + $crate::ThrottleClock::SystemTime => { + static LAST_LOG_SYSTEM_TIME: OnceLock> = OnceLock::new(); + let last_log_time = LAST_LOG_SYSTEM_TIME.get_or_init(|| { + Mutex::new(SystemTime::now()) + }); + + if !first_time { + let now = SystemTime::now(); + let mut previous = last_log_time.lock().unwrap(); + if now >= *previous + throttle { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit here. + return; + } + } + } + $crate::ThrottleClock::Clock(clock) => { + static LAST_LOG_CLOCK_TIME: OnceLock> = OnceLock::new(); + let last_log_time = LAST_LOG_CLOCK_TIME.get_or_init(|| { + Mutex::new(clock.now()) + }); + + if !first_time { + let now = clock.now(); + let mut previous = last_log_time.lock().unwrap(); + + let new_interval = !now.compare_with( + &(previous.clone() + throttle), + |now, interval| now < interval, + ) + .is_some_and(|eval| eval); + + if new_interval { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit here. + return; + } + } } } } @@ -592,8 +637,26 @@ mod tests { assert_eq!(last_severity(), LogSeverity::Error); assert_eq!(count_message("error for custom logger"), 2); - reset_logging_output_handler(); + // Test whether throttling works correctly with a ROS clock + let (clock, source) = Clock::with_source(); + source.set_ros_time_override(0); + + for i in 0..15 { + log!( + "logger" + .throttle(Duration::from_nanos(10)) + .throttle_clock(ThrottleClock::Clock(&clock)), + "custom clock throttled message", + ); + source.set_ros_time_override(i); + } + // The throttle interval is 10ns and the loop shifted the time from 0ns + // to 14ns, triggering the log macro once per nanosecond. That means we + // should see two messages in the log. + assert_eq!(count_message("custom clock throttled message"), 2); + + reset_logging_output_handler(); Ok(()) } diff --git a/rclrs/src/logging/log_params.rs b/rclrs/src/logging/log_params.rs index 79197cd98..4b67edc50 100644 --- a/rclrs/src/logging/log_params.rs +++ b/rclrs/src/logging/log_params.rs @@ -1,4 +1,4 @@ -use crate::rcl_bindings::RCUTILS_LOG_SEVERITY; +use crate::{rcl_bindings::RCUTILS_LOG_SEVERITY, Clock}; use std::{borrow::Borrow, ffi::CString, time::Duration}; /// These parameters determine the behavior of an instance of logging. @@ -16,6 +16,8 @@ pub struct LogParams<'a> { /// e.g. if `log!(logger.throttle(Duration::from_secs(1)), "message");` is called every 10ms, it will /// nevertheless only be published once per second. throttle: Duration, + /// Specify a clock to use for throttling. By default this will be [`ThrottleClock::SteadyTime`]. + throttle_clock: ThrottleClock<'a>, /// The log message will only published if the specified expression evaluates to true only_if: bool, } @@ -28,6 +30,7 @@ impl<'a> LogParams<'a> { severity: Default::default(), occurs: Default::default(), throttle: Duration::new(0, 0), + throttle_clock: Default::default(), only_if: true, } } @@ -52,6 +55,11 @@ impl<'a> LogParams<'a> { self.throttle } + /// Get the throttle clock + pub fn get_throttle_clock(&self) -> ThrottleClock<'a> { + self.throttle_clock + } + /// Get the arbitrary filter set by the user pub fn get_user_filter(&self) -> bool { self.only_if @@ -98,6 +106,13 @@ pub trait ToLogParams<'a>: Sized { params } + /// Set the clock that will be used to control [throttling][Self::throttle]. + fn throttle_clock(self, clock: ThrottleClock<'a>) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.throttle_clock = clock; + params + } + /// The log will not be published if a `false` expression is passed into /// this function. /// @@ -239,6 +254,18 @@ pub enum LogOccurrence { SkipFirst, } +/// This parameter can specify a type of clock for a logger to use when throttling. +#[derive(Debug, Default, Clone, Copy)] +pub enum ThrottleClock<'a> { + /// Use [`std::time::Instant`] as a clock. + #[default] + SteadyTime, + /// Use [`std::time::SystemTime`] as a clock. + SystemTime, + /// Use some [`Clock`] as a clock. + Clock(&'a Clock), +} + impl Default for LogOccurrence { fn default() -> Self { Self::All diff --git a/rclrs/src/logging/logger.rs b/rclrs/src/logging/logger.rs index 2d55f2d7a..30770919c 100644 --- a/rclrs/src/logging/logger.rs +++ b/rclrs/src/logging/logger.rs @@ -1,4 +1,4 @@ -use std::{borrow::Borrow, ffi::CString}; +use std::{borrow::Borrow, ffi::CString, sync::Arc}; use crate::{ rcl_bindings::{rcutils_logging_set_default_logger_level, rcutils_logging_set_logger_level}, @@ -21,26 +21,30 @@ use crate::{ /// /// [1]: crate::log /// [2]: crate::Node::logger +#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] pub struct Logger { - name: Box, - c_name: CString, + name: Arc, + c_name: Arc, } impl Logger { /// Create a new logger with the given name. pub fn new(name: impl Borrow) -> Result { - let name: Box = name.borrow().into(); - let c_name = match CString::new(name.clone().into_string()) { + let name: Arc = name.borrow().into(); + let c_name = match CString::new((*name).to_owned()) { Ok(c_name) => c_name, Err(err) => { return Err(RclrsError::StringContainsNul { - s: name.into_string(), + s: (*name).to_owned(), err, }); } }; - Ok(Self { name, c_name }) + Ok(Self { + name, + c_name: Arc::new(c_name), + }) } /// Create a new logger which will be a child of this logger. @@ -99,3 +103,9 @@ impl Default for Logger { Self::new("").unwrap() } } + +impl std::hash::Hash for Logger { + fn hash(&self, state: &mut H) { + self.name.hash(state); + } +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 5afa1fe03..b51b59817 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -82,7 +82,7 @@ pub struct Node { pub(crate) struct NodeHandle { pub(crate) rcl_node: Mutex, pub(crate) context_handle: Arc, - /// In the humbe distro, rcl is sensitive to the address of the rcl_node_t + /// In the humble distro, rcl is sensitive to the address of the rcl_node_t /// object being moved (this issue seems to be gone in jazzy), so we need /// to initialize the rcl_node_t in-place inside this struct. In the event /// that the initialization fails (e.g. it was created with an invalid name)