diff --git a/src/lib/drivers/zenoh/mod.rs b/src/lib/drivers/zenoh/mod.rs index 19fd69f2..f4f7242c 100644 --- a/src/lib/drivers/zenoh/mod.rs +++ b/src/lib/drivers/zenoh/mod.rs @@ -4,11 +4,9 @@ use anyhow::Result; use mavlink::{self, Message}; use tokio::sync::{RwLock, broadcast}; use tracing::*; -use zenoh; use crate::{ callbacks::{Callbacks, MessageCallback}, - cli::zenoh_config_file, drivers::{Driver, DriverInfo, generic_tasks::SendReceiveContext}, mavlink_json::MAVLinkJSON, protocol::Protocol, @@ -16,6 +14,7 @@ use crate::{ accumulated::driver::{AccumulatedDriverStats, AccumulatedDriverStatsProvider}, driver::DriverUuid, }, + zenoh_service, }; #[derive(Debug)] @@ -66,21 +65,12 @@ impl Zenoh { } #[instrument(level = "debug", skip_all)] - async fn receive_task( - context: &SendReceiveContext, - session: Arc, - ) -> Result<()> { - let subscriber = match session - .declare_subscriber(format!("{}/in", "mavlink")) + async fn receive_task(context: &SendReceiveContext) -> Result<()> { + let session = zenoh_service::get().await?; + let subscriber = session + .declare_subscriber("mavlink/in") .await - { - Ok(subscriber) => subscriber, - Err(error) => { - return Err(anyhow::anyhow!( - "Failed to create subscriber for mavlink data: {error:?}" - )); - } - }; + .map_err(|e| anyhow::anyhow!("Failed to create subscriber for mavlink data: {e}"))?; while let Ok(sample) = subscriber.recv_async().await { let Ok(content) = json5::from_str::>( @@ -121,7 +111,7 @@ impl Zenoh { } #[instrument(level = "debug", skip_all)] - async fn send_task(context: &SendReceiveContext, session: Arc) -> Result<()> { + async fn send_task(context: &SendReceiveContext) -> Result<()> { let mut hub_receiver = context.hub_sender.subscribe(); 'mainloop: loop { @@ -161,7 +151,7 @@ impl Zenoh { let message_name = mavlink_json.message.message_name(); - let json_string = &match json5::to_string(&mavlink_json) { + let json_string = match json5::to_string(&mavlink_json) { Ok(json) => json, Err(error) => { error!("Failed to transform mavlink message {message_name} to json: {error:?}"); @@ -169,42 +159,36 @@ impl Zenoh { } }; - let topic_name = "mavlink/out"; - if let Err(error) = session - .put(topic_name, json_string) - .encoding(zenoh::bytes::Encoding::APPLICATION_JSON) - .await - { - error!("Failed to send message to {topic_name}: {error:?}"); + // Publish to main output topic + if let Err(error) = zenoh_service::publish_json("mavlink/out", &json_string).await { + error!("Failed to send message to mavlink/out: {error:?}"); } else { - trace!("Message sent to {topic_name}: {json_string:?}"); + trace!("Message sent to mavlink/out: {json_string:?}"); } + // Publish to per-message topic let header = &mavlink_json.header.inner; - let topic_name = &format!( + let topic_name = format!( "mavlink/{}/{}/{}", header.system_id, header.component_id, message_name ); - if let Err(error) = session - .put(topic_name, json_string) - .encoding(zenoh::bytes::Encoding::APPLICATION_JSON) - .await - { + if let Err(error) = zenoh_service::publish_json(&topic_name, &json_string).await { error!("Failed to send message to {topic_name}: {error:?}"); } else { trace!("Message sent to {topic_name}: {json_string:?}"); } - // for each key inside mavlink_json and publish under topic_name/field_name + // Publish each field under topic_name/field_name let message_value = serde_json::to_value(&mavlink_json.message).unwrap(); for (field_name, field_value) in message_value.as_object().unwrap() { - let topic_name = &format!("{}/{}", topic_name, field_name); - if let Err(error) = session - .put(topic_name, json5::to_string(field_value).unwrap()) - .encoding(zenoh::bytes::Encoding::APPLICATION_JSON) - .await + let field_topic = format!("{topic_name}/{field_name}"); + if let Err(error) = zenoh_service::publish_json( + &field_topic, + &json5::to_string(field_value).unwrap(), + ) + .await { - error!("Failed to send message to {topic_name}: {error:?}"); + error!("Failed to send message to {field_topic}: {error:?}"); } } } @@ -227,27 +211,6 @@ impl Driver for Zenoh { stats: self.stats.clone(), }; - let mut config = if let Some(zenoh_config_file) = zenoh_config_file() { - zenoh::Config::from_file(zenoh_config_file) - .map_err(|error| anyhow::anyhow!("Failed to load Zenoh config file: {error:?}"))? - } else { - let mut config = zenoh::Config::default(); - config - .insert_json5("mode", r#""client""#) - .expect("Failed to insert client mode"); - config - .insert_json5("connect/endpoints", r#"["tcp/127.0.0.1:7447"]"#) - .expect("Failed to insert connect endpoints"); - config - }; - - config - .insert_json5("adminspace", r#"{"enabled": true}"#) - .expect("Failed to insert adminspace"); - config - .insert_json5("metadata", r#"{"name": "mavlink-server"}"#) - .expect("Failed to insert metadata"); - let mut first = true; loop { if first { @@ -256,21 +219,13 @@ impl Driver for Zenoh { tokio::time::sleep(tokio::time::Duration::from_secs(1)).await; } - let session = match zenoh::open(config.clone()).await { - Ok(session) => Arc::new(session), - Err(error) => { - error!("Failed to start zenoh session: {error:?}"); - continue; - } - }; - tokio::select! { - result = Zenoh::send_task(&context, session.clone()) => { + result = Zenoh::send_task(&context) => { if let Err(error) = result { error!("Error in send task: {error:?}"); } } - result = Zenoh::receive_task(&context, session) => { + result = Zenoh::receive_task(&context) => { if let Err(error) = result { error!("Error in receive task: {error:?}"); } diff --git a/src/lib/logger.rs b/src/lib/logger.rs index 350d0995..c46c54e1 100644 --- a/src/lib/logger.rs +++ b/src/lib/logger.rs @@ -1,20 +1,25 @@ use std::{ + fmt::Write as FmtWrite, io::{self, Write}, sync::{Arc, Mutex}, }; +use chrono::Utc; use lazy_static::lazy_static; use ringbuffer::{AllocRingBuffer, RingBuffer}; use tokio::sync::broadcast::{Receiver, Sender}; use tracing::metadata::LevelFilter; +use tracing::{Event, Level, Subscriber}; use tracing_log::LogTracer; use tracing_subscriber::{ EnvFilter, Layer, fmt::{self, MakeWriter}, - layer::SubscriberExt, + layer::{Context, SubscriberExt}, + registry::LookupSpan, }; -use crate::cli; +use crate::{cli, zenoh_service}; +use zenoh_service::{FoxgloveLog, FoxgloveTimestamp}; struct BroadcastWriter { sender: Sender, @@ -78,9 +83,90 @@ impl History { } } +struct FoxgloveBroadcast { + sender: Sender, +} + +impl FoxgloveBroadcast { + fn new() -> Self { + let (sender, _) = tokio::sync::broadcast::channel(100); + Self { sender } + } + + fn send(&self, log: FoxgloveLog) { + let _ = self.sender.send(log); + } + + fn subscribe(&self) -> Receiver { + self.sender.subscribe() + } +} + lazy_static! { static ref MANAGER: Arc> = Default::default(); pub static ref HISTORY: Arc> = Default::default(); + static ref FOXGLOVE_BROADCAST: FoxgloveBroadcast = FoxgloveBroadcast::new(); +} + +struct FoxgloveLayer; + +impl tracing_subscriber::Layer for FoxgloveLayer +where + S: Subscriber + for<'a> LookupSpan<'a>, +{ + fn on_event(&self, event: &Event<'_>, _ctx: Context<'_, S>) { + // Transform the message to the Foxglove log format + // https://docs.foxglove.dev/docs/visualization/message-schemas/log + let metadata = event.metadata(); + let now = Utc::now(); + let total_ns = now.timestamp_nanos_opt().unwrap_or(0); + + // Extract the message from event fields + let mut message = String::new(); + // The tracing library uses the visitor pattern for field access. + // Event doesn't expose fields directly, it's necessary to call event.record(&mut visitor) + // which invokes your visitor's methods for each field. + let mut visitor = MessageVisitor(&mut message); + event.record(&mut visitor); + + let level = match *metadata.level() { + Level::TRACE => 0, + Level::DEBUG => 1, + Level::INFO => 2, + Level::WARN => 3, + Level::ERROR => 4, + }; + + let foxglove_log = FoxgloveLog { + timestamp: FoxgloveTimestamp { + sec: total_ns / 1_000_000_000, + nsec: total_ns % 1_000_000_000, + }, + level, + message, + name: metadata.target().to_string(), + file: metadata.file().unwrap_or("").to_string(), + line: metadata.line().unwrap_or(0), + }; + + FOXGLOVE_BROADCAST.send(foxglove_log); + } +} + +struct MessageVisitor<'a>(&'a mut String); + +impl tracing::field::Visit for MessageVisitor<'_> { + fn record_debug(&mut self, field: &tracing::field::Field, value: &dyn std::fmt::Debug) { + if field.name() == "message" { + let _ = write!(self.0, "{value:?}"); + } + } + + fn record_str(&mut self, field: &tracing::field::Field, value: &str) { + if field.name() == "message" { + self.0.push_str(value); + } + } } // Start logger, should be done inside main @@ -152,15 +238,31 @@ pub fn init(log_path: String, is_verbose: bool, is_tracing: bool) { .with_thread_names(true) .with_filter(server_env_filter); + let foxglove_layer = FoxgloveLayer; + let history = HISTORY.clone(); + let mut foxglove_rx = FOXGLOVE_BROADCAST.subscribe(); MANAGER.lock().unwrap().process = Some(tokio::spawn(async move { loop { - match rx.recv().await { - Ok(message) => { - history.lock().unwrap().push(message); + tokio::select! { + result = rx.recv() => { + match result { + Ok(message) => { + history.lock().unwrap().push(message); + } + Err(_) => { + tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; + } + } } - Err(_error) => { - tokio::time::sleep(tokio::time::Duration::from_millis(10)).await; + result = foxglove_rx.recv() => { + if let Ok(foxglove_log) = result { + let _ = zenoh_service::publish_foxglove_log( + "services/mavlink-server/log", + &foxglove_log, + ) + .await; + } } } } @@ -170,6 +272,7 @@ pub fn init(log_path: String, is_verbose: bool, is_tracing: bool) { let subscriber = tracing_subscriber::registry() .with(console_layer) .with(file_layer) + .with(foxglove_layer) .with(server_layer); tracing::subscriber::set_global_default(subscriber).expect("Unable to set a global subscriber"); } diff --git a/src/lib/mod.rs b/src/lib/mod.rs index 99b782f9..dae09738 100644 --- a/src/lib/mod.rs +++ b/src/lib/mod.rs @@ -7,3 +7,4 @@ pub mod mavlink_json; pub mod protocol; pub mod stats; pub mod web; +pub mod zenoh_service; diff --git a/src/lib/zenoh_service.rs b/src/lib/zenoh_service.rs new file mode 100644 index 00000000..e55b997e --- /dev/null +++ b/src/lib/zenoh_service.rs @@ -0,0 +1,93 @@ +use std::sync::Arc; + +use anyhow::Result; +use serde::Serialize; +use tokio::sync::OnceCell; +use tracing::*; +use zenoh::bytes::Encoding; + +use crate::cli::zenoh_config_file; + +#[derive(Clone, Serialize)] +pub struct FoxgloveTimestamp { + pub sec: i64, + pub nsec: i64, +} + +#[derive(Clone, Serialize)] +pub struct FoxgloveLog { + pub timestamp: FoxgloveTimestamp, + pub level: u8, + pub message: String, + pub name: String, + pub file: String, + pub line: u32, +} + +static SESSION: OnceCell> = OnceCell::const_new(); + +#[instrument(level = "debug", skip_all)] +pub async fn get() -> Result> { + SESSION.get_or_try_init(init_session).await.map(Arc::clone) +} + + +#[instrument(level = "debug")] +async fn init_session() -> Result> { + let config = if let Some(config_file) = zenoh_config_file() { + info!("Loading Zenoh config from: {config_file}"); + zenoh::Config::from_file(&config_file) + .map_err(|e| anyhow::anyhow!("Failed to load Zenoh config file '{config_file}': {e}"))? + } else { + debug!("Using default Zenoh configuration"); + let mut config = zenoh::Config::default(); + config + .insert_json5("mode", r#""client""#) + .map_err(|e| anyhow::anyhow!("Failed to set Zenoh mode: {e}"))?; + config + .insert_json5("connect/endpoints", r#"["tcp/127.0.0.1:7447"]"#) + .map_err(|e| anyhow::anyhow!("Failed to set Zenoh connect endpoints: {e}"))?; + config + }; + + let session = zenoh::open(config) + .await + .map_err(|e| anyhow::anyhow!("Failed to open Zenoh session: {e}"))?; + + info!("Zenoh session initialized successfully"); + Ok(Arc::new(session)) +} + +#[instrument(level = "trace", skip(payload))] +pub async fn publish(key_expr: &str, payload: &str) -> Result<()> { + let session = get().await?; + session + .put(key_expr, payload) + .await + .map_err(|e| anyhow::anyhow!("Failed to publish to '{key_expr}': {e}"))?; + Ok(()) +} + +#[instrument(level = "trace", skip(payload))] +pub async fn publish_json(key_expr: &str, payload: &str) -> Result<()> { + let session = get().await?; + session + .put(key_expr, payload) + .encoding(Encoding::APPLICATION_JSON) + .await + .map_err(|e| anyhow::anyhow!("Failed to publish JSON to '{key_expr}': {e}"))?; + Ok(()) +} + +#[instrument(level = "trace", skip(log))] +pub async fn publish_foxglove_log(key_expr: &str, log: &FoxgloveLog) -> Result<()> { + let session = get().await?; + let payload = serde_json::to_string(log) + .map_err(|e| anyhow::anyhow!("Failed to serialize FoxgloveLog: {e}"))?; + session + .put(key_expr, payload) + .encoding(Encoding::APPLICATION_JSON.with_schema("foxglove.Log")) + .await + .map_err(|e| anyhow::anyhow!("Failed to publish FoxgloveLog to '{key_expr}': {e}"))?; + Ok(()) +}