From 239dab35d1bb8fa76c084d3d6765f1d2b1e473f4 Mon Sep 17 00:00:00 2001 From: geoff-imdex Date: Fri, 11 Oct 2024 09:12:50 +0800 Subject: [PATCH 01/11] Initial logging implementation --- rclrs/src/context.rs | 25 +++ rclrs/src/lib.rs | 2 + rclrs/src/logging.rs | 334 ++++++++++++++++++++++++++++++++++++++++ rclrs/src/node.rs | 28 +++- rclrs/src/rcl_wrapper.h | 1 + 5 files changed, 389 insertions(+), 1 deletion(-) create mode 100644 rclrs/src/logging.rs diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index 90c8fbd3c..e2a1bc10d 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -36,6 +36,9 @@ impl Drop for rcl_context_t { rcl_shutdown(self); rcl_context_fini(self); } + + // SAFETY: No preconditions for rcl_logging_fini + rcl_logging_fini(); } } } @@ -56,6 +59,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, } @@ -142,6 +149,24 @@ impl Context { rcl_init_options_fini(&mut rcl_init_options).ok()?; // 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: + // * Lock the mutex as we cannot guarantee that rcl_* functions are protecting their global variables + // * Context is validated before we reach this point + // * No other preconditions for calling this function + let ret = { + let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // Note: shadowing the allocator above. The rcutils_get_default_allocator provides us with mechanisms for allocating and freeing + // memory, e.g. calloc/free. As these are function pointers and will be the same each time we call the method, it is safe to + // perform this shadowing + // Alternate is to call rcl_init_options_get_allocator however, we've freed the allocator above and restructuring the call + // sequence is unnecessarily complex + let allocator = rcutils_get_default_allocator(); + + rcl_logging_configure(&rcl_context.global_arguments, &allocator).ok() + }; + ret? } Ok(Self { handle: Arc::new(ContextHandle { 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..e3efd4a4a --- /dev/null +++ b/rclrs/src/logging.rs @@ -0,0 +1,334 @@ +// 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::ffi::CString; +use std::sync::Mutex; +use std::time::Duration; + +use crate::rcl_bindings::*; + +// Used to protect calls to rcl/rcutils in case those functions manipulate global variables +static LOG_GUARD: Mutex<()> = Mutex::new(()); + +/// Calls the underlying rclutils logging function +/// Don't call this directly, use the logging macros instead. +/// +/// # Panics +/// +/// This function might panic in the following scenarios: +/// - A logger_name is provided that is not a valid c-string, e.g. contains extraneous null characters +/// - The user-supplied "msg" is not a valid c-string, e.g. contains extraneous null characters +/// - When called if the lock is already held by the current thread. +/// - If the construction of CString objects used to create the log output fail, +/// although, this highly unlikely to occur in most cases +#[doc(hidden)] +pub fn log(msg: &str, logger_name: &str, file: &str, line: u32, severity: LogSeverity) { + // currently not possible to get function name in rust. + // see https://github.com/rust-lang/rfcs/pull/2818 + let function = CString::new("").unwrap(); + let file = CString::new(file).unwrap(); + let location = rcutils_log_location_t { + function_name: function.as_ptr(), + file_name: file.as_ptr(), + line_number: line as usize, + }; + let format = CString::new("%s").unwrap(); + let logger_name = CString::new(logger_name) + .expect("Logger name is a valid c style string, e.g. check for extraneous null characters"); + let message = CString::new(msg) + .expect("Valid c style string required, e.g. check for extraneous null characters"); + let severity = severity.to_native(); + + let _guard = LOG_GUARD.lock().unwrap(); + // SAFETY: Global variables are protected via LOG_GUARD, no other preconditions are required + unsafe { + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format.as_ptr(), + message.as_ptr(), + ); + } +} + +/// Logging severity +#[doc(hidden)] +pub enum LogSeverity { + Unset, + Debug, + Info, + Warn, + Error, + Fatal, +} + +impl LogSeverity { + fn to_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, + } + } +} + +#[derive(Debug)] +/// Specify when a log message should be published +pub enum LoggingOccurrence { + /// The log message will always be published (assuming all other conditions are met) + Always, + /// 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, +} + +/// Specify conditions that must be met for a log message to be published +/// +/// The struct provides the following convenience functions to construct conditions that match +/// behaviour available in the `rclcpp` and `rclpy` libraries. +/// +/// When will my log message be output? +/// +/// - `Once`: A message with the [`LoggingOccurrence::Once`] value will be published once only +/// regardless of any other conditions +/// - `SkipFirst`: A message with the [`LoggingOccurrence::SkipFirst`] value will never be published +/// on the first encounter regardless of any other conditions. After the first +/// encounter, the behaviour is identical to the [`LoggingOccurrence::Always`] setting. +/// - `Always`: The log message will be output if all additional conditions are true: +/// - The current time + the `publish_interval` > the last time the message was published. +/// - The default value for `publish_interval` is 0, i.e. the interval check will always pass +/// - The `log_if_true` expression evaluates to TRUE. +/// - The default value for the `log_if_true` field is TRUE. +pub struct LogConditions { + /// Specify when a log message should be published (See[`LoggingOccurrence`] above) + pub occurs: LoggingOccurrence, + /// Specify the publication interval of the message. A value of ZERO (0) indicates that the + /// message should be published every time, 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. instead + /// of publishing a log message every 10 milliseconds, the `publish_interval` can be configured + /// such that the message is published every 10 seconds. + pub publish_interval: Duration, + /// The log message will only published if the specified expression evaluates to true + pub log_if_true: bool, +} + +impl LogConditions { + /// Default construct an instance + pub fn new() -> Self { + Self { + occurs: LoggingOccurrence::Always, + publish_interval: Duration::ZERO, + log_if_true: true, + } + } + + /// Only publish this message the first time it is encountered + pub fn once() -> Self { + Self { + occurs: LoggingOccurrence::Once, + publish_interval: Duration::ZERO, + log_if_true: true, + } + } + + /// Do not publish the message the first time it is encountered + pub fn skip_first() -> Self { + Self { + occurs: LoggingOccurrence::SkipFirst, + publish_interval: Duration::ZERO, + log_if_true: true, + } + } + + /// Do not publish the first time this message is encountered and publish + /// at the specified `publish_interval` thereafter + pub fn skip_first_throttle(publish_interval: Duration) -> Self { + Self { + occurs: LoggingOccurrence::SkipFirst, + publish_interval, + log_if_true: true, + } + } + + /// Throttle the message to the supplied publish_interval + /// e.g. set `publish_interval` to 1000ms to limit publishing to once a second + pub fn throttle(publish_interval: Duration) -> Self { + Self { + occurs: LoggingOccurrence::Always, + publish_interval, + log_if_true: true, + } + } + + /// Permitting logging if the supplied expression evaluates to true + /// Uses default LoggingOccurrence (Always) and publish_interval (no throttling) + pub fn log_if_true(log_if_true: bool) -> Self { + Self { + occurs: LoggingOccurrence::Always, + publish_interval: Duration::ZERO, + log_if_true, + } + } +} + +/// log_with_conditions +/// Helper macro to log a message using the ROS2 RCUTILS library +/// +/// The macro supports two general calling formats: +/// 1. Plain message string e.g. as per println! macro +/// 2. With calling conditions that provide some restrictions on the output of the message +/// (see [`LogConditions`] above) +/// +/// It is expected that, typically, the macro will be called using one of the wrapper macros, +/// e.g. log_debug!, etc, however, it is relatively straight forward to call the macro directly +/// if you really want to. +/// +/// # Examples +/// +/// ``` +/// log_debug!(&node.name(), "Simple message"); +/// log_debug!(&node.name(), "Simple message {some_variable}"); +/// log_fatal!(&node.name(), "Simple message from {}", node.name()); +/// log_warn!(&node.name(), LogConditions::once(), "Only log this the first time"); +/// log_error!(&node.name(), +/// LogConditions::skip_first_throttle(Duration::from_millis(1000)), +/// "Noisy error that we expect the first time"); +/// +/// log_info!(&node.name(), LogConditions { occurs: LoggingOccurrence::Always, +/// publish_interval: Duration::from_millis(1000), +/// log_if_true: count % 10 == 0, }, +/// "Manually constructed LogConditions"); +/// ``` +/// +/// # Panics +/// +/// It is theoretically possible for the call to panic if the Mutex used for the throttling is +/// poisoned although this should not be possible. +#[macro_export] +macro_rules! log_with_conditions { + // The variable args captured by the $(, $($args:tt)*)?)) code allows us to omit (or include) + // 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); + ($severity: expr, $logger_name: expr, $msg_start: literal $(, $($args:tt)*)?) => { + $crate::log(&std::fmt::format(format_args!($msg_start, $($($args)*)?)), $logger_name, file!(), line!(), $severity); + }; + ($severity: expr, $logger_name: expr, $conditions: 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; + use std::time::SystemTime; + + let log_conditions: $crate::LogConditions = $conditions; + let mut allow_logging = true; + match log_conditions.occurs { + // Create the static variables here so we get a per-instance static + $crate::LoggingOccurrence::Once => { + static LOG_ONCE: std::sync::Once = std::sync::Once::new(); + LOG_ONCE.call_once(|| { + $crate::log(&std::fmt::format(format_args!($($args)*)), $logger_name, file!(), line!(), $severity); + }); + allow_logging = false; + } + $crate::LoggingOccurrence::SkipFirst => { + // Noop, just make sure we exit the first time... + static SKIP_FIRST: std::sync::Once = std::sync::Once::new(); + SKIP_FIRST.call_once(|| { + // Only disable logging the first time + allow_logging = false; + }); + + } + // Drop out + $crate::LoggingOccurrence::Always => (), + } + + // If we have a throttle period AND logging has not already been disabled due to SkipFirst settings + if log_conditions.publish_interval > std::time::Duration::ZERO && allow_logging { + let mut ignore_first_timeout = false; + // Need to initialise to a constant + static LAST_LOG_TIME: Mutex = Mutex::new(std::time::SystemTime::UNIX_EPOCH); + + static INIT_LAST_LOG_TIME: std::sync::Once = std::sync::Once::new(); + // Set the last log time to "now", but let us log the message the first time we hit this + // code, i.e. initial behaviour is expired. + // Note: If this is part of a SkipFirst macro call, we will only hit this code on the second iteration. + INIT_LAST_LOG_TIME.call_once(|| { + let mut last_log_time = LAST_LOG_TIME.lock().unwrap(); + *last_log_time = std::time::SystemTime::now(); + ignore_first_timeout = true; + }); + + let mut last_log_time = LAST_LOG_TIME.lock().unwrap(); + if std::time::SystemTime::now() >= *last_log_time + log_conditions.publish_interval { + // Update our time stamp + *last_log_time = std::time::SystemTime::now(); + } + else if !ignore_first_timeout { + // Timer has not expired (and this is not the first time through here) + allow_logging = false; + } + } + + // At this point we've validated the skip/throttle operations, and we now check that any + // expression supplied also evaluates to true, e.g. if timer has expired but expression is + // false, we won't print + if (allow_logging && log_conditions.log_if_true) + { + $crate::log(&std::fmt::format(format_args!($($args)*)), $logger_name, file!(), line!(), $severity); + } + }; +} + +/// Debug log message. +#[macro_export] +macro_rules! log_debug { + ($logger_name: expr, $($args:tt)*) => {{ + $crate::log_with_conditions!($crate::LogSeverity::Debug, $logger_name, $($args)*); + }} +} + +/// Info log message. +#[macro_export] +macro_rules! log_info { + ($logger_name: expr, $($args:tt)*) => {{ + $crate::log_with_conditions!($crate::LogSeverity::Info, $logger_name, $($args)*); + }} +} + +/// Warning log message. +#[macro_export] +macro_rules! log_warn { + ($logger_name: expr, $($args:tt)*) => {{ + $crate::log_with_conditions!($crate::LogSeverity::Warn, $logger_name, $($args)*); + }} +} + +/// Error log message. +#[macro_export] +macro_rules! log_error { + ($logger_name: expr, $($args:tt)*) => {{ + $crate::log_with_conditions!($crate::LogSeverity::Error, $logger_name, $($args)*); + }} +} + +/// Fatal log message. +#[macro_export] +macro_rules! log_fatal { + ($logger_name: expr, $($args:tt)*) => {{ + $crate::log_with_conditions!($crate::LogSeverity::Fatal, $logger_name, $($args)*); + }} +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 97684d6bc..ae19a5e4f 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -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 @@ -440,6 +440,21 @@ impl Node { pub fn builder(context: &Context, node_name: &str) -> NodeBuilder { NodeBuilder::new(context, node_name) } + + /// Returns the logger name of the node. + pub fn logger_name(&self) -> &str { + let rcl_node = self.handle.rcl_node.lock().unwrap(); + // SAFETY: No pre-conditions for this function + let name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; + if name_raw_ptr.is_null() { + return ""; + } + // SAFETY: The returned CStr is immediately converted to a string slice, + // so the lifetime is no issue. The ptr is valid as per the documentation + // of rcl_node_get_logger_name. + let name_cstr = unsafe { CStr::from_ptr(name_raw_ptr) }; + name_cstr.to_str().unwrap_or("") + } } // Helper used to implement call_string_getter(), but also used to get the FQN in the Node::new() @@ -514,4 +529,15 @@ mod tests { Ok(()) } + + #[test] + fn test_logger_name() -> Result<(), RclrsError> { + // Use helper to create 2 nodes for us + let graph = construct_test_graph("test_topics_graph")?; + + assert_eq!(graph.node1.logger_name(), "graph_test_node_1"); + assert_eq!(graph.node2.logger_name(), "graph_test_node_2"); + + Ok(()) + } } 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 From 6ad25da679891bb8c3f702868c0bdd6b2a6e3fd1 Mon Sep 17 00:00:00 2001 From: Geoff Talbot Date: Mon, 21 Oct 2024 01:32:07 +0000 Subject: [PATCH 02/11] * Moved logging_fini to follow correct call sequence based on rclcpp --- rclrs/src/context.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index e2a1bc10d..e9b7ad9d5 100644 --- a/rclrs/src/context.rs +++ b/rclrs/src/context.rs @@ -34,11 +34,12 @@ impl Drop for rcl_context_t { // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. rcl_shutdown(self); + + // SAFETY: No preconditions for rcl_logging_fini + rcl_logging_fini(); + rcl_context_fini(self); } - - // SAFETY: No preconditions for rcl_logging_fini - rcl_logging_fini(); } } } From d5a6bc4fb62cd153b7b9017abd19853c11282b4a Mon Sep 17 00:00:00 2001 From: geoff-imdex Date: Wed, 23 Oct 2024 12:19:59 +0800 Subject: [PATCH 03/11] Primary change to fix builder's node variable going out of scope --- rclrs/src/client.rs | 6 +++--- rclrs/src/logging.rs | 10 ++++++++++ rclrs/src/node.rs | 24 ++++++++++++++++++------ rclrs/src/node/builder.rs | 4 ++-- rclrs/src/node/graph.rs | 14 +++++++------- rclrs/src/publisher.rs | 4 ++-- rclrs/src/service.rs | 4 ++-- rclrs/src/subscription.rs | 4 ++-- 8 files changed, 46 insertions(+), 24 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index b308f1de2..f0f8f05e5 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -43,7 +43,7 @@ impl Drop for ClientHandle { // 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_client_fini(rcl_client, &mut *rcl_node); + rcl_client_fini(rcl_client, &mut **rcl_node); } } } @@ -115,7 +115,7 @@ where unsafe { rcl_client_init( &mut rcl_client, - &*rcl_node, + &**rcl_node, type_support, topic_c_string.as_ptr(), &client_options, @@ -263,7 +263,7 @@ where pub fn service_is_ready(&self) -> Result { let mut is_ready = false; let client = &mut *self.handle.rcl_client.lock().unwrap(); - let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap(); + let node = &mut **self.handle.node_handle.rcl_node.lock().unwrap(); unsafe { // SAFETY both node and client are guaranteed to be valid here diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index e3efd4a4a..e50a2290d 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -196,7 +196,16 @@ impl LogConditions { /// # Examples /// /// ``` +/// use rclrs::{log_debug, log_error, log_fatal, log_info, log_warn, LogConditions, LoggingOccurrence}; +/// 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_debug!(&node.name(), "Simple message"); +/// let some_variable = 43; /// log_debug!(&node.name(), "Simple message {some_variable}"); /// log_fatal!(&node.name(), "Simple message from {}", node.name()); /// log_warn!(&node.name(), LogConditions::once(), "Only log this the first time"); @@ -204,6 +213,7 @@ impl LogConditions { /// LogConditions::skip_first_throttle(Duration::from_millis(1000)), /// "Noisy error that we expect the first time"); /// +/// let count = 0; /// log_info!(&node.name(), LogConditions { occurs: LoggingOccurrence::Always, /// publish_interval: Duration::from_millis(1000), /// log_if_true: count % 10 == 0, }, diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index ae19a5e4f..0d141721c 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -71,10 +71,15 @@ pub struct Node { /// 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) rcl_node: Mutex>, pub(crate) context_handle: Arc, } @@ -83,9 +88,10 @@ impl Drop for NodeHandle { 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) }; + unsafe { rcl_node_fini(&mut **rcl_node) }; } } @@ -370,7 +376,7 @@ impl Node { let mut domain_id: usize = 0; let ret = unsafe { // SAFETY: No preconditions for this function. - rcl_node_get_domain_id(&*rcl_node, &mut domain_id) + rcl_node_get_domain_id(&**rcl_node, &mut domain_id) }; debug_assert_eq!(ret, 0); @@ -445,7 +451,7 @@ impl Node { pub fn logger_name(&self) -> &str { let rcl_node = self.handle.rcl_node.lock().unwrap(); // SAFETY: No pre-conditions for this function - let name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; + let name_raw_ptr = unsafe { rcl_node_get_logger_name(&**rcl_node) }; if name_raw_ptr.is_null() { return ""; } @@ -535,8 +541,14 @@ mod tests { // Use helper to create 2 nodes for us let graph = construct_test_graph("test_topics_graph")?; - assert_eq!(graph.node1.logger_name(), "graph_test_node_1"); - assert_eq!(graph.node2.logger_name(), "graph_test_node_2"); + assert_eq!( + graph.node1.logger_name(), + "test_topics_graph.graph_test_node_1" + ); + assert_eq!( + graph.node2.logger_name(), + "test_topics_graph.graph_test_node_2" + ); Ok(()) } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 011d5d2f3..14d4dae27 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -277,7 +277,7 @@ impl NodeBuilder { 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 mut rcl_node = Box::new(unsafe { rcl_get_zero_initialized_node() }); unsafe { // SAFETY: // * The rcl_node is zero-initialized as mandated by this function. @@ -287,7 +287,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 *rcl_node, node_name.as_ptr(), node_namespace.as_ptr(), rcl_context, diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..e18675a21 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -139,7 +139,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_topic_names_and_types( - &*rcl_node, + &**rcl_node, &mut rcutils_get_default_allocator(), false, &mut rcl_names_and_types, @@ -169,7 +169,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_node_names( - &*rcl_node, + &**rcl_node, rcutils_get_default_allocator(), &mut rcl_names, &mut rcl_namespaces, @@ -217,7 +217,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_node_names_with_enclaves( - &*rcl_node, + &**rcl_node, rcutils_get_default_allocator(), &mut rcl_names, &mut rcl_namespaces, @@ -266,7 +266,7 @@ impl Node { // SAFETY: The topic_name string was correctly allocated previously unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); - rcl_count_publishers(&*rcl_node, topic_name.as_ptr(), &mut count).ok()? + rcl_count_publishers(&**rcl_node, topic_name.as_ptr(), &mut count).ok()? }; Ok(count) } @@ -282,7 +282,7 @@ impl Node { // SAFETY: The topic_name string was correctly allocated previously unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); - rcl_count_subscribers(&*rcl_node, topic_name.as_ptr(), &mut count).ok()? + rcl_count_subscribers(&**rcl_node, topic_name.as_ptr(), &mut count).ok()? }; Ok(count) } @@ -333,7 +333,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); getter( - &*rcl_node, + &**rcl_node, &mut rcutils_get_default_allocator(), node_name.as_ptr(), node_namespace.as_ptr(), @@ -369,7 +369,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); getter( - &*rcl_node, + &**rcl_node, &mut rcutils_get_default_allocator(), topic.as_ptr(), false, diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 2935ca322..54273bc19 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -38,7 +38,7 @@ impl Drop for PublisherHandle { // 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_publisher_fini(self.rcl_publisher.get_mut().unwrap(), &mut *rcl_node); + rcl_publisher_fini(self.rcl_publisher.get_mut().unwrap(), &mut **rcl_node); } } } @@ -111,7 +111,7 @@ where // variables in the rmw implementation being unsafely modified during cleanup. rcl_publisher_init( &mut rcl_publisher, - &*rcl_node, + &**rcl_node, type_support_ptr, topic_c_string.as_ptr(), &publisher_options, diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac43e51a8..ac8d1a2a6 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -41,7 +41,7 @@ impl Drop for ServiceHandle { // 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_service_fini(rcl_service, &mut *rcl_node); + rcl_service_fini(rcl_service, &mut **rcl_node); } } } @@ -116,7 +116,7 @@ where // variables in the rmw implementation being unsafely modified during initialization. rcl_service_init( &mut rcl_service, - &*rcl_node, + &**rcl_node, type_support, topic_c_string.as_ptr(), &service_options as *const _, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 36c241d19..47d93de1a 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -49,7 +49,7 @@ impl Drop for SubscriptionHandle { // 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_subscription_fini(rcl_subscription, &mut *rcl_node); + rcl_subscription_fini(rcl_subscription, &mut **rcl_node); } } } @@ -129,7 +129,7 @@ where // variables in the rmw implementation being unsafely modified during cleanup. rcl_subscription_init( &mut rcl_subscription, - &*rcl_node, + &**rcl_node, type_support, topic_c_string.as_ptr(), &subscription_options, From 673d478a908735d940ef5d28b1d1863918976d10 Mon Sep 17 00:00:00 2001 From: geoff-imdex Date: Mon, 28 Oct 2024 07:01:10 +0800 Subject: [PATCH 04/11] Fix linting issue with use statement --- rclrs/src/logging.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index e50a2290d..e4f789c4d 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -3,9 +3,7 @@ // 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::ffi::CString; -use std::sync::Mutex; -use std::time::Duration; +use std::{ffi::CString, sync::Mutex, time::Duration}; use crate::rcl_bindings::*; From c35f48454ca3106e98f25753ef74fd905408a51e Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 4 Nov 2024 13:27:58 +0800 Subject: [PATCH 05/11] Remove the need to Box rcl_node_t (#5) Signed-off-by: Michael X. Grey --- rclrs/src/client.rs | 6 +++--- rclrs/src/node.rs | 28 +++++++++++++++++++++++----- rclrs/src/node/builder.rs | 19 +++++++++++-------- rclrs/src/node/graph.rs | 14 +++++++------- rclrs/src/parameter.rs | 6 +++++- rclrs/src/publisher.rs | 4 ++-- rclrs/src/service.rs | 4 ++-- rclrs/src/subscription.rs | 4 ++-- 8 files changed, 55 insertions(+), 30 deletions(-) diff --git a/rclrs/src/client.rs b/rclrs/src/client.rs index f0f8f05e5..b308f1de2 100644 --- a/rclrs/src/client.rs +++ b/rclrs/src/client.rs @@ -43,7 +43,7 @@ impl Drop for ClientHandle { // 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_client_fini(rcl_client, &mut **rcl_node); + rcl_client_fini(rcl_client, &mut *rcl_node); } } } @@ -115,7 +115,7 @@ where unsafe { rcl_client_init( &mut rcl_client, - &**rcl_node, + &*rcl_node, type_support, topic_c_string.as_ptr(), &client_options, @@ -263,7 +263,7 @@ where pub fn service_is_ready(&self) -> Result { let mut is_ready = false; let client = &mut *self.handle.rcl_client.lock().unwrap(); - let node = &mut **self.handle.node_handle.rcl_node.lock().unwrap(); + let node = &mut *self.handle.node_handle.rcl_node.lock().unwrap(); unsafe { // SAFETY both node and client are guaranteed to be valid here diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 0d141721c..90e229258 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::{Arc, Mutex, Weak, atomic::AtomicBool}, vec::Vec, }; @@ -79,19 +79,37 @@ pub struct Node { /// /// [1]: pub(crate) struct NodeHandle { - pub(crate) rcl_node: Mutex>, + 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 + /// 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) }; + unsafe { rcl_node_fini(&mut *rcl_node) }; } } @@ -376,7 +394,7 @@ impl Node { let mut domain_id: usize = 0; let ret = unsafe { // SAFETY: No preconditions for this function. - rcl_node_get_domain_id(&**rcl_node, &mut domain_id) + rcl_node_get_domain_id(&*rcl_node, &mut domain_id) }; debug_assert_eq!(ret, 0); @@ -451,7 +469,7 @@ impl Node { pub fn logger_name(&self) -> &str { let rcl_node = self.handle.rcl_node.lock().unwrap(); // SAFETY: No pre-conditions for this function - let name_raw_ptr = unsafe { rcl_node_get_logger_name(&**rcl_node) }; + let name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; if name_raw_ptr.is_null() { return ""; } diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 14d4dae27..10787c5cf 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -1,6 +1,6 @@ use std::{ ffi::CString, - sync::{Arc, Mutex}, + sync::{Arc, Mutex, atomic::AtomicBool}, }; use crate::{ @@ -276,8 +276,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 = Box::new(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 +292,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 +301,8 @@ 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( diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index e18675a21..343fa61d3 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -139,7 +139,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_topic_names_and_types( - &**rcl_node, + &*rcl_node, &mut rcutils_get_default_allocator(), false, &mut rcl_names_and_types, @@ -169,7 +169,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_node_names( - &**rcl_node, + &*rcl_node, rcutils_get_default_allocator(), &mut rcl_names, &mut rcl_namespaces, @@ -217,7 +217,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); rcl_get_node_names_with_enclaves( - &**rcl_node, + &*rcl_node, rcutils_get_default_allocator(), &mut rcl_names, &mut rcl_namespaces, @@ -266,7 +266,7 @@ impl Node { // SAFETY: The topic_name string was correctly allocated previously unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); - rcl_count_publishers(&**rcl_node, topic_name.as_ptr(), &mut count).ok()? + rcl_count_publishers(&*rcl_node, topic_name.as_ptr(), &mut count).ok()? }; Ok(count) } @@ -282,7 +282,7 @@ impl Node { // SAFETY: The topic_name string was correctly allocated previously unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); - rcl_count_subscribers(&**rcl_node, topic_name.as_ptr(), &mut count).ok()? + rcl_count_subscribers(&*rcl_node, topic_name.as_ptr(), &mut count).ok()? }; Ok(count) } @@ -333,7 +333,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); getter( - &**rcl_node, + &*rcl_node, &mut rcutils_get_default_allocator(), node_name.as_ptr(), node_namespace.as_ptr(), @@ -369,7 +369,7 @@ impl Node { unsafe { let rcl_node = self.handle.rcl_node.lock().unwrap(); getter( - &**rcl_node, + &*rcl_node, &mut rcutils_get_default_allocator(), topic.as_ptr(), false, diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index f8427a0cc..d9e55715d 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -10,7 +10,10 @@ 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 +763,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)? }; diff --git a/rclrs/src/publisher.rs b/rclrs/src/publisher.rs index 54273bc19..2935ca322 100644 --- a/rclrs/src/publisher.rs +++ b/rclrs/src/publisher.rs @@ -38,7 +38,7 @@ impl Drop for PublisherHandle { // 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_publisher_fini(self.rcl_publisher.get_mut().unwrap(), &mut **rcl_node); + rcl_publisher_fini(self.rcl_publisher.get_mut().unwrap(), &mut *rcl_node); } } } @@ -111,7 +111,7 @@ where // variables in the rmw implementation being unsafely modified during cleanup. rcl_publisher_init( &mut rcl_publisher, - &**rcl_node, + &*rcl_node, type_support_ptr, topic_c_string.as_ptr(), &publisher_options, diff --git a/rclrs/src/service.rs b/rclrs/src/service.rs index ac8d1a2a6..ac43e51a8 100644 --- a/rclrs/src/service.rs +++ b/rclrs/src/service.rs @@ -41,7 +41,7 @@ impl Drop for ServiceHandle { // 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_service_fini(rcl_service, &mut **rcl_node); + rcl_service_fini(rcl_service, &mut *rcl_node); } } } @@ -116,7 +116,7 @@ where // variables in the rmw implementation being unsafely modified during initialization. rcl_service_init( &mut rcl_service, - &**rcl_node, + &*rcl_node, type_support, topic_c_string.as_ptr(), &service_options as *const _, diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 47d93de1a..36c241d19 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -49,7 +49,7 @@ impl Drop for SubscriptionHandle { // 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_subscription_fini(rcl_subscription, &mut **rcl_node); + rcl_subscription_fini(rcl_subscription, &mut *rcl_node); } } } @@ -129,7 +129,7 @@ where // variables in the rmw implementation being unsafely modified during cleanup. rcl_subscription_init( &mut rcl_subscription, - &**rcl_node, + &*rcl_node, type_support, topic_c_string.as_ptr(), &subscription_options, From ae966a482c8afd6d3f754a7239a65431f0d9ec1a Mon Sep 17 00:00:00 2001 From: geoff-imdex Date: Mon, 4 Nov 2024 14:19:14 +0800 Subject: [PATCH 06/11] Fix linting issues --- rclrs/src/node.rs | 2 +- rclrs/src/node/builder.rs | 6 ++++-- rclrs/src/parameter.rs | 3 +-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 90e229258..b94572371 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, atomic::AtomicBool}, + sync::{atomic::AtomicBool, Arc, Mutex, Weak}, vec::Vec, }; diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index 10787c5cf..f8df82101 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -1,6 +1,6 @@ use std::{ ffi::CString, - sync::{Arc, Mutex, atomic::AtomicBool}, + sync::{atomic::AtomicBool, Arc, Mutex}, }; use crate::{ @@ -301,7 +301,9 @@ impl NodeBuilder { .ok()?; }; - handle.initialized.store(true, std::sync::atomic::Ordering::Release); + handle + .initialized + .store(true, std::sync::atomic::Ordering::Release); let parameter = { let rcl_node = handle.rcl_node.lock().unwrap(); diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index d9e55715d..9c0ca55ee 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -11,8 +11,7 @@ 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, - ENTITY_LIFECYCLE_MUTEX, + call_string_getter_with_rcl_node, rcl_bindings::*, Node, RclrsError, ENTITY_LIFECYCLE_MUTEX, }; use std::{ collections::{btree_map::Entry, BTreeMap}, From 09f5664a05c8bec121e381ef6b66e9ae869ab186 Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 4 Nov 2024 14:21:31 +0800 Subject: [PATCH 07/11] Fix logging lifecycles and clean up tests Signed-off-by: Michael X. Grey --- rclrs/src/context.rs | 33 +++++--------- rclrs/src/logging.rs | 12 ++--- rclrs/src/logging/logging_configuration.rs | 53 ++++++++++++++++++++++ rclrs/src/node.rs | 6 +-- rclrs/src/node/graph.rs | 10 ++-- rclrs/src/parameter.rs | 16 +++---- rclrs/src/subscription.rs | 2 +- rclrs/src/time_source.rs | 4 +- 8 files changed, 90 insertions(+), 46 deletions(-) create mode 100644 rclrs/src/logging/logging_configuration.rs diff --git a/rclrs/src/context.rs b/rclrs/src/context.rs index e9b7ad9d5..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 @@ -34,10 +34,6 @@ impl Drop for rcl_context_t { // SAFETY: The entity lifecycle mutex is locked to protect against the risk of // global variables in the rmw implementation being unsafely modified during cleanup. rcl_shutdown(self); - - // SAFETY: No preconditions for rcl_logging_fini - rcl_logging_fini(); - rcl_context_fini(self); } } @@ -76,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 { @@ -150,28 +150,17 @@ impl Context { rcl_init_options_fini(&mut rcl_init_options).ok()?; // 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: - // * Lock the mutex as we cannot guarantee that rcl_* functions are protecting their global variables - // * Context is validated before we reach this point - // * No other preconditions for calling this function - let ret = { - let _lifecycle_lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // Note: shadowing the allocator above. The rcutils_get_default_allocator provides us with mechanisms for allocating and freeing - // memory, e.g. calloc/free. As these are function pointers and will be the same each time we call the method, it is safe to - // perform this shadowing - // Alternate is to call rcl_init_options_get_allocator however, we've freed the allocator above and restructuring the call - // sequence is unnecessarily complex - let allocator = rcutils_get_default_allocator(); + // 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)? }; - rcl_logging_configure(&rcl_context.global_arguments, &allocator).ok() - }; - ret? - } Ok(Self { handle: Arc::new(ContextHandle { rcl_context: Mutex::new(rcl_context), + logging, }), }) } diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index e4f789c4d..bfce6ad5b 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -5,10 +5,10 @@ use std::{ffi::CString, sync::Mutex, time::Duration}; -use crate::rcl_bindings::*; +use crate::{rcl_bindings::*, ENTITY_LIFECYCLE_MUTEX}; -// Used to protect calls to rcl/rcutils in case those functions manipulate global variables -static LOG_GUARD: Mutex<()> = Mutex::new(()); +mod logging_configuration; +pub(crate) use logging_configuration::*; /// Calls the underlying rclutils logging function /// Don't call this directly, use the logging macros instead. @@ -39,8 +39,8 @@ pub fn log(msg: &str, logger_name: &str, file: &str, line: u32, severity: LogSev .expect("Valid c style string required, e.g. check for extraneous null characters"); let severity = severity.to_native(); - let _guard = LOG_GUARD.lock().unwrap(); - // SAFETY: Global variables are protected via LOG_GUARD, no other preconditions are required + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: Global variables are protected via ENTITY_LIFECYCLE_MUTEX, no other preconditions are required unsafe { rcutils_log( &location, @@ -200,7 +200,7 @@ impl LogConditions { /// use std::env; /// /// let context = rclrs::Context::new(env::args()).unwrap(); -/// let node = rclrs::Node::new(&context, "test_node").unwrap(); +/// let node = rclrs::Node::new(&context, "log_example_node").unwrap(); /// /// log_debug!(&node.name(), "Simple message"); /// let some_variable = 43; diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs new file mode 100644 index 000000000..b4cfa626b --- /dev/null +++ b/rclrs/src/logging/logging_configuration.rs @@ -0,0 +1,53 @@ +use std::sync::{Mutex, Weak, LazyLock, Arc}; + +use crate::{ + RclrsError, ENTITY_LIFECYCLE_MUTEX, ToResult, + rcl_bindings::{ + rcl_context_t, rcl_arguments_t, rcutils_get_default_allocator, + rcl_logging_configure, rcl_logging_fini, + }, +}; + + +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: LazyLock = LazyLock::new( + || 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(); } + } +} diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index b94572371..394c5f740 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -557,15 +557,15 @@ mod tests { #[test] fn test_logger_name() -> Result<(), RclrsError> { // Use helper to create 2 nodes for us - let graph = construct_test_graph("test_topics_graph")?; + let graph = construct_test_graph("test_logger_name")?; assert_eq!( graph.node1.logger_name(), - "test_topics_graph.graph_test_node_1" + "test_logger_name.graph_test_node_1" ); assert_eq!( graph.node2.logger_name(), - "test_topics_graph.graph_test_node_2" + "test_logger_name.graph_test_node_2" ); Ok(()) diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 343fa61d3..3a08b3ba2 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -487,12 +487,13 @@ 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 + // Test that the graph has no publishers besides /rosout let names_and_topics = node .get_publisher_names_and_types_by_node(node_name, "") .unwrap(); - assert_eq!(names_and_topics.len(), 0); + assert_eq!(names_and_topics.len(), 1); + assert_eq!(names_and_topics.get("/rosout").unwrap().first().unwrap(), "rcl_interfaces/msg/Log"); let num_publishers = node.count_publishers("/test").unwrap(); @@ -535,10 +536,11 @@ mod tests { assert_eq!(names_and_topics.len(), 0); - // Test that the graph has no topics + // Test that the graph has no topics besides /rosout let names_and_topics = node.get_topic_names_and_types().unwrap(); - assert_eq!(names_and_topics.len(), 0); + assert_eq!(names_and_topics.len(), 1); + assert_eq!(names_and_topics.get("/rosout").unwrap().first().unwrap(), "rcl_interfaces/msg/Log"); } #[test] diff --git a/rclrs/src/parameter.rs b/rclrs/src/parameter.rs index 9c0ca55ee..c7740a2d5 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -885,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 @@ -943,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") @@ -1093,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() @@ -1115,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 @@ -1161,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), @@ -1289,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) @@ -1316,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(); @@ -1369,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/subscription.rs b/rclrs/src/subscription.rs index 36c241d19..05b01beb5 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -381,7 +381,7 @@ mod tests { // Test get_subscriptions_info_by_topic() let expected_subscriptions_info = vec![TopicEndpointInfo { - node_name: String::from("graph_test_node_2"), + node_name: format!("graph_test_node_2"), node_namespace: String::from(namespace), topic_type: String::from("test_msgs/msg/Empty"), }]; diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index a6b600800..15d2ca31b 100644 --- a/rclrs/src/time_source.rs +++ b/rclrs/src/time_source.rs @@ -149,7 +149,7 @@ 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 +162,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); } From 444054647b8d3c0dfa42a5d0ef1384b8d05f713b Mon Sep 17 00:00:00 2001 From: geoff-imdex Date: Mon, 4 Nov 2024 14:37:13 +0800 Subject: [PATCH 08/11] Linting fixes after logging lifecycle improvements --- rclrs/src/logging.rs | 2 +- rclrs/src/logging/logging_configuration.rs | 24 +++++++++++++--------- rclrs/src/node/graph.rs | 10 +++++++-- rclrs/src/time_source.rs | 6 +++++- 4 files changed, 28 insertions(+), 14 deletions(-) diff --git a/rclrs/src/logging.rs b/rclrs/src/logging.rs index bfce6ad5b..3943183c7 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -3,7 +3,7 @@ // 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::{ffi::CString, sync::Mutex, time::Duration}; +use std::{ffi::CString, time::Duration}; use crate::{rcl_bindings::*, ENTITY_LIFECYCLE_MUTEX}; diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs index b4cfa626b..21f13987a 100644 --- a/rclrs/src/logging/logging_configuration.rs +++ b/rclrs/src/logging/logging_configuration.rs @@ -1,14 +1,13 @@ -use std::sync::{Mutex, Weak, LazyLock, Arc}; +use std::sync::{Arc, LazyLock, Mutex, Weak}; use crate::{ - RclrsError, ENTITY_LIFECYCLE_MUTEX, ToResult, rcl_bindings::{ - rcl_context_t, rcl_arguments_t, rcutils_get_default_allocator, - rcl_logging_configure, rcl_logging_fini, + rcl_arguments_t, rcl_context_t, rcl_logging_configure, rcl_logging_fini, + rcutils_get_default_allocator, }, + RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, }; - struct LoggingConfiguration { lifecycle: Mutex>, } @@ -30,10 +29,13 @@ impl LoggingLifecycle { } /// SAFETY: Ensure rcl_context_t is valid before passing it in. - pub(crate) unsafe fn configure(context: &rcl_context_t) -> Result, RclrsError> { - static CONFIGURATION: LazyLock = LazyLock::new( - || LoggingConfiguration { lifecycle: Mutex::new(Weak::new())} - ); + pub(crate) unsafe fn configure( + context: &rcl_context_t, + ) -> Result, RclrsError> { + static CONFIGURATION: LazyLock = + LazyLock::new(|| LoggingConfiguration { + lifecycle: Mutex::new(Weak::new()), + }); let mut lifecycle = CONFIGURATION.lifecycle.lock().unwrap(); if let Some(arc_lifecycle) = lifecycle.upgrade() { @@ -48,6 +50,8 @@ impl LoggingLifecycle { impl Drop for LoggingLifecycle { fn drop(&mut self) { let _lock = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - unsafe { rcl_logging_fini(); } + unsafe { + rcl_logging_fini(); + } } } diff --git a/rclrs/src/node/graph.rs b/rclrs/src/node/graph.rs index 3a08b3ba2..6f9e6d866 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -493,7 +493,10 @@ mod tests { .unwrap(); assert_eq!(names_and_topics.len(), 1); - assert_eq!(names_and_topics.get("/rosout").unwrap().first().unwrap(), "rcl_interfaces/msg/Log"); + assert_eq!( + names_and_topics.get("/rosout").unwrap().first().unwrap(), + "rcl_interfaces/msg/Log" + ); let num_publishers = node.count_publishers("/test").unwrap(); @@ -540,7 +543,10 @@ mod tests { let names_and_topics = node.get_topic_names_and_types().unwrap(); assert_eq!(names_and_topics.len(), 1); - assert_eq!(names_and_topics.get("/rosout").unwrap().first().unwrap(), "rcl_interfaces/msg/Log"); + assert_eq!( + names_and_topics.get("/rosout").unwrap().first().unwrap(), + "rcl_interfaces/msg/Log" + ); } #[test] diff --git a/rclrs/src/time_source.rs b/rclrs/src/time_source.rs index 15d2ca31b..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(), &format!("time_source_test_node_{}", line!())).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); } From 2bbf6b6dba39e8c2f1df02a17b0dfd2f6974bcb8 Mon Sep 17 00:00:00 2001 From: Grey Date: Sun, 10 Nov 2024 14:35:46 +0800 Subject: [PATCH 09/11] Introduce Logger and LogParams Signed-off-by: Michael X. Grey --- rclrs/build.rs | 1 + rclrs/src/logging.rs | 637 ++++++++++++--------- rclrs/src/logging/log_params.rs | 246 ++++++++ rclrs/src/logging/logger.rs | 101 ++++ rclrs/src/logging/logging_configuration.rs | 12 +- rclrs/src/node.rs | 35 +- rclrs/src/node/builder.rs | 29 +- rclrs/src/node/graph.rs | 35 +- rclrs/src/subscription.rs | 2 +- 9 files changed, 787 insertions(+), 311 deletions(-) create mode 100644 rclrs/src/logging/log_params.rs create mode 100644 rclrs/src/logging/logger.rs 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/logging.rs b/rclrs/src/logging.rs index 3943183c7..d008e5d66 100644 --- a/rclrs/src/logging.rs +++ b/rclrs/src/logging.rs @@ -3,340 +3,441 @@ // 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::{ffi::CString, time::Duration}; +use std::{ + collections::HashMap, + ffi::CString, + sync::{Mutex, OnceLock}, +}; use crate::{rcl_bindings::*, ENTITY_LIFECYCLE_MUTEX}; mod logging_configuration; pub(crate) use logging_configuration::*; -/// Calls the underlying rclutils logging function -/// Don't call this directly, use the logging macros instead. -/// -/// # Panics -/// -/// This function might panic in the following scenarios: -/// - A logger_name is provided that is not a valid c-string, e.g. contains extraneous null characters -/// - The user-supplied "msg" is not a valid c-string, e.g. contains extraneous null characters -/// - When called if the lock is already held by the current thread. -/// - If the construction of CString objects used to create the log output fail, -/// although, this highly unlikely to occur in most cases -#[doc(hidden)] -pub fn log(msg: &str, logger_name: &str, file: &str, line: u32, severity: LogSeverity) { - // currently not possible to get function name in rust. - // see https://github.com/rust-lang/rfcs/pull/2818 - let function = CString::new("").unwrap(); - let file = CString::new(file).unwrap(); - let location = rcutils_log_location_t { - function_name: function.as_ptr(), - file_name: file.as_ptr(), - line_number: line as usize, - }; - let format = CString::new("%s").unwrap(); - let logger_name = CString::new(logger_name) - .expect("Logger name is a valid c style string, e.g. check for extraneous null characters"); - let message = CString::new(msg) - .expect("Valid c style string required, e.g. check for extraneous null characters"); - let severity = severity.to_native(); - - let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); - // SAFETY: Global variables are protected via ENTITY_LIFECYCLE_MUTEX, no other preconditions are required - unsafe { - rcutils_log( - &location, - severity as i32, - logger_name.as_ptr(), - format.as_ptr(), - message.as_ptr(), - ); - } -} - -/// Logging severity -#[doc(hidden)] -pub enum LogSeverity { - Unset, - Debug, - Info, - Warn, - Error, - Fatal, -} - -impl LogSeverity { - fn to_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, - } - } -} - -#[derive(Debug)] -/// Specify when a log message should be published -pub enum LoggingOccurrence { - /// The log message will always be published (assuming all other conditions are met) - Always, - /// 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, -} - -/// Specify conditions that must be met for a log message to be published -/// -/// The struct provides the following convenience functions to construct conditions that match -/// behaviour available in the `rclcpp` and `rclpy` libraries. -/// -/// When will my log message be output? -/// -/// - `Once`: A message with the [`LoggingOccurrence::Once`] value will be published once only -/// regardless of any other conditions -/// - `SkipFirst`: A message with the [`LoggingOccurrence::SkipFirst`] value will never be published -/// on the first encounter regardless of any other conditions. After the first -/// encounter, the behaviour is identical to the [`LoggingOccurrence::Always`] setting. -/// - `Always`: The log message will be output if all additional conditions are true: -/// - The current time + the `publish_interval` > the last time the message was published. -/// - The default value for `publish_interval` is 0, i.e. the interval check will always pass -/// - The `log_if_true` expression evaluates to TRUE. -/// - The default value for the `log_if_true` field is TRUE. -pub struct LogConditions { - /// Specify when a log message should be published (See[`LoggingOccurrence`] above) - pub occurs: LoggingOccurrence, - /// Specify the publication interval of the message. A value of ZERO (0) indicates that the - /// message should be published every time, 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. instead - /// of publishing a log message every 10 milliseconds, the `publish_interval` can be configured - /// such that the message is published every 10 seconds. - pub publish_interval: Duration, - /// The log message will only published if the specified expression evaluates to true - pub log_if_true: bool, -} - -impl LogConditions { - /// Default construct an instance - pub fn new() -> Self { - Self { - occurs: LoggingOccurrence::Always, - publish_interval: Duration::ZERO, - log_if_true: true, - } - } - - /// Only publish this message the first time it is encountered - pub fn once() -> Self { - Self { - occurs: LoggingOccurrence::Once, - publish_interval: Duration::ZERO, - log_if_true: true, - } - } +mod log_params; +pub use log_params::*; - /// Do not publish the message the first time it is encountered - pub fn skip_first() -> Self { - Self { - occurs: LoggingOccurrence::SkipFirst, - publish_interval: Duration::ZERO, - log_if_true: true, - } - } - - /// Do not publish the first time this message is encountered and publish - /// at the specified `publish_interval` thereafter - pub fn skip_first_throttle(publish_interval: Duration) -> Self { - Self { - occurs: LoggingOccurrence::SkipFirst, - publish_interval, - log_if_true: true, - } - } - - /// Throttle the message to the supplied publish_interval - /// e.g. set `publish_interval` to 1000ms to limit publishing to once a second - pub fn throttle(publish_interval: Duration) -> Self { - Self { - occurs: LoggingOccurrence::Always, - publish_interval, - log_if_true: true, - } - } +mod logger; +pub use logger::*; - /// Permitting logging if the supplied expression evaluates to true - /// Uses default LoggingOccurrence (Always) and publish_interval (no throttling) - pub fn log_if_true(log_if_true: bool) -> Self { - Self { - occurs: LoggingOccurrence::Always, - publish_interval: Duration::ZERO, - log_if_true, - } - } -} - -/// log_with_conditions -/// Helper macro to log a message using the ROS2 RCUTILS library -/// -/// The macro supports two general calling formats: -/// 1. Plain message string e.g. as per println! macro -/// 2. With calling conditions that provide some restrictions on the output of the message -/// (see [`LogConditions`] above) -/// -/// It is expected that, typically, the macro will be called using one of the wrapper macros, -/// e.g. log_debug!, etc, however, it is relatively straight forward to call the macro directly -/// if you really want to. +/// log a message to rosout /// /// # Examples /// /// ``` -/// use rclrs::{log_debug, log_error, log_fatal, log_info, log_warn, LogConditions, LoggingOccurrence}; +/// 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, "log_example_node").unwrap(); +/// let node = rclrs::Node::new(&context, "test_node").unwrap(); /// -/// log_debug!(&node.name(), "Simple message"); +/// log!(node.debug(), "Simple debug message"); /// let some_variable = 43; -/// log_debug!(&node.name(), "Simple message {some_variable}"); -/// log_fatal!(&node.name(), "Simple message from {}", node.name()); -/// log_warn!(&node.name(), LogConditions::once(), "Only log this the first time"); -/// log_error!(&node.name(), -/// LogConditions::skip_first_throttle(Duration::from_millis(1000)), -/// "Noisy error that we expect the first time"); +/// 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() +/// .interval(Duration::from_millis(1000)), +/// "Noisy error that we expect the first time" +/// ); /// /// let count = 0; -/// log_info!(&node.name(), LogConditions { occurs: LoggingOccurrence::Always, -/// publish_interval: Duration::from_millis(1000), -/// log_if_true: count % 10 == 0, }, -/// "Manually constructed LogConditions"); +/// log!( +/// node +/// .info() +/// .interval(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 not be possible. +/// poisoned although this should never happen. #[macro_export] -macro_rules! log_with_conditions { +macro_rules! log { // The variable args captured by the $(, $($args:tt)*)?)) code allows us to omit (or include) - // parameters in the simple message case, e.g. to write + // 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); - ($severity: expr, $logger_name: expr, $msg_start: literal $(, $($args:tt)*)?) => { - $crate::log(&std::fmt::format(format_args!($msg_start, $($($args)*)?)), $logger_name, file!(), line!(), $severity); - }; - ($severity: expr, $logger_name: expr, $conditions: expr, $($args:tt)*) => { + ($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; + use std::sync::{Once, OnceLock, Mutex}; use std::time::SystemTime; - let log_conditions: $crate::LogConditions = $conditions; - let mut allow_logging = true; - match log_conditions.occurs { - // Create the static variables here so we get a per-instance static - $crate::LoggingOccurrence::Once => { - static LOG_ONCE: std::sync::Once = std::sync::Once::new(); - LOG_ONCE.call_once(|| { - $crate::log(&std::fmt::format(format_args!($($args)*)), $logger_name, file!(), line!(), $severity); - }); - allow_logging = false; - } - $crate::LoggingOccurrence::SkipFirst => { - // Noop, just make sure we exit the first time... - static SKIP_FIRST: std::sync::Once = std::sync::Once::new(); - SKIP_FIRST.call_once(|| { - // Only disable logging the first time - allow_logging = false; - }); + // 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; } - // Drop out - $crate::LoggingOccurrence::Always => (), - } - // If we have a throttle period AND logging has not already been disabled due to SkipFirst settings - if log_conditions.publish_interval > std::time::Duration::ZERO && allow_logging { - let mut ignore_first_timeout = false; - // Need to initialise to a constant - static LAST_LOG_TIME: Mutex = Mutex::new(std::time::SystemTime::UNIX_EPOCH); - - static INIT_LAST_LOG_TIME: std::sync::Once = std::sync::Once::new(); - // Set the last log time to "now", but let us log the message the first time we hit this - // code, i.e. initial behaviour is expired. - // Note: If this is part of a SkipFirst macro call, we will only hit this code on the second iteration. - INIT_LAST_LOG_TIME.call_once(|| { - let mut last_log_time = LAST_LOG_TIME.lock().unwrap(); - *last_log_time = std::time::SystemTime::now(); - ignore_first_timeout = true; - }); - - let mut last_log_time = LAST_LOG_TIME.lock().unwrap(); - if std::time::SystemTime::now() >= *last_log_time + log_conditions.publish_interval { - // Update our time stamp - *last_log_time = std::time::SystemTime::now(); + 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 => (), } - else if !ignore_first_timeout { - // Timer has not expired (and this is not the first time through here) - allow_logging = false; + + // If we have a throttle interval then check if we're inside or outside + // of that interval. + let interval = params.get_interval(); + if interval > 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 + interval { + *previous = now; + } else { + // We are still inside the throttle interval, so just exit + // here. + return; + } + } } - } - // At this point we've validated the skip/throttle operations, and we now check that any - // expression supplied also evaluates to true, e.g. if timer has expired but expression is - // false, we won't print - if (allow_logging && log_conditions.log_if_true) - { - $crate::log(&std::fmt::format(format_args!($($args)*)), $logger_name, file!(), line!(), $severity); - } - }; + // All filters have been checked, so go ahead and publish the message + $crate::log_unconditional!(severity, logger_name, $($args)*); + })(); + }}; } -/// Debug log message. +/// Debug log message. See [`log`] for usage. #[macro_export] macro_rules! log_debug { - ($logger_name: expr, $($args:tt)*) => {{ - $crate::log_with_conditions!($crate::LogSeverity::Debug, $logger_name, $($args)*); + ($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. +/// Info log message. See [`log`] for usage. #[macro_export] macro_rules! log_info { - ($logger_name: expr, $($args:tt)*) => {{ - $crate::log_with_conditions!($crate::LogSeverity::Info, $logger_name, $($args)*); + ($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. +/// Warning log message. See [`log`] for usage. #[macro_export] macro_rules! log_warn { - ($logger_name: expr, $($args:tt)*) => {{ - $crate::log_with_conditions!($crate::LogSeverity::Warn, $logger_name, $($args)*); + ($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. +/// Error log message. See [`log`] for usage. #[macro_export] macro_rules! log_error { - ($logger_name: expr, $($args:tt)*) => {{ - $crate::log_with_conditions!($crate::LogSeverity::Error, $logger_name, $($args)*); + ($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. +/// Fatal log message. See [`log`] for usage. #[macro_export] macro_rules! log_fatal { - ($logger_name: expr, $($args:tt)*) => {{ - $crate::log_with_conditions!($crate::LogSeverity::Fatal, $logger_name, $($args)*); + ($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(|| { + CString::new($crate::function!()).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_CSTR: OnceLock = OnceLock::new(); + let format_cstr = FORMAT_CSTR.get_or_init(|| CString::new("%s").unwrap()); + + let severity = severity.as_native(); + + let _lifecycle = ENTITY_LIFECYCLE_MUTEX.lock().unwrap(); + // SAFETY: Global variables are protected via ENTITY_LIFECYCLE_MUTEX, no other preconditions are required + unsafe { + rcutils_log( + &location, + severity as i32, + logger_name.as_ptr(), + format_cstr.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::{test_helpers::*, *}; + + #[test] + fn test_logging_macros() -> Result<(), RclrsError> { + let graph = construct_test_graph("test_logging_macros")?; + let node = graph.node1; + + log!(&*node, "Logging with node dereference"); + + for _ in 0..10 { + log!(node.once(), "Logging once"); + } + + log!(node.logger(), "Logging with node logger"); + log!(node.debug(), "Debug from node"); + log!(node.info(), "Info from node"); + log!(node.warn(), "Warn from node"); + log!(node.error(), "Error from node"); + log!(node.fatal(), "Fatal from node"); + + 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); + } + + node.logger().set_level(LogSeverity::Debug).unwrap(); + log_debug!(node.logger(), "This debug message appears"); + node.logger().set_level(LogSeverity::Info).unwrap(); + log_debug!(node.logger(), "This debug message does not"); + + log!("custom logger name", "message for custom logger"); + for _ in 0..3 { + log!( + "custom logger name".once(), + "one-time message for custom logger" + ); + } + + for _ in 0..3 { + log!( + "custom logger name".error().skip_first(), + "error for custom logger", + ); + } + + 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..d8cbc8995 --- /dev/null +++ b/rclrs/src/logging/log_params.rs @@ -0,0 +1,246 @@ +use crate::rcl_bindings::RCUTILS_LOG_SEVERITY; +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 the publication interval of the message. A value of ZERO (0) indicates that the + /// message should be published every time, 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. instead + /// of publishing a log message every 10 milliseconds, the `publish_interval` can be configured + /// such that the message is published every 10 seconds. + interval: Duration, + /// 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(), + interval: Duration::new(0, 0), + 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 interval + pub fn get_interval(&self) -> Duration { + self.interval + } + + /// 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 an 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 interval(self, interval: Duration) -> LogParams<'a> { + let mut params = self.to_log_params(); + params.interval = interval; + 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::interval` 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. +#[doc(hidden)] +#[derive(Debug, Clone, Copy)] +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, + } + } +} + +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, +} + +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..2d55f2d7a --- /dev/null +++ b/rclrs/src/logging/logger.rs @@ -0,0 +1,101 @@ +use std::{borrow::Borrow, ffi::CString}; + +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 +pub struct Logger { + name: Box, + c_name: CString, +} + +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()) { + Ok(c_name) => c_name, + Err(err) => { + return Err(RclrsError::StringContainsNul { + s: name.into_string(), + err, + }); + } + }; + + Ok(Self { name, 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() + } +} diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs index 21f13987a..2f7014273 100644 --- a/rclrs/src/logging/logging_configuration.rs +++ b/rclrs/src/logging/logging_configuration.rs @@ -1,4 +1,4 @@ -use std::sync::{Arc, LazyLock, Mutex, Weak}; +use std::sync::{Arc, Mutex, OnceLock, Weak}; use crate::{ rcl_bindings::{ @@ -32,12 +32,12 @@ impl LoggingLifecycle { pub(crate) unsafe fn configure( context: &rcl_context_t, ) -> Result, RclrsError> { - static CONFIGURATION: LazyLock = - LazyLock::new(|| LoggingConfiguration { - lifecycle: Mutex::new(Weak::new()), - }); + static CONFIGURATION: OnceLock = OnceLock::new(); + let configuration = CONFIGURATION.get_or_init(|| LoggingConfiguration { + lifecycle: Mutex::new(Weak::new()), + }); - let mut lifecycle = CONFIGURATION.lifecycle.lock().unwrap(); + let mut lifecycle = configuration.lifecycle.lock().unwrap(); if let Some(arc_lifecycle) = lifecycle.upgrade() { return Ok(arc_lifecycle); } diff --git a/rclrs/src/node.rs b/rclrs/src/node.rs index 394c5f740..5afa1fe03 100644 --- a/rclrs/src/node.rs +++ b/rclrs/src/node.rs @@ -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 @@ -66,6 +66,7 @@ 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 @@ -465,19 +466,15 @@ impl Node { NodeBuilder::new(context, node_name) } - /// Returns the logger name of the node. - pub fn logger_name(&self) -> &str { - let rcl_node = self.handle.rcl_node.lock().unwrap(); - // SAFETY: No pre-conditions for this function - let name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) }; - if name_raw_ptr.is_null() { - return ""; - } - // SAFETY: The returned CStr is immediately converted to a string slice, - // so the lifetime is no issue. The ptr is valid as per the documentation - // of rcl_node_get_logger_name. - let name_cstr = unsafe { CStr::from_ptr(name_raw_ptr) }; - name_cstr.to_str().unwrap_or("") + /// 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() } } @@ -560,11 +557,11 @@ mod tests { let graph = construct_test_graph("test_logger_name")?; assert_eq!( - graph.node1.logger_name(), + graph.node1.logger().name(), "test_logger_name.graph_test_node_1" ); assert_eq!( - graph.node2.logger_name(), + graph.node2.logger().name(), "test_logger_name.graph_test_node_2" ); diff --git a/rclrs/src/node/builder.rs b/rclrs/src/node/builder.rs index f8df82101..1e7a9fc63 100644 --- a/rclrs/src/node/builder.rs +++ b/rclrs/src/node/builder.rs @@ -1,11 +1,12 @@ use std::{ - ffi::CString, + 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]. @@ -313,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![]), @@ -323,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 6f9e6d866..639a38e38 100644 --- a/rclrs/src/node/graph.rs +++ b/rclrs/src/node/graph.rs @@ -487,16 +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 besides /rosout + + 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(), 1); - assert_eq!( - names_and_topics.get("/rosout").unwrap().first().unwrap(), - "rcl_interfaces/msg/Log" - ); + check_rosout(names_and_topics); let num_publishers = node.count_publishers("/test").unwrap(); @@ -539,14 +552,8 @@ mod tests { assert_eq!(names_and_topics.len(), 0); - // Test that the graph has no topics besides /rosout let names_and_topics = node.get_topic_names_and_types().unwrap(); - - assert_eq!(names_and_topics.len(), 1); - assert_eq!( - names_and_topics.get("/rosout").unwrap().first().unwrap(), - "rcl_interfaces/msg/Log" - ); + check_rosout(names_and_topics); } #[test] diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 05b01beb5..36c241d19 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -381,7 +381,7 @@ mod tests { // Test get_subscriptions_info_by_topic() let expected_subscriptions_info = vec![TopicEndpointInfo { - node_name: format!("graph_test_node_2"), + node_name: String::from("graph_test_node_2"), node_namespace: String::from(namespace), topic_type: String::from("test_msgs/msg/Empty"), }]; From 196569ce8ab28e9d727d7c606f671d43af1c6e7f Mon Sep 17 00:00:00 2001 From: Grey Date: Mon, 18 Nov 2024 14:47:14 +0800 Subject: [PATCH 10/11] Cleaner logging tests Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Co-authored-by: Nathan Wiebe Neufeldt --- .../workflows/{rust.yml => rust-minimal.yml} | 9 +- .github/workflows/rust-stable.yml | 130 +++++ rclrs/Cargo.toml | 7 +- rclrs/src/logging.rs | 211 +++++++- rclrs/src/logging/log_params.rs | 54 ++- rclrs/src/logging/logging_configuration.rs | 208 +++++++- rclrs/src/parameter.rs | 18 +- rclrs/src/parameter/range.rs | 2 +- rclrs/src/subscription.rs | 10 +- rclrs/src/subscription/message_info.rs | 36 +- rclrs/src/vendor/action_msgs/mod.rs | 7 + rclrs/src/vendor/action_msgs/msg.rs | 457 ++++++++++++++++++ rclrs/src/vendor/action_msgs/srv.rs | 375 ++++++++++++++ rclrs/src/vendor/builtin_interfaces/mod.rs | 2 + rclrs/src/vendor/builtin_interfaces/msg.rs | 8 +- rclrs/src/vendor/mod.rs | 3 +- rclrs/src/vendor/rcl_interfaces/mod.rs | 2 + rclrs/src/vendor/rcl_interfaces/msg.rs | 44 +- rclrs/src/vendor/rcl_interfaces/srv.rs | 97 ++-- rclrs/src/vendor/rosgraph_msgs/mod.rs | 2 + rclrs/src/vendor/rosgraph_msgs/msg.rs | 4 +- .../src/vendor/unique_identifier_msgs/mod.rs | 5 + .../src/vendor/unique_identifier_msgs/msg.rs | 125 +++++ rclrs/src/wait.rs | 2 +- rclrs/vendor_interfaces.py | 61 +-- ros2_rust_jazzy.repos | 29 ++ rosidl_generator_rs/resource/lib.rs.em | 2 + 27 files changed, 1720 insertions(+), 190 deletions(-) rename .github/workflows/{rust.yml => rust-minimal.yml} (94%) create mode 100644 .github/workflows/rust-stable.yml create mode 100644 rclrs/src/vendor/action_msgs/mod.rs create mode 100644 rclrs/src/vendor/action_msgs/msg.rs create mode 100644 rclrs/src/vendor/action_msgs/srv.rs create mode 100644 rclrs/src/vendor/unique_identifier_msgs/mod.rs create mode 100644 rclrs/src/vendor/unique_identifier_msgs/msg.rs mode change 100644 => 100755 rclrs/vendor_interfaces.py create mode 100644 ros2_rust_jazzy.repos diff --git a/.github/workflows/rust.yml b/.github/workflows/rust-minimal.yml similarity index 94% rename from .github/workflows/rust.yml rename to .github/workflows/rust-minimal.yml index 3fd48dba0..9b1a5bb49 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust-minimal.yml @@ -1,4 +1,4 @@ -name: Rust +name: Rust Minimal on: push: @@ -6,7 +6,10 @@ on: pull_request: branches: [ main ] schedule: - - cron: '0 0 * * *' + # Run the CI at 02:22 UTC every Tuesday + # We pick an arbitrary time outside of most of the world's work hours + # to minimize the likelihood of running alongside a heavy workload. + - cron: '22 2 * * 2' env: CARGO_TERM_COLOR: always @@ -51,7 +54,7 @@ jobs: use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} - name: Setup Rust - uses: dtolnay/rust-toolchain@1.74.0 + uses: dtolnay/rust-toolchain@1.75 with: components: clippy, rustfmt diff --git a/.github/workflows/rust-stable.yml b/.github/workflows/rust-stable.yml new file mode 100644 index 000000000..6dc395496 --- /dev/null +++ b/.github/workflows/rust-stable.yml @@ -0,0 +1,130 @@ +name: Rust Stable + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + schedule: + # Run the CI at 02:22 UTC every Tuesday + # We pick an arbitrary time outside of most of the world's work hours + # to minimize the likelihood of running alongside a heavy workload. + - cron: '22 2 * * 2' + +env: + CARGO_TERM_COLOR: always + +jobs: + build: + strategy: + matrix: + ros_distribution: + - humble + - iron + - rolling + include: + # Humble Hawksbill (May 2022 - May 2027) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest + ros_distribution: humble + ros_version: 2 + # Iron Irwini (May 2023 - November 2024) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest + ros_distribution: iron + ros_version: 2 + # Rolling Ridley (June 2020 - Present) + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + ros_distribution: rolling + ros_version: 2 + runs-on: ubuntu-latest + continue-on-error: ${{ matrix.ros_distribution == 'rolling' }} + container: + image: ${{ matrix.docker_image }} + steps: + - uses: actions/checkout@v4 + + - name: Search packages in this repository + id: list_packages + run: | + echo ::set-output name=package_list::$(colcon list --names-only) + + - name: Setup ROS environment + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.ros_distribution }} + use-ros2-testing: ${{ matrix.ros_distribution == 'rolling' }} + + - name: Setup Rust + uses: dtolnay/rust-toolchain@stable + with: + components: clippy, rustfmt + + - name: Install colcon-cargo and colcon-ros-cargo + run: | + sudo pip3 install git+https://github.com/colcon/colcon-cargo.git + sudo pip3 install git+https://github.com/colcon/colcon-ros-cargo.git + + - name: Check formatting of Rust packages + run: | + for path in $(colcon list | awk '$3 == "(ament_cargo)" { print $2 }'); do + cd $path + rustup toolchain install nightly + cargo +nightly fmt -- --check + cd - + done + + - name: Install cargo-ament-build + run: | + cargo install --debug cargo-ament-build + + - name: Build and test + id: build + uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: ${{ steps.list_packages.outputs.package_list }} + target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: ros2_rust_${{ matrix.ros_distribution }}.repos + + - name: Run clippy on Rust packages + run: | + cd ${{ steps.build.outputs.ros-workspace-directory-name }} + . /opt/ros/${{ matrix.ros_distribution }}/setup.sh + for path in $(colcon list | awk '$3 == "(ament_cargo)" { print $2 }'); do + cd $path + echo "Running clippy in $path" + # Run clippy for all features except generate_docs (needed for docs.rs) + if [ "$(basename $path)" = "rclrs" ]; then + cargo clippy --all-targets -F default,dyn_msg -- -D warnings + else + cargo clippy --all-targets --all-features -- -D warnings + fi + cd - + done + + - name: Run cargo test on Rust packages + run: | + cd ${{ steps.build.outputs.ros-workspace-directory-name }} + . install/setup.sh + for path in $(colcon list | awk '$3 == "(ament_cargo)" && $1 != "examples_rclrs_minimal_pub_sub" && $1 != "examples_rclrs_minimal_client_service" && $1 != "rust_pubsub" { print $2 }'); do + cd $path + echo "Running cargo test in $path" + # Run cargo test for all features except generate_docs (needed for docs.rs) + if [ "$(basename $path)" = "rclrs" ]; then + cargo test -F default,dyn_msg + elif [ "$(basename $path)" = "rosidl_runtime_rs" ]; then + cargo test -F default + else + cargo test --all-features + fi + cd - + done + + - name: Rustdoc check + run: | + cd ${{ steps.build.outputs.ros-workspace-directory-name }} + . /opt/ros/${{ matrix.ros_distribution }}/setup.sh + for path in $(colcon list | awk '$3 == "(ament_cargo)" && $1 != "examples_rclrs_minimal_pub_sub" && $1 != "examples_rclrs_minimal_client_service" && $1 != "rust_pubsub" { print $2 }'); do + cd $path + echo "Running rustdoc check in $path" + cargo rustdoc -- -D warnings + cd - + done diff --git a/rclrs/Cargo.toml b/rclrs/Cargo.toml index f489ea3f0..fe17cc990 100644 --- a/rclrs/Cargo.toml +++ b/rclrs/Cargo.toml @@ -6,7 +6,7 @@ authors = ["Esteve Fernandez ", "Nikolai Morin (), } - // If we have a throttle interval then check if we're inside or outside + // If we have a throttle duration then check if we're inside or outside // of that interval. - let interval = params.get_interval(); - if interval > std::time::Duration::ZERO { + 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()) @@ -136,11 +136,10 @@ macro_rules! log { if !first_time { let now = std::time::SystemTime::now(); let mut previous = last_log_time.lock().unwrap(); - if now >= *previous + interval { + if now >= *previous + throttle { *previous = now; } else { - // We are still inside the throttle interval, so just exit - // here. + // We are still inside the throttle interval, so just exit here. return; } } @@ -209,7 +208,16 @@ macro_rules! log_unconditional { // 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(|| { - CString::new($crate::function!()).unwrap_or( + // 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() ) }); @@ -278,21 +286,60 @@ pub unsafe fn impl_log( file_name: file.as_ptr(), line_number: line as usize, }; - static FORMAT_CSTR: OnceLock = OnceLock::new(); - let format_cstr = FORMAT_CSTR.get_or_init(|| CString::new("%s").unwrap()); + + 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(); - // SAFETY: Global variables are protected via ENTITY_LIFECYCLE_MUTEX, no other preconditions are required - unsafe { - rcutils_log( - &location, - severity as i32, - logger_name.as_ptr(), - format_cstr.as_ptr(), - message.as_ptr(), - ); + + #[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(), + ); + } } }; @@ -380,25 +427,121 @@ macro_rules! function { #[cfg(test)] mod tests { - use crate::{test_helpers::*, *}; + 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"); @@ -412,26 +555,44 @@ mod tests { 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"); + 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(), - "one-time message for custom logger" + "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".error().skip_first(), + "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); + + reset_logging_output_handler(); Ok(()) } diff --git a/rclrs/src/logging/log_params.rs b/rclrs/src/logging/log_params.rs index d8cbc8995..79197cd98 100644 --- a/rclrs/src/logging/log_params.rs +++ b/rclrs/src/logging/log_params.rs @@ -10,13 +10,12 @@ pub struct LogParams<'a> { severity: LogSeverity, /// Specify when a log message should be published (See[`LoggingOccurrence`] above) occurs: LogOccurrence, - /// Specify the publication interval of the message. A value of ZERO (0) indicates that the - /// message should be published every time, 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. instead - /// of publishing a log message every 10 milliseconds, the `publish_interval` can be configured - /// such that the message is published every 10 seconds. - interval: Duration, + /// 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, /// The log message will only published if the specified expression evaluates to true only_if: bool, } @@ -28,7 +27,7 @@ impl<'a> LogParams<'a> { logger_name, severity: Default::default(), occurs: Default::default(), - interval: Duration::new(0, 0), + throttle: Duration::new(0, 0), only_if: true, } } @@ -48,9 +47,9 @@ impl<'a> LogParams<'a> { self.occurs } - /// Get the interval - pub fn get_interval(&self) -> Duration { - self.interval + /// Get the throttle interval duration + pub fn get_throttle(&self) -> Duration { + self.throttle } /// Get the arbitrary filter set by the user @@ -88,14 +87,14 @@ pub trait ToLogParams<'a>: Sized { params } - /// Set an interval during which this log will not publish. A value of zero - /// will never block the message from being published, and this is the + /// 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 interval(self, interval: Duration) -> LogParams<'a> { + fn throttle(self, throttle: Duration) -> LogParams<'a> { let mut params = self.to_log_params(); - params.interval = interval; + params.throttle = throttle; params } @@ -103,7 +102,7 @@ pub trait ToLogParams<'a>: Sized { /// this function. /// /// Other factors may prevent the log from being published if a `true` is - /// passed in, such as `ToLogParams::interval` or `ToLogParams::once` + /// 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(); @@ -162,8 +161,13 @@ pub enum LoggerName<'a> { } /// 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)] +#[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) @@ -199,6 +203,22 @@ impl LogSeverity { 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 { diff --git a/rclrs/src/logging/logging_configuration.rs b/rclrs/src/logging/logging_configuration.rs index 2f7014273..1012608ec 100644 --- a/rclrs/src/logging/logging_configuration.rs +++ b/rclrs/src/logging/logging_configuration.rs @@ -1,12 +1,6 @@ use std::sync::{Arc, Mutex, OnceLock, Weak}; -use crate::{ - rcl_bindings::{ - rcl_arguments_t, rcl_context_t, rcl_logging_configure, rcl_logging_fini, - rcutils_get_default_allocator, - }, - RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX, -}; +use crate::{rcl_bindings::*, RclrsError, ToResult, ENTITY_LIFECYCLE_MUTEX}; struct LoggingConfiguration { lifecycle: Mutex>, @@ -55,3 +49,203 @@ impl Drop for LoggingLifecycle { } } } + +#[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/parameter.rs b/rclrs/src/parameter.rs index c7740a2d5..3c49993b3 100644 --- a/rclrs/src/parameter.rs +++ b/rclrs/src/parameter.rs @@ -27,18 +27,18 @@ use std::{ // Among the most relevant ones: // // * Parameter declaration returns an object which will be the main accessor to the parameter, -// providing getters and, except for read only parameters, setters. Object destruction will -// undeclare the parameter. +// providing getters and, except for read only parameters, setters. Object destruction will +// undeclare the parameter. // * Declaration uses a builder pattern to specify ranges, description, human readable constraints -// instead of an ParameterDescriptor argument. +// instead of an ParameterDescriptor argument. // * Parameters properties of read only and dynamic are embedded in their type rather than being a -// boolean parameter. +// boolean parameter. // * There are no runtime exceptions for common cases such as undeclared parameter, already -// declared, or uninitialized. +// declared, or uninitialized. // * There is no "parameter not set" type, users can instead decide to have a `Mandatory` parameter -// that must always have a value or `Optional` parameter that can be unset. +// that must always have a value or `Optional` parameter that can be unset. // * Explicit API for access to undeclared parameters by having a -// `node.use_undeclared_parameters()` API that allows access to all parameters. +// `node.use_undeclared_parameters()` API that allows access to all parameters. #[derive(Clone, Debug)] struct ParameterOptionsStorage { @@ -703,9 +703,9 @@ impl<'a> Parameters<'a> { /// Returns: /// * `Ok(())` if setting was successful. /// * [`Err(DeclarationError::TypeMismatch)`] if the type of the requested value is different - /// from the parameter's type. + /// from the parameter's type. /// * [`Err(DeclarationError::OutOfRange)`] if the requested value is out of the parameter's - /// range. + /// range. /// * [`Err(DeclarationError::ReadOnly)`] if the parameter is read only. pub fn set( &self, diff --git a/rclrs/src/parameter/range.rs b/rclrs/src/parameter/range.rs index 96f66d6e3..6a46d2ff0 100644 --- a/rclrs/src/parameter/range.rs +++ b/rclrs/src/parameter/range.rs @@ -32,7 +32,7 @@ impl From<()> for ParameterRanges { /// Usually only one of these ranges will be applied, but all have to be stored since: /// /// * A dynamic parameter can change its type at runtime, in which case a different range could be -/// applied. +/// applied. /// * Introspection through service calls requires all the ranges to be reported to the user. #[derive(Clone, Debug, Default)] pub struct ParameterRanges { diff --git a/rclrs/src/subscription.rs b/rclrs/src/subscription.rs index 36c241d19..fbd518c21 100644 --- a/rclrs/src/subscription.rs +++ b/rclrs/src/subscription.rs @@ -272,9 +272,7 @@ where } fn execute(&self) -> Result<(), RclrsError> { - // Immediately evaluated closure, to handle SubscriptionTakeFailed - // outside this match - match (|| { + let evaluate = || { match &mut *self.callback.lock().unwrap() { AnySubscriptionCallback::Regular(cb) => { let (msg, _) = self.take()?; @@ -302,7 +300,11 @@ where } } Ok(()) - })() { + }; + + // Immediately evaluated closure, to handle SubscriptionTakeFailed + // outside this match + match evaluate() { Err(RclrsError::RclError { code: RclReturnCode::SubscriptionTakeFailed, .. diff --git a/rclrs/src/subscription/message_info.rs b/rclrs/src/subscription/message_info.rs index 010bf28ec..1adecd3ec 100644 --- a/rclrs/src/subscription/message_info.rs +++ b/rclrs/src/subscription/message_info.rs @@ -4,26 +4,28 @@ use crate::rcl_bindings::*; /// An identifier for a publisher in the local context. /// -/// To quote the `rmw` documentation: +/// To quote the [`rmw` documentation][1]: /// /// > The identifier uniquely identifies the publisher for the local context, but -/// it will not necessarily be the same identifier given in other contexts or processes -/// for the same publisher. -/// Therefore the identifier will uniquely identify the publisher within your application -/// but may disagree about the identifier for that publisher when compared to another -/// application. -/// Even with this limitation, when combined with the publisher sequence number it can -/// uniquely identify a message within your local context. -/// Publisher GIDs generated by the RMW implementation could collide at some point, in which -/// case it is not possible to distinguish which publisher sent the message. -/// The details of how GIDs are generated are RMW implementation dependent. -/// +/// > it will not necessarily be the same identifier given in other contexts or processes +/// > for the same publisher. +/// > Therefore the identifier will uniquely identify the publisher within your application +/// > but may disagree about the identifier for that publisher when compared to another +/// > application. +/// > Even with this limitation, when combined with the publisher sequence number it can +/// > uniquely identify a message within your local context. +/// > Publisher GIDs generated by the RMW implementation could collide at some point, in which +/// > case it is not possible to distinguish which publisher sent the message. +/// > The details of how GIDs are generated are RMW implementation dependent. +/// > /// > It is possible the the RMW implementation needs to reuse a publisher GID, -/// due to running out of unique identifiers or some other constraint, in which case -/// the RMW implementation may document what happens in that case, but that -/// behavior is not defined here. -/// However, this should be avoided, if at all possible, by the RMW implementation, -/// and should be unlikely to happen in practice. +/// > due to running out of unique identifiers or some other constraint, in which case +/// > the RMW implementation may document what happens in that case, but that +/// > behavior is not defined here. +/// > However, this should be avoided, if at all possible, by the RMW implementation, +/// > and should be unlikely to happen in practice. +/// +/// [1]: https://docs.ros.org/en/rolling/p/rmw/generated/structrmw__message__info__s.html#_CPPv4N18rmw_message_info_s13publisher_gidE #[derive(Clone, Debug, PartialEq, Eq)] pub struct PublisherGid { /// Bytes identifying a publisher in the RMW implementation. diff --git a/rclrs/src/vendor/action_msgs/mod.rs b/rclrs/src/vendor/action_msgs/mod.rs new file mode 100644 index 000000000..f43deda46 --- /dev/null +++ b/rclrs/src/vendor/action_msgs/mod.rs @@ -0,0 +1,7 @@ +#![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] + +pub mod msg; + +pub mod srv; diff --git a/rclrs/src/vendor/action_msgs/msg.rs b/rclrs/src/vendor/action_msgs/msg.rs new file mode 100644 index 000000000..32c7b7089 --- /dev/null +++ b/rclrs/src/vendor/action_msgs/msg.rs @@ -0,0 +1,457 @@ +pub mod rmw { + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalInfo( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__msg__GoalInfo__init(msg: *mut GoalInfo) -> bool; + fn action_msgs__msg__GoalInfo__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__msg__GoalInfo__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__msg__GoalInfo__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__msg__GoalInfo + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct GoalInfo { + pub goal_id: crate::vendor::unique_identifier_msgs::msg::rmw::UUID, + pub stamp: crate::vendor::builtin_interfaces::msg::rmw::Time, + } + + impl Default for GoalInfo { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__msg__GoalInfo__init(&mut msg as *mut _) { + panic!("Call to action_msgs__msg__GoalInfo__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for GoalInfo { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalInfo__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalInfo__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalInfo__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for GoalInfo { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for GoalInfo + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/msg/GoalInfo"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalInfo() + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatus( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__msg__GoalStatus__init(msg: *mut GoalStatus) -> bool; + fn action_msgs__msg__GoalStatus__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__msg__GoalStatus__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__msg__GoalStatus__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__msg__GoalStatus + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct GoalStatus { + pub goal_info: crate::vendor::action_msgs::msg::rmw::GoalInfo, + pub status: i8, + } + + impl GoalStatus { + /// Indicates status has not been properly set. + pub const STATUS_UNKNOWN: i8 = 0; + /// The goal has been accepted and is awaiting execution. + pub const STATUS_ACCEPTED: i8 = 1; + /// The goal is currently being executed by the action server. + pub const STATUS_EXECUTING: i8 = 2; + /// The client has requested that the goal be canceled and the action server has + /// accepted the cancel request. + pub const STATUS_CANCELING: i8 = 3; + /// The goal was achieved successfully by the action server. + pub const STATUS_SUCCEEDED: i8 = 4; + /// The goal was canceled after an external request from an action client. + pub const STATUS_CANCELED: i8 = 5; + /// The goal was terminated by the action server without an external request. + pub const STATUS_ABORTED: i8 = 6; + } + + impl Default for GoalStatus { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__msg__GoalStatus__init(&mut msg as *mut _) { + panic!("Call to action_msgs__msg__GoalStatus__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for GoalStatus { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatus__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatus__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatus__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for GoalStatus { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for GoalStatus + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/msg/GoalStatus"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatus( + ) + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatusArray( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__msg__GoalStatusArray__init(msg: *mut GoalStatusArray) -> bool; + fn action_msgs__msg__GoalStatusArray__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__msg__GoalStatusArray__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__msg__GoalStatusArray__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__msg__GoalStatusArray + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct GoalStatusArray { + pub status_list: + rosidl_runtime_rs::Sequence, + } + + impl Default for GoalStatusArray { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__msg__GoalStatusArray__init(&mut msg as *mut _) { + panic!("Call to action_msgs__msg__GoalStatusArray__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for GoalStatusArray { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatusArray__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatusArray__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__msg__GoalStatusArray__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for GoalStatusArray { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for GoalStatusArray + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/msg/GoalStatusArray"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__msg__GoalStatusArray() + } + } + } +} // mod rmw + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct GoalInfo { + pub goal_id: crate::vendor::unique_identifier_msgs::msg::UUID, + pub stamp: crate::vendor::builtin_interfaces::msg::Time, +} + +impl Default for GoalInfo { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::msg::rmw::GoalInfo::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for GoalInfo { + type RmwMsg = crate::vendor::action_msgs::msg::rmw::GoalInfo; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_id: crate::vendor::unique_identifier_msgs::msg::UUID::into_rmw_message( + std::borrow::Cow::Owned(msg.goal_id), + ) + .into_owned(), + stamp: crate::vendor::builtin_interfaces::msg::Time::into_rmw_message( + std::borrow::Cow::Owned(msg.stamp), + ) + .into_owned(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_id: crate::vendor::unique_identifier_msgs::msg::UUID::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.goal_id), + ) + .into_owned(), + stamp: crate::vendor::builtin_interfaces::msg::Time::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.stamp), + ) + .into_owned(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + goal_id: crate::vendor::unique_identifier_msgs::msg::UUID::from_rmw_message( + msg.goal_id, + ), + stamp: crate::vendor::builtin_interfaces::msg::Time::from_rmw_message(msg.stamp), + } + } +} + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct GoalStatus { + pub goal_info: crate::vendor::action_msgs::msg::GoalInfo, + pub status: i8, +} + +impl GoalStatus { + /// Indicates status has not been properly set. + pub const STATUS_UNKNOWN: i8 = 0; + /// The goal has been accepted and is awaiting execution. + pub const STATUS_ACCEPTED: i8 = 1; + /// The goal is currently being executed by the action server. + pub const STATUS_EXECUTING: i8 = 2; + /// The client has requested that the goal be canceled and the action server has + /// accepted the cancel request. + pub const STATUS_CANCELING: i8 = 3; + /// The goal was achieved successfully by the action server. + pub const STATUS_SUCCEEDED: i8 = 4; + /// The goal was canceled after an external request from an action client. + pub const STATUS_CANCELED: i8 = 5; + /// The goal was terminated by the action server without an external request. + pub const STATUS_ABORTED: i8 = 6; +} + +impl Default for GoalStatus { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::msg::rmw::GoalStatus::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for GoalStatus { + type RmwMsg = crate::vendor::action_msgs::msg::rmw::GoalStatus; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Owned(msg.goal_info), + ) + .into_owned(), + status: msg.status, + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.goal_info), + ) + .into_owned(), + status: msg.status, + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::from_rmw_message(msg.goal_info), + status: msg.status, + } + } +} + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct GoalStatusArray { + pub status_list: Vec, +} + +impl Default for GoalStatusArray { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::msg::rmw::GoalStatusArray::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for GoalStatusArray { + type RmwMsg = crate::vendor::action_msgs::msg::rmw::GoalStatusArray; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + status_list: msg + .status_list + .into_iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalStatus::into_rmw_message( + std::borrow::Cow::Owned(elem), + ) + .into_owned() + }) + .collect(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + status_list: msg + .status_list + .iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalStatus::into_rmw_message( + std::borrow::Cow::Borrowed(elem), + ) + .into_owned() + }) + .collect(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + status_list: msg + .status_list + .into_iter() + .map(crate::vendor::action_msgs::msg::GoalStatus::from_rmw_message) + .collect(), + } + } +} diff --git a/rclrs/src/vendor/action_msgs/srv.rs b/rclrs/src/vendor/action_msgs/srv.rs new file mode 100644 index 000000000..f4cf6fe53 --- /dev/null +++ b/rclrs/src/vendor/action_msgs/srv.rs @@ -0,0 +1,375 @@ +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct CancelGoal_Request { + pub goal_info: crate::vendor::action_msgs::msg::GoalInfo, +} + +impl Default for CancelGoal_Request { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::srv::rmw::CancelGoal_Request::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for CancelGoal_Request { + type RmwMsg = crate::vendor::action_msgs::srv::rmw::CancelGoal_Request; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Owned(msg.goal_info), + ) + .into_owned(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Borrowed(&msg.goal_info), + ) + .into_owned(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + goal_info: crate::vendor::action_msgs::msg::GoalInfo::from_rmw_message(msg.goal_info), + } + } +} + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct CancelGoal_Response { + pub return_code: i8, + pub goals_canceling: Vec, +} + +impl CancelGoal_Response { + /// Indicates the request was accepted without any errors. + /// + /// One or more goals have transitioned to the CANCELING state. The + /// goals_canceling list is not empty. + pub const ERROR_NONE: i8 = 0; + /// Indicates the request was rejected. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_REJECTED: i8 = 1; + /// Indicates the requested goal ID does not exist. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_UNKNOWN_GOAL_ID: i8 = 2; + /// Indicates the goal is not cancelable because it is already in a terminal state. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_GOAL_TERMINATED: i8 = 3; +} + +impl Default for CancelGoal_Response { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::action_msgs::srv::rmw::CancelGoal_Response::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for CancelGoal_Response { + type RmwMsg = crate::vendor::action_msgs::srv::rmw::CancelGoal_Response; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + return_code: msg.return_code, + goals_canceling: msg + .goals_canceling + .into_iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Owned(elem), + ) + .into_owned() + }) + .collect(), + }), + std::borrow::Cow::Borrowed(msg) => std::borrow::Cow::Owned(Self::RmwMsg { + return_code: msg.return_code, + goals_canceling: msg + .goals_canceling + .iter() + .map(|elem| { + crate::vendor::action_msgs::msg::GoalInfo::into_rmw_message( + std::borrow::Cow::Borrowed(elem), + ) + .into_owned() + }) + .collect(), + }), + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { + return_code: msg.return_code, + goals_canceling: msg + .goals_canceling + .into_iter() + .map(crate::vendor::action_msgs::msg::GoalInfo::from_rmw_message) + .collect(), + } + } +} + +#[link(name = "action_msgs__rosidl_typesupport_c")] +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal( + ) -> *const std::ffi::c_void; +} + +// Corresponds to action_msgs__srv__CancelGoal +pub struct CancelGoal; + +impl rosidl_runtime_rs::Service for CancelGoal { + type Request = crate::vendor::action_msgs::srv::CancelGoal_Request; + type Response = crate::vendor::action_msgs::srv::CancelGoal_Response; + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal() + } + } +} + +pub mod rmw { + + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Request( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__srv__CancelGoal_Request__init(msg: *mut CancelGoal_Request) -> bool; + fn action_msgs__srv__CancelGoal_Request__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__srv__CancelGoal_Request__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__srv__CancelGoal_Request__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__srv__CancelGoal_Request + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct CancelGoal_Request { + pub goal_info: crate::vendor::action_msgs::msg::rmw::GoalInfo, + } + + impl Default for CancelGoal_Request { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__srv__CancelGoal_Request__init(&mut msg as *mut _) { + panic!("Call to action_msgs__srv__CancelGoal_Request__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for CancelGoal_Request { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Request__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Request__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { + action_msgs__srv__CancelGoal_Request__Sequence__copy(in_seq, out_seq as *mut _) + } + } + } + + impl rosidl_runtime_rs::Message for CancelGoal_Request { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for CancelGoal_Request + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/srv/CancelGoal_Request"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Request() + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Response( + ) -> *const std::ffi::c_void; + } + + #[link(name = "action_msgs__rosidl_generator_c")] + extern "C" { + fn action_msgs__srv__CancelGoal_Response__init(msg: *mut CancelGoal_Response) -> bool; + fn action_msgs__srv__CancelGoal_Response__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn action_msgs__srv__CancelGoal_Response__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn action_msgs__srv__CancelGoal_Response__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to action_msgs__srv__CancelGoal_Response + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct CancelGoal_Response { + pub return_code: i8, + pub goals_canceling: + rosidl_runtime_rs::Sequence, + } + + impl CancelGoal_Response { + /// Indicates the request was accepted without any errors. + /// + /// One or more goals have transitioned to the CANCELING state. The + /// goals_canceling list is not empty. + pub const ERROR_NONE: i8 = 0; + /// Indicates the request was rejected. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_REJECTED: i8 = 1; + /// Indicates the requested goal ID does not exist. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_UNKNOWN_GOAL_ID: i8 = 2; + /// Indicates the goal is not cancelable because it is already in a terminal state. + /// + /// No goals have transitioned to the CANCELING state. The goals_canceling list is + /// empty. + pub const ERROR_GOAL_TERMINATED: i8 = 3; + } + + impl Default for CancelGoal_Response { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !action_msgs__srv__CancelGoal_Response__init(&mut msg as *mut _) { + panic!("Call to action_msgs__srv__CancelGoal_Response__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for CancelGoal_Response { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Response__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { action_msgs__srv__CancelGoal_Response__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { + action_msgs__srv__CancelGoal_Response__Sequence__copy(in_seq, out_seq as *mut _) + } + } + } + + impl rosidl_runtime_rs::Message for CancelGoal_Response { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for CancelGoal_Response + where + Self: Sized, + { + const TYPE_NAME: &'static str = "action_msgs/srv/CancelGoal_Response"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__action_msgs__srv__CancelGoal_Response() + } + } + } + + #[link(name = "action_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal( + ) -> *const std::ffi::c_void; + } + + // Corresponds to action_msgs__srv__CancelGoal + pub struct CancelGoal; + + impl rosidl_runtime_rs::Service for CancelGoal { + type Request = crate::vendor::action_msgs::srv::rmw::CancelGoal_Request; + type Response = crate::vendor::action_msgs::srv::rmw::CancelGoal_Response; + + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_service_type_support_handle__action_msgs__srv__CancelGoal( + ) + } + } + } +} // mod rmw diff --git a/rclrs/src/vendor/builtin_interfaces/mod.rs b/rclrs/src/vendor/builtin_interfaces/mod.rs index a6365b3b8..4c61d56a1 100644 --- a/rclrs/src/vendor/builtin_interfaces/mod.rs +++ b/rclrs/src/vendor/builtin_interfaces/mod.rs @@ -1,3 +1,5 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] pub mod msg; diff --git a/rclrs/src/vendor/builtin_interfaces/msg.rs b/rclrs/src/vendor/builtin_interfaces/msg.rs index 371803171..fd2ec47cd 100644 --- a/rclrs/src/vendor/builtin_interfaces/msg.rs +++ b/rclrs/src/vendor/builtin_interfaces/msg.rs @@ -5,7 +5,7 @@ pub mod rmw { #[link(name = "builtin_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Duration( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "builtin_interfaces__rosidl_generator_c")] @@ -80,7 +80,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "builtin_interfaces/msg/Duration"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Duration() @@ -91,7 +91,7 @@ pub mod rmw { #[link(name = "builtin_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Time( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "builtin_interfaces__rosidl_generator_c")] @@ -166,7 +166,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "builtin_interfaces/msg/Time"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__builtin_interfaces__msg__Time( diff --git a/rclrs/src/vendor/mod.rs b/rclrs/src/vendor/mod.rs index fe87087b1..df638ee7f 100644 --- a/rclrs/src/vendor/mod.rs +++ b/rclrs/src/vendor/mod.rs @@ -1,7 +1,8 @@ //! Created by vendor_interfaces.py #![allow(dead_code)] -#![allow(clippy::derive_partial_eq_without_eq)] +pub mod action_msgs; pub mod builtin_interfaces; pub mod rcl_interfaces; pub mod rosgraph_msgs; +pub mod unique_identifier_msgs; diff --git a/rclrs/src/vendor/rcl_interfaces/mod.rs b/rclrs/src/vendor/rcl_interfaces/mod.rs index ff96b59a9..f43deda46 100644 --- a/rclrs/src/vendor/rcl_interfaces/mod.rs +++ b/rclrs/src/vendor/rcl_interfaces/mod.rs @@ -1,4 +1,6 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] pub mod msg; diff --git a/rclrs/src/vendor/rcl_interfaces/msg.rs b/rclrs/src/vendor/rcl_interfaces/msg.rs index 39c029ae1..7f6dbcc58 100644 --- a/rclrs/src/vendor/rcl_interfaces/msg.rs +++ b/rclrs/src/vendor/rcl_interfaces/msg.rs @@ -5,7 +5,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__FloatingPointRange( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -83,7 +83,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/FloatingPointRange"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__FloatingPointRange() @@ -94,7 +94,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__IntegerRange( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -170,7 +170,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/IntegerRange"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__IntegerRange() @@ -181,7 +181,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ListParametersResult( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -260,7 +260,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ListParametersResult"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ListParametersResult() @@ -271,7 +271,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -364,7 +364,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/Log"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log() @@ -375,7 +375,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterDescriptor( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -464,7 +464,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterDescriptor"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterDescriptor() @@ -475,7 +475,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEventDescriptors( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -566,7 +566,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterEventDescriptors"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEventDescriptors() @@ -577,7 +577,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEvent( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -660,7 +660,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterEvent"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterEvent() @@ -671,7 +671,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Parameter( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -746,7 +746,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/Parameter"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Parameter() @@ -757,7 +757,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterType( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -845,7 +845,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterType"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterType() @@ -856,7 +856,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterValue( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -941,7 +941,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/ParameterValue"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__ParameterValue() @@ -952,7 +952,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__SetParametersResult( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1029,7 +1029,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/msg/SetParametersResult"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__SetParametersResult() diff --git a/rclrs/src/vendor/rcl_interfaces/srv.rs b/rclrs/src/vendor/rcl_interfaces/srv.rs index 221d7eac1..3f43398a1 100644 --- a/rclrs/src/vendor/rcl_interfaces/srv.rs +++ b/rclrs/src/vendor/rcl_interfaces/srv.rs @@ -582,7 +582,7 @@ impl rosidl_runtime_rs::Message for SetParameters_Response { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__DescribeParameters @@ -592,7 +592,7 @@ impl rosidl_runtime_rs::Service for DescribeParameters { type Request = crate::vendor::rcl_interfaces::srv::DescribeParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::DescribeParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters() @@ -603,7 +603,7 @@ impl rosidl_runtime_rs::Service for DescribeParameters { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameters @@ -613,7 +613,7 @@ impl rosidl_runtime_rs::Service for GetParameters { type Request = crate::vendor::rcl_interfaces::srv::GetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::GetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters() @@ -624,7 +624,7 @@ impl rosidl_runtime_rs::Service for GetParameters { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameterTypes @@ -634,7 +634,7 @@ impl rosidl_runtime_rs::Service for GetParameterTypes { type Request = crate::vendor::rcl_interfaces::srv::GetParameterTypes_Request; type Response = crate::vendor::rcl_interfaces::srv::GetParameterTypes_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes() @@ -645,7 +645,7 @@ impl rosidl_runtime_rs::Service for GetParameterTypes { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__ListParameters @@ -655,7 +655,7 @@ impl rosidl_runtime_rs::Service for ListParameters { type Request = crate::vendor::rcl_interfaces::srv::ListParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::ListParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters() @@ -666,7 +666,7 @@ impl rosidl_runtime_rs::Service for ListParameters { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParametersAtomically @@ -676,7 +676,7 @@ impl rosidl_runtime_rs::Service for SetParametersAtomically { type Request = crate::vendor::rcl_interfaces::srv::SetParametersAtomically_Request; type Response = crate::vendor::rcl_interfaces::srv::SetParametersAtomically_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically() @@ -687,7 +687,7 @@ impl rosidl_runtime_rs::Service for SetParametersAtomically { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParameters @@ -697,7 +697,7 @@ impl rosidl_runtime_rs::Service for SetParameters { type Request = crate::vendor::rcl_interfaces::srv::SetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::SetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters() @@ -706,13 +706,14 @@ impl rosidl_runtime_rs::Service for SetParameters { } pub mod rmw { + #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -799,7 +800,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/DescribeParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Request() @@ -810,7 +811,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -902,7 +903,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/DescribeParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__DescribeParameters_Response() @@ -913,7 +914,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -996,7 +997,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Request() @@ -1007,7 +1008,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1091,7 +1092,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameters_Response() @@ -1102,7 +1103,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1185,7 +1186,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameterTypes_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Request() @@ -1196,7 +1197,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1283,7 +1284,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/GetParameterTypes_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__GetParameterTypes_Response() @@ -1294,7 +1295,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1382,7 +1383,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/ListParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Request() @@ -1393,7 +1394,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1476,7 +1477,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/ListParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__ListParameters_Response() @@ -1487,7 +1488,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1576,7 +1577,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParametersAtomically_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Request() @@ -1587,7 +1588,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1676,7 +1677,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParametersAtomically_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParametersAtomically_Response() @@ -1687,7 +1688,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Request( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1771,7 +1772,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParameters_Request"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Request() @@ -1782,7 +1783,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Response( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rcl_interfaces__rosidl_generator_c")] @@ -1867,7 +1868,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rcl_interfaces/srv/SetParameters_Response"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__srv__SetParameters_Response() @@ -1878,7 +1879,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__DescribeParameters @@ -1888,7 +1889,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::DescribeParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::DescribeParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__DescribeParameters() @@ -1899,7 +1900,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameters @@ -1909,7 +1910,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::GetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::GetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameters() @@ -1920,7 +1921,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__GetParameterTypes @@ -1930,7 +1931,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::GetParameterTypes_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::GetParameterTypes_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__GetParameterTypes() @@ -1941,7 +1942,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__ListParameters @@ -1951,7 +1952,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::ListParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::ListParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__ListParameters() @@ -1962,7 +1963,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParametersAtomically @@ -1972,7 +1973,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::SetParametersAtomically_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::SetParametersAtomically_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParametersAtomically() @@ -1983,7 +1984,7 @@ pub mod rmw { #[link(name = "rcl_interfaces__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } // Corresponds to rcl_interfaces__srv__SetParameters @@ -1993,7 +1994,7 @@ pub mod rmw { type Request = crate::vendor::rcl_interfaces::srv::rmw::SetParameters_Request; type Response = crate::vendor::rcl_interfaces::srv::rmw::SetParameters_Response; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_service_type_support_handle__rcl_interfaces__srv__SetParameters() diff --git a/rclrs/src/vendor/rosgraph_msgs/mod.rs b/rclrs/src/vendor/rosgraph_msgs/mod.rs index a6365b3b8..4c61d56a1 100644 --- a/rclrs/src/vendor/rosgraph_msgs/mod.rs +++ b/rclrs/src/vendor/rosgraph_msgs/mod.rs @@ -1,3 +1,5 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] pub mod msg; diff --git a/rclrs/src/vendor/rosgraph_msgs/msg.rs b/rclrs/src/vendor/rosgraph_msgs/msg.rs index 1f4af8b70..6190bfe62 100644 --- a/rclrs/src/vendor/rosgraph_msgs/msg.rs +++ b/rclrs/src/vendor/rosgraph_msgs/msg.rs @@ -5,7 +5,7 @@ pub mod rmw { #[link(name = "rosgraph_msgs__rosidl_typesupport_c")] extern "C" { fn rosidl_typesupport_c__get_message_type_support_handle__rosgraph_msgs__msg__Clock( - ) -> *const std::os::raw::c_void; + ) -> *const std::ffi::c_void; } #[link(name = "rosgraph_msgs__rosidl_generator_c")] @@ -77,7 +77,7 @@ pub mod rmw { Self: Sized, { const TYPE_NAME: &'static str = "rosgraph_msgs/msg/Clock"; - fn get_type_support() -> *const std::os::raw::c_void { + fn get_type_support() -> *const std::ffi::c_void { // SAFETY: No preconditions for this function. unsafe { rosidl_typesupport_c__get_message_type_support_handle__rosgraph_msgs__msg__Clock() diff --git a/rclrs/src/vendor/unique_identifier_msgs/mod.rs b/rclrs/src/vendor/unique_identifier_msgs/mod.rs new file mode 100644 index 000000000..4c61d56a1 --- /dev/null +++ b/rclrs/src/vendor/unique_identifier_msgs/mod.rs @@ -0,0 +1,5 @@ +#![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] + +pub mod msg; diff --git a/rclrs/src/vendor/unique_identifier_msgs/msg.rs b/rclrs/src/vendor/unique_identifier_msgs/msg.rs new file mode 100644 index 000000000..7d6085fb1 --- /dev/null +++ b/rclrs/src/vendor/unique_identifier_msgs/msg.rs @@ -0,0 +1,125 @@ +pub mod rmw { + #[cfg(feature = "serde")] + use serde::{Deserialize, Serialize}; + + #[link(name = "unique_identifier_msgs__rosidl_typesupport_c")] + extern "C" { + fn rosidl_typesupport_c__get_message_type_support_handle__unique_identifier_msgs__msg__UUID( + ) -> *const std::ffi::c_void; + } + + #[link(name = "unique_identifier_msgs__rosidl_generator_c")] + extern "C" { + fn unique_identifier_msgs__msg__UUID__init(msg: *mut UUID) -> bool; + fn unique_identifier_msgs__msg__UUID__Sequence__init( + seq: *mut rosidl_runtime_rs::Sequence, + size: usize, + ) -> bool; + fn unique_identifier_msgs__msg__UUID__Sequence__fini( + seq: *mut rosidl_runtime_rs::Sequence, + ); + fn unique_identifier_msgs__msg__UUID__Sequence__copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: *mut rosidl_runtime_rs::Sequence, + ) -> bool; + } + + // Corresponds to unique_identifier_msgs__msg__UUID + #[repr(C)] + #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] + #[derive(Clone, Debug, PartialEq, PartialOrd)] + pub struct UUID { + pub uuid: [u8; 16], + } + + impl Default for UUID { + fn default() -> Self { + unsafe { + let mut msg = std::mem::zeroed(); + if !unique_identifier_msgs__msg__UUID__init(&mut msg as *mut _) { + panic!("Call to unique_identifier_msgs__msg__UUID__init() failed"); + } + msg + } + } + } + + impl rosidl_runtime_rs::SequenceAlloc for UUID { + fn sequence_init(seq: &mut rosidl_runtime_rs::Sequence, size: usize) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { unique_identifier_msgs__msg__UUID__Sequence__init(seq as *mut _, size) } + } + fn sequence_fini(seq: &mut rosidl_runtime_rs::Sequence) { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { unique_identifier_msgs__msg__UUID__Sequence__fini(seq as *mut _) } + } + fn sequence_copy( + in_seq: &rosidl_runtime_rs::Sequence, + out_seq: &mut rosidl_runtime_rs::Sequence, + ) -> bool { + // SAFETY: This is safe since the pointer is guaranteed to be valid/initialized. + unsafe { unique_identifier_msgs__msg__UUID__Sequence__copy(in_seq, out_seq as *mut _) } + } + } + + impl rosidl_runtime_rs::Message for UUID { + type RmwMsg = Self; + fn into_rmw_message( + msg_cow: std::borrow::Cow<'_, Self>, + ) -> std::borrow::Cow<'_, Self::RmwMsg> { + msg_cow + } + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + msg + } + } + + impl rosidl_runtime_rs::RmwMessage for UUID + where + Self: Sized, + { + const TYPE_NAME: &'static str = "unique_identifier_msgs/msg/UUID"; + fn get_type_support() -> *const std::ffi::c_void { + // SAFETY: No preconditions for this function. + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__unique_identifier_msgs__msg__UUID() + } + } + } +} // mod rmw + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] +#[derive(Clone, Debug, PartialEq, PartialOrd)] +pub struct UUID { + pub uuid: [u8; 16], +} + +impl Default for UUID { + fn default() -> Self { + ::from_rmw_message( + crate::vendor::unique_identifier_msgs::msg::rmw::UUID::default(), + ) + } +} + +impl rosidl_runtime_rs::Message for UUID { + type RmwMsg = crate::vendor::unique_identifier_msgs::msg::rmw::UUID; + + fn into_rmw_message(msg_cow: std::borrow::Cow<'_, Self>) -> std::borrow::Cow<'_, Self::RmwMsg> { + match msg_cow { + std::borrow::Cow::Owned(msg) => { + std::borrow::Cow::Owned(Self::RmwMsg { uuid: msg.uuid }) + } + std::borrow::Cow::Borrowed(msg) => { + std::borrow::Cow::Owned(Self::RmwMsg { uuid: msg.uuid }) + } + } + } + + fn from_rmw_message(msg: Self::RmwMsg) -> Self { + Self { uuid: msg.uuid } + } +} diff --git a/rclrs/src/wait.rs b/rclrs/src/wait.rs index 2ef99c026..243c9d857 100644 --- a/rclrs/src/wait.rs +++ b/rclrs/src/wait.rs @@ -329,7 +329,7 @@ impl WaitSet { /// /// - Passing a wait set with no wait-able items in it will return an error. /// - The timeout must not be so large so as to overflow an `i64` with its nanosecond - /// representation, or an error will occur. + /// representation, or an error will occur. /// /// This list is not comprehensive, since further errors may occur in the `rmw` or `rcl` layers. /// diff --git a/rclrs/vendor_interfaces.py b/rclrs/vendor_interfaces.py old mode 100644 new mode 100755 index 6a0066869..8ffb4e4e0 --- a/rclrs/vendor_interfaces.py +++ b/rclrs/vendor_interfaces.py @@ -1,65 +1,70 @@ -# This script produces the `vendor` module inside `rclrs` by copying -# the generated code for the `rosgraph_msgs` and `rcl_interfaces` packages and -# its dependency `builtin_interfaces` and adjusting the submodule paths in the -# code. +#!/usr/bin/env python3 +# This script produces the `vendor` module inside `rclrs` by copying the +# generated code for the `rosgraph_msgs`, `rcl_interfaces`, and `action_msgs` +# packages and their dependencies `builtin_interfaces` and +# `unique_identifier_msgs` and adjusting the submodule paths in the code. # If these packages, or the `rosidl_generator_rs`, get changed, you can # update the `vendor` module by running this script. -# The purpose is to avoid an external dependency on `rcl_interfaces`, which -# is not published on crates.io. +# The purpose is to avoid an external dependency on these message packages, +# which are not published on crates.io. import argparse from pathlib import Path import shutil import subprocess +vendored_packages = [ + "action_msgs", + "builtin_interfaces", + "rcl_interfaces", + "rosgraph_msgs", + "unique_identifier_msgs", +] + def get_args(): - parser = argparse.ArgumentParser(description='Vendor the rcl_interfaces, builtin_interfaces and rosgraph_msgs packages into rclrs') + parser = argparse.ArgumentParser(description='Vendor interface packages into rclrs') parser.add_argument('install_base', metavar='install_base', type=Path, help='the install base (must have non-merged layout)') return parser.parse_args() -def adjust(pkg, text): - text = text.replace('builtin_interfaces::', 'crate::vendor::builtin_interfaces::') - text = text.replace('rcl_interfaces::', 'crate::vendor::rcl_interfaces::') - text = text.replace('rosgraph_msgs::', 'crate::vendor::rosgraph_msgs::') - text = text.replace('crate::msg', f'crate::vendor::{pkg}::msg') - text = text.replace('crate::srv', f'crate::vendor::{pkg}::srv') +def adjust(current_package, text): + for pkg in vendored_packages: + text = text.replace(f'{pkg}::', f'crate::vendor::{pkg}::') + text = text.replace('crate::msg', f'crate::vendor::{current_package}::msg') + text = text.replace('crate::srv', f'crate::vendor::{current_package}::srv') + text = text.replace('crate::action', f'crate::vendor::{current_package}::action') return text def copy_adjusted(pkg, src, dst): dst.write_text(adjust(pkg, src.read_text())) subprocess.check_call(['rustfmt', str(dst)]) -mod_contents = """//! Created by {} -#![allow(dead_code)] -#![allow(clippy::derive_partial_eq_without_eq)] - -pub mod builtin_interfaces; -pub mod rcl_interfaces; -pub mod rosgraph_msgs; -""".format(Path(__file__).name) - def main(): args = get_args() assert args.install_base.is_dir(), "Install base does not exist" - assert (args.install_base / 'builtin_interfaces').is_dir(), "Install base does not contain builtin_interfaces" - assert (args.install_base / 'rcl_interfaces').is_dir(), "Install base does not contain rcl_interfaces" - assert (args.install_base / 'rosgraph_msgs').is_dir(), "Install base does not contain rosgraph_msgs" + for pkg in vendored_packages: + assert (args.install_base / pkg).is_dir(), f"Install base does not contain {pkg}" rclrs_root = Path(__file__).parent vendor_dir = rclrs_root / 'src' / 'vendor' if vendor_dir.exists(): shutil.rmtree(vendor_dir) - for pkg in ['builtin_interfaces', 'rcl_interfaces', 'rosgraph_msgs']: + for pkg in vendored_packages: src = args.install_base / pkg / 'share' / pkg / 'rust' / 'src' dst = vendor_dir / pkg dst.mkdir(parents=True) copy_adjusted(pkg, src / 'msg.rs', dst / 'msg.rs') if (src / 'srv.rs').is_file(): copy_adjusted(pkg, src / 'srv.rs', dst / 'srv.rs') + if (src / 'action.rs').is_file(): + copy_adjusted(pkg, src / 'action.rs', dst / 'action.rs') copy_adjusted(pkg, src / 'lib.rs', dst / 'mod.rs') # Rename lib.rs to mod.rs - (vendor_dir / 'mod.rs').write_text(mod_contents) - + mod_contents = "//! Created by {}\n".format(Path(__file__).name) + mod_contents += "#![allow(dead_code)]\n" + mod_contents += "\n" + for pkg in vendored_packages: + mod_contents += f"pub mod {pkg};\n" + (vendor_dir / 'mod.rs').write_text(mod_contents) if __name__ == '__main__': main() diff --git a/ros2_rust_jazzy.repos b/ros2_rust_jazzy.repos new file mode 100644 index 000000000..3f72f560d --- /dev/null +++ b/ros2_rust_jazzy.repos @@ -0,0 +1,29 @@ +repositories: + ros2/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: jazzy + ros2/example_interfaces: + type: git + url: https://github.com/ros2/example_interfaces.git + version: jazzy + ros2/rcl_interfaces: + type: git + url: https://github.com/ros2/rcl_interfaces.git + version: jazzy + ros2/test_interface_files: + type: git + url: https://github.com/ros2/test_interface_files.git + version: jazzy + ros2/rosidl_core: + type: git + url: https://github.com/ros2/rosidl_core.git + version: jazzy + ros2/rosidl_defaults: + type: git + url: https://github.com/ros2/rosidl_defaults.git + version: jazzy + ros2/unique_identifier_msgs: + type: git + url: https://github.com/ros2/unique_identifier_msgs.git + version: jazzy diff --git a/rosidl_generator_rs/resource/lib.rs.em b/rosidl_generator_rs/resource/lib.rs.em index 79a0e1def..1ef77924c 100644 --- a/rosidl_generator_rs/resource/lib.rs.em +++ b/rosidl_generator_rs/resource/lib.rs.em @@ -1,4 +1,6 @@ #![allow(non_camel_case_types)] +#![allow(clippy::derive_partial_eq_without_eq)] +#![allow(clippy::upper_case_acronyms)] @[if len(msg_specs) > 0]@ pub mod msg; From 622e887c7112463c68aef05d12c3b08be68c42ef Mon Sep 17 00:00:00 2001 From: Grey Date: Wed, 20 Nov 2024 14:36:43 +0800 Subject: [PATCH 11/11] Address feedback for logging PR Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Signed-off-by: Michael X. Grey Co-authored-by: Nathan Wiebe Neufeldt --- rclrs/src/logging.rs | 93 +++++++++++++++++++++++++++------ rclrs/src/logging/log_params.rs | 29 +++++++++- rclrs/src/logging/logger.rs | 24 ++++++--- rclrs/src/node.rs | 2 +- 4 files changed, 124 insertions(+), 24 deletions(-) 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)