Add support for the rcl logging api

Cargo.toml

@@ -12,6 +12,7 @@ serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
 failure = "0.1.5"
 failure_derive = "0.1.5"
+lazy_static = "1.4.0"
 common = { path = "common", version = "0.0.1" }
 rcl = { path = "rcl", version = "0.0.1" }
 msg_gen = { path = "msg_gen", version = "0.0.1" }

examples/logging.rs

@@ -0,0 +1,32 @@
+use r2r;
+use failure::Error;
+
+/// try to run like this
+/// cargo run --example logging -- --ros-args --log-level DEBUG
+/// The logs produced with the node logger should show up in /rosout
+
+fn main() -> Result<(), Error> {
+    r2r::log_debug!("before_init", "debug msg");
+    let ctx = r2r::Context::create()?;
+
+
+    let node = r2r::Node::create(ctx, "logger_node", "")?;
+
+    let mut i = 0;
+    loop {
+        r2r::log_debug!(node.logger(), "debug msg: {}", i as f64 / 2.5);
+        std::thread::sleep_ms(10);
+        r2r::log_info!(node.logger(), "info msg {}", i % 2);
+        std::thread::sleep_ms(10);
+        r2r::log_warn!(node.logger(), "warn msg {:?}", i.to_string());
+        std::thread::sleep_ms(10);
+        r2r::log_error!(node.logger(), "error msg {:?}", i.to_string().as_bytes());
+        std::thread::sleep_ms(10);
+        r2r::log_fatal!(node.logger(), "fatal msg {}", i);
+
+        std::thread::sleep_ms(1000);
+
+        r2r::log_debug!("other_logger", "i = {}", i);
+        i+=1;
+    }
+}

rcl/src/rcl_wrapper.h

@@ -4,6 +4,9 @@
 // query the network
 #include <rcl/graph.h>
 
+// logging
+#include <rcl/logging.h>
+
 // errors
 #include <rcutils/error_handling.h>
 

src/lib.rs

@@ -16,6 +16,9 @@ use rcl::*;
 mod error;
 use error::*;
 
+mod utils;
+pub use utils::*;
+
 pub type Result<T> = std::result::Result<T, Error>;
 
 pub trait WrappedTypesupport: Serialize + serde::de::DeserializeOwned + Default {
@@ -391,7 +394,14 @@ impl Context {
             rcl_context_is_valid(ctx.as_mut())
         };
 
-        if is_valid {
+        let logging_ok = unsafe {
+            let _guard = log_guard();
+            let ret = rcl_logging_configure(&ctx.as_ref().global_arguments,
+                                            &rcutils_get_default_allocator());
+            ret == RCL_RET_OK as i32
+        };
+
+        if is_valid && logging_ok {
             Ok(Context {
                 context_handle: Arc::new(Mutex::new(ctx)),
             })
@@ -984,6 +994,15 @@ impl Node {
         Ok(&self.timers[self.timers.len()-1])
     }
 
+    pub fn logger<'a>(&'a self) -> &'a str {
+        let ptr = unsafe { rcl_node_get_logger_name(self.node_handle.as_ref()) };
+        if ptr == std::ptr::null() {
+            return "";
+        }
+        let s = unsafe { CStr::from_ptr(ptr) };
+        s.to_str().unwrap_or("")
+    }
+
 }
 
 pub struct Timer {

src/utils.rs

@@ -0,0 +1,143 @@
+use std::ffi::{CString};
+use std::sync::Mutex;
+use rcl::*;
+
+use lazy_static::lazy_static;
+
+lazy_static! {
+    static ref LOG_GUARD: Mutex<()> = Mutex::new(());
+}
+
+pub(crate) fn log_guard() -> std::sync::MutexGuard<'static, ()> {
+    LOG_GUARD.lock().unwrap()
+}
+
+fn check_init() -> bool {
+    let _guard = log_guard();
+    unsafe { g_rcutils_logging_initialized }
+}
+
+// TODO: cleanup
+// fn shutdown() {
+//     if check_init() {
+//         unsafe { rcutils_logging_shutdown(); }
+//     }
+// }
+
+/// Don't call this directly, use the macros below instead.
+pub fn log(msg: &str, logger_name: &str, file: &str, line: u32, severity: LogSeverity) {
+    if !check_init() {
+        let ret = unsafe {
+            let _guard = log_guard();
+            rcutils_logging_initialize()
+        };
+        if ret != RCL_RET_OK as i32 {
+            eprintln!("could not create logging system (Err: {})", ret);
+            return;
+        }
+    }
+    // currently not possible to get function name in rust.
+    let function_name = CString::new("").unwrap().as_ptr();
+    let file_name = CString::new(file).unwrap().as_ptr();
+    let location = rcutils_log_location_t {
+        function_name,
+        file_name,
+        line_number: line as usize,
+    };
+    let logger_name = CString::new(logger_name).unwrap();
+    let format = CString::new("%s").unwrap();
+    let message = CString::new(msg).unwrap();
+    let severity = severity.to_native();
+    unsafe {
+        let _guard = log_guard();
+        rcutils_log(&location,
+                    severity as i32,
+                    logger_name.as_ptr(),
+                    format.as_ptr(),
+                    message.as_ptr(),
+        );
+    }
+}
+
+// pub because of our macros
+pub enum LogSeverity {
+    Unset,
+    Debug,
+    Info,
+    Warn,
+    Error,
+    Fatal
+}
+
+impl LogSeverity {
+    fn to_native(&self) -> RCUTILS_LOG_SEVERITY {
+        use RCUTILS_LOG_SEVERITY::*;
+        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.
+#[doc(hidden)]
+#[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);
+    }}
+}
+
+#[macro_export]
+macro_rules! log_debug {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Debug)
+    }}
+}
+
+#[macro_export]
+macro_rules! log_info {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Info)
+    }}
+}
+
+#[macro_export]
+macro_rules! log_warn {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Warn)
+    }}
+}
+
+#[macro_export]
+macro_rules! log_error {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Error)
+    }}
+}
+
+#[macro_export]
+macro_rules! log_fatal {
+    ($logger_name:expr, $($args:tt)*) => {{
+        $crate::__impl_log!($logger_name, format_args!($($args)*),
+                            file!(), line!(), $crate::LogSeverity::Fatal)
+    }}
+}
+
+
+#[test]
+fn test_log() {
+    log_debug!("log_test", "debug msg");
+    log_info!("log_test", "info msg");
+    log_warn!("log_test", "warn msg");
+    log_error!("log_test", "error msg");
+    log_fatal!("log_test", "fatal msg");
+}