Add logging to ros2_rust (#371)

mvukov · Sep 8, 2024 · 1d58e98 · 1d58e98
1 parent 4099c8d
commit 1d58e98
Show file tree

Hide file tree

Showing 9 changed files with 189 additions and 5 deletions.
diff --git a/examples/chatter/talker.rs b/examples/chatter/talker.rs
@@ -19,7 +19,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
 
     while !shut_down.load(Ordering::Relaxed) && context.ok() {
         message.data = format!("Hello, world! {}", publish_count);
-        println!("Publishing: [{}]", message.data);
+        rclrs::log_info!(node.logger_name(), "Publishing: {}", message.data);
         publisher.publish(&message)?;
         publish_count += 1;
         std::thread::sleep(std::time::Duration::from_millis(500));

diff --git a/examples/zero_copy/listener.rs b/examples/zero_copy/listener.rs
@@ -23,7 +23,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
                 .as_micros() as u64;
             let delay_us = now - msg.timestamp;
             let data_length = msg.data_length as usize;
-            println!(
+            rclrs::log_info!(node.logger_name(),
                 "Delay {} us, I heard: '{:?}'",
                 delay_us,
                 String::from_utf8(msg.data[..data_length].to_vec()).unwrap()

diff --git a/examples/zero_copy/talker.rs b/examples/zero_copy/talker.rs
@@ -37,7 +37,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         let msg_len = msg.len();
         message.data_length = msg_len as u64;
         message.data[..msg_len].copy_from_slice(msg.as_bytes());
-        println!("Publishing: {}", msg);
+        rclrs::log_info!(node.logger_name(), "Publishing: {}", msg);
         message.publish()?;
         publish_count += 1;
         std::thread::sleep(std::time::Duration::from_millis(callback_period_ms as u64));

diff --git a/repositories/patches/ros2_rust_logging.patch b/repositories/patches/ros2_rust_logging.patch
@@ -0,0 +1,42 @@
+diff --git a/rclrs/src/lib.rs b/rclrs/src/lib.rs
+index 4924b36..3a22c6d 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/node.rs b/rclrs/src/node.rs
+index 97684d6..defd1f3 100644
+--- a/rclrs/src/node.rs
++++ b/rclrs/src/node.rs
+@@ -440,6 +440,17 @@ 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();
++        let name_raw_ptr = unsafe { rcl_node_get_logger_name(&*rcl_node) };
++        if name_raw_ptr.is_null() {
++            return "";
++        }
++        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()
diff --git a/repositories/ros2_rust.BUILD.bazel b/repositories/ros2_rust.BUILD.bazel
@@ -65,7 +65,10 @@ rust_library(
     srcs = glob(
         ["rclrs/src/**/*.rs"],
         exclude = ["rclrs/src/vendor/*.rs"],
-    ) + ["rclrs/src/rcl_bindings_generated.rs"],
+    ) + [
+        "rclrs/src/rcl_bindings_generated.rs",
+        ":logging_generator",
+    ],
     edition = "2021",
     visibility = ["//visibility:public"],
     deps = [
@@ -76,6 +79,12 @@ rust_library(
     ],
 )
 
+copy_file(
+    name = "logging_generator",
+    src = "@com_github_mvukov_rules_ros2//third_party/ros2_rust_logging:logging.rs",
+    out = "rclrs/src/logging.rs",
+)
+
 GENERATOR_APP_PY = "rosidl_generator_rs_app.py"
 
 copy_file(

diff --git a/repositories/rust_setup_stage_1.bzl b/repositories/rust_setup_stage_1.bzl
@@ -21,6 +21,7 @@ def rust_setup_stage_1():
             "@com_github_mvukov_rules_ros2//repositories/patches:ros2_rust_fix_rcl_bindings.patch",
             "@com_github_mvukov_rules_ros2//repositories/patches:ros2_rust_fix_rosidl_generator.patch",
             "@com_github_mvukov_rules_ros2//repositories/patches:ros2_rust_no_msg_vendoring.patch",
+            "@com_github_mvukov_rules_ros2//repositories/patches:ros2_rust_logging.patch",
         ],
     )
 

diff --git a/ros2/test/rust/publisher.rs b/ros2/test/rust/publisher.rs
@@ -15,7 +15,9 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     let mut publish_count: u32 = 1;
 
     while !shut_down.load(Ordering::Relaxed) && context.ok() {
-        message.data = format!("Hello, world! {}", publish_count);
+        let msg = format!("Hello, world! {}", publish_count);
+        message.data = msg.clone();
+        rclrs::log_info!(node.logger_name(), "{}", msg);
         publisher.publish(&message)?;
         publish_count += 1;
         std::thread::sleep(std::time::Duration::from_millis(10));

diff --git a/third_party/ros2_rust_logging/BUILD.bazel b/third_party/ros2_rust_logging/BUILD.bazel
@@ -0,0 +1,3 @@
+exports_files([
+    "logging.rs",
+])
diff --git a/third_party/ros2_rust_logging/logging.rs b/third_party/ros2_rust_logging/logging.rs
@@ -0,0 +1,127 @@
+// 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
+
+use std::{
+    ffi::CString,
+    sync::{Mutex, Once},
+};
+
+use crate::error;
+use crate::rcl_bindings::*;
+
+static INIT: Once = Once::new();
+static LOG_GUARD: Mutex<()> = Mutex::new(());
+
+/// Don't call this directly, use the logging macros instead.
+#[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).unwrap();
+    let message = CString::new(msg).unwrap();
+    let severity = severity.to_native();
+
+    INIT.call_once(|| {
+        let ret = unsafe { rcutils_logging_initialize() };
+        if let Err(code) = error::to_rclrs_result(ret) {
+            panic!("Failed to initialize logging: {:?}", code);
+        }
+    });
+    let _guard = LOG_GUARD.lock().unwrap();
+    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,
+        }
+    }
+}
+
+/// A helper macro to log the message.
+#[macro_export]
+macro_rules! __impl_log {
+    ($logger_name:expr, $msg:expr, $file:expr, $line:expr, $severity:expr) => {{
+        $crate::log(&std::fmt::format($msg), $logger_name, $file, $line, $severity);
+    }};
+}
+
+/// Debug log message.
+#[macro_export]
+macro_rules! log_debug {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Debug)
+    }}
+}
+
+/// Info log message.
+#[macro_export]
+macro_rules! log_info {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Info)
+    }}
+}
+
+/// Warning log message.
+#[macro_export]
+macro_rules! log_warn {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Warn)
+    }}
+}
+
+/// Error log message.
+#[macro_export]
+macro_rules! log_error {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Error)
+    }}
+}
+
+/// Fatal log message.
+#[macro_export]
+macro_rules! log_fatal {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Fatal)
+    }}
+}