use std::collections::BTreeMap;
use std::time::{Duration, Instant};
use moqros::ros::{BridgeStats, QosProfile, now_stamp, r2r};
use serde_json::json;
use tokio::sync::watch;
pub const DIAGNOSTICS_TOPIC: &str = "/diagnostics";
const DIAGNOSTICS_TYPE: &str = "diagnostic_msgs/msg/DiagnosticArray";
const STALE_AFTER: Duration = Duration::from_secs(5);
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Level {
Ok = 0,
Warn = 1,
Error = 2,
}
pub struct Diagnostics {
publisher: r2r::PublisherUntyped,
hardware_id: String,
bridges: BTreeMap<String, Entry>,
}
struct Entry {
kind: String,
state: State,
}
enum State {
Running {
stats: watch::Receiver<BridgeStats>,
started: Instant,
previous: (Instant, BridgeStats),
},
Restarting {
since: Instant,
},
}
impl Diagnostics {
pub fn new(node: &mut r2r::Node) -> anyhow::Result<Self> {
let publisher = node.create_publisher_untyped(DIAGNOSTICS_TOPIC, DIAGNOSTICS_TYPE, QosProfile::default())?;
Ok(Self {
publisher,
hardware_id: node.fully_qualified_name()?,
bridges: BTreeMap::new(),
})
}
pub fn running(&mut self, name: &str, kind: &str, stats: watch::Receiver<BridgeStats>) {
let now = Instant::now();
let previous = (now, *stats.borrow());
self.bridges.insert(
name.to_string(),
Entry {
kind: kind.to_string(),
state: State::Running {
stats,
started: now,
previous,
},
},
);
}
pub fn restarting(&mut self, name: &str) {
if let Some(entry) = self.bridges.get_mut(name) {
entry.state = State::Restarting { since: Instant::now() };
}
}
pub fn publish(&mut self) -> anyhow::Result<()> {
let now = Instant::now();
let status: Vec<_> = self
.bridges
.iter_mut()
.map(|(name, entry)| entry.status(name, &self.hardware_id, now))
.collect();
let stamp = now_stamp();
self.publisher.publish(json!({
"header": { "stamp": { "sec": stamp.sec, "nanosec": stamp.nanosec }, "frame_id": "" },
"status": status,
}))?;
Ok(())
}
}
impl Entry {
fn status(&mut self, name: &str, hardware_id: &str, now: Instant) -> serde_json::Value {
let mut values = vec![("kind", self.kind.clone())];
let (level, message) = match &mut self.state {
State::Running {
stats,
started,
previous,
} => {
let current = *stats.borrow();
let report = Report::new(¤t, previous, *started, now);
*previous = (now, current);
values.extend([
("received", current.received.to_string()),
("sent", current.sent.to_string()),
("dropped", current.dropped.to_string()),
("rate", format!("{:.1}", report.rate)),
]);
if let Some(age) = report.idle {
values.push(("since_last_message", format!("{:.1}s", age.as_secs_f64())));
}
report.assess()
}
State::Restarting { since } => (
Level::Error,
format!("stopped {:.0}s ago, restarting", (now - *since).as_secs_f64()),
),
};
json!({
"level": level as u8,
"name": format!("moqros: {name}"),
"message": message,
"hardware_id": hardware_id,
"values": values
.into_iter()
.map(|(key, value)| json!({ "key": key, "value": value }))
.collect::<Vec<_>>(),
})
}
}
#[derive(Debug, PartialEq)]
struct Report {
rate: f64,
received: u64,
dropped: u64,
idle: Option<Duration>,
}
impl Report {
#[expect(clippy::cast_precision_loss, reason = "message counts are far below 2^52")]
fn new(current: &BridgeStats, (since, previous): &(Instant, BridgeStats), started: Instant, now: Instant) -> Self {
let received = current.received.saturating_sub(previous.received);
let elapsed = now.saturating_duration_since(*since).as_secs_f64();
Self {
rate: if elapsed > 0.0 { received as f64 / elapsed } else { 0.0 },
received,
dropped: current.dropped.saturating_sub(previous.dropped),
idle: Some(now.saturating_duration_since(current.last_received.unwrap_or(started))),
}
}
fn assess(&self) -> (Level, String) {
match self.idle {
Some(idle) if idle >= STALE_AFTER => {
return (Level::Warn, format!("no message for {:.0}s", idle.as_secs_f64()));
}
_ => {}
}
if self.received > 0 && self.dropped * 2 > self.received {
return (
Level::Warn,
format!("dropped {} of {} messages", self.dropped, self.received),
);
}
(Level::Ok, "ok".to_string())
}
}
#[cfg(test)]
mod tests {
use super::*;
fn report(received: u64, dropped: u64, idle_secs: u64) -> Report {
Report {
rate: 0.0,
received,
dropped,
idle: Some(Duration::from_secs(idle_secs)),
}
}
#[test]
fn assesses_bridges() {
assert_eq!(report(30, 0, 0).assess().0, Level::Ok);
assert_eq!(report(30, 15, 0).assess().0, Level::Ok);
assert_eq!(
report(30, 16, 0).assess(),
(Level::Warn, "dropped 16 of 30 messages".into())
);
assert_eq!(report(0, 0, 4).assess().0, Level::Ok);
assert_eq!(report(0, 0, 5).assess(), (Level::Warn, "no message for 5s".into()));
}
#[test]
fn computes_rates_since_the_previous_report() {
let started = Instant::now();
let mut previous = BridgeStats::default();
previous.received = 10;
previous.dropped = 1;
let mut current = BridgeStats::default();
current.received = 30;
current.dropped = 3;
current.last_received = Some(started + Duration::from_secs(2));
let report = Report::new(
¤t,
&(started, previous),
started,
started + Duration::from_secs(2),
);
assert_eq!(
report,
Report {
rate: 10.0,
received: 20,
dropped: 2,
idle: Some(Duration::ZERO),
}
);
let report = Report::new(
&BridgeStats::default(),
&(started, BridgeStats::default()),
started,
started + Duration::from_secs(6),
);
assert_eq!(report.idle, Some(Duration::from_secs(6)));
}
#[test]
fn publishes_diagnostic_arrays() {
let ctx = r2r::Context::create().unwrap();
let mut node = r2r::Node::create(ctx, "moqros_diagnostics_test", "").unwrap();
let mut diagnostics = Diagnostics::new(&mut node).expect("diagnostic_msgs bindings (IDL_PACKAGE_FILTER)");
let (sender, stats) = watch::channel(BridgeStats::default());
diagnostics.running("/camera", "ROS -> MoQ video", stats);
sender.send_modify(|stats| stats.received = 3);
diagnostics.running("imu", "MoQ -> ROS data", watch::channel(BridgeStats::default()).1);
diagnostics.restarting("imu");
diagnostics.publish().unwrap();
}
}