Skip to content

Add log via zenoh #199

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
patrickelectric wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Add log via zenoh #199

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
63039ca
lib: zenoh_service: First commit
patrickelectric Dec 16, 2025
1f749d7
lib: drivers: zenoh: Integrate with new zenoh singleton
patrickelectric Dec 16, 2025
9f8614d
fixup! lib: zenoh_service: First commit
patrickelectric Dec 16, 2025
fa1f8a1
lib: Add foxglove integration to log messages
patrickelectric Dec 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
95 changes: 25 additions & 70 deletions src/lib/drivers/zenoh/mod.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,18 +4,17 @@ 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,
stats::{
accumulated::driver::{AccumulatedDriverStats, AccumulatedDriverStatsProvider},
driver::DriverUuid,
},
zenoh_service,
};

#[derive(Debug)]
Expand Down Expand Up @@ -66,21 +65,12 @@ impl Zenoh {
}

#[instrument(level = "debug", skip_all)]
async fn receive_task(
context: &SendReceiveContext,
session: Arc<zenoh::Session>,
) -> 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::<MAVLinkJSON<mavlink::ardupilotmega::MavMessage>>(
Expand Down Expand Up @@ -121,7 +111,7 @@ impl Zenoh {
}

#[instrument(level = "debug", skip_all)]
async fn send_task(context: &SendReceiveContext, session: Arc<zenoh::Session>) -> Result<()> {
async fn send_task(context: &SendReceiveContext) -> Result<()> {
let mut hub_receiver = context.hub_sender.subscribe();

'mainloop: loop {
Expand Down Expand Up @@ -161,50 +151,44 @@ 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:?}");
continue;
}
};

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 {

@joaoantoniocardoso joaoantoniocardoso Dec 18, 2025

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From my tests/studying in the MCM codebase, this other approach tends to have better performance:

Add before the loop:

        let zenoh_topic = session
            .declare_keyexpr("mavlink/out")
            .await
            .map_err(anyhow::Error::msg)?;
        let zenoh_encoding = zenoh::bytes::Encoding::APPLICATION_JSON;
        let publisher = session
            .declare_publisher(zenoh_topic)
            .encoding(zenoh_encoding)
            .await
            .map_err(anyhow::Error::msg)?;
        let topic = publisher.key_expr().to_string();
        'mainloop: loop {

And then in the loop:

            // Publish to main output topic
            if let Err(error) = publisher.put(json_string).await {
                error!("Failed to send message to zenoh {topic:?}: {error:?}");

In which case, the abstraction/wrapper ends up being thinner, with no publishing helper functions, making the 'user' responsible for that.

@joaoantoniocardoso joaoantoniocardoso Dec 18, 2025

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here in the MCM codebase: 'user', abstraction

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:?}");
}
}
}
Expand All @@ -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 {
Expand All @@ -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:?}");
}
Expand Down
53 changes: 53 additions & 0 deletions src/lib/zenoh_service.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
use std::sync::Arc;

use anyhow::Result;
use tokio::sync::OnceCell;
use tracing::*;
use zenoh::bytes::Encoding;

use crate::cli::zenoh_config_file;

static SESSION: OnceCell<Arc<zenoh::Session>> = OnceCell::const_new();

#[instrument(level = "debug", skip_all)]
pub async fn get() -> Result<Arc<zenoh::Session>> {
SESSION.get_or_try_init(init_session).await.map(Arc::clone)
}


#[instrument(level = "debug")]
async fn init_session() -> Result<Arc<zenoh::Session>> {
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_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(())
}