1use std::collections::HashSet;
4use std::str::FromStr;
5
6use anyhow::{bail, ensure};
7use clap::ValueEnum;
8use moqros::ros::{QosProfile, ROS_COMPRESSED_IMAGE_TYPE, ROS_IMAGE_TYPE, RosTarget, 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.ros_type
116 {
117 check_video_type(&topic.topic, ros_type)?;
118 }
119 ensure!(
120 broadcasts.insert(topic.broadcast.clone()),
121 "several topics publish the broadcast {:?}",
122 topic.broadcast
123 );
124 Ok(topic)
125 })
126 .collect()
127}
128
129pub(crate) fn check_video_type(topic: &str, ros_type: &str) -> anyhow::Result<()> {
131 ensure!(
132 [ROS_IMAGE_TYPE, ROS_COMPRESSED_IMAGE_TYPE].contains(&ros_type),
133 "{topic}: video needs {ROS_IMAGE_TYPE} or {ROS_COMPRESSED_IMAGE_TYPE} messages, not {ros_type}; use format=json or cdr"
134 );
135 Ok(())
136}
137
138#[derive(Debug, Clone, PartialEq, Eq)]
142pub struct BroadcastSpec {
143 pub broadcast: String,
144 pub topic: Option<String>,
145 pub ros_type: Option<String>,
146 pub frame_id: Option<String>,
147 pub stamp: Option<StampSource>,
148 pub max_height: Option<u32>,
149 pub encoding: Option<moqros::PixelFormat>,
150}
151
152impl FromStr for BroadcastSpec {
153 type Err = String;
154
155 fn from_str(spec: &str) -> Result<Self, String> {
156 let (broadcast, options) = split_spec(spec, &["topic", "type", "frame_id", "stamp", "max_height", "encoding"])?;
157 let mut parsed = Self {
158 broadcast: broadcast.to_string(),
159 topic: None,
160 ros_type: None,
161 frame_id: None,
162 stamp: None,
163 max_height: None,
164 encoding: None,
165 };
166 for (key, value) in options {
167 match key {
168 "stamp" => parsed.stamp = Some(value.parse().map_err(|err| format!("{err}"))?),
169 "max_height" => {
170 parsed.max_height = Some(
171 value
172 .parse()
173 .map_err(|_| format!("max_height must be a number, not {value:?}"))?,
174 );
175 }
176 "encoding" => parsed.encoding = Some(parse_encoding(value)?),
177 "topic" => parsed.topic = Some(value.to_string()),
178 "type" => parsed.ros_type = Some(value.to_string()),
179 _ => parsed.frame_id = Some(value.to_string()),
180 }
181 }
182 Ok(parsed)
183 }
184}
185
186pub fn parse_encoding(value: &str) -> Result<moqros::PixelFormat, String> {
189 use moqros::PixelFormat::{Bgr8, Bgra8, Mono8, Rgb8, Rgba8};
190 match value.parse::<moqros::PixelFormat>() {
191 Ok(format @ (Rgb8 | Bgr8 | Rgba8 | Bgra8 | Mono8)) => Ok(format),
192 _ => Err(format!(
193 "unsupported encoding {value:?} (expected one of: rgb8, bgr8, rgba8, bgra8, mono8)"
194 )),
195 }
196}
197
198#[derive(Debug, Clone, PartialEq, Eq)]
200pub struct RepublishOptions {
201 pub frame_id: Option<String>,
203 pub stamp: StampSource,
204 pub max_height: Option<u32>,
206 pub encoding: moqros::PixelFormat,
208}
209
210impl Default for RepublishOptions {
211 fn default() -> Self {
212 Self {
213 frame_id: None,
214 stamp: StampSource::default(),
215 max_height: None,
216 encoding: moqros::PixelFormat::Rgb8,
217 }
218 }
219}
220
221impl RepublishOptions {
222 fn overridden_by(&self, spec: &BroadcastSpec) -> Self {
224 Self {
225 frame_id: spec.frame_id.clone().or_else(|| self.frame_id.clone()),
226 stamp: spec.stamp.unwrap_or(self.stamp),
227 max_height: spec.max_height.or(self.max_height),
228 encoding: spec.encoding.unwrap_or(self.encoding),
229 }
230 }
231}
232
233#[derive(Debug, Clone, PartialEq, Eq)]
235pub struct SubBroadcast {
236 pub broadcast: String,
237 pub topic: String,
238 pub ros_type: Option<String>,
240 pub options: RepublishOptions,
241}
242
243impl SubBroadcast {
244 pub fn subscribe_options(&self) -> moqros::SubscribeOptions {
246 let mut options = moqros::SubscribeOptions::default();
247 options.max_height = self.options.max_height;
248 options
249 }
250
251 pub fn target(&self, qos: QosProfile) -> RosTarget {
253 let mut target = RosTarget::new(self.topic.clone(), qos)
254 .with_stamp(self.options.stamp)
255 .with_encoding(self.options.encoding);
256 target.frame_id = self.options.frame_id.clone();
257 target.ros_type = self.ros_type.clone();
258 target
259 }
260}
261
262pub const DEFAULT_SUB_TOPIC: &str = "/moqros/image";
264
265pub fn sub_broadcasts(
269 specs: Vec<BroadcastSpec>,
270 topic: Option<String>,
271 ros_type: Option<String>,
272 options: &RepublishOptions,
273) -> anyhow::Result<Vec<SubBroadcast>> {
274 ensure!(!specs.is_empty(), "no broadcast to subscribe to");
275 let single = specs.len() == 1;
276 ensure!(
277 single || (topic.is_none() && ros_type.is_none()),
278 "--topic and --type need a single --broadcast; give several broadcasts `topic=` and `type=` options instead"
279 );
280 let mut topics = HashSet::new();
281 specs
282 .into_iter()
283 .map(|spec| {
284 let options = options.overridden_by(&spec);
285 let broadcast = SubBroadcast {
286 topic: spec.topic.or(topic.clone()).unwrap_or_else(|| match single {
287 true => DEFAULT_SUB_TOPIC.to_string(),
288 false => format!("/moqros/{}", moqros::normalize_broadcast_name(&spec.broadcast)),
289 }),
290 options,
291 ros_type: spec.ros_type.or(ros_type.clone()),
292 broadcast: spec.broadcast,
293 };
294 ensure!(
295 topics.insert(broadcast.topic.clone()),
296 "several broadcasts publish on {}",
297 broadcast.topic
298 );
299 Ok(broadcast)
300 })
301 .collect()
302}
303
304#[derive(Debug, Clone, PartialEq, Eq)]
306pub struct Discovery {
307 pub prefixes: Vec<String>,
309 pub topic_template: String,
311 pub options: RepublishOptions,
312}
313
314pub const DEFAULT_TOPIC_TEMPLATE: &str = "/moqros/{name}";
316
317impl Discovery {
318 pub fn new(prefixes: Vec<String>, topic_template: String, options: RepublishOptions) -> anyhow::Result<Self> {
320 ensure!(
321 topic_template.contains("{name}"),
322 "--topic-template {topic_template:?} has no {{name}}"
323 );
324 let discovery = Self {
325 prefixes: prefixes
326 .iter()
327 .map(|p| moqros::normalize_broadcast_name(p).to_string())
328 .collect(),
329 topic_template,
330 options,
331 };
332 if let Err(err) = discovery.broadcast("name") {
333 bail!("--topic-template {:?}: {err}", discovery.topic_template);
334 }
335 Ok(discovery)
336 }
337
338 pub fn covers(&self, name: &str) -> bool {
340 self.prefixes
341 .iter()
342 .any(|prefix| moqros::broadcast_is_under(name, prefix))
343 }
344
345 pub fn broadcast(&self, name: &str) -> Result<SubBroadcast, String> {
348 let topic = self
349 .topic_template
350 .replace("{name}", moqros::normalize_broadcast_name(name));
351 if !is_ros_topic(&topic) {
352 return Err(format!(
353 "{topic:?} isn't a topic name moqros republishes on (letters, digits, `_` and `/` only)"
354 ));
355 }
356 Ok(SubBroadcast {
357 broadcast: name.to_string(),
358 topic,
359 ros_type: None,
360 options: self.options.clone(),
361 })
362 }
363}
364
365fn is_ros_topic(topic: &str) -> bool {
368 let Some(tokens) = topic.strip_prefix('/') else {
369 return false;
370 };
371 let is_token = |token: &str| {
372 let mut chars = token.chars();
373 chars.next().is_some_and(|c| c.is_ascii_alphabetic() || c == '_')
374 && chars.all(|c| c.is_ascii_alphanumeric() || c == '_')
375 && !token.contains("__")
376 };
377 !tokens.is_empty() && tokens.split('/').all(is_token)
378}
379
380pub fn check_no_loop(
383 topics: &[PubTopic],
384 broadcasts: &[SubBroadcast],
385 discovery: Option<&Discovery>,
386) -> anyhow::Result<()> {
387 let name = moqros::normalize_broadcast_name;
388 for topic in topics {
389 if let Some(discovery) = discovery {
390 ensure!(
391 !discovery.covers(&topic.broadcast),
392 "the broadcast {:?} is published (from {}) and under a --broadcast-prefix; that would loop",
393 topic.broadcast,
394 topic.topic
395 );
396 }
397 for broadcast in broadcasts {
398 ensure!(
399 name(&topic.broadcast) != name(&broadcast.broadcast),
400 "the broadcast {:?} is both published (from {}) and subscribed to; that would loop",
401 topic.broadcast,
402 topic.topic
403 );
404 ensure!(
405 name(&topic.topic) != name(&broadcast.topic),
406 "the topic {} is both published to the relay and republished from {:?}; that would loop",
407 topic.topic,
408 broadcast.broadcast
409 );
410 }
411 }
412 Ok(())
413}
414
415#[cfg(test)]
416mod tests {
417 use super::*;
418
419 #[test]
420 fn parses_topic_specs() {
421 let spec: TopicSpec = "/scan, format=cdr ,type=sensor_msgs/msg/LaserScan,mode=stream,broadcast=lidar"
422 .parse()
423 .unwrap();
424 assert_eq!(
425 spec,
426 TopicSpec {
427 topic: "/scan".into(),
428 format: Some(Format::Cdr),
429 ros_type: Some("sensor_msgs/msg/LaserScan".into()),
430 mode: Some(moqros::DataMode::Stream),
431 broadcast: Some("lidar".into()),
432 }
433 );
434 assert_eq!("/image_raw".parse::<TopicSpec>().unwrap().format, None);
435 for bad in [
436 "",
437 ",format=json",
438 "/a,json",
439 "/a,codec=vp8",
440 "/a,format=xml",
441 "/a,mode=x",
442 "/a,type=x,type=y",
443 ] {
444 assert!(bad.parse::<TopicSpec>().is_err(), "{bad:?}");
445 }
446
447 let spec: BroadcastSpec = "camera/image_raw,topic=/out,frame_id=cam,type=sensor_msgs/msg/Image,stamp=receive"
448 .parse()
449 .unwrap();
450 assert_eq!(
451 (
452 spec.broadcast.as_str(),
453 spec.topic.as_deref(),
454 spec.frame_id.as_deref(),
455 spec.stamp
456 ),
457 (
458 "camera/image_raw",
459 Some("/out"),
460 Some("cam"),
461 Some(StampSource::Receive)
462 )
463 );
464 assert!("camera,stamp=now".parse::<BroadcastSpec>().is_err());
465 assert!("chatter,format=json".parse::<BroadcastSpec>().is_err());
466 assert_eq!(
467 "cam,max_height=360".parse::<BroadcastSpec>().unwrap().max_height,
468 Some(360)
469 );
470 assert!("cam,max_height=tall".parse::<BroadcastSpec>().is_err());
471 assert_eq!(
472 "cam,encoding=mono8".parse::<BroadcastSpec>().unwrap().encoding,
473 Some(moqros::PixelFormat::Mono8)
474 );
475 assert!("cam,encoding=yuv422".parse::<BroadcastSpec>().is_err());
476 assert!("cam,encoding=16UC1".parse::<BroadcastSpec>().is_err());
477 }
478
479 #[test]
480 fn resolves_pub_topics() {
481 let specs = |specs: &[&str]| {
482 specs
483 .iter()
484 .map(|s| s.parse::<TopicSpec>().unwrap())
485 .collect::<Vec<_>>()
486 };
487 let snapshot = moqros::DataMode::Snapshot;
488
489 let topics = pub_topics(
490 specs(&[
491 "/camera/image_raw",
492 "/chatter,format=json",
493 "/log,mode=stream,broadcast=logs",
494 ]),
495 Format::Video,
496 snapshot,
497 None,
498 None,
499 )
500 .unwrap();
501 let summary: Vec<_> = topics
502 .iter()
503 .map(|t| (t.broadcast.as_str(), t.format, t.mode))
504 .collect();
505 assert_eq!(
506 summary,
507 [
508 ("camera/image_raw", Format::Video, snapshot),
509 ("chatter", Format::Json, snapshot),
510 ("logs", Format::Video, moqros::DataMode::Stream),
511 ]
512 );
513
514 let one = pub_topics(
516 specs(&["/a"]),
517 Format::Cdr,
518 snapshot,
519 Some("t/msg/T".into()),
520 Some("b".into()),
521 )
522 .unwrap();
523 assert_eq!(
524 (one[0].broadcast.as_str(), one[0].ros_type.as_deref()),
525 ("b", Some("t/msg/T"))
526 );
527 assert!(
528 pub_topics(
529 specs(&["/a", "/b"]),
530 Format::Json,
531 snapshot,
532 Some("t/msg/T".into()),
533 None
534 )
535 .is_err()
536 );
537 assert!(pub_topics(specs(&["/a", "/b"]), Format::Json, snapshot, None, Some("b".into())).is_err());
538
539 assert!(pub_topics(specs(&["/a", "a,format=json"]), Format::Video, snapshot, None, None).is_err());
540 assert!(
541 pub_topics(
542 specs(&["/a,type=std_msgs/msg/String"]),
543 Format::Video,
544 snapshot,
545 None,
546 None
547 )
548 .is_err()
549 );
550 assert!(
551 pub_topics(
552 specs(&["/a,type=sensor_msgs/msg/Image"]),
553 Format::Video,
554 snapshot,
555 None,
556 None
557 )
558 .is_ok()
559 );
560 assert!(pub_topics(Vec::new(), Format::Video, snapshot, None, None).is_err());
561 }
562
563 #[test]
564 fn refuses_loops() {
565 let topics = |specs: &[&str]| {
566 let specs = specs.iter().map(|s| s.parse::<TopicSpec>().unwrap()).collect();
567 pub_topics(specs, Format::Json, moqros::DataMode::default(), None, None).unwrap()
568 };
569 let broadcasts = |specs: &[&str]| {
570 let specs = specs.iter().map(|s| s.parse::<BroadcastSpec>().unwrap()).collect();
571 sub_broadcasts(specs, None, None, &RepublishOptions::default()).unwrap()
572 };
573
574 let odom = topics(&["/odom,broadcast=robot/odom"]);
575 assert!(check_no_loop(&odom, &broadcasts(&["robot/cmd_vel,topic=/cmd_vel"]), None).is_ok());
576 assert!(check_no_loop(&odom, &broadcasts(&["/robot/odom/,topic=/other"]), None).is_err());
578 assert!(check_no_loop(&odom, &broadcasts(&["robot/cmd_vel,topic=/odom"]), None).is_err());
580 assert!(check_no_loop(&odom, &[], None).is_ok());
581 }
582
583 #[test]
584 fn resolves_sub_broadcasts() {
585 let specs = |specs: &[&str]| {
586 specs
587 .iter()
588 .map(|s| s.parse::<BroadcastSpec>().unwrap())
589 .collect::<Vec<_>>()
590 };
591 let topics = |broadcasts: Vec<SubBroadcast>| broadcasts.into_iter().map(|b| b.topic).collect::<Vec<_>>();
592
593 assert_eq!(
594 topics(sub_broadcasts(specs(&["image_raw"]), None, None, &RepublishOptions::default()).unwrap()),
595 [DEFAULT_SUB_TOPIC]
596 );
597 assert_eq!(
598 topics(
599 sub_broadcasts(
600 specs(&["image_raw"]),
601 Some("/x".into()),
602 None,
603 &RepublishOptions::default()
604 )
605 .unwrap()
606 ),
607 ["/x"]
608 );
609 let broadcasts = sub_broadcasts(
610 specs(&[
611 "sample/image_raw,topic=/moqros/image",
612 "chatter",
613 "/front/imu/,frame_id=imu,stamp=receive",
614 ]),
615 None,
616 None,
617 &RepublishOptions {
618 frame_id: Some("camera".into()),
619 stamp: StampSource::Source,
620 max_height: Some(480),
621 encoding: moqros::PixelFormat::Bgr8,
622 },
623 )
624 .unwrap();
625 assert_eq!(broadcasts[0].options.frame_id.as_deref(), Some("camera"));
626 assert_eq!(broadcasts[0].options.stamp, StampSource::Source);
627 assert_eq!(broadcasts[2].options.frame_id.as_deref(), Some("imu"));
628 assert_eq!(broadcasts[2].options.stamp, StampSource::Receive);
629 assert_eq!(broadcasts[2].options.max_height, Some(480));
630 assert_eq!(broadcasts[2].options.encoding, moqros::PixelFormat::Bgr8);
631 assert_eq!(
632 sub_broadcasts(specs(&["image_raw"]), None, None, &RepublishOptions::default()).unwrap()[0]
633 .options
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 &RepublishOptions::default()
648 )
649 .is_err()
650 );
651 assert!(
652 sub_broadcasts(
653 specs(&["a", "b"]),
654 None,
655 Some("t/msg/T".into()),
656 &RepublishOptions::default()
657 )
658 .is_err()
659 );
660 assert!(
661 sub_broadcasts(
662 specs(&["a,topic=/x", "b,topic=/x"]),
663 None,
664 None,
665 &RepublishOptions::default()
666 )
667 .is_err()
668 );
669 }
670
671 #[test]
672 fn discovers_broadcasts_under_prefixes() {
673 let discovery = Discovery::new(
674 vec!["robots/".into(), "/drones".into()],
675 DEFAULT_TOPIC_TEMPLATE.into(),
676 RepublishOptions {
677 frame_id: Some("camera".into()),
678 stamp: StampSource::Receive,
679 max_height: Some(480),
680 encoding: moqros::PixelFormat::Mono8,
681 },
682 )
683 .unwrap();
684 assert!(discovery.covers("robots/r1/camera"));
685 assert!(discovery.covers("drones"));
686 assert!(!discovery.covers("robots2/camera"));
687 assert!(!discovery.covers("cars/robots"));
688
689 let found = discovery.broadcast("robots/r1/camera").unwrap();
690 assert_eq!(found.topic, "/moqros/robots/r1/camera");
691 assert_eq!(found.options.max_height, Some(480));
692 assert_eq!(found.options.encoding, moqros::PixelFormat::Mono8);
693 assert_eq!(
694 (found.options.frame_id.as_deref(), found.options.stamp),
695 (Some("camera"), StampSource::Receive)
696 );
697 for bad in [
699 "robots/r-1/camera",
700 "robots/1/camera",
701 "robots//camera",
702 "robots/a b",
703 "robots/__x",
704 "robots/{x}",
705 ] {
706 assert!(discovery.broadcast(bad).is_err(), "{bad}");
707 }
708
709 assert!(Discovery::new(vec![], "/remote".into(), RepublishOptions::default()).is_err());
710 assert!(Discovery::new(vec![], "remote/{name}".into(), RepublishOptions::default()).is_err());
711 let custom = Discovery::new(vec![], "/remote/{name}/image".into(), RepublishOptions::default()).unwrap();
712 assert_eq!(custom.broadcast("r1").unwrap().topic, "/remote/r1/image");
713 }
714
715 #[test]
716 fn refuses_to_discover_its_own_broadcasts() {
717 let topics = pub_topics(
718 vec!["/robots/me/camera".parse().unwrap()],
719 Format::Video,
720 moqros::DataMode::Snapshot,
721 None,
722 None,
723 )
724 .unwrap();
725 let discovery = |prefix: &str| {
726 Discovery::new(
727 vec![prefix.into()],
728 DEFAULT_TOPIC_TEMPLATE.into(),
729 RepublishOptions::default(),
730 )
731 .unwrap()
732 };
733 assert!(check_no_loop(&topics, &[], Some(&discovery("robots"))).is_err());
734 assert!(check_no_loop(&topics, &[], Some(&discovery("drones"))).is_ok());
735 }
736}