maps_io_ros 0.3.0

Fundamental IO for 2D ROS grid maps.
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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121

//! Map metadata.
//! Supports loading files that follow the ROS map server yaml format:
//! <https://wiki.ros.org/map_server#YAML_format>

use emath;
use serde::{Deserialize, Serialize};
use std::path::{Path, PathBuf};

use crate::error::{Error, Result};
use crate::os_helpers::resolve_symlink;
use crate::value_interpretation::{Mode, ValueInterpretation};

/// Plain ROS map metadata yaml file format.
#[derive(Deserialize)]
pub struct MetaYaml {
    pub image: PathBuf,
    pub resolution: f32,
    pub origin: [f32; 3], // x, y, theta
    pub negate: i32,
    pub occupied_thresh: f32,
    pub free_thresh: f32,
    pub mode: Option<Mode>,
}

// Annotated yaml meta to keep track of the yaml file path.
struct MetaYamlAnnotated {
    pub meta_yaml: MetaYaml,
    pub yaml_path: PathBuf,
}

impl MetaYamlAnnotated {
    fn from(yaml_path: &Path) -> Result<MetaYamlAnnotated> {
        let yaml_path = resolve_symlink(yaml_path);
        let buffer = std::fs::read_to_string(&yaml_path)
            .map_err(|e| Error::io(format!("Cannot read {yaml_path:?}"), e))?;

        let meta_yaml = serde_yaml_ng::from_str::<MetaYaml>(&buffer)
            .map_err(|e| Error::yaml(format!("Cannot parse {yaml_path:?}"), e))?;

        Ok(MetaYamlAnnotated {
            meta_yaml,
            yaml_path,
        })
    }

    fn from_bytes(bytes: &[u8], yaml_name: &str) -> Result<MetaYamlAnnotated> {
        let meta_yaml = serde_yaml_ng::from_slice::<MetaYaml>(bytes)
            .map_err(|e| Error::yaml(format!("Cannot parse {yaml_name}"), e))?;

        Ok(MetaYamlAnnotated {
            yaml_path: PathBuf::from(yaml_name),
            meta_yaml,
        })
    }
}

/// Internal representation of the metadata.
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct Meta {
    pub image_path: PathBuf,
    pub yaml_path: PathBuf,
    pub resolution: f32,
    pub origin_xy: emath::Vec2,
    pub origin_theta: emath::Rot2,
    #[serde(default)]
    pub value_interpretation: ValueInterpretation,
    #[serde(default)]
    original_value_interpretation: ValueInterpretation,
}

impl From<MetaYamlAnnotated> for Meta {
    fn from(meta_yaml_annotated: MetaYamlAnnotated) -> Meta {
        let meta_yaml = &meta_yaml_annotated.meta_yaml;
        Meta {
            // Resolve image path, it can be absolute or relative to the yaml file.
            image_path: if meta_yaml.image.is_absolute() {
                resolve_symlink(&meta_yaml.image)
            } else {
                meta_yaml_annotated
                    .yaml_path
                    .parent()
                    .expect("YAML file has no parent directory")
                    .join(&meta_yaml.image)
            },
            yaml_path: meta_yaml_annotated.yaml_path,
            resolution: meta_yaml.resolution,
            origin_xy: emath::Vec2::new(meta_yaml.origin[0], meta_yaml.origin[1]),
            origin_theta: emath::Rot2::from_angle(emath::normalized_angle(meta_yaml.origin[2])),
            value_interpretation: ValueInterpretation::from_meta_yaml(meta_yaml),
            original_value_interpretation: ValueInterpretation::from_meta_yaml(meta_yaml),
        }
    }
}

impl Meta {
    pub fn load_from_file(yaml_path: &Path) -> Result<Meta> {
        let meta_yaml_annotated = MetaYamlAnnotated::from(yaml_path)?;
        let meta = Meta::from(meta_yaml_annotated);
        if !meta.image_path.exists() {
            return Err(Error::io(
                format!(
                    "Metadata from {} points to an image that does not exist: {}",
                    yaml_path.display(),
                    meta.image_path.display()
                ),
                std::io::Error::new(std::io::ErrorKind::NotFound, "Image file not found"),
            ));
        }
        Ok(meta)
    }

    pub fn load_from_bytes(bytes: &[u8], yaml_name: &str) -> Result<Meta> {
        let meta_yaml_annotated = MetaYamlAnnotated::from_bytes(bytes, yaml_name)?;
        let meta = Meta::from(meta_yaml_annotated);
        Ok(meta)
    }

    pub fn reset_value_interpretation(&mut self) {
        self.value_interpretation = self.original_value_interpretation;
    }
}