1use std::collections::{HashMap, HashSet};
4use std::time::{Duration, Instant};
5
6use anyhow::{Context, bail, ensure};
7use futures::StreamExt;
8use futures::stream::FuturesUnordered;
9use moqros::ros::{BitrateControl, Bridge, MoqToRos, QosProfile, ROS_COMPRESSED_IMAGE_TYPE, ROS_IMAGE_TYPE, r2r};
10use tokio::task::JoinError;
11
12use crate::args::Format;
13use crate::diagnostics::Diagnostics;
14use crate::runtime::{SharedNode, Spinner, shutdown_signal, spawn_spinner};
15use crate::spec::{Discovery, PubTopic, SubBroadcast};
16
17pub struct Bridges {
20 pub topics: Vec<PubTopic>,
21 pub broadcasts: Vec<SubBroadcast>,
22 pub video: moqros::ImageBroadcastSettings,
24 pub qos: QosProfile,
25 pub diagnostics_period: Option<Duration>,
27 pub discovery: Option<Discovery>,
29}
30
31pub struct Relay {
34 publisher: Option<moqros::Publisher>,
35 subscriber: Option<moqros::Subscriber>,
36 session: moqros::Session,
37}
38
39impl Relay {
40 pub fn publisher(publisher: moqros::Publisher) -> Self {
42 Self {
43 session: publisher.session().clone(),
44 publisher: Some(publisher),
45 subscriber: None,
46 }
47 }
48
49 pub fn subscriber(subscriber: moqros::Subscriber) -> Self {
51 Self {
52 session: subscriber.session().clone(),
53 publisher: None,
54 subscriber: Some(subscriber),
55 }
56 }
57
58 pub fn both(publisher: moqros::Publisher, subscriber: moqros::Subscriber) -> Self {
60 Self {
61 session: publisher.session().clone(),
62 publisher: Some(publisher),
63 subscriber: Some(subscriber),
64 }
65 }
66}
67
68pub async fn run_bridges(node: r2r::Node, relay: Relay, bridges: Bridges) -> anyhow::Result<()> {
81 let Bridges {
82 topics,
83 broadcasts,
84 mut video,
85 qos,
86 diagnostics_period,
87 discovery,
88 } = bridges;
89 ensure!(
90 topics.is_empty() || relay.publisher.is_some(),
91 "topics to publish need a publisher"
92 );
93 ensure!(
94 (broadcasts.is_empty() && discovery.is_none()) || relay.subscriber.is_some(),
95 "broadcasts to republish need a subscriber"
96 );
97 let mut known_broadcasts: HashSet<String> = broadcasts
99 .iter()
100 .map(|b| b.broadcast.trim_matches('/').to_string())
101 .collect();
102 let mut used_topics: HashSet<String> = broadcasts
103 .iter()
104 .map(|b| b.topic.clone())
105 .chain(topics.iter().map(|t| t.topic.clone()))
106 .collect();
107 let mut announcements = relay
108 .subscriber
109 .as_ref()
110 .filter(|_| discovery.is_some())
111 .map(|subscriber| subscriber.announced(""));
112
113 let node = SharedNode::new(node);
114 let mut spinner = match topics.is_empty() {
117 true => Spinner::idle(),
118 false => spawn_spinner(node.clone()),
119 };
120
121 let mut subscribing: FuturesUnordered<_> = broadcasts
124 .into_iter()
125 .filter_map(|broadcast| Some(subscribe_after(relay.subscriber.clone()?, broadcast, Duration::ZERO)))
126 .collect();
127 let mut pub_bridges = FuturesUnordered::new();
128 let mut sub_bridges = FuturesUnordered::new();
129 let has_video = topics.iter().any(|topic| topic.format == Format::Video);
130 let mut pending = PendingTopics::new(topics);
131 let mut restarts = Restarts::default();
132 let mut parameters = match has_video {
134 true => Some(node.with(watch_parameters)?),
135 false => None,
136 };
137 let mut bitrates: HashMap<String, BitrateControl> = HashMap::new();
138 let mut diagnostics = diagnostics_period.and_then(|period| match node.with(Diagnostics::new) {
139 Ok(diagnostics) => Some((diagnostics, tokio::time::interval(period))),
140 Err(err) => {
141 tracing::warn!(%err, "not publishing diagnostics; build with diagnostic_msgs in IDL_PACKAGE_FILTER");
142 None
143 }
144 });
145 let shutdown = shutdown_signal();
147 tokio::pin!(shutdown);
148
149 loop {
150 if let Some(publisher) = &relay.publisher {
151 for (topic, bridge) in pending.start_ready(&node, publisher, &video, &qos, &mut restarts)? {
152 let stats = bridge.watch_stats();
153 if let Some((diagnostics, _)) = &mut diagnostics {
154 diagnostics.running(
155 &topic.topic,
156 &format!("ROS -> MoQ {}", topic.format.name()),
157 stats.clone(),
158 );
159 }
160 if let Some(control) = bridge.bitrate_control() {
161 bitrates.insert(topic.topic.clone(), control);
162 }
163 pub_bridges.push(async move { (topic, stats, bridge.await) });
164 }
165 }
166
167 tokio::select! {
168 Some((ready, broadcast)) = subscribing.next() => {
169 match node.with(|node| spawn_sub_broadcast(ready, node, &broadcast, qos.clone())) {
170 Ok(mut bridge) => {
171 let stats = bridge.watch_stats();
172 if let Some((diagnostics, _)) = &mut diagnostics {
173 diagnostics.running(&broadcast.broadcast, "MoQ -> ROS", stats.clone());
174 }
175 let teardown = bridge.take_teardown();
176 sub_bridges.push(async move { (broadcast, stats, teardown, bridge.await) });
177 }
178 Err(err) => {
181 tracing::error!(%err, broadcast = broadcast.broadcast, topic = broadcast.topic, "skipping broadcast");
182 if discovery.is_none() && subscribing.is_empty() && sub_bridges.is_empty() && pub_bridges.is_empty() && pending.is_empty() {
183 bail!("nothing left to bridge: no broadcast could be republished");
184 }
185 }
186 }
187 }
188 Some(name) = next_announcement(&mut announcements) => {
189 let Some(discovery) = &discovery else { continue };
190 if !discovery.covers(&name) || !known_broadcasts.insert(name.trim_matches('/').to_string()) {
191 continue;
192 }
193 match discovery.broadcast(&name) {
194 Ok(broadcast) if used_topics.insert(broadcast.topic.clone()) => {
195 tracing::info!(broadcast = name, topic = broadcast.topic, "found a broadcast");
196 if let Some(subscriber) = relay.subscriber.clone() {
197 subscribing.push(subscribe_after(subscriber, broadcast, Duration::ZERO));
198 }
199 }
200 Ok(broadcast) => tracing::warn!(broadcast = name, topic = broadcast.topic, "skipping a found broadcast: its topic is taken"),
201 Err(err) => tracing::warn!(broadcast = name, %err, "skipping a found broadcast"),
202 }
203 }
204 Some((name, value)) = next_parameter(&mut parameters) => {
205 match runtime_bitrate(&name, &value) {
206 Some(Ok(bitrate)) => {
207 tracing::info!(bitrate, "changing the video bitrate");
208 video.encoder.bitrate = bitrate;
209 for control in bitrates.values() {
210 control.set(bitrate);
211 }
212 }
213 Some(Err(err)) => tracing::warn!(%err, "ignoring the new bitrate"),
214 None => tracing::warn!(parameter = name, "only `bitrate` changes at runtime; restart the node to apply this parameter"),
215 }
216 }
217 Some((topic, stats, res)) = pub_bridges.next() => {
218 let stats = *stats.borrow();
219 log_stopped("topic", &topic.topic, &stats, res);
220 if let Some((diagnostics, _)) = &mut diagnostics {
221 diagnostics.restarting(&topic.topic);
222 }
223 let delay = restarts.next(&format!("topic {}", topic.topic), stats.sent > 0);
224 tracing::info!(topic = topic.topic, ?delay, "restarting the topic bridge");
225 pending.restart(topic, delay);
226 }
227 Some((broadcast, stats, teardown, res)) = sub_bridges.next() => {
228 let stats = *stats.borrow();
229 log_stopped("broadcast", &broadcast.broadcast, &stats, res);
230 if let Some((diagnostics, _)) = &mut diagnostics {
231 diagnostics.restarting(&broadcast.broadcast);
232 }
233 node.with(|node| teardown.run(node));
234 let delay = restarts.next(&format!("broadcast {}", broadcast.broadcast), stats.sent > 0);
235 tracing::info!(broadcast = broadcast.broadcast, ?delay, "restarting the broadcast bridge");
236 if let Some(subscriber) = relay.subscriber.clone() {
237 subscribing.push(subscribe_after(subscriber, broadcast, delay));
238 }
239 }
240 _ = tokio::time::sleep(Duration::from_millis(500)), if !pending.is_empty() => {}
241 Some(diagnostics) = next_report(&mut diagnostics) => {
242 if let Err(err) = diagnostics.publish() {
243 tracing::warn!(%err, "publishing diagnostics failed");
244 }
245 }
246 res = relay.session.closed() => return res.context("relay connection closed"),
248 err = spinner.failed() => return Err(err),
249 res = &mut shutdown => {
250 tracing::info!(signal = res?, "shutting down");
251 if let Err(err) = relay.session.close().await {
252 tracing::warn!(%err, "the relay connection didn't close cleanly");
253 }
254 return Ok(());
255 }
256 }
257 }
258}
259
260type ParameterChanges = std::pin::Pin<Box<dyn futures::Stream<Item = (String, r2r::ParameterValue)> + Send>>;
262
263fn watch_parameters(node: &mut r2r::Node) -> anyhow::Result<ParameterChanges> {
265 let (serve, changes) = node
266 .make_parameter_handler()
267 .context("failed to serve ROS parameters")?;
268 tokio::spawn(serve);
269 Ok(Box::pin(changes))
270}
271
272async fn next_announcement(announcements: &mut Option<moqros::Announcements>) -> Option<String> {
274 match announcements {
275 Some(announcements) => announcements.next().await,
276 None => std::future::pending().await,
277 }
278}
279
280async fn next_parameter(parameters: &mut Option<ParameterChanges>) -> Option<(String, r2r::ParameterValue)> {
282 match parameters {
283 Some(changes) => changes.next().await,
284 None => std::future::pending().await,
285 }
286}
287
288fn runtime_bitrate(name: &str, value: &r2r::ParameterValue) -> Option<Result<u32, String>> {
291 (name == "bitrate").then(|| match value {
292 r2r::ParameterValue::Integer(bitrate) => u32::try_from(*bitrate)
293 .ok()
294 .filter(|&bitrate| bitrate > 0)
295 .ok_or_else(|| format!("{bitrate} is not a bitrate in bit/s")),
296 other => Err(format!("{other:?} is not an integer")),
297 })
298}
299
300async fn next_report(diagnostics: &mut Option<(Diagnostics, tokio::time::Interval)>) -> Option<&mut Diagnostics> {
302 match diagnostics {
303 Some((diagnostics, interval)) => {
304 interval.tick().await;
305 Some(diagnostics)
306 }
307 None => std::future::pending().await,
308 }
309}
310
311async fn subscribe_after(
313 subscriber: moqros::Subscriber,
314 broadcast: SubBroadcast,
315 delay: Duration,
316) -> (MoqToRos, SubBroadcast) {
317 tokio::time::sleep(delay).await;
318 let mut options = moqros::ImageOptions::default();
319 options.max_height = broadcast.max_height;
320 (
321 MoqToRos::subscribe_with(subscriber, &broadcast.broadcast, options).await,
322 broadcast,
323 )
324}
325
326fn log_stopped(
328 kind: &str,
329 name: &str,
330 stats: &moqros::ros::BridgeStats,
331 res: Result<moqros::Result<moqros::ros::BridgeStats>, JoinError>,
332) {
333 match res {
334 Ok(Ok(_)) => tracing::warn!(kind, name, ?stats, "bridge stopped"),
335 Ok(Err(err)) => tracing::warn!(kind, name, ?stats, %err, "bridge failed"),
336 Err(err) => tracing::error!(kind, name, ?stats, %err, "bridge panicked"),
337 }
338}
339
340#[derive(Debug, Default)]
343struct Restarts {
344 delays: HashMap<String, Duration>,
345}
346
347impl Restarts {
348 const MIN: Duration = Duration::from_secs(1);
349 const MAX: Duration = Duration::from_secs(30);
350
351 fn next(&mut self, bridge: &str, progressed: bool) -> Duration {
354 let delay = self.delays.entry(bridge.to_string()).or_default();
355 *delay = match progressed || delay.is_zero() {
356 true => Self::MIN,
357 false => (*delay * 2).min(Self::MAX),
358 };
359 *delay
360 }
361}
362
363struct Pending {
365 topic: PubTopic,
366 due: Instant,
368 restart: bool,
370}
371
372struct PendingTopics {
375 topics: Vec<Pending>,
376 logged: bool,
378}
379
380impl PendingTopics {
381 fn new(topics: Vec<PubTopic>) -> Self {
382 let now = Instant::now();
383 let topics = topics
384 .into_iter()
385 .map(|topic| Pending {
386 topic,
387 due: now,
388 restart: false,
389 })
390 .collect();
391 Self { topics, logged: false }
392 }
393
394 fn is_empty(&self) -> bool {
395 self.topics.is_empty()
396 }
397
398 fn restart(&mut self, topic: PubTopic, delay: Duration) {
400 self.topics.push(Pending {
401 topic,
402 due: Instant::now() + delay,
403 restart: true,
404 });
405 }
406
407 fn start_ready(
413 &mut self,
414 node: &SharedNode,
415 publisher: &moqros::Publisher,
416 video: &moqros::ImageBroadcastSettings,
417 qos: &QosProfile,
418 restarts: &mut Restarts,
419 ) -> anyhow::Result<Vec<(PubTopic, Bridge)>> {
420 let now = Instant::now();
421 let mut started = Vec::new();
422 let mut waiting = Vec::new();
423 for mut pending in std::mem::take(&mut self.topics) {
424 if pending.due > now {
425 self.topics.push(pending);
426 continue;
427 }
428 match node.with(|node| start_pub_topic(node, publisher, &pending.topic, video, qos.clone())) {
429 Ok(Some(bridge)) => started.push((pending.topic, bridge)),
430 Ok(None) => {
431 waiting.push(pending.topic.topic.clone());
432 self.topics.push(pending);
433 }
434 Err(err) if pending.restart => {
435 let delay = restarts.next(&format!("topic {}", pending.topic.topic), false);
436 tracing::warn!(topic = pending.topic.topic, %err, ?delay, "restarting the topic bridge failed");
437 pending.due = now + delay;
438 self.topics.push(pending);
439 }
440 Err(err) => return Err(err),
441 }
442 }
443 if !waiting.is_empty() && !self.logged {
444 tracing::info!(topics = ?waiting, "waiting for topics to appear to learn their types");
445 self.logged = true;
446 }
447 Ok(started)
448 }
449}
450
451fn start_pub_topic(
454 node: &mut r2r::Node,
455 publisher: &moqros::Publisher,
456 topic: &PubTopic,
457 video: &moqros::ImageBroadcastSettings,
458 qos: QosProfile,
459) -> anyhow::Result<Option<Bridge>> {
460 let ros_type = match &topic.ros_type {
461 Some(ros_type) => ros_type.clone(),
462 None => match moqros::ros::topic_type(node, &topic.topic)? {
463 Some(ros_type) => ros_type,
464 None => return Ok(None),
465 },
466 };
467 let bridge = match topic.format.data() {
468 None => {
469 let images = publisher.create_image_broadcast(&topic.broadcast, video.clone())?;
470 match ros_type.as_str() {
471 ROS_IMAGE_TYPE => moqros::ros::spawn_ros_to_moq(node, &topic.topic, qos, images)?,
472 ROS_COMPRESSED_IMAGE_TYPE => moqros::ros::spawn_ros_to_moq_compressed(node, &topic.topic, qos, images)?,
473 other => bail!(
474 "{}: video needs {ROS_IMAGE_TYPE} or {ROS_COMPRESSED_IMAGE_TYPE} messages, not {other}; use format=json or cdr",
475 topic.topic
476 ),
477 }
478 }
479 Some(format) => {
480 let settings = moqros::DataSettings {
481 format,
482 mode: topic.mode,
483 ros_type: Some(ros_type),
484 };
485 let data = publisher.create_data_broadcast(&topic.broadcast, settings)?;
486 moqros::ros::spawn_ros_to_moq_data(node, &topic.topic, qos, data)?
487 }
488 };
489 tracing::info!(
490 topic = topic.topic,
491 format = ?topic.format,
492 broadcast = topic.broadcast,
493 "bridging ROS -> MoQ"
494 );
495 Ok(Some(bridge))
496}
497
498fn spawn_sub_broadcast(
500 ready: moqros::ros::MoqToRos,
501 node: &mut r2r::Node,
502 broadcast: &SubBroadcast,
503 qos: QosProfile,
504) -> moqros::Result<Bridge> {
505 let target = moqros::ros::RosTarget {
506 topic: broadcast.topic.clone(),
507 qos,
508 frame_id: broadcast.frame_id.clone(),
509 stamp: broadcast.stamp,
510 encoding: broadcast.encoding,
511 ros_type: broadcast.ros_type.clone(),
512 };
513 let bridge = ready.spawn(node, target)?;
514 tracing::info!(
515 broadcast = broadcast.broadcast,
516 topic = broadcast.topic,
517 "bridging MoQ -> ROS"
518 );
519 Ok(bridge)
520}
521
522#[cfg(test)]
523mod tests {
524 use super::*;
525
526 #[test]
527 fn reads_runtime_bitrates() {
528 use r2r::ParameterValue::{Double, Integer};
529 assert_eq!(runtime_bitrate("bitrate", &Integer(500_000)), Some(Ok(500_000)));
530 assert!(matches!(runtime_bitrate("bitrate", &Integer(0)), Some(Err(_))));
531 assert!(matches!(runtime_bitrate("bitrate", &Integer(-1)), Some(Err(_))));
532 assert!(matches!(runtime_bitrate("bitrate", &Integer(1 << 40)), Some(Err(_))));
533 assert!(matches!(runtime_bitrate("bitrate", &Double(5e5)), Some(Err(_))));
534 assert_eq!(runtime_bitrate("fps", &Integer(10)), None);
535 }
536
537 #[test]
538 fn restart_delays_back_off_and_reset() {
539 let mut restarts = Restarts::default();
540 let delays: Vec<u64> = (0..7).map(|_| restarts.next("topic /a", false).as_secs()).collect();
541 assert_eq!(delays, [1, 2, 4, 8, 16, 30, 30]);
542 assert_eq!(restarts.next("topic /b", false), Restarts::MIN);
544 assert_eq!(restarts.next("topic /a", true), Restarts::MIN);
546 assert_eq!(restarts.next("topic /a", false), Duration::from_secs(2));
547 }
548}