use std::sync::{Arc, Condvar, Mutex};
use std::time::{Duration, SystemTime, UNIX_EPOCH};
use futures::StreamExt;
use r2r::builtin_interfaces::msg::Time;
use r2r::std_msgs::msg::Header;
use tokio::task::JoinHandle;
use crate::{Error, Image, ImagePublisher, Result, Subscriber};
pub use r2r;
pub use r2r::QosProfile;
pub type RosImage = r2r::sensor_msgs::msg::Image;
pub fn image_from_ros(msg: RosImage) -> Result<Image> {
if msg.is_bigendian != 0 && msg.encoding.contains("16") {
return Err(Error::UnsupportedEncoding(format!("{} (big endian)", msg.encoding)));
}
let format = msg.encoding.parse()?;
let stamp = &msg.header.stamp;
let timestamp = Duration::new(stamp.sec.max(0) as u64, stamp.nanosec);
Image::with_step(msg.width, msg.height, format, msg.step, msg.data, timestamp)
}
pub fn image_to_ros(image: &Image, header: Header) -> RosImage {
RosImage {
header,
height: image.height,
width: image.width,
encoding: image.format.as_ros_encoding().to_string(),
is_bigendian: 0,
step: image.step,
data: image.data.to_vec(),
}
}
pub fn now_stamp() -> Time {
let now = SystemTime::now().duration_since(UNIX_EPOCH).unwrap_or_default();
Time {
sec: now.as_secs() as i32,
nanosec: now.subsec_nanos(),
}
}
#[derive(Debug, Default, Clone, Copy)]
pub struct BridgeStats {
pub received: u64,
pub sent: u64,
pub dropped: u64,
}
pub fn spawn_ros_to_moq(
node: &mut r2r::Node,
topic: &str,
qos: QosProfile,
mut broadcast: ImagePublisher,
) -> Result<JoinHandle<Result<BridgeStats>>> {
let mut stream = node.subscribe::<RosImage>(topic, qos).map_err(ros_error)?;
let topic = topic.to_string();
let slot = Arc::new(LatestSlot::default());
let encoder = tokio::task::spawn_blocking({
let slot = slot.clone();
move || -> Result<u64> {
let mut sent = 0;
let mut warned = false;
while let Some(msg) = slot.take() {
let image = match image_from_ros(msg) {
Ok(image) => image,
Err(err) => {
if !warned {
tracing::warn!(%err, "skipping image");
warned = true;
}
continue;
}
};
if broadcast.publish(&image)? {
sent += 1;
}
}
Ok(sent)
}
});
Ok(tokio::spawn(async move {
let slot = CloseOnDrop(slot);
let mut stats = BridgeStats::default();
let mut meter = RateMeter::new("ROS -> MoQ");
while let Some(msg) = stream.next().await {
stats.received += 1;
meter.tick();
if slot.0.put(msg) {
stats.dropped += 1;
tracing::debug!(topic, "encoder busy, dropped an older image");
}
if encoder.is_finished() {
break;
}
}
drop(slot);
stats.sent = encoder.await.expect("encoder thread panicked")?;
Ok(stats)
}))
}
#[derive(Default)]
struct LatestSlot<T> {
state: Mutex<(Option<T>, bool)>,
ready: Condvar,
}
impl<T> LatestSlot<T> {
fn put(&self, value: T) -> bool {
let mut state = self.state.lock().unwrap();
let replaced = state.0.replace(value).is_some();
self.ready.notify_one();
replaced
}
fn take(&self) -> Option<T> {
let mut state = self.state.lock().unwrap();
loop {
if let Some(value) = state.0.take() {
return Some(value);
}
if state.1 {
return None;
}
state = self.ready.wait(state).unwrap();
}
}
fn close(&self) {
self.state.lock().unwrap().1 = true;
self.ready.notify_all();
}
}
struct CloseOnDrop<T>(Arc<LatestSlot<T>>);
impl<T> Drop for CloseOnDrop<T> {
fn drop(&mut self) {
self.0.close();
}
}
pub fn spawn_moq_to_ros(
node: &mut r2r::Node,
topic: &str,
qos: QosProfile,
frame_id: &str,
subscriber: Subscriber,
broadcast: &str,
) -> Result<JoinHandle<Result<()>>> {
let publisher = node.create_publisher::<RosImage>(topic, qos).map_err(ros_error)?;
let frame_id = frame_id.to_string();
let broadcast = broadcast.to_string();
Ok(tokio::spawn(async move {
loop {
let mut images = match subscriber.subscribe_images(&broadcast).await {
Ok(images) => images,
Err(err) => {
tracing::warn!(%err, broadcast, "subscribe failed, retrying");
tokio::time::sleep(Duration::from_secs(1)).await;
continue;
}
};
let mut count = 0u64;
let mut meter = RateMeter::new("MoQ -> ROS");
let ended = loop {
match images.next_image().await {
Ok(Some(image)) => {
let header = Header {
stamp: now_stamp(),
frame_id: frame_id.clone(),
};
publisher.publish(&image_to_ros(&image, header)).map_err(ros_error)?;
count += 1;
meter.tick();
}
Ok(None) => break None,
Err(err) => break Some(err),
}
};
match ended {
None => tracing::info!(broadcast, count, "broadcast ended, waiting for it to return"),
Some(err) => tracing::warn!(%err, broadcast, count, "subscription failed, resubscribing"),
}
}
}))
}
struct RateMeter {
label: &'static str,
count: u64,
since: std::time::Instant,
}
impl RateMeter {
const PERIOD: Duration = Duration::from_secs(10);
fn new(label: &'static str) -> Self {
Self {
label,
count: 0,
since: std::time::Instant::now(),
}
}
fn tick(&mut self) {
self.count += 1;
let elapsed = self.since.elapsed();
if elapsed >= Self::PERIOD {
let fps = self.count as f64 / elapsed.as_secs_f64();
tracing::info!(bridge = self.label, fps = format!("{fps:.1}"), "image rate");
self.count = 0;
self.since = std::time::Instant::now();
}
}
}
fn ros_error(err: r2r::Error) -> Error {
Error::Ros(err.to_string())
}
#[cfg(test)]
mod tests {
use std::thread;
use super::*;
use crate::PixelFormat;
fn ros_image(encoding: &str, width: u32, height: u32, step: u32) -> RosImage {
RosImage {
header: Header {
stamp: Time { sec: 12, nanosec: 34 },
frame_id: "camera".into(),
},
height,
width,
encoding: encoding.into(),
is_bigendian: 0,
step,
data: (0..step * height).map(|i| i as u8).collect(),
}
}
#[test]
fn converts_ros_images() {
let image = image_from_ros(ros_image("bgr8", 2, 2, 8)).unwrap();
assert_eq!(
(image.width, image.height, image.format, image.step),
(2, 2, PixelFormat::Bgr8, 8)
);
assert_eq!(image.timestamp, Duration::new(12, 34));
assert_eq!(image.data.len(), 16);
let header = Header {
stamp: Time { sec: 1, nanosec: 2 },
frame_id: "out".into(),
};
let back = image_to_ros(&image, header.clone());
assert_eq!(back.header, header);
assert_eq!(back.encoding, "bgr8");
assert_eq!((back.width, back.height, back.step, back.is_bigendian), (2, 2, 8, 0));
assert_eq!(back.data, ros_image("bgr8", 2, 2, 8).data);
}
#[test]
fn clamps_negative_stamps() {
let mut msg = ros_image("mono8", 2, 2, 2);
msg.header.stamp.sec = -5;
assert_eq!(image_from_ros(msg).unwrap().timestamp, Duration::new(0, 34));
}
#[test]
fn rejects_unsupported_ros_images() {
assert!(matches!(
image_from_ros(ros_image("16UC1", 2, 2, 4)),
Err(Error::UnsupportedEncoding(_))
));
let mut big_endian = ros_image("mono16", 2, 2, 4);
big_endian.is_bigendian = 1;
assert!(matches!(image_from_ros(big_endian), Err(Error::UnsupportedEncoding(e)) if e.contains("big endian")));
assert!(matches!(
image_from_ros(ros_image("rgb8", 4, 2, 8)),
Err(Error::InvalidImage(_))
));
}
#[test]
fn latest_slot_keeps_only_the_newest_value() {
let slot = LatestSlot::default();
assert!(!slot.put(1));
assert!(slot.put(2));
assert_eq!(slot.take(), Some(2));
assert!(!slot.put(3));
slot.close();
assert_eq!(slot.take(), Some(3));
assert_eq!(slot.take(), None);
}
#[test]
fn latest_slot_take_waits_for_put_or_close() {
let slot = Arc::new(LatestSlot::default());
let taker = thread::spawn({
let slot = slot.clone();
move || (slot.take(), slot.take())
});
thread::sleep(Duration::from_millis(50));
slot.put(7);
thread::sleep(Duration::from_millis(50));
drop(CloseOnDrop(slot));
assert_eq!(taker.join().unwrap(), (Some(7), None));
}
}