Skip to main content

rust_robotics_mapping/
lidar_to_grid_map.rs

1//! LIDAR to 2D Grid Map conversion.
2//!
3//! Converts lidar scan data (angle, distance pairs) into an occupancy grid map
4//! using either Bresenham ray tracing or flood fill.
5//!
6//! Based on the work by Erno Horvath, Csaba Hajdu, and Atsushi Sakai (@Atsushi_twi).
7
8use nalgebra::DMatrix;
9use std::collections::VecDeque;
10use std::fs::File;
11use std::io::Write;
12
13/// Grid map extension area \[m\].
14pub const EXTEND_AREA: f64 = 1.0;
15
16/// Occupancy value for free cells.
17pub const FREE: f64 = 0.0;
18/// Occupancy value for unknown cells.
19pub const UNKNOWN: f64 = 0.5;
20/// Occupancy value for occupied cells.
21pub const OCCUPIED: f64 = 1.0;
22
23/// Method used to generate the occupancy grid map.
24#[derive(Debug, Clone, Copy, PartialEq, Eq)]
25pub enum GridMapMethod {
26    /// Bresenham ray tracing from sensor origin to each obstacle point.
27    Bresenham,
28    /// Flood fill from sensor origin after initial ray-based boundary marking.
29    FloodFill,
30}
31
32/// Grid map configuration computed from obstacle positions.
33#[derive(Debug, Clone)]
34struct GridMapConfig {
35    min_x: f64,
36    min_y: f64,
37    max_x: f64,
38    max_y: f64,
39    x_width: usize,
40    y_width: usize,
41}
42
43/// LIDAR-based occupancy grid map.
44///
45/// Cell values:
46/// - `0.0` = free space
47/// - `0.5` = unknown
48/// - `1.0` = occupied (obstacle)
49pub struct LidarGridMap {
50    pub data: DMatrix<f64>,
51    pub min_x: f64,
52    pub min_y: f64,
53    pub max_x: f64,
54    pub max_y: f64,
55    pub resolution: f64,
56    pub x_width: usize,
57    pub y_width: usize,
58    pub center_ix: usize,
59    pub center_iy: usize,
60}
61
62impl LidarGridMap {
63    /// Create a new LIDAR grid map from obstacle positions in Cartesian coordinates.
64    ///
65    /// # Arguments
66    /// * `ox` - obstacle x coordinates \[m\]
67    /// * `oy` - obstacle y coordinates \[m\]
68    /// * `resolution` - grid cell size \[m\]
69    /// * `method` - algorithm to use for free-space detection
70    pub fn new(ox: &[f64], oy: &[f64], resolution: f64, method: GridMapMethod) -> Self {
71        assert_eq!(ox.len(), oy.len(), "ox and oy must have the same length");
72        assert!(resolution > 0.0, "resolution must be positive");
73
74        let config = calc_grid_map_config(ox, oy, resolution);
75
76        // Sensor is at the origin (0, 0) in world coordinates.
77        let center_ix = ((-config.min_x) / resolution).round() as usize;
78        let center_iy = ((-config.min_y) / resolution).round() as usize;
79
80        let mut data = DMatrix::from_element(config.x_width, config.y_width, UNKNOWN);
81
82        match method {
83            GridMapMethod::Bresenham => {
84                generate_bresenham(&mut data, ox, oy, &config, resolution, center_ix, center_iy);
85            }
86            GridMapMethod::FloodFill => {
87                generate_flood_fill(&mut data, ox, oy, &config, resolution, center_ix, center_iy);
88            }
89        }
90
91        LidarGridMap {
92            data,
93            min_x: config.min_x,
94            min_y: config.min_y,
95            max_x: config.max_x,
96            max_y: config.max_y,
97            resolution,
98            x_width: config.x_width,
99            y_width: config.y_width,
100            center_ix,
101            center_iy,
102        }
103    }
104
105    /// Create a LIDAR grid map from angle and distance pairs.
106    ///
107    /// # Arguments
108    /// * `angles` - lidar beam angles \[rad\]
109    /// * `distances` - corresponding distances \[m\]
110    /// * `resolution` - grid cell size \[m\]
111    /// * `method` - algorithm to use
112    pub fn from_scan(
113        angles: &[f64],
114        distances: &[f64],
115        resolution: f64,
116        method: GridMapMethod,
117    ) -> Self {
118        assert_eq!(
119            angles.len(),
120            distances.len(),
121            "angles and distances must have the same length"
122        );
123        let ox: Vec<f64> = angles
124            .iter()
125            .zip(distances.iter())
126            .map(|(a, d)| a.sin() * d)
127            .collect();
128        let oy: Vec<f64> = angles
129            .iter()
130            .zip(distances.iter())
131            .map(|(a, d)| a.cos() * d)
132            .collect();
133        Self::new(&ox, &oy, resolution, method)
134    }
135
136    /// Get grid index from world coordinates.
137    pub fn get_index(&self, x: f64, y: f64) -> Option<(usize, usize)> {
138        let ix = ((x - self.min_x) / self.resolution).floor() as i32;
139        let iy = ((y - self.min_y) / self.resolution).floor() as i32;
140
141        if ix >= 0 && ix < self.x_width as i32 && iy >= 0 && iy < self.y_width as i32 {
142            Some((ix as usize, iy as usize))
143        } else {
144            None
145        }
146    }
147
148    /// Get occupancy value at world coordinates.
149    pub fn get_occupancy(&self, x: f64, y: f64) -> Option<f64> {
150        self.get_index(x, y).map(|(ix, iy)| self.data[(ix, iy)])
151    }
152}
153
154/// Compute grid map extents from obstacle positions.
155fn calc_grid_map_config(ox: &[f64], oy: &[f64], resolution: f64) -> GridMapConfig {
156    let min_x = ox.iter().cloned().fold(f64::INFINITY, f64::min).floor() - EXTEND_AREA / 2.0;
157    let min_y = oy.iter().cloned().fold(f64::INFINITY, f64::min).floor() - EXTEND_AREA / 2.0;
158    let max_x = ox.iter().cloned().fold(f64::NEG_INFINITY, f64::max).ceil() + EXTEND_AREA / 2.0;
159    let max_y = oy.iter().cloned().fold(f64::NEG_INFINITY, f64::max).ceil() + EXTEND_AREA / 2.0;
160
161    let x_width = ((max_x - min_x) / resolution).round() as usize;
162    let y_width = ((max_y - min_y) / resolution).round() as usize;
163
164    GridMapConfig {
165        min_x,
166        min_y,
167        max_x,
168        max_y,
169        x_width,
170        y_width,
171    }
172}
173
174/// Bresenham's line algorithm returning a list of (x, y) grid points.
175fn bresenham(start: (i32, i32), end: (i32, i32)) -> Vec<(i32, i32)> {
176    let (mut x1, mut y1) = start;
177    let (mut x2, mut y2) = end;
178    let dx = x2 - x1;
179    let dy = y2 - y1;
180    let is_steep = dy.abs() > dx.abs();
181
182    if is_steep {
183        std::mem::swap(&mut x1, &mut y1);
184        std::mem::swap(&mut x2, &mut y2);
185    }
186
187    let mut swapped = false;
188    if x1 > x2 {
189        std::mem::swap(&mut x1, &mut x2);
190        std::mem::swap(&mut y1, &mut y2);
191        swapped = true;
192    }
193
194    let dx = x2 - x1;
195    let dy = y2 - y1;
196    let mut error = dx / 2;
197    let y_step = if y1 < y2 { 1 } else { -1 };
198
199    let mut y = y1;
200    let mut points = Vec::new();
201    for x in x1..=x2 {
202        let coord = if is_steep { (y, x) } else { (x, y) };
203        points.push(coord);
204        error -= dy.abs();
205        if error < 0 {
206            y += y_step;
207            error += dx;
208        }
209    }
210
211    if swapped {
212        points.reverse();
213    }
214    points
215}
216
217/// Generate occupancy grid using Bresenham ray tracing.
218fn generate_bresenham(
219    data: &mut DMatrix<f64>,
220    ox: &[f64],
221    oy: &[f64],
222    config: &GridMapConfig,
223    resolution: f64,
224    center_ix: usize,
225    center_iy: usize,
226) {
227    let xw = config.x_width;
228    let yw = config.y_width;
229
230    for (&x, &y) in ox.iter().zip(oy.iter()) {
231        let ix = ((x - config.min_x) / resolution).round() as i32;
232        let iy = ((y - config.min_y) / resolution).round() as i32;
233
234        let laser_beams = bresenham((center_ix as i32, center_iy as i32), (ix, iy));
235
236        // Mark cells along the ray as free.
237        for &(bx, by) in &laser_beams {
238            if bx >= 0 && bx < xw as i32 && by >= 0 && by < yw as i32 {
239                data[(bx as usize, by as usize)] = FREE;
240            }
241        }
242
243        // Mark the obstacle cell and its immediate neighbors as occupied.
244        for di in 0..=1_i32 {
245            for dj in 0..=1_i32 {
246                let oi = ix + di;
247                let oj = iy + dj;
248                if oi >= 0 && oi < xw as i32 && oj >= 0 && oj < yw as i32 {
249                    data[(oi as usize, oj as usize)] = OCCUPIED;
250                }
251            }
252        }
253    }
254}
255
256/// Generate occupancy grid using flood fill from the sensor origin.
257fn generate_flood_fill(
258    data: &mut DMatrix<f64>,
259    ox: &[f64],
260    oy: &[f64],
261    config: &GridMapConfig,
262    resolution: f64,
263    center_ix: usize,
264    center_iy: usize,
265) {
266    let xw = config.x_width;
267    let yw = config.y_width;
268
269    // Phase 1: Trace rays between consecutive obstacle points to mark boundary free cells.
270    let mut prev_ix = center_ix as i32 - 1;
271    let mut prev_iy = center_iy as i32;
272    for (&x, &y) in ox.iter().zip(oy.iter()) {
273        let ix = ((x - config.min_x) / resolution).round() as i32;
274        let iy = ((y - config.min_y) / resolution).round() as i32;
275
276        let line = bresenham((prev_ix, prev_iy), (ix, iy));
277        for &(bx, by) in &line {
278            if bx >= 0 && bx < xw as i32 && by >= 0 && by < yw as i32 {
279                data[(bx as usize, by as usize)] = FREE;
280            }
281        }
282
283        prev_ix = ix;
284        prev_iy = iy;
285    }
286
287    // Phase 2: Flood fill from the sensor center to mark all reachable unknown cells as free.
288    flood_fill(center_ix, center_iy, data);
289
290    // Phase 3: Mark obstacle cells and their neighbors as occupied.
291    for (&x, &y) in ox.iter().zip(oy.iter()) {
292        let ix = ((x - config.min_x) / resolution).round() as i32;
293        let iy = ((y - config.min_y) / resolution).round() as i32;
294        for di in 0..=1_i32 {
295            for dj in 0..=1_i32 {
296                let oi = ix + di;
297                let oj = iy + dj;
298                if oi >= 0 && oi < xw as i32 && oj >= 0 && oj < yw as i32 {
299                    data[(oi as usize, oj as usize)] = OCCUPIED;
300                }
301            }
302        }
303    }
304}
305
306/// BFS flood fill from the given starting point. Cells with value `UNKNOWN` (0.5)
307/// that are reachable from the start are set to `FREE` (0.0).
308fn flood_fill(start_x: usize, start_y: usize, data: &mut DMatrix<f64>) {
309    let (sx, sy) = data.shape();
310    let mut queue = VecDeque::new();
311    queue.push_back((start_x, start_y));
312
313    while let Some((nx, ny)) = queue.pop_back() {
314        // West
315        if nx > 0 && (data[(nx - 1, ny)] - UNKNOWN).abs() < f64::EPSILON {
316            data[(nx - 1, ny)] = FREE;
317            queue.push_front((nx - 1, ny));
318        }
319        // East
320        if nx < sx - 1 && (data[(nx + 1, ny)] - UNKNOWN).abs() < f64::EPSILON {
321            data[(nx + 1, ny)] = FREE;
322            queue.push_front((nx + 1, ny));
323        }
324        // North
325        if ny > 0 && (data[(nx, ny - 1)] - UNKNOWN).abs() < f64::EPSILON {
326            data[(nx, ny - 1)] = FREE;
327            queue.push_front((nx, ny - 1));
328        }
329        // South
330        if ny < sy - 1 && (data[(nx, ny + 1)] - UNKNOWN).abs() < f64::EPSILON {
331            data[(nx, ny + 1)] = FREE;
332            queue.push_front((nx, ny + 1));
333        }
334    }
335}
336
337fn occupancy_style(value: f64) -> (&'static str, f64) {
338    if value >= 0.99 {
339        ("#b91c1c", 0.95) // occupied - red
340    } else if value >= 0.49 {
341        ("#94a3b8", 0.60) // unknown - grey
342    } else {
343        ("#f0fdf4", 0.85) // free - light green
344    }
345}
346
347/// Build an SVG visualization of the LIDAR grid map.
348pub fn build_lidar_grid_map_svg(grid_map: &LidarGridMap, ox: &[f64], oy: &[f64]) -> String {
349    let width = 920.0;
350    let height = 700.0;
351    let margin = 72.0;
352    let side_panel_width = 220.0;
353    let available_plot_width = width - 2.0 * margin - side_panel_width;
354    let available_plot_height = height - 2.0 * margin;
355    let x_range = grid_map.max_x - grid_map.min_x;
356    let y_range = grid_map.max_y - grid_map.min_y;
357    let scale = (available_plot_width / x_range)
358        .min(available_plot_height / y_range)
359        .max(1.0);
360    let plot_width = x_range * scale;
361    let plot_height = y_range * scale;
362    let plot_left = margin;
363    let plot_top = margin + (available_plot_height - plot_height) / 2.0;
364    let legend_x = plot_left + plot_width + 36.0;
365    let legend_y = plot_top + 24.0;
366
367    let to_svg_x = |x: f64| plot_left + (x - grid_map.min_x) * scale;
368    let to_svg_y = |y: f64| plot_top + plot_height - (y - grid_map.min_y) * scale;
369    let cell_size = grid_map.resolution * scale;
370    let origin_x = grid_map.min_x + grid_map.center_ix as f64 * grid_map.resolution;
371    let origin_y = grid_map.min_y + grid_map.center_iy as f64 * grid_map.resolution;
372
373    let mut svg = String::new();
374    svg.push_str(&format!(
375        "<?xml version='1.0' encoding='UTF-8'?>\n<svg xmlns='http://www.w3.org/2000/svg' width='{width}' height='{height}' viewBox='0 0 {width} {height}'>\n"
376    ));
377    svg.push_str("<rect width='100%' height='100%' fill='white'/>\n");
378    svg.push_str(&format!(
379        "<text x='{:.1}' y='38' text-anchor='middle' font-size='24' font-family='sans-serif' font-weight='700' fill='#111827'>LIDAR to Grid Map</text>\n",
380        width / 2.0
381    ));
382    svg.push_str(&format!(
383        "<rect x='{plot_left:.1}' y='{plot_top:.1}' width='{plot_width:.1}' height='{plot_height:.1}' fill='#fffdf7' stroke='#d6d3d1' stroke-width='2'/>\n"
384    ));
385
386    // X axis ticks
387    let tick_step = pick_tick_step(x_range);
388    let x_tick_start = (grid_map.min_x / tick_step).ceil() as i32;
389    let x_tick_end = (grid_map.max_x / tick_step).floor() as i32;
390    for tick in x_tick_start..=x_tick_end {
391        let x = tick as f64 * tick_step;
392        let sx = to_svg_x(x);
393        svg.push_str(&format!(
394            "<line x1='{sx:.2}' y1='{plot_top:.2}' x2='{sx:.2}' y2='{:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
395            plot_top + plot_height
396        ));
397        svg.push_str(&format!(
398            "<text x='{sx:.2}' y='{:.2}' text-anchor='middle' font-size='14' font-family='sans-serif' fill='#374151'>{x:.1}</text>\n",
399            plot_top + plot_height + 24.0
400        ));
401    }
402
403    // Y axis ticks
404    let y_tick_step = pick_tick_step(y_range);
405    let y_tick_start = (grid_map.min_y / y_tick_step).ceil() as i32;
406    let y_tick_end = (grid_map.max_y / y_tick_step).floor() as i32;
407    for tick in y_tick_start..=y_tick_end {
408        let y = tick as f64 * y_tick_step;
409        let sy = to_svg_y(y);
410        svg.push_str(&format!(
411            "<line x1='{plot_left:.2}' y1='{sy:.2}' x2='{:.2}' y2='{sy:.2}' stroke='#ece7dc' stroke-width='1'/>\n",
412            plot_left + plot_width
413        ));
414        svg.push_str(&format!(
415            "<text x='{:.2}' y='{:.2}' text-anchor='end' font-size='14' font-family='sans-serif' fill='#374151'>{y:.1}</text>\n",
416            plot_left - 10.0,
417            sy + 5.0
418        ));
419    }
420
421    // Grid cells
422    for iy in 0..grid_map.y_width {
423        for ix in 0..grid_map.x_width {
424            let value = grid_map.data[(ix, iy)];
425            let x = grid_map.min_x + ix as f64 * grid_map.resolution;
426            let y = grid_map.min_y + iy as f64 * grid_map.resolution;
427            let sx = to_svg_x(x);
428            let sy = to_svg_y(y + grid_map.resolution);
429            let (fill, opacity) = occupancy_style(value);
430            svg.push_str(&format!(
431                "<rect x='{sx:.2}' y='{sy:.2}' width='{cell_size:.2}' height='{cell_size:.2}' fill='{fill}' fill-opacity='{opacity:.3}' stroke='none'/>\n"
432            ));
433        }
434    }
435
436    // Obstacle points
437    for (x, y) in ox.iter().zip(oy.iter()) {
438        svg.push_str(&format!(
439            "<circle cx='{:.2}' cy='{:.2}' r='3.8' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
440            to_svg_x(*x),
441            to_svg_y(*y)
442        ));
443    }
444
445    // Sensor origin
446    svg.push_str(&format!(
447        "<circle cx='{:.2}' cy='{:.2}' r='6' fill='#2563eb' stroke='white' stroke-width='2'/>\n",
448        to_svg_x(origin_x),
449        to_svg_y(origin_y)
450    ));
451
452    // Legend
453    svg.push_str(&format!(
454        "<rect x='{legend_x:.1}' y='{legend_y:.1}' width='182' height='180' rx='18' fill='white' stroke='#d6d3d1'/>\n"
455    ));
456    let legend_rows = [
457        ("#b91c1c", "Occupied cell"),
458        ("#94a3b8", "Unknown cell"),
459        ("#f0fdf4", "Free space"),
460    ];
461    for (index, (color, label)) in legend_rows.iter().enumerate() {
462        let row_y = legend_y + 28.0 + index as f64 * 26.0;
463        svg.push_str(&format!(
464            "<rect x='{:.1}' y='{:.1}' width='22' height='14' rx='4' fill='{color}' stroke='#cbd5e1'/>\n",
465            legend_x + 14.0,
466            row_y - 10.0
467        ));
468        svg.push_str(&format!(
469            "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>{label}</text>\n",
470            legend_x + 48.0,
471            row_y + 1.0
472        ));
473    }
474    let obstacle_row_y = legend_y + 112.0;
475    svg.push_str(&format!(
476        "<circle cx='{:.1}' cy='{obstacle_row_y:.1}' r='4.0' fill='#111827' stroke='white' stroke-width='1.2'/>\n",
477        legend_x + 25.0
478    ));
479    svg.push_str(&format!(
480        "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Obstacle points</text>\n",
481        legend_x + 48.0,
482        obstacle_row_y + 1.0
483    ));
484    let sensor_row_y = legend_y + 138.0;
485    svg.push_str(&format!(
486        "<circle cx='{:.1}' cy='{sensor_row_y:.1}' r='5.5' fill='#2563eb' stroke='white' stroke-width='1.5'/>\n",
487        legend_x + 25.0
488    ));
489    svg.push_str(&format!(
490        "<text x='{:.1}' y='{:.1}' font-size='16' font-family='sans-serif' fill='#111827'>Sensor origin</text>\n",
491        legend_x + 48.0,
492        sensor_row_y + 1.0
493    ));
494
495    // Axis labels
496    svg.push_str(&format!(
497        "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827'>x [m]</text>\n",
498        plot_left + plot_width / 2.0,
499        plot_top + plot_height + 52.0
500    ));
501    svg.push_str(&format!(
502        "<text x='{:.1}' y='{:.1}' text-anchor='middle' font-size='16' font-family='sans-serif' fill='#111827' transform='rotate(-90, {:.1}, {:.1})'>y [m]</text>\n",
503        plot_left - 48.0,
504        plot_top + plot_height / 2.0,
505        plot_left - 48.0,
506        plot_top + plot_height / 2.0
507    ));
508    svg.push_str("</svg>\n");
509
510    svg
511}
512
513/// Choose a reasonable tick step for the given axis range.
514fn pick_tick_step(range: f64) -> f64 {
515    if range > 20.0 {
516        10.0
517    } else if range > 4.0 {
518        2.0
519    } else if range > 1.0 {
520        0.5
521    } else {
522        0.1
523    }
524}
525
526/// Save a LIDAR grid map SVG to a file.
527pub fn save_lidar_grid_map_svg(
528    filename: &str,
529    grid_map: &LidarGridMap,
530    ox: &[f64],
531    oy: &[f64],
532) -> std::io::Result<()> {
533    let svg = build_lidar_grid_map_svg(grid_map, ox, oy);
534    let mut file = File::create(filename)?;
535    file.write_all(svg.as_bytes())?;
536    Ok(())
537}
538
539#[cfg(test)]
540mod tests {
541    use super::*;
542    use std::f64::consts::PI;
543
544    /// Generate a simple circular scan for testing.
545    fn circular_scan(n: usize, radius: f64) -> (Vec<f64>, Vec<f64>) {
546        let angles: Vec<f64> = (0..n).map(|i| 2.0 * PI * i as f64 / n as f64).collect();
547        let distances: Vec<f64> = vec![radius; n];
548        (angles, distances)
549    }
550
551    #[test]
552    fn test_bresenham_horizontal() {
553        let pts = bresenham((0, 0), (5, 0));
554        assert_eq!(pts.len(), 6);
555        assert_eq!(pts[0], (0, 0));
556        assert_eq!(pts[5], (5, 0));
557    }
558
559    #[test]
560    fn test_bresenham_vertical() {
561        let pts = bresenham((0, 0), (0, 5));
562        assert_eq!(pts.len(), 6);
563        assert_eq!(pts[0], (0, 0));
564        assert_eq!(pts[5], (0, 5));
565    }
566
567    #[test]
568    fn test_bresenham_diagonal() {
569        let pts = bresenham((0, 0), (3, 3));
570        assert_eq!(pts.len(), 4);
571        for (i, &(x, y)) in pts.iter().enumerate() {
572            assert_eq!(x, i as i32);
573            assert_eq!(y, i as i32);
574        }
575    }
576
577    #[test]
578    fn test_bresenham_reversed() {
579        let fwd = bresenham((1, 1), (5, 3));
580        let rev = bresenham((5, 3), (1, 1));
581        // Reversed line should produce same set of points in opposite order.
582        assert_eq!(fwd.len(), rev.len());
583        for (a, b) in fwd.iter().zip(rev.iter().rev()) {
584            assert_eq!(a, b);
585        }
586    }
587
588    #[test]
589    fn test_grid_map_bresenham_basic() {
590        let (angles, distances) = circular_scan(36, 5.0);
591        let grid = LidarGridMap::from_scan(&angles, &distances, 0.5, GridMapMethod::Bresenham);
592
593        assert!(grid.x_width > 0);
594        assert!(grid.y_width > 0);
595
596        // Sensor origin should be free.
597        let origin_val = grid.data[(grid.center_ix, grid.center_iy)];
598        assert!(
599            origin_val < 0.01,
600            "origin should be free, got {}",
601            origin_val
602        );
603
604        // At least some cells should be occupied.
605        let occupied_count = grid.data.iter().filter(|&&v| v >= 0.99).count();
606        assert!(occupied_count > 0, "expected some occupied cells");
607    }
608
609    #[test]
610    fn test_grid_map_flood_fill_basic() {
611        let (angles, distances) = circular_scan(36, 5.0);
612        let grid = LidarGridMap::from_scan(&angles, &distances, 0.5, GridMapMethod::FloodFill);
613
614        // Sensor origin should be free.
615        let origin_val = grid.data[(grid.center_ix, grid.center_iy)];
616        assert!(
617            origin_val < 0.01,
618            "origin should be free, got {}",
619            origin_val
620        );
621
622        // At least some cells should be occupied.
623        let occupied_count = grid.data.iter().filter(|&&v| v >= 0.99).count();
624        assert!(occupied_count > 0, "expected some occupied cells");
625    }
626
627    #[test]
628    fn test_grid_map_from_cartesian() {
629        let ox = vec![1.0, 0.0, -1.0, 0.0];
630        let oy = vec![0.0, 1.0, 0.0, -1.0];
631        let grid = LidarGridMap::new(&ox, &oy, 0.5, GridMapMethod::Bresenham);
632
633        // All obstacle positions should be within bounds.
634        for (&x, &y) in ox.iter().zip(oy.iter()) {
635            assert!(grid.get_index(x, y).is_some());
636        }
637    }
638
639    #[test]
640    fn test_get_occupancy_out_of_bounds() {
641        let grid = LidarGridMap::new(&[1.0], &[1.0], 0.5, GridMapMethod::Bresenham);
642        assert!(grid.get_occupancy(1000.0, 1000.0).is_none());
643    }
644
645    #[test]
646    fn test_svg_output() {
647        let ox = vec![1.0, 0.0, -1.0, 0.0];
648        let oy = vec![0.0, 1.0, 0.0, -1.0];
649        let grid = LidarGridMap::new(&ox, &oy, 0.5, GridMapMethod::Bresenham);
650        let svg = build_lidar_grid_map_svg(&grid, &ox, &oy);
651
652        assert!(svg.contains("<rect width='100%' height='100%' fill='white'/>"));
653        assert!(svg.contains("LIDAR to Grid Map"));
654        assert!(svg.contains("Occupied cell"));
655        assert!(svg.contains("Free space"));
656        assert!(svg.contains("Sensor origin"));
657    }
658
659    #[test]
660    fn test_both_methods_produce_occupied_and_free() {
661        let (angles, distances) = circular_scan(72, 3.0);
662
663        for method in [GridMapMethod::Bresenham, GridMapMethod::FloodFill] {
664            let grid = LidarGridMap::from_scan(&angles, &distances, 0.25, method);
665            let free_count = grid.data.iter().filter(|&&v| v < 0.01).count();
666            let occupied_count = grid.data.iter().filter(|&&v| v >= 0.99).count();
667            assert!(free_count > 0, "{:?}: expected free cells", method);
668            assert!(occupied_count > 0, "{:?}: expected occupied cells", method);
669        }
670    }
671}