use std::sync::{Arc, Mutex};
use std::time::Duration;
use r2r::std_msgs::msg::Header;
use super::bridge::StatsSender;
use super::meter::{RateMeter, WarnOnce};
use super::{Bridge, QosProfile, RosImage, Teardown, image_into_ros, now_stamp, stamp};
use crate::{
DataFormat, DataSettings, DataSubscriber, Error, ImageSubscriber, PixelFormat, Result, SubscribeOptions,
Subscriber, Subscription,
};
pub async fn spawn_moq_to_ros(
node: &mut r2r::Node,
subscriber: Subscriber,
broadcast: &str,
target: RosTarget,
) -> Result<Bridge> {
MoqToRos::subscribe(subscriber, broadcast).await.spawn(node, target)
}
#[derive(Debug, Clone)]
#[non_exhaustive]
pub struct RosTarget {
pub topic: String,
pub qos: QosProfile,
pub frame_id: Option<String>,
pub stamp: StampSource,
pub ros_type: Option<String>,
pub encoding: PixelFormat,
}
#[derive(Debug, Clone, Copy, Default, PartialEq, Eq, Hash)]
#[non_exhaustive]
pub enum StampSource {
#[default]
Source,
Receive,
}
crate::names::named_enum!(StampSource, "stamp source", {
Source => "source",
Receive => "receive",
});
impl RosTarget {
pub fn new(topic: impl Into<String>, qos: QosProfile) -> Self {
Self {
topic: topic.into(),
qos,
frame_id: None,
stamp: StampSource::default(),
ros_type: None,
encoding: PixelFormat::Rgb8,
}
}
pub fn with_encoding(mut self, encoding: PixelFormat) -> Self {
self.encoding = encoding;
self
}
pub fn with_frame_id(mut self, frame_id: impl Into<String>) -> Self {
self.frame_id = Some(frame_id.into());
self
}
pub fn with_stamp(mut self, stamp: StampSource) -> Self {
self.stamp = stamp;
self
}
pub fn with_ros_type(mut self, ros_type: impl Into<String>) -> Self {
self.ros_type = Some(ros_type.into());
self
}
}
pub struct MoqToRos {
subscriber: Subscriber,
broadcast: String,
options: SubscribeOptions,
first: Subscription,
}
impl MoqToRos {
pub async fn subscribe(subscriber: Subscriber, broadcast: &str) -> Self {
Self::subscribe_with(subscriber, broadcast, SubscribeOptions::default()).await
}
pub async fn subscribe_with(subscriber: Subscriber, broadcast: &str, options: SubscribeOptions) -> Self {
let first = subscribe(&subscriber, broadcast, &options).await;
Self {
subscriber,
broadcast: broadcast.to_string(),
options,
first,
}
}
pub fn broadcast(&self) -> &str {
&self.broadcast
}
pub fn data_settings(&self) -> Option<&crate::DataSettings> {
match &self.first {
Subscription::Images(_) => None,
Subscription::Data(data) => Some(data.settings()),
}
}
pub fn spawn(self, node: &mut r2r::Node, target: RosTarget) -> Result<Bridge> {
let RosTarget {
topic,
qos,
frame_id,
stamp,
ros_type,
encoding,
} = target;
let output = match &self.first {
Subscription::Images(_) => {
if matches!(encoding, PixelFormat::Uyvy | PixelFormat::Yuyv) {
return Err(Error::InvalidSettings(format!("can't republish images as {encoding}")));
}
RosOutput::Images {
publisher: node.create_publisher::<RosImage>(&topic, qos)?,
frame_id,
stamp,
encoding,
}
}
Subscription::Data(data) => {
let settings = data.settings();
let ros_type = ros_type
.or_else(|| settings.ros_type.clone())
.ok_or_else(|| Error::MissingRosType(self.broadcast.clone()))?;
RosOutput::Data {
publisher: node.create_publisher_untyped(&topic, &ros_type, qos)?,
catalog: settings.clone(),
}
}
};
let released = Arc::new(Mutex::new(None));
let slot = released.clone();
let bridge = Bridge::spawn(|stats| async move {
let mut output = output;
let result = republish(
self.subscriber,
self.broadcast,
self.options,
self.first,
&mut output,
stats,
)
.await;
*slot.lock().unwrap_or_else(|err| err.into_inner()) = Some(output);
result
});
Ok(bridge.with_teardown(Teardown::new(move |node| {
if let Some(output) = released.lock().unwrap_or_else(|err| err.into_inner()).take() {
output.destroy(node);
}
})))
}
}
async fn subscribe(subscriber: &Subscriber, broadcast: &str, options: &SubscribeOptions) -> Subscription {
loop {
match subscriber.subscribe_with(broadcast, options).await {
Ok(subscription) => return subscription,
Err(err) => {
tracing::warn!(%err, broadcast, "subscribe failed, retrying");
tokio::time::sleep(Duration::from_secs(1)).await;
}
}
}
}
enum RosOutput {
Images {
publisher: r2r::Publisher<RosImage>,
frame_id: Option<String>,
stamp: StampSource,
encoding: PixelFormat,
},
Data {
publisher: r2r::PublisherUntyped,
catalog: DataSettings,
},
}
impl RosOutput {
fn destroy(self, node: &mut r2r::Node) {
match self {
Self::Images { publisher, .. } => node.destroy_publisher(publisher),
Self::Data { publisher, .. } => node.destroy_publisher_untyped(publisher),
}
}
}
async fn republish(
subscriber: Subscriber,
broadcast: String,
options: SubscribeOptions,
first: Subscription,
output: &mut RosOutput,
stats: StatsSender,
) -> Result<()> {
let mut first = Some(first);
loop {
let subscription = match first.take() {
Some(subscription) => subscription,
None => subscribe(&subscriber, &broadcast, &options).await,
};
let ended = match (subscription, &mut *output) {
(
Subscription::Images(images),
RosOutput::Images {
publisher,
frame_id,
stamp,
encoding,
},
) => republish_images(images, publisher, frame_id.as_deref(), *stamp, *encoding, &stats).await?,
(Subscription::Data(data), RosOutput::Data { publisher, catalog }) => {
if let Some((from, to)) = data_changed(catalog, data.settings()) {
return Err(Error::BroadcastChanged { broadcast, from, to });
}
republish_data(data, publisher, catalog.format, &stats).await
}
(Subscription::Images(_), RosOutput::Data { .. }) => {
return Err(Error::BroadcastChanged {
broadcast,
from: "data".into(),
to: "video".into(),
});
}
(Subscription::Data(_), RosOutput::Images { .. }) => {
return Err(Error::NoVideo(broadcast));
}
};
let sent = stats.snapshot().sent;
match ended {
None => tracing::info!(broadcast, sent, "broadcast ended, waiting for it to return"),
Some(err) => tracing::warn!(%err, broadcast, sent, "subscription failed, resubscribing"),
}
}
}
fn data_changed(first: &DataSettings, now: &DataSettings) -> Option<(String, String)> {
let name = |settings: &DataSettings| {
format!(
"{} {}",
settings.format,
settings.ros_type.as_deref().unwrap_or("(untyped)")
)
};
(first.format != now.format || first.ros_type != now.ros_type).then(|| (name(first), name(now)))
}
async fn republish_images(
mut images: ImageSubscriber,
publisher: &mut r2r::Publisher<RosImage>,
frame_id: Option<&str>,
stamp_source: StampSource,
encoding: PixelFormat,
stats: &StatsSender,
) -> Result<Option<Error>> {
images.set_output_format(encoding)?;
let mut meter = RateMeter::new("MoQ -> ROS");
loop {
match images.next_image().await {
Ok(Some(decoded)) => {
stats.received();
let header = Header {
stamp: match (stamp_source, decoded.capture_time) {
(StampSource::Source, Some(capture_time)) => stamp(capture_time),
_ => now_stamp(),
},
frame_id: frame_id.or(images.frame_id()).unwrap_or_default().to_string(),
};
publisher.publish(&image_into_ros(decoded.image, header))?;
stats.sent();
meter.tick();
}
Ok(None) => return Ok(None),
Err(err) => return Ok(Some(err)),
}
}
}
async fn republish_data(
mut data: DataSubscriber,
publisher: &mut r2r::PublisherUntyped,
format: DataFormat,
stats: &StatsSender,
) -> Option<Error> {
let mut meter = RateMeter::new("MoQ -> ROS");
let mut warned = WarnOnce::default();
loop {
let message = match data.next_message().await {
Ok(Some(message)) => message,
Ok(None) => return None,
Err(err) => return Some(err),
};
stats.received();
let published = match format {
DataFormat::Json => serde_json::from_slice(&message.payload)
.map_err(Error::from)
.and_then(|value| publisher.publish(value).map_err(Error::from)),
DataFormat::Cdr => publisher.publish_raw(&message.payload).map_err(Error::from),
};
match published {
Ok(()) => {
stats.sent();
meter.tick();
}
Err(err) => {
stats.dropped();
if warned.first() {
tracing::warn!(%err, "skipping message");
}
}
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn detects_data_track_changes() {
let settings = |format, mode, ros_type: &str| DataSettings {
format,
mode,
ros_type: Some(ros_type.into()),
};
let first = settings(DataFormat::Json, crate::DataMode::Snapshot, "std_msgs/msg/Header");
assert_eq!(data_changed(&first, &first), None);
let stream = settings(DataFormat::Json, crate::DataMode::Stream, "std_msgs/msg/Header");
assert_eq!(data_changed(&first, &stream), None);
let cdr = settings(DataFormat::Cdr, crate::DataMode::Snapshot, "std_msgs/msg/Header");
assert_eq!(
data_changed(&first, &cdr),
Some(("json std_msgs/msg/Header".into(), "cdr std_msgs/msg/Header".into()))
);
let string = settings(DataFormat::Json, crate::DataMode::Snapshot, "std_msgs/msg/String");
assert!(data_changed(&first, &string).is_some());
}
}