Skip to main content

rust_robotics_slam/
carmen.rs

1//! CARMEN log reader for 2D laser datasets (e.g. the Intel Research Lab and
2//! Freiburg logs of the Kümmerle et al. SLAM benchmark).
3//!
4//! Supported lines:
5//!
6//! ```text
7//! FLASER n r1 … rn x y theta odom_x odom_y odom_theta ipc_timestamp ipc_hostname logger_timestamp
8//! ROBOTLASER1 type start_angle fov resolution max_range accuracy remission_mode
9//!             n r1 … rn m e1 … em laser_x laser_y laser_theta robot_x robot_y robot_theta
10//!             tv rv forward_safety side_safety turn_axis ipc_timestamp ipc_hostname logger_timestamp
11//! ```
12//!
13//! `FLASER` carries no beam geometry; the CARMEN convention for front lasers
14//! is used: `angle_min = -π/2`, `angle_increment = π / n`. Other message types
15//! (`ODOM`, `PARAM`, comments, …) are skipped.
16
17use nalgebra::Vector2;
18use rust_robotics_core::Pose2D;
19
20/// One laser scan from a CARMEN log.
21#[derive(Debug, Clone, PartialEq)]
22pub struct CarmenScan {
23    /// IPC timestamp \[s\] (the time the relations of the SLAM benchmark use).
24    pub timestamp: f64,
25    /// Bearing of the first beam in the laser frame \[rad\].
26    pub angle_min: f64,
27    /// Angle between consecutive beams \[rad\].
28    pub angle_increment: f64,
29    /// Maximum range of the sensor \[m\] (`f64::INFINITY` when unknown).
30    pub max_range: f64,
31    /// Measured ranges \[m\], in beam order.
32    pub ranges: Vec<f64>,
33    /// Laser pose from odometry (the pose of the scan in the odometry frame).
34    pub laser_odometry: Pose2D,
35}
36
37impl CarmenScan {
38    /// Beam-ordered points in the laser frame, keeping ranges in
39    /// `(min_range, max_usable_range)` that are also below the sensor maximum.
40    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/// Error raised for a malformed laser line.
55#[derive(Debug, Clone, PartialEq, Eq)]
56pub struct CarmenParseError {
57    /// 1-based line number.
58    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
122/// Parses every `FLASER` and `ROBOTLASER1` line of a CARMEN log.
123pub 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
187/// Formats a scan as a CARMEN `FLASER` line (robot odometry = laser pose).
188///
189/// The scan must follow the `FLASER` beam convention (`-π/2`, `π / n`).
190pub 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        // The 60 m reading exceeds the 50 m sensor maximum.
240        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}