1use std::collections::HashSet;
4use std::str::FromStr;
5
6use anyhow::{bail, ensure};
7use clap::ValueEnum;
8use moqros::ros::ROS_IMAGE_TYPE;
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.as_deref().filter(|t| *t != ROS_IMAGE_TYPE)
116 {
117 bail!(
118 "{}: video needs {ROS_IMAGE_TYPE} messages, not {ros_type}; use format=json or cdr",
119 topic.topic
120 );
121 }
122 ensure!(
123 broadcasts.insert(topic.broadcast.clone()),
124 "several topics publish the broadcast {:?}",
125 topic.broadcast
126 );
127 Ok(topic)
128 })
129 .collect()
130}
131
132#[derive(Debug, Clone, PartialEq, Eq)]
136pub struct BroadcastSpec {
137 pub broadcast: String,
138 pub topic: Option<String>,
139 pub ros_type: Option<String>,
140 pub frame_id: Option<String>,
141}
142
143impl FromStr for BroadcastSpec {
144 type Err = String;
145
146 fn from_str(spec: &str) -> Result<Self, String> {
147 let (broadcast, options) = split_spec(spec, &["topic", "type", "frame_id"])?;
148 let mut parsed = Self {
149 broadcast: broadcast.to_string(),
150 topic: None,
151 ros_type: None,
152 frame_id: None,
153 };
154 for (key, value) in options {
155 let value = Some(value.to_string());
156 match key {
157 "topic" => parsed.topic = value,
158 "type" => parsed.ros_type = value,
159 _ => parsed.frame_id = value,
160 }
161 }
162 Ok(parsed)
163 }
164}
165
166#[derive(Debug, Clone, PartialEq, Eq)]
168pub struct SubBroadcast {
169 pub broadcast: String,
170 pub topic: String,
171 pub ros_type: Option<String>,
173 pub frame_id: String,
174}
175
176pub const DEFAULT_SUB_TOPIC: &str = "/moqros/image";
178
179pub fn sub_broadcasts(
183 specs: Vec<BroadcastSpec>,
184 topic: Option<String>,
185 ros_type: Option<String>,
186 frame_id: &str,
187) -> anyhow::Result<Vec<SubBroadcast>> {
188 ensure!(!specs.is_empty(), "no broadcast to subscribe to");
189 let single = specs.len() == 1;
190 ensure!(
191 single || (topic.is_none() && ros_type.is_none()),
192 "--topic and --type need a single --broadcast; give several broadcasts `topic=` and `type=` options instead"
193 );
194 let mut topics = HashSet::new();
195 specs
196 .into_iter()
197 .map(|spec| {
198 let broadcast = SubBroadcast {
199 topic: spec.topic.or(topic.clone()).unwrap_or_else(|| match single {
200 true => DEFAULT_SUB_TOPIC.to_string(),
201 false => format!("/moqros/{}", spec.broadcast.trim_matches('/')),
202 }),
203 ros_type: spec.ros_type.or(ros_type.clone()),
204 frame_id: spec.frame_id.unwrap_or_else(|| frame_id.to_string()),
205 broadcast: spec.broadcast,
206 };
207 ensure!(
208 topics.insert(broadcast.topic.clone()),
209 "several broadcasts publish on {}",
210 broadcast.topic
211 );
212 Ok(broadcast)
213 })
214 .collect()
215}
216
217pub fn check_no_loop(topics: &[PubTopic], broadcasts: &[SubBroadcast]) -> anyhow::Result<()> {
220 let name = |name: &str| name.trim_matches('/').to_string();
221 for topic in topics {
222 for broadcast in broadcasts {
223 ensure!(
224 name(&topic.broadcast) != name(&broadcast.broadcast),
225 "the broadcast {:?} is both published (from {}) and subscribed to; that would loop",
226 topic.broadcast,
227 topic.topic
228 );
229 ensure!(
230 name(&topic.topic) != name(&broadcast.topic),
231 "the topic {} is both published to the relay and republished from {:?}; that would loop",
232 topic.topic,
233 broadcast.broadcast
234 );
235 }
236 }
237 Ok(())
238}
239
240#[cfg(test)]
241mod tests {
242 use super::*;
243
244 #[test]
245 fn parses_topic_specs() {
246 let spec: TopicSpec = "/scan, format=cdr ,type=sensor_msgs/msg/LaserScan,mode=stream,broadcast=lidar"
247 .parse()
248 .unwrap();
249 assert_eq!(
250 spec,
251 TopicSpec {
252 topic: "/scan".into(),
253 format: Some(Format::Cdr),
254 ros_type: Some("sensor_msgs/msg/LaserScan".into()),
255 mode: Some(moqros::DataMode::Stream),
256 broadcast: Some("lidar".into()),
257 }
258 );
259 assert_eq!("/image_raw".parse::<TopicSpec>().unwrap().format, None);
260 for bad in [
261 "",
262 ",format=json",
263 "/a,json",
264 "/a,codec=vp8",
265 "/a,format=xml",
266 "/a,mode=x",
267 "/a,type=x,type=y",
268 ] {
269 assert!(bad.parse::<TopicSpec>().is_err(), "{bad:?}");
270 }
271
272 let spec: BroadcastSpec = "camera/image_raw,topic=/out,frame_id=cam,type=sensor_msgs/msg/Image"
273 .parse()
274 .unwrap();
275 assert_eq!(
276 (spec.broadcast.as_str(), spec.topic.as_deref(), spec.frame_id.as_deref()),
277 ("camera/image_raw", Some("/out"), Some("cam"))
278 );
279 assert!("chatter,format=json".parse::<BroadcastSpec>().is_err());
280 }
281
282 #[test]
283 fn resolves_pub_topics() {
284 let specs = |specs: &[&str]| {
285 specs
286 .iter()
287 .map(|s| s.parse::<TopicSpec>().unwrap())
288 .collect::<Vec<_>>()
289 };
290 let snapshot = moqros::DataMode::Snapshot;
291
292 let topics = pub_topics(
293 specs(&[
294 "/camera/image_raw",
295 "/chatter,format=json",
296 "/log,mode=stream,broadcast=logs",
297 ]),
298 Format::Video,
299 snapshot,
300 None,
301 None,
302 )
303 .unwrap();
304 let summary: Vec<_> = topics
305 .iter()
306 .map(|t| (t.broadcast.as_str(), t.format, t.mode))
307 .collect();
308 assert_eq!(
309 summary,
310 [
311 ("camera/image_raw", Format::Video, snapshot),
312 ("chatter", Format::Json, snapshot),
313 ("logs", Format::Video, moqros::DataMode::Stream),
314 ]
315 );
316
317 let one = pub_topics(
319 specs(&["/a"]),
320 Format::Cdr,
321 snapshot,
322 Some("t/msg/T".into()),
323 Some("b".into()),
324 )
325 .unwrap();
326 assert_eq!(
327 (one[0].broadcast.as_str(), one[0].ros_type.as_deref()),
328 ("b", Some("t/msg/T"))
329 );
330 assert!(
331 pub_topics(
332 specs(&["/a", "/b"]),
333 Format::Json,
334 snapshot,
335 Some("t/msg/T".into()),
336 None
337 )
338 .is_err()
339 );
340 assert!(pub_topics(specs(&["/a", "/b"]), Format::Json, snapshot, None, Some("b".into())).is_err());
341
342 assert!(pub_topics(specs(&["/a", "a,format=json"]), Format::Video, snapshot, None, None).is_err());
343 assert!(
344 pub_topics(
345 specs(&["/a,type=std_msgs/msg/String"]),
346 Format::Video,
347 snapshot,
348 None,
349 None
350 )
351 .is_err()
352 );
353 assert!(
354 pub_topics(
355 specs(&["/a,type=sensor_msgs/msg/Image"]),
356 Format::Video,
357 snapshot,
358 None,
359 None
360 )
361 .is_ok()
362 );
363 assert!(pub_topics(Vec::new(), Format::Video, snapshot, None, None).is_err());
364 }
365
366 #[test]
367 fn refuses_loops() {
368 let topics = |specs: &[&str]| {
369 let specs = specs.iter().map(|s| s.parse::<TopicSpec>().unwrap()).collect();
370 pub_topics(specs, Format::Json, moqros::DataMode::default(), None, None).unwrap()
371 };
372 let broadcasts = |specs: &[&str]| {
373 let specs = specs.iter().map(|s| s.parse::<BroadcastSpec>().unwrap()).collect();
374 sub_broadcasts(specs, None, None, "camera").unwrap()
375 };
376
377 let odom = topics(&["/odom,broadcast=robot/odom"]);
378 assert!(check_no_loop(&odom, &broadcasts(&["robot/cmd_vel,topic=/cmd_vel"])).is_ok());
379 assert!(check_no_loop(&odom, &broadcasts(&["/robot/odom/,topic=/other"])).is_err());
381 assert!(check_no_loop(&odom, &broadcasts(&["robot/cmd_vel,topic=/odom"])).is_err());
383 assert!(check_no_loop(&odom, &[]).is_ok());
384 }
385
386 #[test]
387 fn resolves_sub_broadcasts() {
388 let specs = |specs: &[&str]| {
389 specs
390 .iter()
391 .map(|s| s.parse::<BroadcastSpec>().unwrap())
392 .collect::<Vec<_>>()
393 };
394 let topics = |broadcasts: Vec<SubBroadcast>| broadcasts.into_iter().map(|b| b.topic).collect::<Vec<_>>();
395
396 assert_eq!(
397 topics(sub_broadcasts(specs(&["image_raw"]), None, None, "camera").unwrap()),
398 [DEFAULT_SUB_TOPIC]
399 );
400 assert_eq!(
401 topics(sub_broadcasts(specs(&["image_raw"]), Some("/x".into()), None, "camera").unwrap()),
402 ["/x"]
403 );
404 let broadcasts = sub_broadcasts(
405 specs(&[
406 "sample/image_raw,topic=/moqros/image",
407 "chatter",
408 "/front/imu/,frame_id=imu",
409 ]),
410 None,
411 None,
412 "camera",
413 )
414 .unwrap();
415 assert_eq!(broadcasts[2].frame_id, "imu");
416 assert_eq!(
417 topics(broadcasts),
418 ["/moqros/image", "/moqros/chatter", "/moqros/front/imu"]
419 );
420
421 assert!(sub_broadcasts(specs(&["a", "b"]), Some("/x".into()), None, "camera").is_err());
422 assert!(sub_broadcasts(specs(&["a", "b"]), None, Some("t/msg/T".into()), "camera").is_err());
423 assert!(sub_broadcasts(specs(&["a,topic=/x", "b,topic=/x"]), None, None, "camera").is_err());
424 }
425}