use std::time::Duration;
use r2r::std_msgs::msg::Header;
use tokio::task::JoinHandle;
use super::meter::{RateMeter, WarnOnce};
use super::{QosProfile, RosImage, image_to_ros, now_stamp};
use crate::{DataFormat, DataSettings, DataSubscriber, Error, ImageSubscriber, Result, Subscriber, Subscription};
pub async fn spawn_moq_to_ros(
node: &mut r2r::Node,
subscriber: Subscriber,
broadcast: &str,
target: RosTarget,
) -> Result<JoinHandle<Result<()>>> {
MoqToRos::subscribe(subscriber, broadcast).await.spawn(node, target)
}
#[derive(Debug, Clone)]
pub struct RosTarget {
pub topic: String,
pub qos: QosProfile,
pub frame_id: String,
pub ros_type: Option<String>,
}
impl RosTarget {
pub fn new(topic: impl Into<String>, qos: QosProfile) -> Self {
Self {
topic: topic.into(),
qos,
frame_id: String::new(),
ros_type: None,
}
}
pub fn with_frame_id(mut self, frame_id: impl Into<String>) -> Self {
self.frame_id = frame_id.into();
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,
first: Subscription,
}
impl MoqToRos {
pub async fn subscribe(subscriber: Subscriber, broadcast: &str) -> Self {
let first = subscribe(&subscriber, broadcast).await;
Self {
subscriber,
broadcast: broadcast.to_string(),
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<JoinHandle<Result<()>>> {
let RosTarget {
topic,
qos,
frame_id,
ros_type,
} = target;
let output = match &self.first {
Subscription::Images(_) => RosOutput::Images {
publisher: node.create_publisher::<RosImage>(&topic, qos)?,
frame_id,
},
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(),
}
}
};
Ok(tokio::spawn(republish(
self.subscriber,
self.broadcast,
self.first,
output,
)))
}
}
async fn subscribe(subscriber: &Subscriber, broadcast: &str) -> Subscription {
loop {
match subscriber.subscribe(broadcast).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: String,
},
Data {
publisher: r2r::PublisherUntyped,
catalog: DataSettings,
},
}
async fn republish(
subscriber: Subscriber,
broadcast: String,
first: Subscription,
mut output: RosOutput,
) -> Result<()> {
let mut first = Some(first);
loop {
let subscription = match first.take() {
Some(subscription) => subscription,
None => subscribe(&subscriber, &broadcast).await,
};
let mut count = 0u64;
let ended = match (subscription, &mut output) {
(Subscription::Images(images), RosOutput::Images { publisher, frame_id }) => {
republish_images(images, publisher, frame_id, &mut count).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, &mut count).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));
}
};
match ended {
None => tracing::info!(broadcast, count, "broadcast ended, waiting for it to return"),
Some(err) => tracing::warn!(%err, broadcast, count, "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: &str,
count: &mut u64,
) -> Result<Option<Error>> {
let mut meter = RateMeter::new("MoQ -> ROS");
loop {
match images.next_image().await {
Ok(Some(image)) => {
let header = Header {
stamp: now_stamp(),
frame_id: frame_id.to_string(),
};
publisher.publish(&image_to_ros(&image, header))?;
*count += 1;
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,
count: &mut u64,
) -> 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),
};
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(()) => {
*count += 1;
meter.tick();
}
Err(err) => {
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());
}
}