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 = { version = "0.25", default-features = false, features = ["png"] } # Keep in sync with bevy image version
 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;
+
 #[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: occupancy_info.cell_size,
+            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,42 @@ 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 num_grids_saved = 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));
+        if let Err(err_str) = img.save(path) {
+            error!("Could not save image: {:?}", err_str);
+        } else {
+            num_grids_saved += 1;
+        }
+    }
+    info!("Successfully exported {} occupancy grids", num_grids_saved);
+}
+
 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 +1955,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(

crates/rmf_site_editor/src/widgets/occupancy_export_menu.rs

@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2025 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 crate::{
+    occupancy::{CalculateGrid, ExportGridRequest, OccupancyInfo},
+    AppState, WorkspaceSaver,
+};
+use bevy::{ecs::hierarchy::ChildOf, prelude::*};
+use bevy_egui::{
+    egui::{self, DragValue},
+    EguiContexts,
+};
+use rmf_site_egui::*;
+
+/// Keeps track of which entity is associated to the export sdf button.
+#[derive(Resource)]
+pub struct OccupancyExportMenu {
+    export_sdf: Entity,
+}
+
+impl OccupancyExportMenu {
+    pub fn get(&self) -> Entity {
+        self.export_sdf
+    }
+}
+
+impl FromWorld for OccupancyExportMenu {
+    fn from_world(world: &mut World) -> Self {
+        let file_header = world.resource::<FileMenu>().get();
+        let export_sdf = world
+            .spawn((
+                MenuItem::Text(TextMenuItem::new("Export Occupancy")),
+                ChildOf(file_header),
+            ))
+            .id();
+
+        OccupancyExportMenu { export_sdf }
+    }
+}
+
+#[derive(Default)]
+struct ExportOccupancyConfig {
+    visible: bool,
+}
+
+fn handle_export_occupancy_menu_events(
+    mut menu_events: EventReader<MenuEvent>,
+    mut export_event: EventWriter<ExportGridRequest>,
+    occupancy_menu: Res<OccupancyExportMenu>,
+    mut occupancy_info: ResMut<OccupancyInfo>,
+    mut egui_context: EguiContexts,
+    mut configuration: Local<ExportOccupancyConfig>,
+) {
+    for event in menu_events.read() {
+        if event.clicked() && event.source() == occupancy_menu.get() {
+            configuration.visible = true;
+        }
+    }
+
+    if !configuration.visible {
+        return;
+    }
+
+    egui::Window::new("Occupancy Export Options").show(egui_context.ctx_mut(), |ui| {
+        ui.horizontal(|ui| {
+            ui.label("Cell Size:");
+            ui.add(DragValue::new(&mut occupancy_info.cell_size));
+        });
+        if ui.button("Export").clicked() {
+            configuration.visible = false;
+            export_event.write(ExportGridRequest);
+        }
+    });
+}
+
+#[derive(Default)]
+pub struct OccupancyExportMenuPlugin {}
+
+impl Plugin for OccupancyExportMenuPlugin {
+    fn build(&self, app: &mut App) {
+        app.init_resource::<OccupancyExportMenu>().add_systems(
+            Update,
+            handle_export_occupancy_menu_events.run_if(AppState::in_displaying_mode()),
+        );
+    }
+}

crates/rmf_site_editor/src/workspace.rs

@@ -56,6 +56,7 @@ pub enum ExportFormat {
     Default,
     Sdf,
     NavGraph,
+    OccupancyGrid,
 }
 
 /// Used to keep track of visibility when switching workspace
@@ -518,6 +519,8 @@ pub struct WorkspaceSavingServices {
     pub export_nav_graphs_to_dialog: Service<(), ()>,
     /// Exports the nav graphs from the requested site to the requested path.
     pub export_nav_graphs_to_path: Service<PathBuf, ()>,
+    /// Exports Occupancy Grid from the requested
+    pub export_occupancy_grid: Service<(), ()>,
 }
 
 impl FromWorld for WorkspaceSavingServices {
@@ -616,6 +619,16 @@ impl FromWorld for WorkspaceSavingServices {
                 .connect(scope.terminate)
         });
 
+        let export_occupancy_grid = world.spawn_workflow(|scope, builder| {
+            scope
+                .input
+                .chain(builder)
+                .then(pick_folder)
+                .map_block(|path| (path, ExportFormat::OccupancyGrid))
+                .then(send_file_save)
+                .connect(scope.terminate)
+        });
+
         Self {
             save_workspace_to_dialog,
             save_workspace_to_path,
@@ -624,6 +637,7 @@ impl FromWorld for WorkspaceSavingServices {
             export_sdf_to_path,
             export_nav_graphs_to_dialog,
             export_nav_graphs_to_path,
+            export_occupancy_grid,
         }
     }
 }
@@ -681,6 +695,13 @@ impl<'w, 's> WorkspaceSaver<'w, 's> {
             .request(path, self.workspace_saving.export_nav_graphs_to_path)
             .detach();
     }
+
+    /// Request to export the occupancy grid
+    pub fn export_occupancy_to_dialog(&mut self) {
+        self.commands
+            .request((), self.workspace_saving.export_occupancy_grid)
+            .detach();
+    }
 }
 
 /// `SystemParam` used to request for workspace loading operations