Skip to main content

moqros_cli/
spec.rs

1//! `--topic` and `--broadcast` specs, resolved against the global flags.
2
3use 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
12/// `NAME[,key=value...]` split into the name and its options.
13type Spec<'a> = (&'a str, Vec<(&'a str, &'a str)>);
14
15/// Split `NAME[,key=value...]` into the name and its options, rejecting keys not in `keys`.
16fn 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/// A topic for `moqros-pub`: `TOPIC[,format=…][,type=…][,mode=…][,broadcast=…]`.
40///
41/// Options left out fall back to the matching command-line flags.
42#[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/// What `moqros-pub` does with one topic.
76#[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	/// Looked up in the ROS graph when `None` (unless the format is video).
83	pub ros_type: Option<String>,
84}
85
86/// Fill in each topic's options from the flags: `format` and `mode` apply to every topic,
87/// while `ros_type` and `broadcast` name one topic, so they need exactly one.
88pub 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/// A broadcast for `moqros-sub`: `BROADCAST[,topic=…][,type=…][,frame_id=…]`.
133///
134/// Options left out fall back to the matching command-line flags.
135#[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/// What `moqros-sub` does with one broadcast.
167#[derive(Debug, Clone, PartialEq, Eq)]
168pub struct SubBroadcast {
169	pub broadcast: String,
170	pub topic: String,
171	/// Taken from the catalog when `None`.
172	pub ros_type: Option<String>,
173	pub frame_id: String,
174}
175
176/// Topic of a lone broadcast that neither `topic=` nor `--topic` names.
177pub const DEFAULT_SUB_TOPIC: &str = "/moqros/image";
178
179/// Fill in each broadcast's options from the flags. `topic` and `ros_type` name one
180/// broadcast, so they need exactly one. Without either `topic=` or `--topic`, a lone
181/// broadcast goes to [`DEFAULT_SUB_TOPIC`], and each of several to `/moqros/<broadcast>`.
182pub 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
217/// Refuse a bridge that would feed its own output back in: a broadcast or a ROS topic that
218/// both directions use. Every message would go round the loop forever.
219pub 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		// Flags that name one topic.
318		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		// The same broadcast both ways, however it's spelled.
380		assert!(check_no_loop(&odom, &broadcasts(&["/robot/odom/,topic=/other"])).is_err());
381		// The same topic both ways.
382		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}