costmap 0.2.0

2D costmaps, occupancy grids, and raycasting for robotics navigation - a Nav2 alternative in pure Rust
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

use costmap::types::{FREE, OCCUPIED, UNKNOWN};
use costmap::{RosMapLoader, loaders::ros2::RosMapMetadata};
use glam::UVec2;
use image::GenericImageView;
use std::path::Path;

#[test]
fn loads_trinary_ros2_map() {
    let manifest_dir = Path::new(env!("CARGO_MANIFEST_DIR"));
    let yaml_path = manifest_dir.join("tests/fixtures/simple.yaml");

    let grid = RosMapLoader::load_from_yaml(&yaml_path).expect("grid should load");

    assert_eq!(grid.width(), 2);
    assert_eq!(grid.height(), 2);

    assert_eq!(grid.get(UVec2::new(0, 0)), Some(&FREE));
    assert_eq!(grid.get(UVec2::new(1, 0)), Some(&OCCUPIED));
    assert_eq!(grid.get(UVec2::new(0, 1)), Some(&OCCUPIED));
    assert_eq!(grid.get(UVec2::new(1, 1)), Some(&UNKNOWN));
}

#[test]
fn loads_real_map_fixture() {
    let manifest_dir = Path::new(env!("CARGO_MANIFEST_DIR"));
    let yaml_path = manifest_dir.join("tests/fixtures/warehouse.yaml");
    let image_path = manifest_dir.join("tests/fixtures/warehouse.png");

    let grid = RosMapLoader::load_from_yaml(&yaml_path).expect("grid should load");
    let image = image::open(&image_path).expect("image should load");
    let (width, height) = image.dimensions();

    assert_eq!(grid.width(), width);
    assert_eq!(grid.height(), height);
}

#[test]
fn loads_image_direct() {
    let manifest_dir = Path::new(env!("CARGO_MANIFEST_DIR"));
    let image_path = manifest_dir.join("tests/fixtures/simple.pgm");
    let metadata = RosMapMetadata {
        image: image_path.to_path_buf(),
        ..Default::default()
    };
    let grid = RosMapLoader::load_from_image(&metadata).expect("grid should load");

    assert_eq!(grid.width(), 2);
    assert_eq!(grid.height(), 2);

    assert_eq!(grid.get(UVec2::new(0, 0)), Some(&FREE));
    assert_eq!(grid.get(UVec2::new(1, 0)), Some(&OCCUPIED));
    assert_eq!(grid.get(UVec2::new(0, 1)), Some(&OCCUPIED));
    assert_eq!(grid.get(UVec2::new(1, 1)), Some(&UNKNOWN));
}