diff --git a/rclrs/build.rs b/rclrs/build.rs index 3ac6dc414..91d0b190f 100644 --- a/rclrs/build.rs +++ b/rclrs/build.rs @@ -35,6 +35,7 @@ fn main() { } }; + println!("cargo:rustc-check-cfg=cfg(ros_distro, values(\"humble\", \"iron\", \"jazzy\", \"rolling\"))"); println!("cargo:rustc-cfg=ros_distro=\"{ros_distro}\""); let mut builder = bindgen::Builder::default() diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 90c8fbd3c..524169bb2 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -6,7 +6,7 @@ use std::{ vec::Vec, }; -use crate::{rcl_bindings::*, RclrsError, ToResult}; +use crate::{rcl_bindings::*, LoggingLifecycle, RclrsError, ToResult}; /// This is locked whenever initializing or dropping any middleware entity /// because we have found issues in RCL and some RMW implementations that @@ -56,6 +56,10 @@ unsafe impl Send for rcl_context_t {} /// - middleware-specific data, e.g. the domain participant in DDS /// - the allocator used (left as the default by `rclrs`) /// +/// The context also configures the rcl_logging_* layer to allow publication to /rosout +/// (as well as the terminal). TODO: This behaviour should be configurable using an +/// "auto logging initialise" flag as per rclcpp and rclpy. +/// pub struct Context { pub(crate) handle: Arc, } @@ -68,6 +72,10 @@ pub struct Context { /// bindings in this library. pub(crate) struct ContextHandle { pub(crate) rcl_context: Mutex, + /// This ensures that logging does not get cleaned up until after this ContextHandle + /// has dropped. + #[allow(unused)] + logging: Arc, } impl Context { @@ -143,9 +151,16 @@ impl Context { // Move the check after the last fini() ret?; } + + // TODO: "Auto set-up logging" is forced but should be configurable as per rclcpp and rclpy + // SAFETY: We created this context a moment ago and verified that it is valid. + // No other conditions are needed. + let logging = unsafe { LoggingLifecycle::configure(&rcl_context)? }; + Ok(Self { handle: Arc::new(ContextHandle { rcl_context: Mutex::new(rcl_context), + logging, }), }) } diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs index 4924b36cb..3a22c6da8 100644 --- a/rclrs/src/lib.rs +++ b/rclrs/src/lib.rs @@ -11,6 +11,7 @@ mod clock; mod context; mod error; mod executor; +mod logging; mod node; mod parameter; mod publisher; @@ -38,6 +39,7 @@ pub use clock::*; pub use context::*; pub use error::*; pub use executor::*; +pub use logging::*; pub use node::*; pub use parameter::*; pub use publisher::*; diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs new file mode 100644 index 000000000..5143ae35c --- /dev/null +++ b/rclrs/src/logging.rs @@ -0,0 +1,667 @@ +// Copyright (c) 2019 Sequence Planner +// SPDX-License-Identifier: Apache-2.0 AND MIT +// Adapted from https://github.com/sequenceplanner/r2r/blob/89cec03d07a1496a225751159cbc7bfb529d9dd1/r2r/src/utils.rs +// Further adapted from https://github.com/mvukov/rules_ros2/pull/371 + +use std::{ + collections::HashMap, + ffi::CString, + sync::{Mutex, OnceLock}, +}; + +use crate::{rcl_bindings::*, ENTITY_LIFECYCLE_MUTEX}; + +mod logging_configuration; +pub(crate) use logging_configuration::*; + +mod log_params; +pub use log_params::*; + +mod logger; +pub use logger::*; + +/// log a message to rosout +/// +/// # Examples +/// +/// ``` +/// use rclrs::{log, ToLogParams}; +/// use std::sync::Mutex; +/// use std::time::Duration; +/// use std::env; +/// +/// let context = rclrs::Context::new(env::args()).unwrap(); +/// let node = rclrs::Node::new(&context, "test_node").unwrap(); +/// +/// log!(node.debug(), "Simple debug message"); +/// let some_variable = 43; +/// log!(node.debug(), "Formatted debug message: {some_variable}"); +/// log!(node.fatal(), "Fatal message from {}", node.name()); +/// log!(node.warn().once(), "Only log this the first time"); +/// log!( +/// node +/// .error() +/// .skip_first() +/// .throttle(Duration::from_millis(1000)), +/// "Noisy error that we expect the first time" +/// ); +/// +/// let count = 0; +/// log!( +/// node +/// .info() +/// .throttle(Duration::from_millis(1000)) +/// .only_if(count % 10 == 0), +/// "Manually constructed LogConditions", +/// ); +/// ``` +/// +/// All of the above examples will also work with the severity-specific log macros, +/// but any severity that gets passed in will be overridden: +/// - [`log_debug`][crate::log_debug] +/// - [`log_info`][crate::log_info] +/// - [`log_warn`][crate::log_warn] +/// - [`log_error`][crate::log_error] +/// - [`log_fatal`][crate::log_fatal] +/// +/// # Panics +/// +/// It is theoretically possible for the call to panic if the Mutex used for the throttling is +/// poisoned although this should never happen. +#[macro_export] +macro_rules! log { + // The variable args captured by the $(, $($args:tt)*)?)) code allows us to omit (or include) + // formatting parameters in the simple message case, e.g. to write + // ``` + // log_error!(, "Log with no params"); // OR + // log_error!(, "Log with useful info {}", error_reason); + ($to_log_params: expr, $($args:tt)*) => {{ + // Adding these use statements here due an issue like this one: + // https://github.com/intellij-rust/intellij-rust/issues/9853 + // 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, 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 + // macro early without causing the calling function to also try to + // return early. + (|| { + let params = $crate::ToLogParams::to_log_params($to_log_params); + + if !params.get_user_filter() { + // The user filter is telling us not to log this time, so exit + // before doing anything else. + return; + } + + let mut first_time = false; + static REMEMBER_FIRST_TIME: Once = Once::new(); + REMEMBER_FIRST_TIME.call_once(|| first_time = true); + + let logger_name = params.get_logger_name(); + let severity = params.get_severity(); + + match params.get_occurence() { + // Create the static variables here so we get a per-instance static + $crate::LogOccurrence::Once => { + if first_time { + $crate::log_unconditional!(severity, logger_name, $($args)*); + } + // Since we've already logged once, we should never log again, + // so just exit right now. + return; + } + $crate::LogOccurrence::SkipFirst => { + if first_time { + // This is the first time that we're seeing this log, and we + // were told to skip the first one, so just exit right away. + return; + } + } + // Do nothing + $crate::LogOccurrence::All => (), + } + + // If we have a throttle duration then check if we're inside or outside + // of that interval. + let throttle = params.get_throttle(); + if throttle > std::time::Duration::ZERO { + 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; + } + } + } + } + } + + // All filters have been checked, so go ahead and publish the message + $crate::log_unconditional!(severity, logger_name, $($args)*); + })(); + }}; +} + +/// Debug log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_debug { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.debug(), $($args)*); + }} +} + +/// Info log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_info { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.info(), $($args)*); + }} +} + +/// Warning log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_warn { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.warn(), $($args)*); + }} +} + +/// Error log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_error { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.error(), $($args)*); + }} +} + +/// Fatal log message. See [`log`] for usage. +#[macro_export] +macro_rules! log_fatal { + ($to_log_params: expr, $($args:tt)*) => {{ + let log_params = $crate::ToLogParams::to_log_params($to_log_params); + $crate::log!(log_params.fatal(), $($args)*); + }} +} + +/// A logging mechanism that does not have any conditions: It will definitely +/// publish the log. This is only meant for internal use, but needs to be exported +/// in order for [`log`] to work. +#[doc(hidden)] +#[macro_export] +macro_rules! log_unconditional { + ($severity: expr, $logger_name: expr, $($args:tt)*) => {{ + use std::{ffi::CString, sync::OnceLock}; + + // Only allocate a CString for the function name once per call to this macro. + static FUNCTION_NAME: OnceLock = OnceLock::new(); + let function_name = FUNCTION_NAME.get_or_init(|| { + // This call to function! is nested within two layers of closures, + // so we need to strip away those suffixes or else users will be + // misled. If we ever restructure these macros or if Rust changes + // the way it names closures, this implementation detail may need to + // change. + let function_name = $crate::function!() + .strip_suffix("::{{closure}}::{{closure}}") + .unwrap(); + + CString::new(function_name).unwrap_or( + CString::new("").unwrap() + ) + }); + + // Only allocate a CString for the file name once per call to this macro. + static FILE_NAME: OnceLock = OnceLock::new(); + let file_name = FILE_NAME.get_or_init(|| { + CString::new(file!()).unwrap_or( + CString::new("").unwrap() + ) + }); + + // We have to allocate a CString for the message every time because the + // formatted data may have changed. We could consider having an alternative + // macro for string literals that only allocates once, but it not obvious + // how to guarantee that the user only passes in an unchanging string literal. + match CString::new(std::fmt::format(format_args!($($args)*))) { + Ok(message) => { + // SAFETY: impl_log is actually completely safe to call, we just + // mark it as unsafe to discourage downstream users from calling it + unsafe { $crate::impl_log($severity, $logger_name, &message, &function_name, &file_name, line!()) }; + } + Err(err) => { + let message = CString::new(format!( + "Unable to format log message into a valid c-string. Error: {}", err + )).unwrap(); + + // SAFETY: impl_log is actually completely safe to call, we just + // mark it as unsafe to discourage downstream users from calling it + unsafe { + $crate::impl_log( + $crate::LogSeverity::Error, + &$crate::LoggerName::Unvalidated("logger"), + &message, + &function_name, + &file_name, + line!(), + ); + } + } + } + }} +} + +/// Calls the underlying rclutils logging function +/// Don't call this directly, use the logging macros instead, i.e. [`log`]. +/// +/// SAFETY: This function is not actually unsafe, but it is not meant to be part of the public +/// API, so we mark it as unsafe to discourage users from trying to use it. They should use +/// one of the of log! macros instead. We can't make it private because it needs to be used +/// by exported macros. +#[doc(hidden)] +pub unsafe fn impl_log( + severity: LogSeverity, + logger_name: &LoggerName, + message: &CString, + function: &CString, + file: &CString, + line: u32, +) { + // We use a closure here because there are several different points in this + // function where we may need to run this same logic. + let send_log = |severity: LogSeverity, logger_name: &CString, message: &CString| { + let location = rcutils_log_location_t { + function_name: function.as_ptr(), + file_name: file.as_ptr(), + line_number: line as usize, + }; + + static FORMAT_STRING: OnceLock = OnceLock::new(); + let format_string = FORMAT_STRING.get_or_init(|| CString::new("%s").unwrap()); + + let severity = severity.as_native(); + + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + + #[cfg(test)] + { + // If we are compiling for testing purposes, when the default log + // output handler is being used we need to use the format_string, + // but when our custom log output handler is being used we need to + // pass the raw message string so that it can be viewed by the + // custom log output handler, allowing us to use it for test assertions. + if log_handler::is_using_custom_handler() { + // We are using the custom log handler that is only used during + // logging tests, so pass the raw message as the format string. + unsafe { + // SAFETY: The global mutex is locked as _lifecycle + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + message.as_ptr(), + ); + } + } else { + // We are using the normal log handler so call rcutils_log the normal way. + unsafe { + // SAFETY: The global mutex is locked as _lifecycle + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format_string.as_ptr(), + message.as_ptr(), + ); + } + } + } + + #[cfg(not(test))] + { + unsafe { + // SAFETY: The global mutex is locked as _lifecycle + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format_string.as_ptr(), + message.as_ptr(), + ); + } + } + }; + + match logger_name { + LoggerName::Validated(c_name) => { + // The logger name is pre-validated, so just go ahead and use it. + send_log(severity, c_name, message); + } + LoggerName::Unvalidated(str_name) => { + // The name was not validated before being passed in. + // + // We maintain a hashmap of previously validated loggers so + // we don't need to reallocate the CString on every log instance. + // This is done inside of the function impl_log instead of in a macro + // so that this map is global for the entire application. + static NAME_MAP: OnceLock>> = OnceLock::new(); + let name_map = NAME_MAP.get_or_init(Default::default); + + { + // We need to keep name_map locked while we call send_log, but + // we also need to make sure it gets unlocked right afterward + // if the condition fails, otherwise this function would + // deadlock on itself when handling the error case of the logger + // name being invalid. So we keep name_map_guard in this extra + // scope to isolate its lifespan. + let name_map_guard = name_map.lock().unwrap(); + if let Some(c_name) = name_map_guard.get(*str_name) { + // The map name has been used before, so we just use the + // pre-existing CString + send_log(severity, c_name, message); + + // We return right away because the remainder of this + // function just allocates and validates a new CString for + // the logger name. + return; + } + } + + // The unvalidated logger name has not been used before, so we need + // to convert it and add it to the name_map now. + let c_name = match CString::new(str_name.to_string()) { + Ok(c_name) => c_name, + Err(_) => { + static INVALID_MSG: OnceLock = OnceLock::new(); + let invalid_msg = INVALID_MSG.get_or_init(|| { + CString::new( + "Failed to convert logger name into a c-string. \ + Check for null terminators inside the string.", + ) + .unwrap() + }); + static INTERNAL_LOGGER_NAME: OnceLock = OnceLock::new(); + let internal_logger_name = + INTERNAL_LOGGER_NAME.get_or_init(|| CString::new("logger").unwrap()); + send_log(severity, internal_logger_name, invalid_msg); + return; + } + }; + + send_log(severity, &c_name, message); + name_map + .lock() + .unwrap() + .insert(str_name.to_string(), c_name); + } + } +} + +/// Used internally by logging macros to get the name of the function that called the +/// logging macro. This is not meant for public use, but we need to export it so the +/// other exported macros can use it. We should remove it if an official function! macro +/// is ever offered. +#[macro_export] +#[doc(hidden)] +macro_rules! function { + () => {{ + fn f() {} + fn type_name_of(_: T) -> &'static str { + std::any::type_name::() + } + let name = type_name_of(f); + name.strip_suffix("::f").unwrap() + }}; +} + +#[cfg(test)] +mod tests { + use crate::{log_handler::*, test_helpers::*, *}; + use std::sync::Mutex; + + #[test] + fn test_logging_macros() -> Result<(), RclrsError> { + // This test ensures that strings which are being sent to the logger are + // being sanitized correctly. Rust generally and our logging macro in + // particular do not use C-style formatting strings, but rcutils expects + // to receive C-style formatting strings alongside variadic arguments + // that describe how to fill in the formatting. + // + // If we pass the final string into rcutils as the format with no + // variadic arguments, then it may trigger a crash or undefined behavior + // if the message happens to contain any % symbols. In particular %n + // will trigger a crash when no variadic arguments are given because it + // attempts to write to a buffer. If no buffer is given, a seg fault + // happens. + log!("please do not crash", "%n"); + + let graph = construct_test_graph("test_logging_macros")?; + + let log_collection: Arc>>> = Arc::new(Mutex::new(Vec::new())); + let inner_log_collection = log_collection.clone(); + + log_handler::set_logging_output_handler(move |log_entry: log_handler::LogEntry| { + inner_log_collection + .lock() + .unwrap() + .push(log_entry.into_owned()); + }) + .unwrap(); + + let last_logger_name = || { + log_collection + .lock() + .unwrap() + .last() + .unwrap() + .logger_name + .clone() + }; + + let last_message = || { + log_collection + .lock() + .unwrap() + .last() + .unwrap() + .message + .clone() + }; + + let last_location = || { + log_collection + .lock() + .unwrap() + .last() + .unwrap() + .location + .clone() + }; + + let last_severity = || log_collection.lock().unwrap().last().unwrap().severity; + + let count_message = |message: &str| { + let mut count = 0; + for log in log_collection.lock().unwrap().iter() { + if log.message == message { + count += 1; + } + } + count + }; + + let node = graph.node1; + + log!(&*node, "Logging with node dereference"); + assert_eq!(last_logger_name(), node.logger().name()); + assert_eq!(last_message(), "Logging with node dereference"); + assert_eq!(last_severity(), LogSeverity::Info); + assert_eq!( + last_location().function_name, + "rclrs::logging::tests::test_logging_macros", + ); + + for _ in 0..10 { + log!(node.once(), "Logging once"); + } + assert_eq!(count_message("Logging once"), 1); + assert_eq!(last_severity(), LogSeverity::Info); + + log!(node.logger(), "Logging with node logger"); + assert_eq!(last_message(), "Logging with node logger"); + assert_eq!(last_severity(), LogSeverity::Info); + + log!(node.debug(), "Debug from node"); + // The default severity level is Info so we should not see the last message + assert_ne!(last_message(), "Debug from node"); + assert_ne!(last_severity(), LogSeverity::Debug); + + log!(node.info(), "Info from node"); + assert_eq!(last_message(), "Info from node"); + assert_eq!(last_severity(), LogSeverity::Info); + + log!(node.warn(), "Warn from node"); + assert_eq!(last_message(), "Warn from node"); + assert_eq!(last_severity(), LogSeverity::Warn); + + log!(node.error(), "Error from node"); + assert_eq!(last_message(), "Error from node"); + assert_eq!(last_severity(), LogSeverity::Error); + + log!(node.fatal(), "Fatal from node"); + assert_eq!(last_message(), "Fatal from node"); + assert_eq!(last_severity(), LogSeverity::Fatal); + + log_debug!(node.logger(), "log_debug macro"); + log_info!(node.logger(), "log_info macro"); + log_warn!(node.logger(), "log_warn macro"); + log_error!(node.logger(), "log_error macro"); + log_fatal!(node.logger(), "log_fatal macro"); + + log!(node.only_if(false), "This should not be logged",); + log!(node.only_if(true), "This should be logged",); + + for i in 0..3 { + log!(node.warn().skip_first(), "Formatted warning #{}", i); + } + assert_eq!(count_message("Formatted warning #0"), 0); + assert_eq!(count_message("Formatted warning #1"), 1); + assert_eq!(count_message("Formatted warning #2"), 1); + + node.logger().set_level(LogSeverity::Debug).unwrap(); + log_debug!(node.logger(), "This debug message appears"); + assert_eq!(last_message(), "This debug message appears"); + assert_eq!(last_severity(), LogSeverity::Debug); + + node.logger().set_level(LogSeverity::Info).unwrap(); + log_debug!(node.logger(), "This debug message does not appear"); + assert_ne!(last_message(), "This debug message does not appear"); + + log!("custom logger name", "message for custom logger"); + assert_eq!(last_logger_name(), "custom logger name"); + assert_eq!(last_message(), "message for custom logger"); + + for _ in 0..3 { + log!( + "custom logger name once".once(), + "one-time message for custom logger", + ); + } + assert_eq!(last_logger_name(), "custom logger name once"); + assert_eq!(last_severity(), LogSeverity::Info); + assert_eq!(count_message("one-time message for custom logger"), 1); + + for _ in 0..3 { + log!( + "custom logger name skip".error().skip_first(), + "error for custom logger", + ); + } + assert_eq!(last_logger_name(), "custom logger name skip"); + assert_eq!(last_severity(), LogSeverity::Error); + assert_eq!(count_message("error for custom logger"), 2); + + // 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(()) + } + + #[test] + fn test_function_macro() { + assert_eq!(function!(), "rclrs::logging::tests::test_function_macro"); + } +} diff --git a/rclrs/src/logging/log_params.rs b/rclrs/src/logging/log_params.rs new file mode 100644 index 000000000..4b67edc50 --- /dev/null +++ b/rclrs/src/logging/log_params.rs @@ -0,0 +1,293 @@ +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. +#[derive(Debug, Clone, Copy)] +pub struct LogParams<'a> { + /// The name of the logger + logger_name: LoggerName<'a>, + /// The severity of the logging instance. + severity: LogSeverity, + /// Specify when a log message should be published (See[`LoggingOccurrence`] above) + occurs: LogOccurrence, + /// Specify a publication throttling interval for the message. A value of ZERO (0) indicates that the + /// message should not be throttled. Otherwise, the message will only be published once the specified + /// interval has elapsed. This field is typically used to limit the output from high-frequency messages, + /// 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, +} + +impl<'a> LogParams<'a> { + /// Create a set of default log parameters, given the name of a logger + pub fn new(logger_name: LoggerName<'a>) -> Self { + Self { + logger_name, + severity: Default::default(), + occurs: Default::default(), + throttle: Duration::new(0, 0), + throttle_clock: Default::default(), + only_if: true, + } + } + + /// Get the logger name + pub fn get_logger_name(&self) -> &LoggerName { + &self.logger_name + } + + /// Get the severity of the log + pub fn get_severity(&self) -> LogSeverity { + self.severity + } + + /// Get the occurrence + pub fn get_occurence(&self) -> LogOccurrence { + self.occurs + } + + /// Get the throttle interval duration + pub fn get_throttle(&self) -> Duration { + 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 + } +} + +/// Methods for defining the behavior of a logging instance. +/// +/// This trait is implemented by Logger, Node, and anything that a `&str` can be +/// [borrowed][1] from, such as string literals (`"my_str"`), [`String`], or +/// [`Cow`][2]. +/// +/// [1]: Borrow +/// [2]: std::borrow::Cow +pub trait ToLogParams<'a>: Sized { + /// Convert the object into LogParams with default settings + fn to_log_params(self) -> LogParams<'a>; + + /// The logging should only happen once + fn once(self) -> LogParams<'a> { + self.occurs(LogOccurrence::Once) + } + + /// The first time the logging happens, we should skip it + fn skip_first(self) -> LogParams<'a> { + self.occurs(LogOccurrence::SkipFirst) + } + + /// Set the occurrence behavior of the log instance + fn occurs(self, occurs: LogOccurrence) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.occurs = occurs; + params + } + + /// Set a throttling interval during which this log will not publish. A value + /// of zero will never block the message from being published, and this is the + /// default behavior. + /// + /// A negative duration is not valid, but will be treated as a zero duration. + fn throttle(self, throttle: Duration) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.throttle = throttle; + 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. + /// + /// Other factors may prevent the log from being published if a `true` is + /// passed in, such as `ToLogParams::throttle` or `ToLogParams::once` + /// filtering the log. + fn only_if(self, only_if: bool) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.only_if = only_if; + params + } + + /// Log as a debug message. + fn debug(self) -> LogParams<'a> { + self.severity(LogSeverity::Debug) + } + + /// Log as an informative message. This is the default, so you don't + /// generally need to use this. + fn info(self) -> LogParams<'a> { + self.severity(LogSeverity::Info) + } + + /// Log as a warning message. + fn warn(self) -> LogParams<'a> { + self.severity(LogSeverity::Warn) + } + + /// Log as an error message. + fn error(self) -> LogParams<'a> { + self.severity(LogSeverity::Error) + } + + /// Log as a fatal message. + fn fatal(self) -> LogParams<'a> { + self.severity(LogSeverity::Fatal) + } + + /// Set the severity for this instance of logging. The default value will be + /// [`LogSeverity::Info`]. + fn severity(self, severity: LogSeverity) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.severity = severity; + params + } +} + +/// This is used to borrow a logger name which might be validated (e.g. came +/// from a [`Logger`][1] struct) or not (e.g. a user-defined `&str`). If an +/// unvalidated logger name is used with one of the logging macros, we will log +/// an error about it, and the original log message will be logged with the +/// default logger. +/// +/// [1]: crate::Logger +#[derive(Debug, Clone, Copy)] +pub enum LoggerName<'a> { + /// The logger name is already available as a valid CString + Validated(&'a CString), + /// The logger name has not been validated yet + Unvalidated(&'a str), +} + +/// Logging severity. +// +// TODO(@mxgrey): Consider whether this is redundant with RCUTILS_LOG_SEVERITY. +// Perhaps we can customize the output of bindgen to automatically change the name +// of RCUTILS_LOG_SEVERITY to just LogSeverity so it's more idiomatic and then +// export it from the rclrs module. +#[doc(hidden)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)] +pub enum LogSeverity { + /// Use the severity level of the parent logger (or the root logger if the + /// current logger has no parent) + Unset, + /// For messages that are not needed outside of debugging. + Debug, + /// For messages that provide useful information about the state of the + /// application. + Info, + /// For messages that indicate something unusual or unintended might have happened. + Warn, + /// For messages that indicate an error has occurred which may cause the application + /// to misbehave. + Error, + /// For messages that indicate an error has occurred which is so severe that the + /// application should terminate because it cannot recover. + /// + /// Using this severity level will not automatically cause the application to + /// terminate, the application developer must decide how to do that on a + /// case-by-case basis. + Fatal, +} + +impl LogSeverity { + pub(super) fn as_native(&self) -> RCUTILS_LOG_SEVERITY { + use crate::rcl_bindings::rcl_log_severity_t::*; + match self { + LogSeverity::Unset => RCUTILS_LOG_SEVERITY_UNSET, + LogSeverity::Debug => RCUTILS_LOG_SEVERITY_DEBUG, + LogSeverity::Info => RCUTILS_LOG_SEVERITY_INFO, + LogSeverity::Warn => RCUTILS_LOG_SEVERITY_WARN, + LogSeverity::Error => RCUTILS_LOG_SEVERITY_ERROR, + LogSeverity::Fatal => RCUTILS_LOG_SEVERITY_FATAL, + } + } + + /// This is only used by the log output handler during testing, so it will + /// not be compiled when testing is not configured + #[cfg(test)] + pub(crate) fn from_native(native: i32) -> Self { + use crate::rcl_bindings::rcl_log_severity_t::*; + match native { + _ if native == RCUTILS_LOG_SEVERITY_UNSET as i32 => LogSeverity::Unset, + _ if native == RCUTILS_LOG_SEVERITY_DEBUG as i32 => LogSeverity::Debug, + _ if native == RCUTILS_LOG_SEVERITY_INFO as i32 => LogSeverity::Info, + _ if native == RCUTILS_LOG_SEVERITY_WARN as i32 => LogSeverity::Warn, + _ if native == RCUTILS_LOG_SEVERITY_ERROR as i32 => LogSeverity::Error, + _ if native == RCUTILS_LOG_SEVERITY_FATAL as i32 => LogSeverity::Fatal, + _ => panic!("Invalid native severity received: {}", native), + } + } +} + +impl Default for LogSeverity { + fn default() -> Self { + Self::Info + } +} + +/// Specify when a log message should be published +#[derive(Debug, Clone, Copy)] +pub enum LogOccurrence { + /// Every message will be published if all other conditions are met + All, + /// The message will only be published on the first occurrence (Note: no other conditions apply) + Once, + /// The log message will not be published on the first occurrence, but will be published on + /// each subsequent occurrence (assuming all other conditions are met) + 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 + } +} + +// Anything that we can borrow a string from can be used as if it's a logger and +// turned into LogParams +impl<'a, T: Borrow> ToLogParams<'a> for &'a T { + fn to_log_params(self) -> LogParams<'a> { + LogParams::new(LoggerName::Unvalidated(self.borrow())) + } +} + +impl<'a> ToLogParams<'a> for &'a str { + fn to_log_params(self) -> LogParams<'a> { + LogParams::new(LoggerName::Unvalidated(self)) + } +} + +impl<'a> ToLogParams<'a> for LogParams<'a> { + fn to_log_params(self) -> LogParams<'a> { + self + } +} diff --git a/rclrs/src/logging/logger.rs b/rclrs/src/logging/logger.rs new file mode 100644 index 000000000..30770919c --- /dev/null +++ b/rclrs/src/logging/logger.rs @@ -0,0 +1,111 @@ +use std::{borrow::Borrow, ffi::CString, sync::Arc}; + +use crate::{ + rcl_bindings::{rcutils_logging_set_default_logger_level, rcutils_logging_set_logger_level}, + LogParams, LogSeverity, LoggerName, RclrsError, ToLogParams, ToResult, ENTITY_LIFECYCLE_MUTEX, +}; + +/// Logger can be passed in as the first argument into one of the [logging][1] +/// macros provided by rclrs. When passing it into one of the logging macros, +/// you can optionally apply any of the methods from [`ToLogParams`] to tweak +/// the logging behavior. +/// +/// You can obtain a Logger in the following ways: +/// - Calling [`Node::logger`][2] to get the logger of a Node +/// - Using [`Logger::create_child`] to create a child logger for an existing logger +/// - Using [`Logger::new`] to create a new logger with any name that you'd like +/// - Using [`Logger::default()`] to access the global default logger +/// +/// Note that if you are passing the Logger of a Node into one of the logging macros, +/// then you can choose to simply pass in `&node` instead of `node.logger()`. +/// +/// [1]: crate::log +/// [2]: crate::Node::logger +#[derive(Debug, Clone, PartialEq, Eq, PartialOrd, Ord)] +pub struct Logger { + name: Arc, + c_name: Arc, +} + +impl Logger { + /// Create a new logger with the given name. + pub fn new(name: impl Borrow) -> Result { + 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).to_owned(), + err, + }); + } + }; + + Ok(Self { + name, + c_name: Arc::new(c_name), + }) + } + + /// Create a new logger which will be a child of this logger. + /// + /// If the name of this logger is `parent_name`, then the name for the new + /// child will be '"parent_name.child_name"`. + /// + /// If this is the default logger (whose name is an empty string), then the + /// name for the new child will simply be the value in `child_name`. + pub fn create_child(&self, child_name: impl Borrow) -> Result { + if self.name.is_empty() { + Self::new(child_name) + } else { + Self::new(format!("{}.{}", &self.name, child_name.borrow())) + } + } + + /// Get the name of this logger + pub fn name(&self) -> &str { + &self.name + } + + /// Set the severity level of this logger + pub fn set_level(&self, severity: LogSeverity) -> Result<(), RclrsError> { + // SAFETY: The precondition are: + // - we are passing in a valid CString, which is already taken care of during construction of the Logger + // - the severity level is a valid value, which is guaranteed by the rigid enum definition + // - not thread-safe, so we lock the global mutex before calling this + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + rcutils_logging_set_logger_level(self.c_name.as_ptr(), severity.as_native() as i32).ok() + } + } + + /// Set the severity level of the default logger which acts as the root ancestor + /// of all other loggers. + pub fn set_default_level(severity: LogSeverity) { + // SAFETY: The preconditions are: + // - the severity level is a valid value, which is guaranteed by the rigid enum definition + // - not thread-safe, so we lock the global mutex before calling this + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + rcutils_logging_set_default_logger_level(severity.as_native() as i32); + } + } +} + +impl<'a> ToLogParams<'a> for &'a Logger { + fn to_log_params(self) -> LogParams<'a> { + LogParams::new(LoggerName::Validated(&self.c_name)) + } +} + +impl Default for Logger { + fn default() -> Self { + Self::new("").unwrap() + } +} + +impl std::hash::Hash for Logger { + fn hash(&self, state: &mut H) { + self.name.hash(state); + } +} diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs new file mode 100644 index 000000000..1012608ec --- /dev/null +++ b/rclrs/src/logging/logging_configuration.rs @@ -0,0 +1,251 @@ +use std::sync::{Arc, Mutex, OnceLock, Weak}; + +use crate::{rcl_bindings::*, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX}; + +struct LoggingConfiguration { + lifecycle: Mutex>, +} + +pub(crate) struct LoggingLifecycle; + +impl LoggingLifecycle { + fn new(args: &rcl_arguments_t) -> Result { + // SAFETY: + // * Lock the mutex as we cannot guarantee that rcl_* functions are protecting their global variables + // * This is only called by Self::configure, which requires that a valid context was passed to it + // * No other preconditions for calling this function + unsafe { + let allocator = rcutils_get_default_allocator(); + let _lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + rcl_logging_configure(args, &allocator).ok()?; + } + Ok(Self) + } + + /// SAFETY: Ensure rcl_context_t is valid before passing it in. + pub(crate) unsafe fn configure( + context: &rcl_context_t, + ) -> Result, RclrsError> { + static CONFIGURATION: OnceLock = OnceLock::new(); + let configuration = CONFIGURATION.get_or_init(|| LoggingConfiguration { + lifecycle: Mutex::new(Weak::new()), + }); + + let mut lifecycle = configuration.lifecycle.lock().unwrap(); + if let Some(arc_lifecycle) = lifecycle.upgrade() { + return Ok(arc_lifecycle); + } + let arc_lifecycle = Arc::new(LoggingLifecycle::new(&context.global_arguments)?); + *lifecycle = Arc::downgrade(&arc_lifecycle); + Ok(arc_lifecycle) + } +} + +impl Drop for LoggingLifecycle { + fn drop(&mut self) { + let _lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + rcl_logging_fini(); + } + } +} + +#[cfg(test)] +pub(crate) mod log_handler { + //! This module provides a way to customize how log output is handled. For + //! now we are not making this a private API and are only using it for tests + //! in rclrs. We can consider making it public in the future, but we should + //! put more consideration into the API before doing so, and more crucially + //! we need to figure out a way to process C-style formatting strings with + //! a [`va_list`] from inside of Rust, which seems to be very messy. + + use std::{ + borrow::Cow, + ffi::CStr, + sync::{ + atomic::{AtomicBool, Ordering}, + OnceLock, + }, + }; + + use crate::{rcl_bindings::*, LogSeverity, ENTITY_LIFECYCLE_MUTEX}; + + /// Global variable that allows a custom log handler to be set. This log + /// handler will be applied throughout the entire application and cannot be + /// replaced with a different custom log handler. If you want to be able to + /// change the log handler over the lifetime of your application, you should + /// design your own custom handler with an Arc> inside that allows + /// its own behavior to be modified. + static LOGGING_OUTPUT_HANDLER: OnceLock = OnceLock::new(); + + /// Alias for an arbitrary log handler that is compatible with raw rcl types + pub(crate) type RawLogHandler = Box< + dyn Fn( + *const rcutils_log_location_t, // location + std::os::raw::c_int, // severity + *const std::os::raw::c_char, // logger name + rcutils_time_point_value_t, // timestamp + *const std::os::raw::c_char, // format + *mut va_list, // formatting arguments + ) + + 'static + + Send + + Sync, + >; + + /// This is an idiomatic representation of all the information for a log entry + #[derive(Clone)] + pub(crate) struct LogEntry<'a> { + pub(crate) location: LogLocation<'a>, + pub(crate) severity: LogSeverity, + pub(crate) logger_name: Cow<'a, str>, + pub(crate) timestamp: i64, + pub(crate) message: Cow<'a, str>, + } + + impl<'a> LogEntry<'a> { + /// Change the entry from something borrowed into something owned + pub(crate) fn into_owned(self) -> LogEntry<'static> { + LogEntry { + location: self.location.into_owned(), + severity: self.severity, + logger_name: Cow::Owned(self.logger_name.into_owned()), + timestamp: self.timestamp, + message: Cow::Owned(self.message.into_owned()), + } + } + } + + /// Rust-idiomatic representation of the location of a log + #[derive(Debug, Clone)] + pub(crate) struct LogLocation<'a> { + pub function_name: Cow<'a, str>, + pub file_name: Cow<'a, str>, + pub line_number: usize, + } + + impl<'a> LogLocation<'a> { + pub(crate) fn into_owned(self) -> LogLocation<'static> { + LogLocation { + function_name: Cow::Owned(self.function_name.into_owned()), + file_name: Cow::Owned(self.file_name.into_owned()), + line_number: self.line_number, + } + } + } + + #[derive(Debug)] + pub(crate) struct OutputHandlerAlreadySet; + + static USING_CUSTOM_HANDLER: OnceLock = OnceLock::new(); + + /// Set an idiomatic log hander + pub(crate) fn set_logging_output_handler( + handler: impl Fn(LogEntry) + 'static + Send + Sync, + ) -> Result<(), OutputHandlerAlreadySet> { + let raw_handler = Box::new( + move |raw_location: *const rcutils_log_location_t, + raw_severity: std::os::raw::c_int, + raw_logger_name: *const std::os::raw::c_char, + raw_timestamp: rcutils_time_point_value_t, + raw_format: *const std::os::raw::c_char, + // NOTE: In the rclrs logging test we are choosing to format + // the full message in advance when using the custom handler, + // so the format field always contains the finished formatted + // message. Therefore we can just ignore the raw formatting + // arguments. + _raw_formatting_arguments: *mut va_list| { + unsafe { + // NOTE: We use .unwrap() extensively inside this function because + // it only gets used during tests. We should reconsider this if + // we ever make this public. + let location = LogLocation { + function_name: Cow::Borrowed( + CStr::from_ptr((*raw_location).function_name) + .to_str() + .unwrap(), + ), + file_name: Cow::Borrowed( + CStr::from_ptr((*raw_location).file_name).to_str().unwrap(), + ), + line_number: (*raw_location).line_number, + }; + let severity = LogSeverity::from_native(raw_severity); + let logger_name = + Cow::Borrowed(CStr::from_ptr(raw_logger_name).to_str().unwrap()); + let timestamp: i64 = raw_timestamp; + let message = Cow::Borrowed(CStr::from_ptr(raw_format).to_str().unwrap()); + handler(LogEntry { + location, + severity, + logger_name, + timestamp, + message, + }); + } + }, + ); + + set_raw_logging_output_handler(raw_handler) + } + + /// Set the logging output handler directly + pub(crate) fn set_raw_logging_output_handler( + handler: RawLogHandler, + ) -> Result<(), OutputHandlerAlreadySet> { + LOGGING_OUTPUT_HANDLER + .set(handler) + .map_err(|_| OutputHandlerAlreadySet)?; + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: + // - We have locked the global mutex + rcutils_logging_set_output_handler(Some(rclrs_logging_output_handler)); + } + + USING_CUSTOM_HANDLER + .get_or_init(|| AtomicBool::new(false)) + .store(true, Ordering::Release); + Ok(()) + } + + pub(crate) fn is_using_custom_handler() -> bool { + USING_CUSTOM_HANDLER + .get_or_init(|| AtomicBool::new(false)) + .load(Ordering::Acquire) + } + + /// This function exists so that we can give a raw function pointer to + /// rcutils_logging_set_output_handler, which is needed by its API. + extern "C" fn rclrs_logging_output_handler( + location: *const rcutils_log_location_t, + severity: std::os::raw::c_int, + logger_name: *const std::os::raw::c_char, + timestamp: rcutils_time_point_value_t, + message: *const std::os::raw::c_char, + logging_output: *mut va_list, + ) { + let handler = LOGGING_OUTPUT_HANDLER.get().unwrap(); + (*handler)( + location, + severity, + logger_name, + timestamp, + message, + logging_output, + ); + } + + /// Reset the logging output handler to the default one + pub(crate) fn reset_logging_output_handler() { + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + unsafe { + // SAFETY: The global mutex is locked. No other precondition is + // required. + rcutils_logging_set_output_handler(Some(rcl_logging_multiple_output_handler)); + } + USING_CUSTOM_HANDLER + .get_or_init(|| AtomicBool::new(false)) + .store(false, Ordering::Release); + } +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..b51b59817 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -5,7 +5,7 @@ use std::{ ffi::CStr, fmt, os::raw::c_char, - sync::{Arc, Mutex, Weak}, + sync::{atomic::AtomicBool, Arc, Mutex, Weak}, vec::Vec, }; @@ -13,10 +13,10 @@ use rosidl_runtime_rs::Message; pub use self::{builder::*, graph::*}; use crate::{ - rcl_bindings::*, Client, ClientBase, Clock, Context, ContextHandle, GuardCondition, - ParameterBuilder, ParameterInterface, ParameterVariant, Parameters, Publisher, QoSProfile, - RclrsError, Service, ServiceBase, Subscription, SubscriptionBase, SubscriptionCallback, - TimeSource, ENTITY_LIFECYCLE_MUTEX, + 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, }; // SAFETY: The functions accessing this type, including drop(), shouldn't care about the thread @@ -44,7 +44,7 @@ unsafe impl Send for rcl_node_t {} /// It's a good idea for node names in the same executable to be unique. /// /// ## Remapping -/// The namespace and name given when creating the node can be overriden through the command line. +/// The namespace and name given when creating the node can be overridden through the command line. /// In that sense, the parameters to the node creation functions are only the _default_ namespace and /// name. /// See also the [official tutorial][1] on the command line arguments for ROS nodes, and the @@ -66,23 +66,48 @@ pub struct Node { time_source: TimeSource, parameter: ParameterInterface, pub(crate) handle: Arc, + logger: Logger, } /// This struct manages the lifetime of an `rcl_node_t`, and accounts for its /// dependency on the lifetime of its `rcl_context_t` by ensuring that this /// dependency is [dropped after][1] the `rcl_node_t`. +/// Note: we capture the rcl_node_t returned from rcl_get_zero_initialized_node() +/// to guarantee that the node handle exists until we drop the NodeHandle +/// instance. This addresses an issue where previously the address of the variable +/// in the builder.rs was being used, and whose lifespan was (just) shorter than the +/// NodeHandle instance. /// /// [1]: pub(crate) struct NodeHandle { pub(crate) rcl_node: Mutex, pub(crate) context_handle: Arc, + /// 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) + /// we need to make sure that we do not call rcl_node_fini on it while + /// dropping the NodeHandle, so we keep track of successful initialization + /// with this variable. + /// + /// We may be able to restructure this in the future when we no longer need + /// to support Humble. + pub(crate) initialized: AtomicBool, } impl Drop for NodeHandle { fn drop(&mut self) { + if !self.initialized.load(std::sync::atomic::Ordering::Acquire) { + // The node was not correctly initialized, e.g. it was created with + // an invalid name, so we must not try to finalize it or else we + // will get undefined behavior. + return; + } + let _context_lock = self.context_handle.rcl_context.lock().unwrap(); let mut rcl_node = self.rcl_node.lock().unwrap(); let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. unsafe { rcl_node_fini(&mut *rcl_node) }; @@ -440,6 +465,17 @@ impl Node { pub fn builder(context: &Context, node_name: &str) -> NodeBuilder { NodeBuilder::new(context, node_name) } + + /// Get the logger associated with this Node. + pub fn logger(&self) -> &Logger { + &self.logger + } +} + +impl<'a> ToLogParams<'a> for &'a Node { + fn to_log_params(self) -> LogParams<'a> { + self.logger().to_log_params() + } } // Helper used to implement call_string_getter(), but also used to get the FQN in the Node::new() @@ -514,4 +550,21 @@ mod tests { Ok(()) } + + #[test] + fn test_logger_name() -> Result<(), RclrsError> { + // Use helper to create 2 nodes for us + let graph = construct_test_graph("test_logger_name")?; + + assert_eq!( + graph.node1.logger().name(), + "test_logger_name.graph_test_node_1" + ); + assert_eq!( + graph.node2.logger().name(), + "test_logger_name.graph_test_node_2" + ); + + Ok(()) + } } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..1e7a9fc63 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -1,11 +1,12 @@ use std::{ - ffi::CString, - sync::{Arc, Mutex}, + ffi::{CStr, CString}, + sync::{atomic::AtomicBool, Arc, Mutex}, }; use crate::{ - rcl_bindings::*, ClockType, Context, ContextHandle, Node, NodeHandle, ParameterInterface, - QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, QOS_PROFILE_CLOCK, + rcl_bindings::*, ClockType, Context, ContextHandle, Logger, Node, NodeHandle, + ParameterInterface, QoSProfile, RclrsError, TimeSource, ToResult, ENTITY_LIFECYCLE_MUTEX, + QOS_PROFILE_CLOCK, }; /// A builder for creating a [`Node`][1]. @@ -276,8 +277,13 @@ impl NodeBuilder { let rcl_node_options = self.create_rcl_node_options()?; let rcl_context = &mut *self.context.rcl_context.lock().unwrap(); - // SAFETY: Getting a zero-initialized value is always safe. - let mut rcl_node = unsafe { rcl_get_zero_initialized_node() }; + let handle = Arc::new(NodeHandle { + // SAFETY: Getting a zero-initialized value is always safe. + rcl_node: Mutex::new(unsafe { rcl_get_zero_initialized_node() }), + context_handle: Arc::clone(&self.context), + initialized: AtomicBool::new(false), + }); + unsafe { // SAFETY: // * The rcl_node is zero-initialized as mandated by this function. @@ -287,7 +293,7 @@ impl NodeBuilder { // global variables in the rmw implementation being unsafely modified during cleanup. let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); rcl_node_init( - &mut rcl_node, + &mut *handle.rcl_node.lock().unwrap(), node_name.as_ptr(), node_namespace.as_ptr(), rcl_context, @@ -296,10 +302,10 @@ impl NodeBuilder { .ok()?; }; - let handle = Arc::new(NodeHandle { - rcl_node: Mutex::new(rcl_node), - context_handle: Arc::clone(&self.context), - }); + handle + .initialized + .store(true, std::sync::atomic::Ordering::Release); + let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); ParameterInterface::new( @@ -308,6 +314,26 @@ impl NodeBuilder { &rcl_context.global_arguments, )? }; + + let logger_name = { + let rcl_node = handle.rcl_node.lock().unwrap(); + let logger_name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; + if logger_name_raw_ptr.is_null() { + "" + } else { + // SAFETY: rcl_node_get_logger_name will either return a nullptr + // if the provided node was invalid or provide a valid null-terminated + // const char* if the provided node was valid. We have already + // verified that it is not a nullptr. We are also preventing the + // pointed-to value from being modified while we view it by locking + // the mutex of rcl_node while we view it. This means all the + // safety conditions of CStr::from_ptr are met. + unsafe { CStr::from_ptr(logger_name_raw_ptr) } + .to_str() + .unwrap_or("") + } + }; + let node = Arc::new(Node { handle, clients_mtx: Mutex::new(vec![]), @@ -318,7 +344,9 @@ impl NodeBuilder { .clock_qos(self.clock_qos) .build(), parameter, + logger: Logger::new(logger_name)?, }); + node.time_source.attach_node(&node); if self.start_parameter_services { node.parameter.create_services(&node)?; diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..639a38e38 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -487,12 +487,29 @@ mod tests { .unwrap(); let node_name = "test_publisher_names_and_types"; let node = Node::new(&context, node_name).unwrap(); - // Test that the graph has no publishers + + let check_rosout = |topics: HashMap>| { + // rosout shows up in humble and iron, even if the graph is empty + #[cfg(any(ros_distro = "humble", ros_distro = "iron"))] + { + assert_eq!(topics.len(), 1); + assert_eq!( + topics.get("/rosout").unwrap().first().unwrap(), + "rcl_interfaces/msg/Log" + ); + } + + // rosout does not automatically show up in jazzy when the graph is empty + #[cfg(any(ros_distro = "jazzy", ros_distro = "rolling"))] + { + assert_eq!(topics.len(), 0); + } + }; + let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") .unwrap(); - - assert_eq!(names_and_topics.len(), 0); + check_rosout(names_and_topics); let num_publishers = node.count_publishers("/test").unwrap(); @@ -535,10 +552,8 @@ mod tests { assert_eq!(names_and_topics.len(), 0); - // Test that the graph has no topics let names_and_topics = node.get_topic_names_and_types().unwrap(); - - assert_eq!(names_and_topics.len(), 0); + check_rosout(names_and_topics); } #[test] diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index c7153ce01..3c49993b3 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -10,7 +10,9 @@ pub use value::*; use crate::vendor::rcl_interfaces::msg::rmw::{ParameterType, ParameterValue as RmwParameterValue}; -use crate::{call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError}; +use crate::{ + call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, +}; use std::{ collections::{btree_map::Entry, BTreeMap}, fmt::Debug, @@ -760,6 +762,7 @@ impl ParameterInterface { global_arguments: &rcl_arguments_t, ) -> Result { let override_map = unsafe { + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); let fqn = call_string_getter_with_rcl_node(rcl_node, rcl_node_get_fully_qualified_name); resolve_parameter_overrides(&fqn, node_arguments, global_arguments)? }; @@ -882,7 +885,7 @@ mod tests { String::from("declared_int:=10"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); // Declaring a parameter with a different type than what was overridden should return an // error @@ -940,7 +943,7 @@ mod tests { String::from("non_declared_string:='param'"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); let overridden_int = node .declare_parameter("declared_int") @@ -1090,7 +1093,7 @@ mod tests { String::from("declared_int:=10"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); // If a parameter was set as an override and as an undeclared parameter, the undeclared // value should get priority node.use_undeclared_parameters() @@ -1112,7 +1115,7 @@ mod tests { String::from("declared_int:=10"), ]) .unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); { // Setting a parameter with an override let param = node @@ -1158,7 +1161,7 @@ mod tests { #[test] fn test_parameter_ranges() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); // Setting invalid ranges should fail let range = ParameterRange { lower: Some(10), @@ -1286,7 +1289,7 @@ mod tests { #[test] fn test_readonly_parameters() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); let param = node .declare_parameter("int_param") .default(100) @@ -1313,7 +1316,7 @@ mod tests { #[test] fn test_preexisting_value_error() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); node.use_undeclared_parameters() .set("int_param", 100) .unwrap(); @@ -1366,7 +1369,7 @@ mod tests { #[test] fn test_optional_parameter_apis() { let ctx = Context::new([]).unwrap(); - let node = create_node(&ctx, "param_test_node").unwrap(); + let node = create_node(&ctx, &format!("param_test_node_{}", line!())).unwrap(); node.declare_parameter::("int_param") .optional() .unwrap(); diff --git a/rclrs/src/rcl_wrapper.h b/rclrs/src/rcl_wrapper.h index fe97cb4e5..ececf491f 100644 --- a/rclrs/src/rcl_wrapper.h +++ b/rclrs/src/rcl_wrapper.h @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..0be0c07ec 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -149,7 +149,11 @@ mod tests { #[test] fn time_source_default_clock() { - let node = create_node(&Context::new([]).unwrap(), "test_node").unwrap(); + let node = create_node( + &Context::new([]).unwrap(), + &format!("time_source_test_node_{}", line!()), + ) + .unwrap(); // Default clock should be above 0 (use_sim_time is default false) assert!(node.get_clock().now().nsec > 0); } @@ -162,7 +166,7 @@ mod tests { String::from("use_sim_time:=true"), ]) .unwrap(); - let node = create_node(&ctx, "test_node").unwrap(); + let node = create_node(&ctx, &format!("time_source_test_node_{}", line!())).unwrap(); // Default sim time value should be 0 (no message received) assert_eq!(node.get_clock().now().nsec, 0); }