Export occupancy graphs

rmf_site_editor/src/occupancy.rs

@@ -29,8 +29,9 @@ use bevy::{
     },
 };
 use itertools::Itertools;
+use serde::{Deserialize, Serialize};
 use std::{
-    collections::{HashMap, HashSet},
+    collections::{BTreeMap, BTreeSet, HashMap, HashSet},
     time::Instant,
 };
 
@@ -91,6 +92,29 @@ pub struct Grid {
     pub range: GridRange,
 }
 
+impl Grid {
+    pub fn to_format(&self) -> rmf_site_format::Occupancy {
+        let mut cells: BTreeMap<i64, BTreeSet<i64>> = BTreeMap::new();
+        for cell in &self.occupied {
+            cells.entry(cell.x).or_default().insert(cell.y);
+        }
+
+        rmf_site_format::Occupancy {
+            cell_size: self.cell_size,
+            cells,
+        }
+    }
+}
+
+#[derive(Serialize, Deserialize)]
+pub struct GridFile {
+    pub resolution: f32, // m/cell
+    pub origin: [f32; 2],
+    pub width: u64,
+    pub height: u64,
+    pub occupied: BTreeMap<u64, BTreeSet<u64>>,
+}
+
 #[derive(Clone, Copy, Debug)]
 pub struct GridRange {
     min: [i64; 2],
@@ -129,6 +153,13 @@ impl GridRange {
     pub fn iter(&self) -> impl Iterator<Item = (i64, i64)> {
         (self.min[0]..=self.max[0]).cartesian_product(self.min[1]..=self.max[1])
     }
+
+    pub fn width(&self) -> u64 {
+        (self.max[0] - self.min[0]) as u64
+    }
+    pub fn height(&self) -> u64 {
+        (self.max[1] - self.min[1]) as u64
+    }
 }
 
 pub struct CalculateGrid {

rmf_site_editor/src/site/save.rs

@@ -19,10 +19,13 @@ use bevy::{
     ecs::{event::Events, system::SystemState},
     prelude::*,
 };
-use std::{collections::BTreeMap, path::PathBuf};
+use std::{
+    collections::{BTreeMap, HashMap},
+    path::PathBuf,
+};
 use thiserror::Error as ThisError;
 
-use crate::site::*;
+use crate::{occupancy::Grid as OccupancyGrid, site::*};
 use rmf_site_format::*;
 
 pub struct SaveSite {
@@ -917,7 +920,34 @@ pub fn save_nav_graphs(world: &mut World) {
             }
         };
 
-        for (name, nav_graph) in legacy::nav_graph::NavGraph::from_site(&site) {
+        let occupancy: HashMap<String, Occupancy> = {
+            let mut state: SystemState<(
+                Query<(&OccupancyGrid, &Parent)>,
+                Query<(Entity, &LevelProperties, &Parent)>,
+            )> = SystemState::new(world);
+
+            let (grids, levels) = state.get_mut(world);
+            let level_names: HashMap<Entity, String> = levels
+                .iter()
+                .filter(|(_, _, parent)| parent.get() == save_event.site)
+                .map(|(e, prop, _)| (e, prop.name.clone()))
+                .collect();
+
+            grids
+                .iter()
+                .filter_map(|(grid, parent)| {
+                    level_names
+                        .get(&parent.get())
+                        .map(|name| (name.clone(), grid.to_format()))
+                })
+                .collect()
+        };
+
+        for (name, mut nav_graph) in legacy::nav_graph::NavGraph::from_site(&site) {
+            for (level_name, level) in &mut nav_graph.levels {
+                level.occupancy = occupancy.get(level_name).map(|grid| grid.clone());
+            }
+
             let mut graph_file = path.clone();
             graph_file.set_file_name(name + ".nav.yaml");
             println!(

rmf_site_format/src/legacy/nav_graph.rs

@@ -1,11 +1,11 @@
 use crate::*;
-use serde::Serialize;
+use serde::{Deserialize, Serialize};
 use std::collections::{HashMap, HashSet};
 
-#[derive(Serialize, Clone)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct NavGraph {
-    building_name: String,
-    levels: HashMap<String, NavLevel>,
+    pub building_name: String,
+    pub levels: HashMap<String, NavLevel>,
 }
 
 impl NavGraph {
@@ -25,13 +25,16 @@ impl NavGraph {
             };
 
             let lanes_with_anchor = {
-                let mut lanes_with_anchor = HashMap::new();
+                let mut lanes_with_anchor: HashMap<u32, Vec<(u32, &Lane<u32>)>> = HashMap::new();
                 for (lane_id, lane) in &site.navigation.guided.lanes {
                     if !lane.graphs.includes(graph_id) {
                         continue;
                     }
                     for a in lane.anchors.array() {
-                        lanes_with_anchor.insert(a, (*lane_id, lane));
+                        lanes_with_anchor
+                            .entry(a)
+                            .or_default()
+                            .push((*lane_id, lane));
                     }
                 }
                 lanes_with_anchor
@@ -45,12 +48,22 @@ impl NavGraph {
                 let mut vertices = Vec::new();
                 let mut lanes_to_include = HashSet::new();
                 for (id, anchor) in &level.anchors {
-                    let (lane, _) = match lanes_with_anchor.get(id) {
+                    let lanes = match lanes_with_anchor.get(id) {
                         Some(v) => v,
-                        None => continue,
+                        None => {
+                            if let Some(location) = location_at_anchor.get(id) {
+                                // Even if the location is not connected by any
+                                // lane, include it in the nav graph.
+                                anchor_to_vertex.insert(*id, vertices.len());
+                                vertices.push(NavVertex::from_anchor(anchor, Some(location)));
+                            }
+                            continue;
+                        }
                     };
 
-                    lanes_to_include.insert(*lane);
+                    for (lane_id, _) in lanes {
+                        lanes_to_include.insert(*lane_id);
+                    }
                     anchor_to_vertex.insert(*id, vertices.len());
                     vertices.push(NavVertex::from_anchor(anchor, location_at_anchor.get(id)));
                 }
@@ -84,7 +97,14 @@ impl NavGraph {
                     }
                 }
 
-                levels.insert(level.properties.name.clone(), NavLevel { lanes, vertices });
+                levels.insert(
+                    level.properties.name.clone(),
+                    NavLevel {
+                        lanes,
+                        vertices,
+                        occupancy: None,
+                    },
+                );
             }
 
             graphs.push((
@@ -100,20 +120,22 @@ impl NavGraph {
     }
 }
 
-#[derive(Serialize, Clone)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct NavLevel {
-    lanes: Vec<NavLane>,
-    vertices: Vec<NavVertex>,
+    pub lanes: Vec<NavLane>,
+    pub vertices: Vec<NavVertex>,
+    #[serde(default, skip_serializing_if = "Option::is_none")]
+    pub occupancy: Option<Occupancy>,
 }
 
-#[derive(Serialize, Clone)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct NavLane(pub usize, pub usize, pub NavLaneProperties);
 
-#[derive(Serialize, Clone)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct NavLaneProperties {
-    speed_limit: f32,
-    #[serde(skip_serializing_if = "Option::is_none")]
-    dock_name: Option<String>,
+    pub speed_limit: f32,
+    #[serde(default, skip_serializing_if = "Option::is_none")]
+    pub dock_name: Option<String>,
     // TODO(MXG): Add other lane properties
     // door_name,
     // orientation_constraint,
@@ -129,7 +151,7 @@ impl NavLaneProperties {
     }
 }
 
-#[derive(Serialize, Clone)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct NavVertex(pub f32, pub f32, pub NavVertexProperties);
 
 impl NavVertex {
@@ -139,17 +161,17 @@ impl NavVertex {
     }
 }
 
-#[derive(Serialize, Clone)]
+#[derive(Serialize, Deserialize, Clone)]
 pub struct NavVertexProperties {
-    #[serde(skip_serializing_if = "Option::is_none")]
-    lift: Option<String>,
-    #[serde(skip_serializing_if = "is_false")]
-    is_charger: bool,
-    #[serde(skip_serializing_if = "is_false")]
-    is_holding_point: bool,
-    #[serde(skip_serializing_if = "is_false")]
-    is_parking_spot: bool,
-    name: String,
+    #[serde(default, skip_serializing_if = "Option::is_none")]
+    pub lift: Option<String>,
+    #[serde(default = "bool_false", skip_serializing_if = "is_false")]
+    pub is_charger: bool,
+    #[serde(default = "bool_false", skip_serializing_if = "is_false")]
+    pub is_holding_point: bool,
+    #[serde(default = "bool_false", skip_serializing_if = "is_false")]
+    pub is_parking_spot: bool,
+    pub name: String,
 }
 
 impl Default for NavVertexProperties {
@@ -172,13 +194,19 @@ impl NavVertexProperties {
             None => return props,
         };
         props.name = location.name.0.clone();
-        props.is_charger = location.tags.iter().find(|t| t.is_charger()).is_some();
+        props.is_charger = location.tags.0.iter().find(|t| t.is_charger()).is_some();
         props.is_holding_point = location
             .tags
+            .0
             .iter()
             .find(|t| t.is_holding_point())
             .is_some();
-        props.is_parking_spot = location.tags.iter().find(|t| t.is_parking_spot()).is_some();
+        props.is_parking_spot = location
+            .tags
+            .0
+            .iter()
+            .find(|t| t.is_parking_spot())
+            .is_some();
 
         props
     }
@@ -187,3 +215,7 @@ impl NavVertexProperties {
 fn is_false(b: &bool) -> bool {
     !b
 }
+
+fn bool_false() -> bool {
+    false
+}

rmf_site_format/src/lib.rs

@@ -75,6 +75,9 @@ pub use nav_graph::*;
 pub mod navigation;
 pub use navigation::*;
 
+pub mod occupancy;
+pub use occupancy::*;
+
 pub mod path;
 pub use path::*;
 

rmf_site_format/src/occupancy.rs

@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+use serde::{Deserialize, Serialize};
+use std::collections::{BTreeMap, BTreeSet};
+
+#[derive(Serialize, Deserialize, Clone, Debug)]
+pub struct Occupancy {
+    /// How large is each cell
+    pub cell_size: f32,
+    /// Sparse description of cell occupancy.
+    /// Key is x coordinate and Value is all the occupied y coordinates.
+    pub cells: BTreeMap<i64, BTreeSet<i64>>,
+}