Re-Introduces Occupancy Grid Export

Cargo.toml

@@ -23,7 +23,7 @@ opt-level = 3
 [workspace.dependencies]
 serde = { version = "1.0" }
 strum = { version = "0.26", features = ["derive"] }
-
+image = "0.24"
 rmf_site_picking = { path = "crates/rmf_site_picking", version = "0.0.2"}
 rmf_site_format = { path = "crates/rmf_site_format", version = "0.0.2" }
 rmf_site_editor = { path = "crates/rmf_site_editor", version = "0.0.3"}

crates/rmf_site_editor/Cargo.toml

@@ -56,6 +56,7 @@ sdformat = {workspace = true}
 gz-fuel = {workspace = true}
 pathdiff = {workspace = true}
 ehttp = { workspace = true, features = ["native-async"] }
+image = { workspace = true}
 nalgebra = {workspace = true}
 anyhow = {workspace = true}
 uuid = { workspace = true, features = ["v4"] }

crates/rmf_site_editor/src/occupancy.rs

@@ -19,6 +19,7 @@ use crate::{
     layers::ZLayer,
     mapf_rse::{MAPFDebugDisplay, NegotiationRequest},
     site::{Category, LevelElevation, NameOfSite, SiteAssets},
+    workspace::WorkspaceSaver,
 };
 use bevy::{
     ecs::{hierarchy::ChildOf, relationship::AncestorIter},
@@ -41,9 +42,10 @@ impl Plugin for OccupancyPlugin {
     fn build(&self, app: &mut App) {
         app.add_event::<CalculateGridRequest>()
             .add_event::<NegotiationRequest>()
+            .add_event::<ExportGridRequest>()
             .init_resource::<MAPFDebugDisplay>()
             .init_resource::<OccupancyInfo>()
-            .add_systems(Update, handle_calculate_grid_request);
+            .add_systems(Update, (handle_mapf_request, handle_export_request));
     }
 }
 
@@ -128,6 +130,14 @@ impl GridRange {
         }
     }
 
+    pub fn width(&self) -> usize {
+        self.min[0].abs_diff(self.max[0]) as usize
+    }
+
+    pub fn height(&self) -> usize {
+        self.min[1].abs_diff(self.max[1]) as usize
+    }
+
     pub fn include(&mut self, cell: Cell) {
         self.min[0] = self.min[0].min(cell.x);
         self.min[1] = self.min[1].min(cell.y);
@@ -154,6 +164,9 @@ impl GridRange {
     }
 }
 
+#[derive(Event)]
+pub struct ExportGridRequest(pub f32);
+
 #[derive(Event)]
 pub struct CalculateGridRequest;
 
@@ -185,7 +198,50 @@ enum Group {
     None,
 }
 
-fn handle_calculate_grid_request(
+fn handle_export_request(
+    mut request: EventReader<ExportGridRequest>,
+    //occupancy_info: Res<OccupancyInfo>,
+    robots: Query<Entity, With<Robot>>,
+    mut commands: Commands,
+    bodies: Query<(Entity, &Mesh3d, &Aabb, &GlobalTransform)>,
+    meta: Query<(
+        Option<&ChildOf>,
+        Option<&Category>,
+        Option<&ComputedVisualCue>,
+    )>,
+    child_of: Query<&ChildOf>,
+    levels: Query<Entity, With<LevelElevation>>,
+    sites: Query<(), With<NameOfSite>>,
+    mut meshes: ResMut<Assets<Mesh>>,
+    assets: Res<SiteAssets>,
+    grids: Query<Entity, With<Grid>>,
+    display_mapf_debug: Res<MAPFDebugDisplay>,
+    mut workspace_saver: WorkspaceSaver,
+) {
+    if let Some(export_req) = request.read().last() {
+        let grid = CalculateGrid {
+            cell_size: export_req.0,
+            ignore: robots.iter().collect(),
+            ..default()
+        };
+        calculate_grid(
+            &grid,
+            &mut commands,
+            &bodies,
+            &meta,
+            &child_of,
+            &levels,
+            &sites,
+            &mut meshes,
+            &assets,
+            &grids,
+            &display_mapf_debug,
+        );
+        workspace_saver.export_occupancy_to_dialog();
+    }
+}
+
+fn handle_mapf_request(
     mut request: EventReader<CalculateGridRequest>,
     occupancy_info: Res<OccupancyInfo>,
     robots: Query<Entity, With<Robot>>,

crates/rmf_site_editor/src/site/save.rs

@@ -32,7 +32,9 @@ use std::{
 };
 use thiserror::Error as ThisError;
 
-use crate::{exit_confirmation::SiteChanged, recency::RecencyRanking, site::*, ExportFormat};
+use crate::{
+    exit_confirmation::SiteChanged, occupancy::Grid, recency::RecencyRanking, site::*, ExportFormat,
+};
 use rmf_site_format::*;
 use sdformat::yaserde;
 
@@ -1759,6 +1761,39 @@ pub fn generate_site(
     });
 }
 
+pub fn export_grid(world: &mut World, path: &PathBuf) {
+    let mut system_state: SystemState<(Query<(&Grid, &ChildOf)>, Query<&NameInSite>)> =
+        SystemState::new(world);
+    let (grids, names) = system_state.get(&world);
+    let mut i = 0;
+    for (grid, parent) in grids {
+        let mut img = image::RgbImage::new(
+            (grid.range.width() + 1) as u32,
+            (grid.range.height() + 1) as u32,
+        );
+
+        img.fill(255u8);
+        let Ok(name) = names.get(parent.0) else {
+            error!("Could not get level name");
+            continue;
+        };
+
+        for cell in &grid.occupied {
+            img.put_pixel(
+                (cell.x - grid.range.min_cell().x) as u32,
+                (cell.y - grid.range.min_cell().y) as u32,
+                image::Rgb([0, 0, 0]),
+            );
+        }
+
+        let mut path = path.clone();
+        path.push(format!("occupancy.{}.png", name.0));
+        img.save(path).unwrap();
+        i += 1;
+    }
+    info!("Successfully exported {} occupancy grids", i);
+}
+
 pub fn save_site(world: &mut World) {
     let save_events: Vec<_> = world.resource_mut::<Events<SaveSite>>().drain().collect();
     for save_event in save_events {
@@ -1917,6 +1952,9 @@ pub fn save_site(world: &mut World) {
                     new_path.to_str().unwrap_or("<failed to render??>")
                 );
             }
+            ExportFormat::OccupancyGrid => {
+                export_grid(world, &new_path);
+            }
         }
     }
 }

crates/rmf_site_editor/src/widgets/mod.rs

@@ -71,6 +71,9 @@ pub use inspector::*;
 pub mod move_layer;
 pub use move_layer::*;
 
+pub mod occupancy_export_menu;
+pub use occupancy_export_menu::*;
+
 pub mod sdf_export_menu;
 use rmf_site_egui::*;
 use rmf_site_picking::{Hover, SelectionServiceStages, UiFocused};
@@ -168,6 +171,8 @@ impl Plugin for StandardUiPlugin {
                 SdfExportMenuPlugin::default(),
                 #[cfg(not(target_arch = "wasm32"))]
                 NavGraphIoPlugin::default(),
+                #[cfg(not(target_arch = "wasm32"))]
+                OccupancyExportMenuPlugin::default(),
             ))
             .add_systems(Startup, init_ui_style)
             .add_systems(