1#[derive(Clone, Debug)]
8pub struct OccupancyGridConfig {
9 pub resolution: f64,
11 pub width: usize,
13 pub height: usize,
15 pub prior_log_odds: f64,
17 pub occupied_log_odds: f64,
19 pub free_log_odds: f64,
21 pub max_log_odds: f64,
23 pub min_log_odds: f64,
25}
26
27impl Default for OccupancyGridConfig {
28 fn default() -> Self {
29 Self {
30 resolution: 0.5,
31 width: 100,
32 height: 100,
33 prior_log_odds: 0.0,
34 occupied_log_odds: 0.85,
35 free_log_odds: -0.4,
36 max_log_odds: 5.0,
37 min_log_odds: -5.0,
38 }
39 }
40}
41
42pub struct OccupancyGridMap {
48 pub grid: Vec<Vec<f64>>,
50 pub config: OccupancyGridConfig,
52}
53
54impl OccupancyGridMap {
55 pub fn new(config: OccupancyGridConfig) -> Self {
57 let grid = vec![vec![config.prior_log_odds; config.height]; config.width];
58 Self { grid, config }
59 }
60
61 pub fn update_with_scan(
70 &mut self,
71 robot_x: f64,
72 robot_y: f64,
73 robot_yaw: f64,
74 scan_ranges: &[f64],
75 angle_min: f64,
76 angle_increment: f64,
77 ) {
78 let origin = match self.world_to_grid(robot_x, robot_y) {
79 Some(o) => o,
80 None => return,
81 };
82
83 for (i, &range) in scan_ranges.iter().enumerate() {
84 if range <= 0.0 || !range.is_finite() {
85 continue;
86 }
87
88 let angle = robot_yaw + angle_min + i as f64 * angle_increment;
89 let end_x = robot_x + range * angle.cos();
90 let end_y = robot_y + range * angle.sin();
91
92 let end_cell = self.world_to_grid(end_x, end_y);
93
94 let end_ix = end_cell.map(|(ix, _)| ix as i32).unwrap_or({
95 let ix = (end_x / self.config.resolution + self.config.width as f64 / 2.0).round()
97 as i32;
98 ix.clamp(0, self.config.width as i32 - 1)
99 });
100 let end_iy = end_cell.map(|(_, iy)| iy as i32).unwrap_or({
101 let iy = (end_y / self.config.resolution + self.config.height as f64 / 2.0).round()
102 as i32;
103 iy.clamp(0, self.config.height as i32 - 1)
104 });
105
106 let ray_cells = bresenham_line(origin.0 as i32, origin.1 as i32, end_ix, end_iy);
108
109 let free_delta = self.config.free_log_odds;
110 let occ_delta = self.config.occupied_log_odds;
111 let min_l = self.config.min_log_odds;
112 let max_l = self.config.max_log_odds;
113 let w = self.config.width;
114 let h = self.config.height;
115
116 let ray_len = ray_cells.len();
118 for &(cx, cy) in ray_cells.iter().take(ray_len.saturating_sub(1)) {
119 if cx >= 0 && cx < w as i32 && cy >= 0 && cy < h as i32 {
120 let l = &mut self.grid[cx as usize][cy as usize];
121 *l = (*l + free_delta).clamp(min_l, max_l);
122 }
123 }
124
125 if let Some((ex, ey)) = end_cell {
127 let l = &mut self.grid[ex][ey];
128 *l = (*l + occ_delta).clamp(min_l, max_l);
129 }
130 }
131 }
132
133 pub fn get_probability(&self, ix: usize, iy: usize) -> f64 {
137 let l = self.grid[ix][iy];
138 1.0 - 1.0 / (1.0 + l.exp())
139 }
140
141 pub fn world_to_grid(&self, x: f64, y: f64) -> Option<(usize, usize)> {
145 let ix = (x / self.config.resolution + self.config.width as f64 / 2.0).floor() as i32;
146 let iy = (y / self.config.resolution + self.config.height as f64 / 2.0).floor() as i32;
147
148 if ix >= 0 && ix < self.config.width as i32 && iy >= 0 && iy < self.config.height as i32 {
149 Some((ix as usize, iy as usize))
150 } else {
151 None
152 }
153 }
154
155 pub fn is_occupied(&self, ix: usize, iy: usize, threshold: f64) -> bool {
158 self.get_probability(ix, iy) > threshold
159 }
160}
161
162fn bresenham_line(x0: i32, y0: i32, x1: i32, y1: i32) -> Vec<(i32, i32)> {
165 let mut cells = Vec::new();
166
167 let dx = (x1 - x0).abs();
168 let dy = (y1 - y0).abs();
169 let sx = if x0 < x1 { 1_i32 } else { -1 };
170 let sy = if y0 < y1 { 1_i32 } else { -1 };
171
172 let mut x = x0;
173 let mut y = y0;
174 let mut err = dx - dy;
175
176 loop {
177 cells.push((x, y));
178 if x == x1 && y == y1 {
179 break;
180 }
181 let e2 = 2 * err;
182 if e2 > -dy {
183 err -= dy;
184 x += sx;
185 }
186 if e2 < dx {
187 err += dx;
188 y += sy;
189 }
190 }
191
192 cells
193}
194
195#[cfg(test)]
196mod tests {
197 use super::*;
198
199 #[test]
201 fn test_initial_grid_is_prior_probability() {
202 let map = OccupancyGridMap::new(OccupancyGridConfig::default());
203 let p = map.get_probability(50, 50);
204 println!("Prior probability at (50,50): {p}");
205 assert!((p - 0.5).abs() < 1e-10, "Expected 0.5 but got {p}");
206 }
207
208 #[test]
211 fn test_update_marks_occupied_cell() {
212 let mut map = OccupancyGridMap::new(OccupancyGridConfig::default());
213 map.update_with_scan(0.0, 0.0, 0.0, &[5.0], 0.0, 0.0);
215
216 let (ex, ey) = map.world_to_grid(5.0, 0.0).unwrap();
217 let p = map.get_probability(ex, ey);
218 println!("Occupied cell probability at ({ex},{ey}): {p}");
219 assert!(p > 0.5, "Expected occupied (p > 0.5) but got {p}");
220 }
221
222 #[test]
225 fn test_update_marks_free_cells_along_ray() {
226 let mut map = OccupancyGridMap::new(OccupancyGridConfig::default());
227 for _ in 0..5 {
229 map.update_with_scan(0.0, 0.0, 0.0, &[5.0], 0.0, 0.0);
230 }
231
232 let (fx, fy) = map.world_to_grid(2.0, 0.0).unwrap();
234 let p = map.get_probability(fx, fy);
235 println!("Free cell probability at ({fx},{fy}): {p}");
236 assert!(p < 0.5, "Expected free (p < 0.5) but got {p}");
237 }
238
239 #[test]
241 fn test_world_to_grid_conversion() {
242 let cfg = OccupancyGridConfig::default();
243 let map = OccupancyGridMap::new(cfg.clone());
244
245 let (cx, cy) = map.world_to_grid(0.0, 0.0).unwrap();
247 println!("Grid centre: ({cx}, {cy})");
248 assert_eq!(cx, cfg.width / 2);
249 assert_eq!(cy, cfg.height / 2);
250
251 let outside = map.world_to_grid(1_000.0, 1_000.0);
253 assert!(outside.is_none());
254 }
255}