1use nalgebra::Vector2;
18use rust_robotics_core::Pose2D;
19
20#[derive(Debug, Clone, PartialEq)]
22pub struct CarmenScan {
23 pub timestamp: f64,
25 pub angle_min: f64,
27 pub angle_increment: f64,
29 pub max_range: f64,
31 pub ranges: Vec<f64>,
33 pub laser_odometry: Pose2D,
35}
36
37impl CarmenScan {
38 pub fn points(&self, min_range: f64, max_usable_range: f64) -> Vec<Vector2<f64>> {
41 let limit = max_usable_range.min(self.max_range);
42 self.ranges
43 .iter()
44 .enumerate()
45 .filter(|(_, range)| range.is_finite() && **range > min_range && **range < limit)
46 .map(|(beam, range)| {
47 let angle = self.angle_min + self.angle_increment * beam as f64;
48 Vector2::new(range * angle.cos(), range * angle.sin())
49 })
50 .collect()
51 }
52}
53
54#[derive(Debug, Clone, PartialEq, Eq)]
56pub struct CarmenParseError {
57 pub line: usize,
59 pub message: String,
60}
61
62impl std::fmt::Display for CarmenParseError {
63 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
64 write!(f, "line {}: {}", self.line, self.message)
65 }
66}
67
68impl std::error::Error for CarmenParseError {}
69
70struct Fields<'a> {
71 tokens: Vec<&'a str>,
72 next: usize,
73 line: usize,
74}
75
76impl<'a> Fields<'a> {
77 fn error(&self, message: impl Into<String>) -> CarmenParseError {
78 CarmenParseError {
79 line: self.line,
80 message: message.into(),
81 }
82 }
83
84 fn token(&mut self, what: &str) -> Result<&'a str, CarmenParseError> {
85 let token = self
86 .tokens
87 .get(self.next)
88 .copied()
89 .ok_or_else(|| self.error(format!("missing {what}")))?;
90 self.next += 1;
91 Ok(token)
92 }
93
94 fn number(&mut self, what: &str) -> Result<f64, CarmenParseError> {
95 let token = self.token(what)?;
96 token
97 .parse::<f64>()
98 .map_err(|_| self.error(format!("invalid {what} `{token}`")))
99 }
100
101 fn count(&mut self, what: &str) -> Result<usize, CarmenParseError> {
102 let value = self.number(what)?;
103 if value < 0.0 || value.fract() != 0.0 || value > 100_000.0 {
104 return Err(self.error(format!("invalid {what} `{value}`")));
105 }
106 Ok(value as usize)
107 }
108
109 fn numbers(&mut self, count: usize, what: &str) -> Result<Vec<f64>, CarmenParseError> {
110 (0..count).map(|_| self.number(what)).collect()
111 }
112
113 fn pose(&mut self, what: &str) -> Result<Pose2D, CarmenParseError> {
114 Ok(Pose2D::new(
115 self.number(what)?,
116 self.number(what)?,
117 self.number(what)?,
118 ))
119 }
120}
121
122pub fn parse_carmen_log(text: &str) -> Result<Vec<CarmenScan>, CarmenParseError> {
124 let mut scans = Vec::new();
125 for (index, line) in text.lines().enumerate() {
126 let tokens: Vec<&str> = line.split_whitespace().collect();
127 let Some(kind) = tokens.first().copied() else {
128 continue;
129 };
130 let mut fields = Fields {
131 tokens,
132 next: 1,
133 line: index + 1,
134 };
135 let scan = match kind {
136 "FLASER" => parse_flaser(&mut fields)?,
137 "ROBOTLASER1" => parse_robotlaser(&mut fields)?,
138 _ => continue,
139 };
140 scans.push(scan);
141 }
142 Ok(scans)
143}
144
145fn parse_flaser(fields: &mut Fields<'_>) -> Result<CarmenScan, CarmenParseError> {
146 let count = fields.count("number of readings")?;
147 let ranges = fields.numbers(count, "range")?;
148 let laser_odometry = fields.pose("laser pose")?;
149 let _robot_odometry = fields.pose("odometry pose")?;
150 let timestamp = fields.number("ipc timestamp")?;
151 Ok(CarmenScan {
152 timestamp,
153 angle_min: -std::f64::consts::FRAC_PI_2,
154 angle_increment: std::f64::consts::PI / count.max(1) as f64,
155 max_range: f64::INFINITY,
156 ranges,
157 laser_odometry,
158 })
159}
160
161fn parse_robotlaser(fields: &mut Fields<'_>) -> Result<CarmenScan, CarmenParseError> {
162 let _laser_type = fields.number("laser type")?;
163 let angle_min = fields.number("start angle")?;
164 let _field_of_view = fields.number("field of view")?;
165 let angle_increment = fields.number("angular resolution")?;
166 let max_range = fields.number("maximum range")?;
167 let _accuracy = fields.number("accuracy")?;
168 let _remission_mode = fields.number("remission mode")?;
169 let count = fields.count("number of readings")?;
170 let ranges = fields.numbers(count, "range")?;
171 let remissions = fields.count("number of remissions")?;
172 fields.numbers(remissions, "remission")?;
173 let laser_odometry = fields.pose("laser pose")?;
174 let _robot_odometry = fields.pose("robot pose")?;
175 fields.numbers(5, "velocity / safety field")?;
176 let timestamp = fields.number("ipc timestamp")?;
177 Ok(CarmenScan {
178 timestamp,
179 angle_min,
180 angle_increment,
181 max_range,
182 ranges,
183 laser_odometry,
184 })
185}
186
187pub fn format_flaser(scan: &CarmenScan) -> String {
191 let pose = scan.laser_odometry;
192 let ranges: Vec<String> = scan
193 .ranges
194 .iter()
195 .map(|range| format!("{range:.3}"))
196 .collect();
197 format!(
198 "FLASER {} {} {:.6} {:.6} {:.6} {:.6} {:.6} {:.6} {:.6} sim {:.6}",
199 scan.ranges.len(),
200 ranges.join(" "),
201 pose.x,
202 pose.y,
203 pose.yaw,
204 pose.x,
205 pose.y,
206 pose.yaw,
207 scan.timestamp,
208 scan.timestamp,
209 )
210}
211
212#[cfg(test)]
213mod tests {
214 use super::*;
215
216 #[test]
217 fn parses_flaser_and_robotlaser_and_skips_other_lines() {
218 let log = "\
219# comment
220PARAM robot_front_laser_max 81.9
221ODOM 0.0 0.0 0.0 0.0 0.0 0.0 10.0 host 10.0
222FLASER 4 1.0 2.0 3.0 4.0 0.5 -0.5 0.1 0.6 -0.4 0.2 12.5 host 12.6
223ROBOTLASER1 0 -1.5708 3.1416 0.0175 50.0 0.01 0 3 1.0 60.0 2.0 0 1.0 2.0 0.3 1.1 2.1 0.31 0 0 0 0 0 13.0 host 13.1
224";
225 let scans = parse_carmen_log(log).expect("valid log");
226 assert_eq!(scans.len(), 2);
227
228 let flaser = &scans[0];
229 assert_eq!(flaser.ranges, vec![1.0, 2.0, 3.0, 4.0]);
230 assert_eq!(flaser.laser_odometry, Pose2D::new(0.5, -0.5, 0.1));
231 assert_eq!(flaser.timestamp, 12.5);
232 assert!((flaser.angle_increment - std::f64::consts::PI / 4.0).abs() < 1e-12);
233
234 let robotlaser = &scans[1];
235 assert_eq!(robotlaser.ranges, vec![1.0, 60.0, 2.0]);
236 assert_eq!(robotlaser.max_range, 50.0);
237 assert_eq!(robotlaser.laser_odometry, Pose2D::new(1.0, 2.0, 0.3));
238 assert_eq!(robotlaser.timestamp, 13.0);
239 assert_eq!(robotlaser.points(0.05, 80.0).len(), 2);
241 }
242
243 #[test]
244 fn points_follow_the_beam_geometry() {
245 let scan = CarmenScan {
246 timestamp: 0.0,
247 angle_min: -std::f64::consts::FRAC_PI_2,
248 angle_increment: std::f64::consts::FRAC_PI_2,
249 max_range: f64::INFINITY,
250 ranges: vec![1.0, 2.0, 3.0],
251 laser_odometry: Pose2D::origin(),
252 };
253 let points = scan.points(0.0, 10.0);
254 assert!((points[0] - Vector2::new(0.0, -1.0)).norm() < 1e-12);
255 assert!((points[1] - Vector2::new(2.0, 0.0)).norm() < 1e-12);
256 assert!((points[2] - Vector2::new(0.0, 3.0)).norm() < 1e-12);
257 assert_eq!(scan.points(0.0, 2.5).len(), 2);
258 }
259
260 #[test]
261 fn flaser_round_trips_through_format_and_parse() {
262 let scan = CarmenScan {
263 timestamp: 42.125,
264 angle_min: -std::f64::consts::FRAC_PI_2,
265 angle_increment: std::f64::consts::PI / 3.0,
266 max_range: f64::INFINITY,
267 ranges: vec![1.25, 2.5, 3.75],
268 laser_odometry: Pose2D::new(1.0, -2.0, 0.5),
269 };
270 let parsed = parse_carmen_log(&format_flaser(&scan)).expect("valid line");
271 assert_eq!(parsed, vec![scan]);
272 }
273
274 #[test]
275 fn reports_truncated_lines() {
276 let error = parse_carmen_log("ODOM 1 2 3\nFLASER 3 1.0 2.0\n").unwrap_err();
277 assert_eq!(error.line, 2);
278 assert!(error.message.contains("range"), "{}", error.message);
279 }
280}