use std::pin::Pin;
use std::sync::{Arc, Condvar, Mutex};
use bytes::Bytes;
use futures::{Stream, StreamExt};
#[cfg(feature = "compressed")]
use super::compressed_image_from_ros;
use super::meter::{RateMeter, WarnOnce};
use super::{BitrateControl, Bridge, QosProfile, RosImage, image_from_ros};
use crate::{DataFormat, DataPublisher, Error, ImagePublisher, Result};
pub fn spawn_ros_to_moq(
node: &mut r2r::Node,
topic: &str,
qos: QosProfile,
broadcast: ImagePublisher,
) -> Result<Bridge> {
spawn_image_bridge(node, topic, qos, broadcast, image_from_ros)
}
#[cfg(feature = "compressed")]
pub fn spawn_ros_to_moq_compressed(
node: &mut r2r::Node,
topic: &str,
qos: QosProfile,
broadcast: ImagePublisher,
) -> Result<Bridge> {
spawn_image_bridge(node, topic, qos, broadcast, compressed_image_from_ros)
}
trait ImageMessage: r2r::WrappedTypesupport + Send + 'static {
fn frame_id(&self) -> &str;
}
impl ImageMessage for RosImage {
fn frame_id(&self) -> &str {
&self.header.frame_id
}
}
#[cfg(feature = "compressed")]
impl ImageMessage for super::RosCompressedImage {
fn frame_id(&self) -> &str {
&self.header.frame_id
}
}
fn spawn_image_bridge<M>(
node: &mut r2r::Node,
topic: &str,
qos: QosProfile,
mut broadcast: ImagePublisher,
convert: fn(M) -> Result<crate::Image>,
) -> Result<Bridge>
where
M: ImageMessage,
{
let mut stream = node.subscribe::<M>(topic, qos)?;
let topic = topic.to_string();
let (bitrate_control, mut bitrate) = BitrateControl::new(broadcast.settings().bitrate);
let bridge = Bridge::spawn(|stats| async move {
let slot = Arc::new(LatestSlot::<M>::default());
let encoder = tokio::task::spawn_blocking({
let slot = slot.clone();
let stats = stats.clone();
move || -> Result<()> {
let mut warned = WarnOnce::default();
while let Some(msg) = slot.take() {
if bitrate.has_changed().unwrap_or(false) {
broadcast.set_bitrate(*bitrate.borrow_and_update());
}
broadcast.set_frame_id(msg.frame_id())?;
let image = match convert(msg) {
Ok(image) => image,
Err(err) => {
stats.dropped();
if warned.first() {
tracing::warn!(%err, "skipping image");
}
continue;
}
};
if broadcast.publish(&image)? {
stats.sent();
}
}
Ok(())
}
});
let slot = CloseOnDrop(slot);
let mut meter = RateMeter::new("ROS -> MoQ");
while let Some(msg) = stream.next().await {
stats.received();
meter.tick();
if slot.0.put(msg) {
stats.dropped();
tracing::debug!(topic, "encoder busy, dropped an older image");
}
if encoder.is_finished() {
break;
}
}
drop(slot);
encoder.await.expect("encoder thread panicked")
});
Ok(bridge.with_bitrate_control(bitrate_control))
}
#[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_ros_to_moq_data(
node: &mut r2r::Node,
topic: &str,
qos: QosProfile,
mut broadcast: DataPublisher,
) -> Result<Bridge> {
let settings = broadcast.settings();
let ros_type = settings
.ros_type
.clone()
.ok_or_else(|| Error::MissingRosType(broadcast.name().to_string()))?;
let mut messages: Pin<Box<dyn Stream<Item = Result<Bytes>> + Send>> = match settings.format {
DataFormat::Json => Box::pin(
node.subscribe_untyped(topic, &ros_type, qos)?
.map(|msg| Ok(serde_json::to_vec(&msg?)?.into())),
),
DataFormat::Cdr => Box::pin(node.subscribe_raw(topic, &ros_type, qos)?.map(|msg| Ok(msg.into()))),
};
Ok(Bridge::spawn(|stats| async move {
let mut meter = RateMeter::new("ROS -> MoQ");
let mut warned = WarnOnce::default();
while let Some(msg) = messages.next().await {
stats.received();
meter.tick();
match msg {
Ok(payload) => {
broadcast.publish(payload)?;
stats.sent();
}
Err(err) => {
stats.dropped();
if warned.first() {
tracing::warn!(%err, "skipping message");
}
}
}
}
Ok(())
}))
}
#[cfg(test)]
mod tests {
use std::thread;
use std::time::Duration;
use super::*;
#[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));
}
}