1use std::collections::HashSet;
4use std::str::FromStr;
5
6use anyhow::{bail, ensure};
7use clap::ValueEnum;
8use moqros::ros::{ROS_COMPRESSED_IMAGE_TYPE, ROS_IMAGE_TYPE, StampSource};
9
10use crate::args::Format;
11
12type Spec<'a> = (&'a str, Vec<(&'a str, &'a str)>);
14
15fn split_spec<'a>(spec: &'a str, keys: &[&str]) -> Result<Spec<'a>, String> {
17 let mut parts = spec.split(',');
18 let name = parts.next().unwrap_or_default().trim();
19 if name.is_empty() {
20 return Err(format!("{spec:?} doesn't start with a name"));
21 }
22 let mut options: Vec<(&str, &str)> = Vec::new();
23 for part in parts {
24 let (key, value) = part
25 .split_once('=')
26 .ok_or_else(|| format!("expected key=value, got {part:?}"))?;
27 let (key, value) = (key.trim(), value.trim());
28 if !keys.contains(&key) {
29 return Err(format!("unknown option {key:?} (expected one of: {})", keys.join(", ")));
30 }
31 if options.iter().any(|(k, _)| *k == key) {
32 return Err(format!("option {key:?} given twice"));
33 }
34 options.push((key, value));
35 }
36 Ok((name, options))
37}
38
39#[derive(Debug, Clone, PartialEq, Eq)]
43pub struct TopicSpec {
44 pub topic: String,
45 pub format: Option<Format>,
46 pub ros_type: Option<String>,
47 pub mode: Option<moqros::DataMode>,
48 pub broadcast: Option<String>,
49}
50
51impl FromStr for TopicSpec {
52 type Err = String;
53
54 fn from_str(spec: &str) -> Result<Self, String> {
55 let (topic, options) = split_spec(spec, &["format", "type", "mode", "broadcast"])?;
56 let mut parsed = Self {
57 topic: topic.to_string(),
58 format: None,
59 ros_type: None,
60 mode: None,
61 broadcast: None,
62 };
63 for (key, value) in options {
64 match key {
65 "format" => parsed.format = Some(Format::from_str(value, true)?),
66 "type" => parsed.ros_type = Some(value.to_string()),
67 "mode" => parsed.mode = Some(value.parse().map_err(|err| format!("{err}"))?),
68 _ => parsed.broadcast = Some(value.to_string()),
69 }
70 }
71 Ok(parsed)
72 }
73}
74
75#[derive(Debug, Clone, PartialEq, Eq)]
77pub struct PubTopic {
78 pub topic: String,
79 pub broadcast: String,
80 pub format: Format,
81 pub mode: moqros::DataMode,
82 pub ros_type: Option<String>,
84}
85
86pub fn pub_topics(
89 specs: Vec<TopicSpec>,
90 format: Format,
91 mode: moqros::DataMode,
92 ros_type: Option<String>,
93 broadcast: Option<String>,
94) -> anyhow::Result<Vec<PubTopic>> {
95 ensure!(!specs.is_empty(), "no topic to publish");
96 ensure!(
97 specs.len() == 1 || (ros_type.is_none() && broadcast.is_none()),
98 "--type and --broadcast need a single --topic; give several topics `type=` and `broadcast=` options instead"
99 );
100 let mut broadcasts = HashSet::new();
101 specs
102 .into_iter()
103 .map(|spec| {
104 let topic = PubTopic {
105 broadcast: spec
106 .broadcast
107 .or(broadcast.clone())
108 .unwrap_or_else(|| moqros::broadcast_name_for_topic(&spec.topic)),
109 format: spec.format.unwrap_or(format),
110 mode: spec.mode.unwrap_or(mode),
111 ros_type: spec.ros_type.or(ros_type.clone()),
112 topic: spec.topic,
113 };
114 if topic.format == Format::Video
115 && let Some(ros_type) = topic
116 .ros_type
117 .as_deref()
118 .filter(|t| ![ROS_IMAGE_TYPE, ROS_COMPRESSED_IMAGE_TYPE].contains(t))
119 {
120 bail!(
121 "{}: video needs {ROS_IMAGE_TYPE} or {ROS_COMPRESSED_IMAGE_TYPE} messages, not {ros_type}; use format=json or cdr",
122 topic.topic
123 );
124 }
125 ensure!(
126 broadcasts.insert(topic.broadcast.clone()),
127 "several topics publish the broadcast {:?}",
128 topic.broadcast
129 );
130 Ok(topic)
131 })
132 .collect()
133}
134
135#[derive(Debug, Clone, PartialEq, Eq)]
139pub struct BroadcastSpec {
140 pub broadcast: String,
141 pub topic: Option<String>,
142 pub ros_type: Option<String>,
143 pub frame_id: Option<String>,
144 pub stamp: Option<StampSource>,
145 pub max_height: Option<u32>,
146 pub encoding: Option<moqros::PixelFormat>,
147}
148
149impl FromStr for BroadcastSpec {
150 type Err = String;
151
152 fn from_str(spec: &str) -> Result<Self, String> {
153 let (broadcast, options) = split_spec(spec, &["topic", "type", "frame_id", "stamp", "max_height", "encoding"])?;
154 let mut parsed = Self {
155 broadcast: broadcast.to_string(),
156 topic: None,
157 ros_type: None,
158 frame_id: None,
159 stamp: None,
160 max_height: None,
161 encoding: None,
162 };
163 for (key, value) in options {
164 match key {
165 "stamp" => parsed.stamp = Some(value.parse().map_err(|err| format!("{err}"))?),
166 "max_height" => {
167 parsed.max_height = Some(
168 value
169 .parse()
170 .map_err(|_| format!("max_height must be a number, not {value:?}"))?,
171 );
172 }
173 "encoding" => parsed.encoding = Some(parse_encoding(value)?),
174 "topic" => parsed.topic = Some(value.to_string()),
175 "type" => parsed.ros_type = Some(value.to_string()),
176 _ => parsed.frame_id = Some(value.to_string()),
177 }
178 }
179 Ok(parsed)
180 }
181}
182
183pub fn parse_encoding(value: &str) -> Result<moqros::PixelFormat, String> {
186 use moqros::PixelFormat::{Bgr8, Bgra8, Mono8, Rgb8, Rgba8};
187 match value.parse::<moqros::PixelFormat>() {
188 Ok(format @ (Rgb8 | Bgr8 | Rgba8 | Bgra8 | Mono8)) => Ok(format),
189 _ => Err(format!(
190 "unsupported encoding {value:?} (expected one of: rgb8, bgr8, rgba8, bgra8, mono8)"
191 )),
192 }
193}
194
195#[derive(Debug, Clone, PartialEq, Eq)]
197pub struct SubBroadcast {
198 pub broadcast: String,
199 pub topic: String,
200 pub ros_type: Option<String>,
202 pub frame_id: Option<String>,
204 pub stamp: StampSource,
205 pub max_height: Option<u32>,
207 pub encoding: moqros::PixelFormat,
209}
210
211pub const DEFAULT_SUB_TOPIC: &str = "/moqros/image";
213
214pub fn sub_broadcasts(
218 specs: Vec<BroadcastSpec>,
219 topic: Option<String>,
220 ros_type: Option<String>,
221 frame_id: Option<&str>,
222 stamp: StampSource,
223 max_height: Option<u32>,
224 encoding: moqros::PixelFormat,
225) -> anyhow::Result<Vec<SubBroadcast>> {
226 ensure!(!specs.is_empty(), "no broadcast to subscribe to");
227 let single = specs.len() == 1;
228 ensure!(
229 single || (topic.is_none() && ros_type.is_none()),
230 "--topic and --type need a single --broadcast; give several broadcasts `topic=` and `type=` options instead"
231 );
232 let mut topics = HashSet::new();
233 specs
234 .into_iter()
235 .map(|spec| {
236 let broadcast = SubBroadcast {
237 topic: spec.topic.or(topic.clone()).unwrap_or_else(|| match single {
238 true => DEFAULT_SUB_TOPIC.to_string(),
239 false => format!("/moqros/{}", spec.broadcast.trim_matches('/')),
240 }),
241 ros_type: spec.ros_type.or(ros_type.clone()),
242 frame_id: spec.frame_id.or_else(|| frame_id.map(String::from)),
243 stamp: spec.stamp.unwrap_or(stamp),
244 max_height: spec.max_height.or(max_height),
245 encoding: spec.encoding.unwrap_or(encoding),
246 broadcast: spec.broadcast,
247 };
248 ensure!(
249 topics.insert(broadcast.topic.clone()),
250 "several broadcasts publish on {}",
251 broadcast.topic
252 );
253 Ok(broadcast)
254 })
255 .collect()
256}
257
258#[derive(Debug, Clone, PartialEq, Eq)]
260pub struct Discovery {
261 pub prefixes: Vec<String>,
263 pub topic_template: String,
265 pub frame_id: Option<String>,
267 pub stamp: StampSource,
268 pub max_height: Option<u32>,
269 pub encoding: moqros::PixelFormat,
270}
271
272pub const DEFAULT_TOPIC_TEMPLATE: &str = "/moqros/{name}";
274
275impl Discovery {
276 pub fn new(
278 prefixes: Vec<String>,
279 topic_template: String,
280 frame_id: Option<String>,
281 stamp: StampSource,
282 max_height: Option<u32>,
283 encoding: moqros::PixelFormat,
284 ) -> anyhow::Result<Self> {
285 ensure!(
286 topic_template.contains("{name}"),
287 "--topic-template {topic_template:?} has no {{name}}"
288 );
289 let discovery = Self {
290 prefixes: prefixes.iter().map(|p| p.trim_matches('/').to_string()).collect(),
291 topic_template,
292 frame_id,
293 stamp,
294 max_height,
295 encoding,
296 };
297 if let Err(err) = discovery.broadcast("name") {
298 bail!("--topic-template {:?}: {err}", discovery.topic_template);
299 }
300 Ok(discovery)
301 }
302
303 pub fn covers(&self, name: &str) -> bool {
305 self.prefixes.iter().any(|prefix| is_under(name, prefix))
306 }
307
308 pub fn broadcast(&self, name: &str) -> Result<SubBroadcast, String> {
311 let topic = self.topic_template.replace("{name}", name.trim_matches('/'));
312 if !is_ros_topic(&topic) {
313 return Err(format!(
314 "{topic:?} isn't a topic name moqros republishes on (letters, digits, `_` and `/` only)"
315 ));
316 }
317 Ok(SubBroadcast {
318 broadcast: name.to_string(),
319 topic,
320 ros_type: None,
321 frame_id: self.frame_id.clone(),
322 stamp: self.stamp,
323 max_height: self.max_height,
324 encoding: self.encoding,
325 })
326 }
327}
328
329fn is_under(name: &str, prefix: &str) -> bool {
332 let name = name.trim_matches('/');
333 prefix.is_empty() || name == prefix || name.strip_prefix(prefix).is_some_and(|rest| rest.starts_with('/'))
334}
335
336fn is_ros_topic(topic: &str) -> bool {
339 let Some(tokens) = topic.strip_prefix('/') else {
340 return false;
341 };
342 let is_token = |token: &str| {
343 let mut chars = token.chars();
344 chars.next().is_some_and(|c| c.is_ascii_alphabetic() || c == '_')
345 && chars.all(|c| c.is_ascii_alphanumeric() || c == '_')
346 && !token.contains("__")
347 };
348 !tokens.is_empty() && tokens.split('/').all(is_token)
349}
350
351pub fn check_no_loop(
354 topics: &[PubTopic],
355 broadcasts: &[SubBroadcast],
356 discovery: Option<&Discovery>,
357) -> anyhow::Result<()> {
358 let name = |name: &str| name.trim_matches('/').to_string();
359 for topic in topics {
360 if let Some(discovery) = discovery {
361 ensure!(
362 !discovery.covers(&topic.broadcast),
363 "the broadcast {:?} is published (from {}) and under a --broadcast-prefix; that would loop",
364 topic.broadcast,
365 topic.topic
366 );
367 }
368 for broadcast in broadcasts {
369 ensure!(
370 name(&topic.broadcast) != name(&broadcast.broadcast),
371 "the broadcast {:?} is both published (from {}) and subscribed to; that would loop",
372 topic.broadcast,
373 topic.topic
374 );
375 ensure!(
376 name(&topic.topic) != name(&broadcast.topic),
377 "the topic {} is both published to the relay and republished from {:?}; that would loop",
378 topic.topic,
379 broadcast.broadcast
380 );
381 }
382 }
383 Ok(())
384}
385
386#[cfg(test)]
387mod tests {
388 use super::*;
389
390 #[test]
391 fn parses_topic_specs() {
392 let spec: TopicSpec = "/scan, format=cdr ,type=sensor_msgs/msg/LaserScan,mode=stream,broadcast=lidar"
393 .parse()
394 .unwrap();
395 assert_eq!(
396 spec,
397 TopicSpec {
398 topic: "/scan".into(),
399 format: Some(Format::Cdr),
400 ros_type: Some("sensor_msgs/msg/LaserScan".into()),
401 mode: Some(moqros::DataMode::Stream),
402 broadcast: Some("lidar".into()),
403 }
404 );
405 assert_eq!("/image_raw".parse::<TopicSpec>().unwrap().format, None);
406 for bad in [
407 "",
408 ",format=json",
409 "/a,json",
410 "/a,codec=vp8",
411 "/a,format=xml",
412 "/a,mode=x",
413 "/a,type=x,type=y",
414 ] {
415 assert!(bad.parse::<TopicSpec>().is_err(), "{bad:?}");
416 }
417
418 let spec: BroadcastSpec = "camera/image_raw,topic=/out,frame_id=cam,type=sensor_msgs/msg/Image,stamp=receive"
419 .parse()
420 .unwrap();
421 assert_eq!(
422 (
423 spec.broadcast.as_str(),
424 spec.topic.as_deref(),
425 spec.frame_id.as_deref(),
426 spec.stamp
427 ),
428 (
429 "camera/image_raw",
430 Some("/out"),
431 Some("cam"),
432 Some(StampSource::Receive)
433 )
434 );
435 assert!("camera,stamp=now".parse::<BroadcastSpec>().is_err());
436 assert!("chatter,format=json".parse::<BroadcastSpec>().is_err());
437 assert_eq!(
438 "cam,max_height=360".parse::<BroadcastSpec>().unwrap().max_height,
439 Some(360)
440 );
441 assert!("cam,max_height=tall".parse::<BroadcastSpec>().is_err());
442 assert_eq!(
443 "cam,encoding=mono8".parse::<BroadcastSpec>().unwrap().encoding,
444 Some(moqros::PixelFormat::Mono8)
445 );
446 assert!("cam,encoding=yuv422".parse::<BroadcastSpec>().is_err());
447 assert!("cam,encoding=16UC1".parse::<BroadcastSpec>().is_err());
448 }
449
450 #[test]
451 fn resolves_pub_topics() {
452 let specs = |specs: &[&str]| {
453 specs
454 .iter()
455 .map(|s| s.parse::<TopicSpec>().unwrap())
456 .collect::<Vec<_>>()
457 };
458 let snapshot = moqros::DataMode::Snapshot;
459
460 let topics = pub_topics(
461 specs(&[
462 "/camera/image_raw",
463 "/chatter,format=json",
464 "/log,mode=stream,broadcast=logs",
465 ]),
466 Format::Video,
467 snapshot,
468 None,
469 None,
470 )
471 .unwrap();
472 let summary: Vec<_> = topics
473 .iter()
474 .map(|t| (t.broadcast.as_str(), t.format, t.mode))
475 .collect();
476 assert_eq!(
477 summary,
478 [
479 ("camera/image_raw", Format::Video, snapshot),
480 ("chatter", Format::Json, snapshot),
481 ("logs", Format::Video, moqros::DataMode::Stream),
482 ]
483 );
484
485 let one = pub_topics(
487 specs(&["/a"]),
488 Format::Cdr,
489 snapshot,
490 Some("t/msg/T".into()),
491 Some("b".into()),
492 )
493 .unwrap();
494 assert_eq!(
495 (one[0].broadcast.as_str(), one[0].ros_type.as_deref()),
496 ("b", Some("t/msg/T"))
497 );
498 assert!(
499 pub_topics(
500 specs(&["/a", "/b"]),
501 Format::Json,
502 snapshot,
503 Some("t/msg/T".into()),
504 None
505 )
506 .is_err()
507 );
508 assert!(pub_topics(specs(&["/a", "/b"]), Format::Json, snapshot, None, Some("b".into())).is_err());
509
510 assert!(pub_topics(specs(&["/a", "a,format=json"]), Format::Video, snapshot, None, None).is_err());
511 assert!(
512 pub_topics(
513 specs(&["/a,type=std_msgs/msg/String"]),
514 Format::Video,
515 snapshot,
516 None,
517 None
518 )
519 .is_err()
520 );
521 assert!(
522 pub_topics(
523 specs(&["/a,type=sensor_msgs/msg/Image"]),
524 Format::Video,
525 snapshot,
526 None,
527 None
528 )
529 .is_ok()
530 );
531 assert!(pub_topics(Vec::new(), Format::Video, snapshot, None, None).is_err());
532 }
533
534 #[test]
535 fn refuses_loops() {
536 let topics = |specs: &[&str]| {
537 let specs = specs.iter().map(|s| s.parse::<TopicSpec>().unwrap()).collect();
538 pub_topics(specs, Format::Json, moqros::DataMode::default(), None, None).unwrap()
539 };
540 let broadcasts = |specs: &[&str]| {
541 let specs = specs.iter().map(|s| s.parse::<BroadcastSpec>().unwrap()).collect();
542 sub_broadcasts(
543 specs,
544 None,
545 None,
546 None,
547 StampSource::Source,
548 None,
549 moqros::PixelFormat::Rgb8,
550 )
551 .unwrap()
552 };
553
554 let odom = topics(&["/odom,broadcast=robot/odom"]);
555 assert!(check_no_loop(&odom, &broadcasts(&["robot/cmd_vel,topic=/cmd_vel"]), None).is_ok());
556 assert!(check_no_loop(&odom, &broadcasts(&["/robot/odom/,topic=/other"]), None).is_err());
558 assert!(check_no_loop(&odom, &broadcasts(&["robot/cmd_vel,topic=/odom"]), None).is_err());
560 assert!(check_no_loop(&odom, &[], None).is_ok());
561 }
562
563 #[test]
564 fn resolves_sub_broadcasts() {
565 let specs = |specs: &[&str]| {
566 specs
567 .iter()
568 .map(|s| s.parse::<BroadcastSpec>().unwrap())
569 .collect::<Vec<_>>()
570 };
571 let topics = |broadcasts: Vec<SubBroadcast>| broadcasts.into_iter().map(|b| b.topic).collect::<Vec<_>>();
572
573 assert_eq!(
574 topics(
575 sub_broadcasts(
576 specs(&["image_raw"]),
577 None,
578 None,
579 None,
580 StampSource::Source,
581 None,
582 moqros::PixelFormat::Rgb8
583 )
584 .unwrap()
585 ),
586 [DEFAULT_SUB_TOPIC]
587 );
588 assert_eq!(
589 topics(
590 sub_broadcasts(
591 specs(&["image_raw"]),
592 Some("/x".into()),
593 None,
594 None,
595 StampSource::Source,
596 None,
597 moqros::PixelFormat::Rgb8
598 )
599 .unwrap()
600 ),
601 ["/x"]
602 );
603 let broadcasts = sub_broadcasts(
604 specs(&[
605 "sample/image_raw,topic=/moqros/image",
606 "chatter",
607 "/front/imu/,frame_id=imu,stamp=receive",
608 ]),
609 None,
610 None,
611 Some("camera"),
612 StampSource::Source,
613 Some(480),
614 moqros::PixelFormat::Bgr8,
615 )
616 .unwrap();
617 assert_eq!(broadcasts[0].frame_id.as_deref(), Some("camera"));
618 assert_eq!(broadcasts[0].stamp, StampSource::Source);
619 assert_eq!(broadcasts[2].frame_id.as_deref(), Some("imu"));
620 assert_eq!(broadcasts[2].stamp, StampSource::Receive);
621 assert_eq!(broadcasts[2].max_height, Some(480));
622 assert_eq!(broadcasts[2].encoding, moqros::PixelFormat::Bgr8);
623 assert_eq!(
624 sub_broadcasts(
625 specs(&["image_raw"]),
626 None,
627 None,
628 None,
629 StampSource::Source,
630 None,
631 moqros::PixelFormat::Rgb8
632 )
633 .unwrap()[0]
634 .frame_id,
635 None
636 );
637 assert_eq!(
638 topics(broadcasts),
639 ["/moqros/image", "/moqros/chatter", "/moqros/front/imu"]
640 );
641
642 assert!(
643 sub_broadcasts(
644 specs(&["a", "b"]),
645 Some("/x".into()),
646 None,
647 None,
648 StampSource::Source,
649 None,
650 moqros::PixelFormat::Rgb8
651 )
652 .is_err()
653 );
654 assert!(
655 sub_broadcasts(
656 specs(&["a", "b"]),
657 None,
658 Some("t/msg/T".into()),
659 None,
660 StampSource::Source,
661 None,
662 moqros::PixelFormat::Rgb8
663 )
664 .is_err()
665 );
666 assert!(
667 sub_broadcasts(
668 specs(&["a,topic=/x", "b,topic=/x"]),
669 None,
670 None,
671 None,
672 StampSource::Source,
673 None,
674 moqros::PixelFormat::Rgb8
675 )
676 .is_err()
677 );
678 }
679
680 #[test]
681 fn discovers_broadcasts_under_prefixes() {
682 let discovery = Discovery::new(
683 vec!["robots/".into(), "/drones".into()],
684 DEFAULT_TOPIC_TEMPLATE.into(),
685 Some("camera".into()),
686 StampSource::Receive,
687 Some(480),
688 moqros::PixelFormat::Mono8,
689 )
690 .unwrap();
691 assert!(discovery.covers("robots/r1/camera"));
692 assert!(discovery.covers("drones"));
693 assert!(!discovery.covers("robots2/camera"));
694 assert!(!discovery.covers("cars/robots"));
695
696 let found = discovery.broadcast("robots/r1/camera").unwrap();
697 assert_eq!(found.topic, "/moqros/robots/r1/camera");
698 assert_eq!(found.max_height, Some(480));
699 assert_eq!(found.encoding, moqros::PixelFormat::Mono8);
700 assert_eq!(
701 (found.frame_id.as_deref(), found.stamp),
702 (Some("camera"), StampSource::Receive)
703 );
704 for bad in [
706 "robots/r-1/camera",
707 "robots/1/camera",
708 "robots//camera",
709 "robots/a b",
710 "robots/__x",
711 "robots/{x}",
712 ] {
713 assert!(discovery.broadcast(bad).is_err(), "{bad}");
714 }
715
716 assert!(
717 Discovery::new(
718 vec![],
719 "/remote".into(),
720 None,
721 StampSource::Source,
722 None,
723 moqros::PixelFormat::Rgb8
724 )
725 .is_err()
726 );
727 assert!(
728 Discovery::new(
729 vec![],
730 "remote/{name}".into(),
731 None,
732 StampSource::Source,
733 None,
734 moqros::PixelFormat::Rgb8
735 )
736 .is_err()
737 );
738 let custom = Discovery::new(
739 vec![],
740 "/remote/{name}/image".into(),
741 None,
742 StampSource::Source,
743 None,
744 moqros::PixelFormat::Rgb8,
745 )
746 .unwrap();
747 assert_eq!(custom.broadcast("r1").unwrap().topic, "/remote/r1/image");
748 }
749
750 #[test]
751 fn refuses_to_discover_its_own_broadcasts() {
752 let topics = pub_topics(
753 vec!["/robots/me/camera".parse().unwrap()],
754 Format::Video,
755 moqros::DataMode::Snapshot,
756 None,
757 None,
758 )
759 .unwrap();
760 let discovery = |prefix: &str| {
761 Discovery::new(
762 vec![prefix.into()],
763 DEFAULT_TOPIC_TEMPLATE.into(),
764 None,
765 StampSource::Source,
766 None,
767 moqros::PixelFormat::Rgb8,
768 )
769 .unwrap()
770 };
771 assert!(check_no_loop(&topics, &[], Some(&discovery("robots"))).is_err());
772 assert!(check_no_loop(&topics, &[], Some(&discovery("drones"))).is_ok());
773 }
774}