diff --git a/assets/textures/add.png b/assets/textures/add.png new file mode 100644 index 00000000..0a586af5 Binary files /dev/null and b/assets/textures/add.png differ diff --git a/assets/textures/alignment.png b/assets/textures/alignment.png new file mode 100644 index 00000000..28bf6e05 Binary files /dev/null and b/assets/textures/alignment.png differ diff --git a/assets/textures/alignment.svg b/assets/textures/alignment.svg new file mode 100644 index 00000000..7fe16006 --- /dev/null +++ b/assets/textures/alignment.svg @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/textures/attribution.md b/assets/textures/attribution.md index 11eef4fd..3a6d66e2 100644 --- a/assets/textures/attribution.md +++ b/assets/textures/attribution.md @@ -1,4 +1,11 @@ * [`select.png`](https://thenounproject.com/icon/select-3324735/) * [`edit.png`](https://thenounproject.com/icon/edit-2162449/) +* [`exit.png`](https://thenounproject.com/icon/exit-1826632/) +* [`confirm.png`](https://thenounproject.com/icon/confirm-2261637/) +* [`reject.png`](https://thenounproject.com/icon/x-2289933/) +* [`empty.png`](https://thenounproject.com/icon/empty-194055/) +* [`add.png`](https://thenounproject.com/icon/plus-1809810/) +* [`search.png`](https://thenounproject.com/icon/search-3743008/) * `trash.png`: @mxgrey - +* `selected.png`: @mxgrey +* `alignment.png`: @mxgrey diff --git a/assets/textures/confirm.png b/assets/textures/confirm.png new file mode 100644 index 00000000..5e7ae0e7 Binary files /dev/null and b/assets/textures/confirm.png differ diff --git a/assets/textures/empty.png b/assets/textures/empty.png new file mode 100644 index 00000000..5c20903e Binary files /dev/null and b/assets/textures/empty.png differ diff --git a/assets/textures/exit.png b/assets/textures/exit.png new file mode 100644 index 00000000..bc6c6be1 Binary files /dev/null and b/assets/textures/exit.png differ diff --git a/assets/textures/reject.png b/assets/textures/reject.png new file mode 100644 index 00000000..e2f52070 Binary files /dev/null and b/assets/textures/reject.png differ diff --git a/assets/textures/search.png b/assets/textures/search.png new file mode 100644 index 00000000..70293f9c Binary files /dev/null and b/assets/textures/search.png differ diff --git a/assets/textures/selected.png b/assets/textures/selected.png new file mode 100644 index 00000000..5aa9fee1 Binary files /dev/null and b/assets/textures/selected.png differ diff --git a/assets/textures/selected.svg b/assets/textures/selected.svg new file mode 100644 index 00000000..d551232c --- /dev/null +++ b/assets/textures/selected.svg @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index 159d2621..f1f2221b 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -21,7 +21,6 @@ bevy_polyline = "0.4" bevy_stl = "0.7.0" bevy_obj = { git = "https://github.com/luca-della-vedova/bevy_obj", branch = "luca/scene_0.9", features = ["scene"]} bevy_rapier3d = "0.20.0" -optimization_engine = "0.7.7" smallvec = "*" serde = { version = "1.0", features = ["derive"] } serde_yaml = "0.8.23" @@ -45,6 +44,7 @@ tracing-subscriber = "0.3.1" rfd = "0.11" urdf-rs = "0.7" sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "a5daef0"} +pathdiff = "*" # only enable the 'dynamic' feature if we're not building for web or windows [target.'cfg(all(not(target_arch = "wasm32"), not(target_os = "windows")))'.dependencies] diff --git a/rmf_site_editor/src/interaction/camera_controls.rs b/rmf_site_editor/src/interaction/camera_controls.rs index 0350c610..bf13cfc8 100644 --- a/rmf_site_editor/src/interaction/camera_controls.rs +++ b/rmf_site_editor/src/interaction/camera_controls.rs @@ -78,6 +78,18 @@ impl ProjectionMode { } } +pub struct ChangeProjectionMode(pub ProjectionMode); + +impl ChangeProjectionMode { + pub fn to_perspective() -> ChangeProjectionMode { + ChangeProjectionMode(ProjectionMode::Perspective) + } + + pub fn to_orthographic() -> ChangeProjectionMode { + ChangeProjectionMode(ProjectionMode::Orthographic) + } +} + #[derive(Debug, Clone, Reflect, Resource)] pub struct CameraControls { mode: ProjectionMode, @@ -152,6 +164,23 @@ impl CameraControls { self.use_perspective(!choice, cameras, visibilities, headlights_on); } + pub fn use_mode( + &mut self, + mode: ProjectionMode, + cameras: &mut Query<&mut Camera>, + visibilities: &mut Query<&mut Visibility>, + headlights_on: bool, + ) { + match mode { + ProjectionMode::Perspective => { + self.use_perspective(true, cameras, visibilities, headlights_on); + } + ProjectionMode::Orthographic => { + self.use_orthographic(true, cameras, visibilities, headlights_on); + } + } + } + pub fn mode(&self) -> ProjectionMode { self.mode } @@ -329,10 +358,21 @@ fn camera_controls( mut previous_mouse_location: ResMut, mut controls: ResMut, mut cameras: Query<(&mut Projection, &mut Transform)>, + mut bevy_cameras: Query<&mut Camera>, mut visibility: Query<&mut Visibility>, headlight_toggle: Res, picking_blockers: Res, + mut change_mode: EventReader, ) { + if let Some(mode) = change_mode.iter().last() { + controls.use_mode( + mode.0, + &mut bevy_cameras, + &mut visibility, + headlight_toggle.0, + ); + } + if headlight_toggle.is_changed() { controls.toggle_lights(headlight_toggle.0, &mut visibility); } @@ -488,6 +528,7 @@ impl Plugin for CameraControlsPlugin { app.insert_resource(MouseLocation::default()) .init_resource::() .init_resource::() + .add_event::() .add_system(camera_controls); } } diff --git a/rmf_site_editor/src/interaction/highlight.rs b/rmf_site_editor/src/interaction/highlight.rs new file mode 100644 index 00000000..e9c66596 --- /dev/null +++ b/rmf_site_editor/src/interaction/highlight.rs @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2023 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::{interaction::*, site::DrawingMarker}; +use bevy::prelude::*; + +#[derive(Component)] +pub struct Highlight { + pub select: Color, + pub hover: Color, + pub hover_select: Color, +} + +#[derive(Component)] +pub struct SuppressHighlight; + +impl Highlight { + pub fn for_drawing() -> Self { + Self { + select: Color::rgb(1., 0.7, 1.), + hover: Color::rgb(0.7, 1., 1.), + hover_select: Color::rgb(1.0, 0.5, 0.7), + } + } +} + +pub fn add_highlight_visualization( + mut commands: Commands, + new_drawings: Query>, +) { + for e in &new_drawings { + commands.entity(e).insert(Highlight::for_drawing()); + } +} + +pub fn update_highlight_visualization( + highlightable: Query< + ( + &Hovered, + &Selected, + &Handle, + &Highlight, + Option<&SuppressHighlight>, + ), + Or<( + Changed, + Changed, + Changed, + Changed>, + )>, + >, + mut materials: ResMut>, +) { + for (hovered, selected, m, highlight, suppress) in &highlightable { + if let Some(material) = materials.get_mut(m) { + let mut color = if suppress.is_some() { + Color::WHITE + } else if hovered.cue() && selected.cue() { + highlight.hover_select + } else if hovered.cue() { + highlight.hover + } else if selected.cue() { + highlight.select + } else { + Color::WHITE + }; + color.set_a(material.base_color.a()); + + material.base_color = color; + } + } +} diff --git a/rmf_site_editor/src/interaction/lane.rs b/rmf_site_editor/src/interaction/lane.rs index 6f9e3816..4d9a7f0c 100644 --- a/rmf_site_editor/src/interaction/lane.rs +++ b/rmf_site_editor/src/interaction/lane.rs @@ -43,18 +43,18 @@ pub fn update_lane_visual_cues( site_assets: Res, cursor: Res, ) { - for (hovering, selected, pieces, mut tf) in &mut lanes { - if hovering.is_hovered { + for (hovered, selected, pieces, mut tf) in &mut lanes { + if hovered.is_hovered { set_visibility(cursor.frame, &mut visibility, false); } - let (m, h, v) = if hovering.cue() && selected.cue() { + let (m, h, v) = if hovered.cue() && selected.cue() { ( &site_assets.hover_select_material, HOVERED_LANE_OFFSET, true, ) - } else if hovering.cue() { + } else if hovered.cue() { (&site_assets.hover_material, HOVERED_LANE_OFFSET, true) } else if selected.cue() { (&site_assets.select_material, SELECTED_LANE_OFFSET, true) diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 95d88e68..5b064534 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -42,6 +42,9 @@ pub use edge::*; pub mod gizmo; pub use gizmo::*; +pub mod highlight; +pub use highlight::*; + pub mod lane; pub use lane::*; @@ -66,6 +69,9 @@ pub use picking::*; pub mod point; pub use point::*; +pub mod popup; +pub use popup::*; + pub mod preview; pub use preview::*; @@ -171,6 +177,7 @@ impl Plugin for InteractionPlugin { .with_system(handle_select_anchor_mode.after(maintain_selected_entities)) .with_system(handle_select_anchor_3d_mode.after(maintain_selected_entities)) .with_system(update_anchor_visual_cues.after(maintain_selected_entities)) + .with_system(update_popups.after(maintain_selected_entities)) .with_system(update_unassigned_anchor_cues) .with_system(update_anchor_cues_for_mode) .with_system(update_anchor_proximity_xray.after(update_cursor_transform)) @@ -181,6 +188,7 @@ impl Plugin for InteractionPlugin { .with_system(update_point_visual_cues.after(maintain_selected_entities)) .with_system(update_path_visual_cues.after(maintain_selected_entities)) .with_system(update_outline_visualization.after(maintain_selected_entities)) + .with_system(update_highlight_visualization.after(maintain_selected_entities)) .with_system( update_cursor_hover_visualization.after(maintain_selected_entities), ) @@ -207,8 +215,10 @@ impl Plugin for InteractionPlugin { .with_system(add_point_visual_cues) .with_system(add_path_visual_cues) .with_system(add_outline_visualization) + .with_system(add_highlight_visualization) .with_system(add_cursor_hover_visualization) - .with_system(add_physical_light_visual_cues), + .with_system(add_physical_light_visual_cues) + .with_system(add_popups), ) .add_system_set_to_stage( InteractionUpdateStage::ProcessVisuals, diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs index 4a3338f6..d404b8a4 100644 --- a/rmf_site_editor/src/interaction/outline.rs +++ b/rmf_site_editor/src/interaction/outline.rs @@ -15,7 +15,7 @@ * */ -use crate::interaction::*; +use crate::{interaction::*, site::DrawingMarker}; use bevy::render::view::RenderLayers; use bevy_mod_outline::{OutlineBundle, OutlineRenderLayers, OutlineVolume, SetOutlineDepth}; use rmf_site_format::{ @@ -105,6 +105,10 @@ impl OutlineVisualization { } } +/// Use this to temporarily prevent objects from being highlighted. +#[derive(Component)] +pub struct SuppressOutline; + pub fn add_outline_visualization( mut commands: Commands, new_entities: Query< @@ -117,6 +121,7 @@ pub fn add_outline_visualization( Added, Added, Added, + Added, Added, Added, Added, @@ -135,13 +140,27 @@ pub fn add_outline_visualization( pub fn update_outline_visualization( mut commands: Commands, outlinable: Query< - (Entity, &Hovered, &Selected, &OutlineVisualization), - Or<(Changed, Changed)>, + ( + Entity, + &Hovered, + &Selected, + &OutlineVisualization, + Option<&SuppressOutline>, + ), + Or<( + Changed, + Changed, + Changed, + )>, >, descendants: Query<(Option<&Children>, Option<&ComputedVisualCue>)>, ) { - for (e, hovered, selected, vis) in &outlinable { - let color = vis.color(hovered, selected); + for (e, hovered, selected, vis, suppress) in &outlinable { + let color = if suppress.is_some() { + None + } else { + vis.color(hovered, selected) + }; let layers = vis.layers(hovered, selected); let depth = vis.depth(); let root = vis.root().unwrap_or(e); diff --git a/rmf_site_editor/src/interaction/popup.rs b/rmf_site_editor/src/interaction/popup.rs new file mode 100644 index 00000000..914af825 --- /dev/null +++ b/rmf_site_editor/src/interaction/popup.rs @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2023 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::{interaction::*, site::*}; + +#[derive(Component, Clone, Copy)] +pub struct Popup { + regular: f32, + hovered: f32, + selected: f32, +} + +pub fn add_popups( + mut commands: Commands, + new_poppers: Query, Added)>>, +) { + for e in &new_poppers { + commands.entity(e).insert(Popup { + regular: 0., + hovered: HOVERED_LANE_OFFSET, + selected: SELECTED_LANE_OFFSET, + }); + } +} + +// TODO(@mxgrey): Merge this implementation with the popup implementation for +// lanes at some point. +pub fn update_popups( + mut objects: Query< + (&Hovered, &Selected, &Popup, &mut Transform), + Or<(Changed, Changed, Changed)>, + >, +) { + for (hovered, selected, popup, mut tf) in &mut objects { + if hovered.is_hovered { + tf.translation.z = popup.hovered; + } else if selected.cue() { + tf.translation.z = popup.selected; + } else { + tf.translation.z = popup.regular; + } + } +} diff --git a/rmf_site_editor/src/interaction/select_anchor.rs b/rmf_site_editor/src/interaction/select_anchor.rs index 4f75c9be..9b50881b 100644 --- a/rmf_site_editor/src/interaction/select_anchor.rs +++ b/rmf_site_editor/src/interaction/select_anchor.rs @@ -18,16 +18,17 @@ use crate::{ interaction::*, site::{ - Anchor, AnchorBundle, Category, Dependents, DrawingMarker, Original, PathBehavior, Pending, + drawing_editor::CurrentEditDrawing, Anchor, AnchorBundle, Category, Dependents, + DrawingMarker, Original, PathBehavior, Pending, }, CurrentWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; use rmf_site_format::{ Constraint, ConstraintDependents, Door, Edge, Fiducial, Floor, Lane, LiftProperties, Location, - Measurement, MeshConstraint, MeshElement, Model, ModelMarker, NameInWorkcell, Path, - PixelsPerMeter, Point, Pose, Side, SiteProperties, Wall, WorkcellCollisionMarker, - WorkcellModel, WorkcellVisualMarker, + Measurement, MeshConstraint, MeshElement, Model, ModelMarker, NameInWorkcell, NameOfSite, Path, + PixelsPerMeter, Point, Pose, Side, Wall, WorkcellCollisionMarker, WorkcellModel, + WorkcellVisualMarker, }; use std::collections::HashSet; use std::sync::Arc; @@ -588,9 +589,6 @@ impl Placement for EdgePlacement { params: &mut SelectAnchorPlacementParams<'w, 's>, ) -> Result { // Restore visibility to anchors that were hidden in this mode - for e in params.hidden_entities.selected_drawing_anchors.drain() { - set_visibility(e, &mut params.visibility, true); - } for e in params.hidden_entities.drawing_anchors.drain() { set_visibility(e, &mut params.visibility, true); } @@ -1056,11 +1054,6 @@ impl Placement for PathPlacement { #[derive(Resource, Default)] pub struct HiddenSelectAnchorEntities { - /// Level anchors but not assigned to a drawing, hidden when entering constraint creation mode - pub level_anchors: HashSet, - /// Anchors assigned to the the selected drawing, hidden when the user chose the first anchor - /// of a constraint to make sure it is drawn between two different drawings - pub selected_drawing_anchors: HashSet, /// All drawing anchors, hidden when users draw level entities such as walls, lanes, floors to /// make sure they don't connect to drawing anchors pub drawing_anchors: HashSet, @@ -1145,14 +1138,6 @@ impl<'w, 's> SelectAnchorPlacementParams<'w, 's> { Ok(()) } - fn get_visible_drawing(&self) -> Option<(Entity, &PixelsPerMeter)> { - self.drawings.iter().find(|(e, _)| { - self.visibility - .get(*e) - .is_ok_and(|vis| vis.is_visible == true) - }) - } - /// Use this when exiting SelectAnchor mode fn cleanup(&mut self) { self.cursor @@ -1169,12 +1154,6 @@ impl<'w, 's> SelectAnchorPlacementParams<'w, 's> { ); set_visibility(self.cursor.frame_placement, &mut self.visibility, false); self.cursor.set_model_preview(&mut self.commands, None); - for e in self.hidden_entities.level_anchors.drain() { - set_visibility(e, &mut self.visibility, true); - } - for e in self.hidden_entities.selected_drawing_anchors.drain() { - set_visibility(e, &mut self.visibility, true); - } for e in self.hidden_entities.drawing_anchors.drain() { set_visibility(e, &mut self.visibility, true); } @@ -1206,15 +1185,6 @@ impl SelectAnchorEdgeBuilder { } } - pub fn for_constraint(self) -> SelectAnchor { - SelectAnchor { - target: self.for_element, - placement: EdgePlacement::new::>(self.placement), - continuity: self.continuity, - scope: Scope::MultipleDrawings, - } - } - pub fn for_wall(self) -> SelectAnchor { SelectAnchor { target: self.for_element, @@ -1269,7 +1239,16 @@ impl SelectAnchorPointBuilder { } } - pub fn for_fiducial(self) -> SelectAnchor { + pub fn for_site_fiducial(self) -> SelectAnchor { + SelectAnchor { + target: self.for_element, + placement: PointPlacement::new::>(), + continuity: self.continuity, + scope: Scope::Site, + } + } + + pub fn for_drawing_fiducial(self) -> SelectAnchor { SelectAnchor { target: self.for_element, placement: PointPlacement::new::>(), @@ -1337,7 +1316,6 @@ type PlacementArc = Arc; #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] pub enum Scope { Drawing, - MultipleDrawings, General, Site, } @@ -1904,7 +1882,8 @@ pub fn handle_select_anchor_mode( mut hover: EventWriter, blockers: Option>, workspace: Res, - open_sites: Query>, + open_sites: Query>, + current_drawing: Res, ) { let mut request = match &*mode { InteractionMode::SelectAnchor(request) => request.clone(), @@ -1950,20 +1929,6 @@ pub fn handle_select_anchor_mode( } match request.scope { - Scope::MultipleDrawings => { - // If we are working with requests that span multiple drawings, - // (constraints) hide all non fiducials - for (e, _) in ¶ms.anchors { - if !params.dependents.get(e).is_ok_and(|deps| { - deps.0.iter().any(|dep| params.fiducials.get(*dep).is_ok()) - }) { - if let Ok(mut visibility) = params.visibility.get_mut(e) { - visibility.is_visible = false; - params.hidden_entities.level_anchors.insert(e); - } - } - } - } Scope::General | Scope::Site => { // If we are working with normal level or site requests, hide all drawing anchors for anchor in params.anchors.iter().filter(|(e, _)| { @@ -2073,9 +2038,14 @@ pub fn handle_select_anchor_mode( new_anchor } Scope::Drawing => { + let drawing_entity = current_drawing + .target() + .expect("No drawing while spawning drawing anchor") + .drawing; let (parent, ppm) = params - .get_visible_drawing() - .expect("No drawing while spawning drawing anchor"); + .drawings + .get(drawing_entity) + .expect("Entity being edited is not a drawing"); // We also need to have a transform such that the anchor will spawn in the // right spot let pose = compute_parent_inverse_pose(&tf, &transforms, parent); @@ -2084,14 +2054,10 @@ pub fn handle_select_anchor_mode( .commands .spawn(AnchorBundle::new([pose.trans[0], pose.trans[1]].into())) .insert(Transform::from_scale(Vec3::new(ppm, ppm, 1.0))) + .set_parent(parent) .id(); - params.commands.entity(parent).add_child(new_anchor); new_anchor } - Scope::MultipleDrawings => { - warn!("Only existing fiducials can be connected through constraints"); - return; - } Scope::General => params.commands.spawn(AnchorBundle::at_transform(tf)).id(), }; @@ -2143,25 +2109,7 @@ pub fn handle_select_anchor_mode( .filter(|s| anchors.contains(*s)) { request = match request.next(AnchorSelection::existing(new_selection), &mut params) { - Some(next_mode) => { - // We need to hide anchors connected to this drawing to force the user to - // connect different drawing - if matches!(request.scope, Scope::MultipleDrawings) { - params - .parents - .get(new_selection) - .and_then(|p| params.children.get(**p)) - .map(|children| { - for c in children.iter().filter(|c| params.anchors.get(**c).is_ok()) - { - set_visibility(*c, &mut params.visibility, false); - params.hidden_entities.selected_drawing_anchors.insert(*c); - } - }) - .ok(); - } - next_mode - } + Some(next_mode) => next_mode, None => { params.cleanup(); *mode = InteractionMode::Inspect; diff --git a/rmf_site_editor/src/keyboard.rs b/rmf_site_editor/src/keyboard.rs index d9a948e1..f8773463 100644 --- a/rmf_site_editor/src/keyboard.rs +++ b/rmf_site_editor/src/keyboard.rs @@ -18,9 +18,9 @@ use crate::{ interaction::{ camera_controls::{CameraControls, HeadlightToggle}, - ChangeMode, InteractionMode, Selection, + ChangeMode, ChangeProjectionMode, InteractionMode, Selection, }, - site::{AlignLevelDrawings, AlignSiteDrawings, CurrentLevel, Delete}, + site::{AlignSiteDrawings, CurrentLevel, Delete}, CreateNewWorkspace, CurrentWorkspace, LoadWorkspace, SaveWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; @@ -47,7 +47,6 @@ impl Plugin for KeyboardInputPlugin { // TODO(luca) get rid of this once 16 parameters limit is lifted in bevy 0.10 #[derive(SystemParam)] struct KeyboardParams<'w, 's> { - align_drawings: EventWriter<'w, 's, AlignLevelDrawings>, align_site: EventWriter<'w, 's, AlignSiteDrawings>, current_workspace: Res<'w, CurrentWorkspace>, } @@ -56,17 +55,14 @@ fn handle_keyboard_input( keyboard_input: Res>, selection: Res, current_mode: Res, - mut camera_controls: ResMut, - mut cameras: Query<&mut Camera>, - mut visibilities: Query<&mut Visibility>, mut egui_context: ResMut, mut change_mode: EventWriter, mut delete: EventWriter, mut save_workspace: EventWriter, mut new_workspace: EventWriter, mut load_workspace: EventWriter, + mut change_camera_mode: EventWriter, current_level: Res, - headlight_toggle: Res, mut debug_mode: ResMut, mut params: KeyboardParams, ) { @@ -80,11 +76,11 @@ fn handle_keyboard_input( } if keyboard_input.just_pressed(KeyCode::F2) { - camera_controls.use_orthographic(true, &mut cameras, &mut visibilities, headlight_toggle.0); + change_camera_mode.send(ChangeProjectionMode::to_orthographic()); } if keyboard_input.just_pressed(KeyCode::F3) { - camera_controls.use_perspective(true, &mut cameras, &mut visibilities, headlight_toggle.0); + change_camera_mode.send(ChangeProjectionMode::to_perspective()); } if keyboard_input.just_pressed(KeyCode::Escape) { @@ -117,14 +113,8 @@ fn handle_keyboard_input( } if keyboard_input.just_pressed(KeyCode::T) { - if keyboard_input.any_pressed([KeyCode::LShift, KeyCode::RShift]) { - if let Some(site) = params.current_workspace.root { - params.align_site.send(AlignSiteDrawings(site)); - } - } else { - if let Some(level) = **current_level { - params.align_drawings.send(AlignLevelDrawings(level)); - } + if let Some(site) = params.current_workspace.root { + params.align_site.send(AlignSiteDrawings(site)); } } diff --git a/rmf_site_editor/src/occupancy.rs b/rmf_site_editor/src/occupancy.rs index 1af75c53..877fe3c0 100644 --- a/rmf_site_editor/src/occupancy.rs +++ b/rmf_site_editor/src/occupancy.rs @@ -18,7 +18,7 @@ use crate::{ interaction::ComputedVisualCue, shapes::*, - site::{Category, LevelProperties, SiteAssets, SiteProperties, LANE_LAYER_START}, + site::{Category, LevelElevation, NameOfSite, SiteAssets, LANE_LAYER_START}, }; use bevy::{ math::{swizzles::*, Affine3A, Mat3A, Vec2, Vec3A}, @@ -156,8 +156,8 @@ fn calculate_grid( Option<&ComputedVisualCue>, )>, parents: Query<&Parent>, - levels: Query>, - sites: Query<(), With>, + levels: Query>, + sites: Query<(), With>, mut meshes: ResMut>, assets: Res, grids: Query>, @@ -296,7 +296,7 @@ fn calculate_grid( } fn get_levels_of_sites( - levels: &Query>, + levels: &Query>, parents: &Query<&Parent>, ) -> HashMap> { let mut levels_of_sites: HashMap> = HashMap::new(); @@ -312,8 +312,8 @@ fn get_levels_of_sites( fn get_group( e: Entity, parents: &Query<&Parent>, - levels: &Query>, - sites: &Query<(), With>, + levels: &Query>, + sites: &Query<(), With>, ) -> Group { let mut e_meta = e; loop { diff --git a/rmf_site_editor/src/site/anchor.rs b/rmf_site_editor/src/site/anchor.rs index 3ec59880..10d4ddd3 100644 --- a/rmf_site_editor/src/site/anchor.rs +++ b/rmf_site_editor/src/site/anchor.rs @@ -162,8 +162,9 @@ pub fn assign_orphan_anchors_to_parent( // No level is currently assigned, so we should create one. let new_level_id = commands .spawn(LevelProperties { - name: "".to_string(), - elevation: 0., + name: NameInSite("".to_owned()), + elevation: LevelElevation(0.), + ..default() }) .insert(Category::Level) .id(); diff --git a/rmf_site_editor/src/site/assets.rs b/rmf_site_editor/src/site/assets.rs index 224d67ae..3ec8078d 100644 --- a/rmf_site_editor/src/site/assets.rs +++ b/rmf_site_editor/src/site/assets.rs @@ -193,16 +193,18 @@ impl FromWorld for SiteAssets { ); let location_mesh = meshes.add( Mesh::from( - make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 6) - .transform_by(Affine3A::from_translation(0.00125 * Vec3::Z)), + make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 6).transform_by( + Affine3A::from_translation((0.00125 + LOCATION_LAYER_HEIGHT) * Vec3::Z), + ), ) .with_generated_outline_normals() .unwrap(), ); let fiducial_mesh = meshes.add( Mesh::from( - make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 4) - .transform_by(Affine3A::from_translation(0.00125 * Vec3::Z)), + make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 4).transform_by( + Affine3A::from_translation((0.00125 + LOCATION_LAYER_HEIGHT) * Vec3::Z), + ), ) .with_generated_outline_normals() .unwrap(), diff --git a/rmf_site_editor/src/site/constraint.rs b/rmf_site_editor/src/site/constraint.rs index 666bde80..0a2e0dbd 100644 --- a/rmf_site_editor/src/site/constraint.rs +++ b/rmf_site_editor/src/site/constraint.rs @@ -37,8 +37,8 @@ pub fn assign_orphan_constraints_to_parent( constraints: Query<(Entity, &Edge), (Without, With)>, current_workspace: Res, parents: Query<&Parent>, - levels: Query>, - open_sites: Query>, + levels: Query>, + open_sites: Query>, ) { if let Some(current_site) = current_workspace.to_site(&open_sites) { for (e, edge) in &constraints { @@ -164,7 +164,7 @@ pub fn update_constraint_for_changed_labels( dependents: Query<&Dependents>, fiducials: Query>, constraints: Query<(Entity, &Edge, &Parent), With>, - open_sites: Query>, + open_sites: Query>, ) { let get_fiducial_label = |e: Entity| -> Option<&Label> { let fiducial = dependents diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index a0305e33..35436066 100644 --- a/rmf_site_editor/src/site/deletion.rs +++ b/rmf_site_editor/src/site/deletion.rs @@ -18,7 +18,10 @@ use crate::{ interaction::{Select, Selection}, log::Log, - site::{Category, CurrentLevel, Dependents, LevelProperties, SiteUpdateStage}, + site::{ + Category, CurrentLevel, Dependents, LevelElevation, LevelProperties, NameInSite, + SiteUpdateStage, + }, }; use bevy::{ecs::system::SystemParam, prelude::*}; use rmf_site_format::{ConstraintDependents, Edge, MeshConstraint, Path, Point}; @@ -85,7 +88,7 @@ struct DeletionParams<'w, 's> { children: Query<'w, 's, &'static Children>, selection: Res<'w, Selection>, current_level: ResMut<'w, CurrentLevel>, - levels: Query<'w, 's, Entity, With>, + levels: Query<'w, 's, Entity, With>, select: EventWriter<'w, 's, Select>, log: EventWriter<'w, 's, Log>, } @@ -345,8 +348,9 @@ fn perform_deletions(all_to_delete: HashSet, params: &mut DeletionParams .commands .spawn(SpatialBundle::default()) .insert(LevelProperties { - elevation: 0.0, - name: "".to_string(), + name: NameInSite("".to_owned()), + elevation: LevelElevation(0.0), + ..default() }) .insert(Category::Level) .id(); diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index 44053297..2d1f0711 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -19,25 +19,20 @@ use crate::{ interaction::Selectable, shapes::make_flat_rect_mesh, site::{ - get_current_workspace_path, Anchor, DefaultFile, FiducialMarker, GlobalFloorVisibility, + get_current_workspace_path, Anchor, DefaultFile, FiducialMarker, GlobalDrawingVisibility, LayerVisibility, MeasurementMarker, MeasurementSegment, RecencyRank, DEFAULT_MEASUREMENT_OFFSET, FLOOR_LAYER_START, }, CurrentWorkspace, }; use bevy::{asset::LoadState, math::Affine3A, prelude::*}; -use rmf_site_format::{ - AssetSource, Category, Drawing, IsPrimary, NameInSite, PixelsPerMeter, Pose, -}; +use rmf_site_format::{AssetSource, Category, DrawingProperties, NameInSite, PixelsPerMeter, Pose}; +use std::path::PathBuf; #[derive(Bundle, Debug, Clone)] pub struct DrawingBundle { + pub properties: DrawingProperties, pub category: Category, - pub name: NameInSite, - pub source: AssetSource, - pub pose: Pose, - pub pixels_per_meter: PixelsPerMeter, - pub is_primary: IsPrimary, pub transform: Transform, pub global_transform: GlobalTransform, pub visibility: Visibility, @@ -46,19 +41,15 @@ pub struct DrawingBundle { } impl DrawingBundle { - pub fn new(drawing: &Drawing) -> Self { + pub fn new(properties: DrawingProperties) -> Self { DrawingBundle { + properties, category: Category::Drawing, - name: drawing.name.clone(), - source: drawing.source.clone(), - pose: drawing.pose.clone(), - pixels_per_meter: drawing.pixels_per_meter.clone(), - is_primary: drawing.is_primary.clone(), - transform: Transform::IDENTITY, - global_transform: GlobalTransform::IDENTITY, - visibility: Visibility::VISIBLE, - computed: ComputedVisibility::default(), - marker: DrawingMarker::default(), + transform: default(), + global_transform: default(), + visibility: default(), + computed: default(), + marker: default(), } } } @@ -66,24 +57,8 @@ impl DrawingBundle { #[derive(Component, Clone, Copy, Debug, Default)] pub struct DrawingMarker; -#[derive(Debug, Clone, Copy, Default, Deref, DerefMut, Resource)] -pub struct GlobalDrawingVisibility(pub LayerVisibility); - pub const DRAWING_LAYER_START: f32 = 0.0; -// Semi transparency for drawings, more opaque than floors to make them visible -const DEFAULT_DRAWING_SEMI_TRANSPARENCY: f32 = 0.5; - -/// Resource used to set what the alpha value for partially transparent drawings should be -#[derive(Clone, Resource, Deref, DerefMut)] -pub struct DrawingSemiTransparency(f32); - -impl Default for DrawingSemiTransparency { - fn default() -> Self { - DrawingSemiTransparency(DEFAULT_DRAWING_SEMI_TRANSPARENCY) - } -} - #[derive(Debug, Clone, Copy, Component)] pub struct DrawingSegments { leaf: Entity, @@ -101,24 +76,24 @@ fn drawing_layer_height(rank: Option<&RecencyRank>) -> f32 { pub fn add_drawing_visuals( mut commands: Commands, - changed_drawings: Query< - (Entity, &AssetSource, &IsPrimary), - (With, Changed), - >, + changed_drawings: Query<(Entity, &AssetSource), (With, Changed)>, asset_server: Res, current_workspace: Res, site_files: Query<&DefaultFile>, - mut default_floor_vis: ResMut, - drawing_transparency: Res, + default_drawing_vis: Query<&GlobalDrawingVisibility>, ) { + if changed_drawings.is_empty() { + return; + } + // TODO(luca) depending on when this system is executed, this function might be called between // the creation of the drawing and the change of the workspace, making this silently fail // Look into reordering systems, or adding a marker component, to make sure this doesn't happen let file_path = match get_current_workspace_path(current_workspace, site_files) { Some(file_path) => file_path, - None => return, + None => PathBuf::new(), }; - for (e, source, is_primary) in &changed_drawings { + for (e, source) in &changed_drawings { // Append file name to path if it's a local file // TODO(luca) cleanup let asset_source = match source { @@ -128,19 +103,7 @@ pub fn add_drawing_visuals( _ => source.clone(), }; let texture_handle: Handle = asset_server.load(&String::from(&asset_source)); - let visibility = if is_primary.0 == true { - LayerVisibility::Opaque - } else { - LayerVisibility::Alpha(**drawing_transparency) - }; - commands - .entity(e) - .insert(LoadingDrawing(texture_handle)) - .insert(visibility); - } - - if !changed_drawings.is_empty() { - **default_floor_vis = LayerVisibility::new_semi_transparent(); + commands.entity(e).insert(LoadingDrawing(texture_handle)); } } @@ -155,15 +118,18 @@ pub fn handle_loaded_drawing( &PixelsPerMeter, &LoadingDrawing, Option<&LayerVisibility>, + Option<&Parent>, + Option<&RecencyRank>, )>, mut mesh_assets: ResMut>, asset_server: Res, mut materials: ResMut>, - rank: Query<&RecencyRank>, segments: Query<&DrawingSegments>, - default_drawing_vis: Res, + default_drawing_vis: Query<&GlobalDrawingVisibility>, ) { - for (entity, source, pose, pixels_per_meter, handle, vis) in loading_drawings.iter() { + for (entity, source, pose, pixels_per_meter, handle, vis, parent, rank) in + loading_drawings.iter() + { match asset_server.get_load_state(&handle.0) { LoadState::Loaded => { let img = assets.get(&handle.0).unwrap(); @@ -175,6 +141,16 @@ pub fn handle_loaded_drawing( Affine3A::from_translation(Vec3::new(width / 2.0, -height / 2.0, 0.0)), ); let mesh = mesh_assets.add(mesh.into()); + let default = parent + .map(|p| default_drawing_vis.get(p.get()).ok()) + .flatten(); + let (alpha, alpha_mode) = drawing_alpha(vis, rank, default); + let material = materials.add(StandardMaterial { + base_color_texture: Some(handle.0.clone()), + base_color: *Color::default().set_a(alpha), + alpha_mode, + ..Default::default() + }); let leaf = if let Ok(segment) = segments.get(entity) { segment.leaf @@ -191,20 +167,22 @@ pub fn handle_loaded_drawing( .insert(Selectable::new(entity)); leaf }; - let z = drawing_layer_height(rank.get(entity).ok()); - let (alpha, alpha_mode) = drawing_alpha(vis, &default_drawing_vis); - commands.entity(leaf).insert(PbrBundle { - mesh, - material: materials.add(StandardMaterial { - base_color_texture: Some(handle.0.clone()), - base_color: *Color::default().set_a(alpha), - alpha_mode, - ..default() - }), - transform: Transform::from_xyz(0.0, 0.0, z), - ..default() - }); - commands.entity(entity).remove::(); + let z = drawing_layer_height(rank); + commands + .entity(leaf) + .insert(PbrBundle { + mesh, + material: material.clone(), + transform: Transform::from_xyz(0.0, 0.0, z), + ..Default::default() + }) + .insert(Selectable::new(entity)); + commands + .entity(entity) + // Put a handle for the material into the main entity + // so that we can modify it during interactions. + .insert(material) + .remove::(); } LoadState::Failed => { error!("Failed loading drawing {:?}", String::from(source)); @@ -257,7 +235,10 @@ pub fn update_drawing_pixels_per_meter( pub fn update_drawing_children_to_pixel_coordinates( mut commands: Commands, - changed_drawings: Query<(&PixelsPerMeter, &Children), Changed>, + changed_drawings: Query< + (&PixelsPerMeter, &Children), + Or<(Changed, Changed)>, + >, meshes: Query, With, With)>>, mut transforms: Query<&mut Transform>, ) { @@ -278,11 +259,28 @@ pub fn update_drawing_children_to_pixel_coordinates( } } +#[inline] fn drawing_alpha( specific: Option<&LayerVisibility>, - general: &LayerVisibility, + rank: Option<&RecencyRank>, + general: Option<&GlobalDrawingVisibility>, ) -> (f32, AlphaMode) { - let alpha = specific.map(|s| s.alpha()).unwrap_or(general.alpha()); + let alpha = specific + .copied() + .unwrap_or_else(|| { + general + .map(|v| { + if let Some(r) = rank { + if r.rank() < v.bottom_count { + return v.bottom; + } + } + v.general + }) + .unwrap_or(LayerVisibility::Opaque) + }) + .alpha(); + let alpha_mode = if alpha < 1.0 { AlphaMode::Blend } else { @@ -291,16 +289,27 @@ fn drawing_alpha( (alpha, alpha_mode) } +#[inline] fn iter_update_drawing_visibility<'a>( - iter: impl Iterator, &'a DrawingSegments)>, + iter: impl Iterator< + Item = ( + Option<&'a LayerVisibility>, + Option<&'a Parent>, + Option<&'a RecencyRank>, + &'a DrawingSegments, + ), + >, material_handles: &Query<&Handle>, material_assets: &mut ResMut>, - default_drawing_vis: &LayerVisibility, + default_drawing_vis: &Query<&GlobalDrawingVisibility>, ) { - for (vis, segments) in iter { + for (vis, parent, rank, segments) in iter { if let Ok(handle) = material_handles.get(segments.leaf) { if let Some(mat) = material_assets.get_mut(handle) { - let (alpha, alpha_mode) = drawing_alpha(vis, &default_drawing_vis); + let default = parent + .map(|p| default_drawing_vis.get(p.get()).ok()) + .flatten(); + let (alpha, alpha_mode) = drawing_alpha(vis, rank, default); mat.base_color = *mat.base_color.set_a(alpha); mat.alpha_mode = alpha_mode; } @@ -309,35 +318,49 @@ fn iter_update_drawing_visibility<'a>( } // TODO(luca) RemovedComponents is brittle, maybe wrap component in an option? -// TODO(luca) This is copy-pasted from floor.rs, consider having a generic plugin pub fn update_drawing_visibility( - changed_floors: Query<(Option<&LayerVisibility>, &DrawingSegments), Changed>, + changed_drawings: Query< + Entity, + Or<( + Changed, + Changed, + Changed>, + )>, + >, removed_vis: RemovedComponents, - all_floors: Query<(Option<&LayerVisibility>, &DrawingSegments)>, + all_drawings: Query<( + Option<&LayerVisibility>, + Option<&Parent>, + Option<&RecencyRank>, + &DrawingSegments, + )>, material_handles: Query<&Handle>, mut material_assets: ResMut>, - default_drawing_vis: Res, + default_drawing_vis: Query<&GlobalDrawingVisibility>, + changed_default_drawing_vis: Query<&Children, Changed>, ) { - if default_drawing_vis.is_changed() { - iter_update_drawing_visibility( - all_floors.iter(), - &material_handles, - &mut material_assets, - &default_drawing_vis, - ); - } else { - iter_update_drawing_visibility( - changed_floors.iter(), - &material_handles, - &mut material_assets, - &default_drawing_vis, - ); + iter_update_drawing_visibility( + changed_drawings + .iter() + .filter_map(|e| all_drawings.get(e).ok()), + &material_handles, + &mut material_assets, + &default_drawing_vis, + ); + + iter_update_drawing_visibility( + removed_vis.iter().filter_map(|e| all_drawings.get(e).ok()), + &material_handles, + &mut material_assets, + &default_drawing_vis, + ); + for children in &changed_default_drawing_vis { iter_update_drawing_visibility( - removed_vis.iter().filter_map(|e| all_floors.get(e).ok()), + children.iter().filter_map(|e| all_drawings.get(*e).ok()), &material_handles, &mut material_assets, &default_drawing_vis, ); - }; + } } diff --git a/rmf_site_editor/src/site/drawing_editor/alignment.rs b/rmf_site_editor/src/site/drawing_editor/alignment.rs new file mode 100644 index 00000000..a69d8cae --- /dev/null +++ b/rmf_site_editor/src/site/drawing_editor/alignment.rs @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2023 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 bevy::ecs::system::SystemParam; +use bevy::{ + math::{DVec2, Vec2}, + prelude::*, +}; + +use crate::site::{ + Affiliation, AlignSiteDrawings, Anchor, Angle, Category, Change, ConstraintMarker, Distance, + DrawingMarker, Edge, FiducialMarker, LevelElevation, MeasurementMarker, NameOfSite, + PixelsPerMeter, Point, Pose, Rotation, SiteProperties, +}; +use itertools::{Either, Itertools}; +use rmf_site_format::alignment::{ + align_site, DrawingVariables, FiducialVariables, MeasurementVariables, SiteVariables, +}; +use std::collections::HashSet; + +#[derive(SystemParam)] +pub struct OptimizationParams<'w, 's> { + drawings: Query< + 'w, + 's, + ( + &'static Children, + &'static mut Pose, + &'static mut PixelsPerMeter, + ), + With, + >, + anchors: Query<'w, 's, &'static Anchor>, + fiducials: + Query<'w, 's, (&'static Affiliation, &'static Point), With>, + measurements: + Query<'w, 's, (&'static Edge, &'static Distance), With>, +} + +pub fn align_site_drawings( + levels: Query<&Children, With>, + sites: Query<&Children, With>, + mut events: EventReader, + mut params: OptimizationParams, +) { + for AlignSiteDrawings(site) in events.iter() { + let mut site_variables = SiteVariables::::default(); + let Ok(children) = sites.get(*site) else { continue }; + for child in children { + let Ok((group, point)) = params.fiducials.get(*child) else { continue }; + let Ok(anchor) = params.anchors.get(point.0) else { continue }; + let Some(group) = group.0 else { continue }; + let p = anchor.translation_for_category(Category::Fiducial); + site_variables.fiducials.push(FiducialVariables { + group, + position: DVec2::new(p[0] as f64, p[1] as f64), + }); + } + + for child in children { + let Ok(level_children) = levels.get(*child) else { continue }; + for level_child in level_children { + let Ok((drawing_children, pose, ppm)) = params.drawings.get(*level_child) else { continue }; + let mut drawing_variables = DrawingVariables::::new( + Vec2::from_slice(&pose.trans).as_dvec2(), + pose.rot.yaw().radians() as f64, + (1.0 / ppm.0) as f64, + ); + for child in drawing_children { + if let Ok((group, point)) = params.fiducials.get(*child) { + let Ok(anchor) = params.anchors.get(point.0) else { continue }; + let Some(group) = group.0 else { continue }; + let p = anchor.translation_for_category(Category::Fiducial); + drawing_variables.fiducials.push(FiducialVariables { + group, + position: DVec2::new(p[0] as f64, p[1] as f64), + }); + } + + if let Ok((edge, distance)) = params.measurements.get(*child) { + let Ok([anchor0, anchor1]) = params.anchors.get_many(edge.array()) else { continue }; + let Some(in_meters) = distance.0 else { continue }; + let in_meters = in_meters as f64; + let p0 = + Vec2::from_slice(anchor0.translation_for_category(Category::Fiducial)); + let p1 = + Vec2::from_slice(anchor1.translation_for_category(Category::Fiducial)); + let in_pixels = (p1 - p0).length() as f64; + drawing_variables.measurements.push(MeasurementVariables { + in_pixels, + in_meters, + }); + } + } + + site_variables + .drawings + .insert(*level_child, drawing_variables); + } + } + + // TODO(@mxgrey): When we implement an undo buffer, remember to make an + // undo operation for this set of changes. + let alignments = align_site(&site_variables); + for (e, alignment) in alignments { + let Ok((_, mut pose, mut ppm)) = params.drawings.get_mut(e) else { continue }; + pose.trans[0] = alignment.translation.x as f32; + pose.trans[1] = alignment.translation.y as f32; + pose.rot = + Rotation::Yaw(Angle::Rad(alignment.rotation as f32).match_variant(pose.rot.yaw())); + ppm.0 = 1.0 / alignment.scale as f32; + } + } +} diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs index e19b1e29..0be419f2 100644 --- a/rmf_site_editor/src/site/drawing_editor/mod.rs +++ b/rmf_site_editor/src/site/drawing_editor/mod.rs @@ -15,174 +15,251 @@ * */ -use bevy::prelude::*; +use bevy::{prelude::*, render::view::visibility::RenderLayers}; -pub mod optimizer; -pub use optimizer::*; +pub mod alignment; +pub use alignment::*; -use crate::interaction::{CameraControls, HeadlightToggle, Selection}; -use crate::site::{ - Anchor, DrawingMarker, Edge, FiducialMarker, MeasurementMarker, Pending, PixelsPerMeter, Point, +use crate::AppState; +use crate::{ + interaction::{ChangeProjectionMode, Selection, SuppressHighlight, SuppressOutline}, + site::{ + Anchor, DrawingMarker, Edge, FiducialMarker, MeasurementMarker, NameOfSite, Pending, + PixelsPerMeter, Point, PreventDeletion, SiteProperties, WorkcellProperties, + }, + CurrentWorkspace, WorkspaceMarker, }; -use crate::{AppState, VisibilityEvents}; use std::collections::HashSet; -#[derive(Default)] -pub struct DrawingEditorPlugin; +#[derive(Clone, Copy)] +pub struct BeginEditDrawing(pub Entity); -#[derive(Resource, Default, Deref, DerefMut)] -pub struct DrawingEditorHiddenEntities(HashSet); +/// Command to finish editing a drawing. Use None to command any drawing to finish. +#[derive(Clone, Copy)] +pub struct FinishEditDrawing(pub Option); -// TODO(luca) should these events be defined somewhere else? -#[derive(Deref, DerefMut)] -pub struct ScaleDrawing(pub Entity); +#[derive(Clone, Copy)] +pub struct EditDrawing { + /// What drawing is being edited + pub drawing: Entity, + /// What is the original parent level for the drawing + pub level: Entity, +} -#[derive(Deref, DerefMut)] -pub struct AlignLevelDrawings(pub Entity); +#[derive(Clone, Copy, Resource)] +pub struct CurrentEditDrawing { + editor: Entity, + target: Option, +} + +impl FromWorld for CurrentEditDrawing { + fn from_world(world: &mut World) -> Self { + let editor = world.spawn(SpatialBundle::default()).id(); + Self { + editor, + target: None, + } + } +} + +impl CurrentEditDrawing { + pub fn target(&self) -> &Option { + &self.target + } +} + +#[derive(Default)] +pub struct DrawingEditorPlugin; #[derive(Deref, DerefMut)] pub struct AlignSiteDrawings(pub Entity); -fn hide_level_entities( - mut visibilities: Query<&mut Visibility>, - mut camera_controls: ResMut, - mut cameras: Query<&mut Camera>, - headlight_toggle: Res, - mut visibility_events: VisibilityEvents, +fn switch_edit_drawing_mode( + mut commands: Commands, + mut begin: EventReader, + mut finish: EventReader, + mut current: ResMut, + mut workspace_visibility: Query<&mut Visibility, With>, + mut app_state: ResMut>, + mut local_tf: Query<&mut Transform>, + mut change_camera_mode: EventWriter, + global_tf: Query<&GlobalTransform>, + current_workspace: Res, + parent: Query<&Parent, With>, + is_site: Query<(), With>, + is_workcell: Query<(), With>, ) { - camera_controls.use_orthographic(true, &mut cameras, &mut visibilities, headlight_toggle.0); - visibility_events.constraints.send(false.into()); - visibility_events.doors.send(false.into()); - visibility_events.lanes.send(false.into()); - visibility_events.lift_cabins.send(false.into()); - visibility_events.lift_cabin_doors.send(false.into()); - visibility_events.locations.send(false.into()); - visibility_events.floors.send(false.into()); - visibility_events.models.send(false.into()); - visibility_events.walls.send(false.into()); -} + // TODO(@mxgrey): We can make this implementation much cleaner after we + // update to the latest version of bevy that distinguishes between inherited + // vs independent visibility. + // + // We should also consider using an edit mode stack instead of simply + // CurrentWorkspace and AppState. + 'handle_begin: { + if let Some(BeginEditDrawing(e)) = begin.iter().last() { + if current.target().is_some_and(|c| c.drawing == *e) { + break 'handle_begin; + } -fn hide_non_drawing_entities( - mut anchors: Query<(Entity, &mut Visibility), (With, Without)>, - parents: Query<&Parent>, - mut drawings: Query<(Entity, &mut Visibility), (Without, With)>, - mut anchor_set: ResMut, - selection: Res, -) { - for (e, mut vis) in &mut anchors { - if let Ok(parent) = parents.get(e) { - if drawings.get(**parent).is_err() { - if vis.is_visible { - vis.is_visible = false; - anchor_set.insert(e); + if let Some(c) = current.target() { + // A drawing was being edited and now we're switching to a + // different drawing, so we need to reset the previous drawing. + restore_edited_drawing(c, &mut commands); + } + + let level = if let Ok(p) = parent.get(*e) { + p.get() + } else { + error!("Cannot edit {e:?} as a drawing"); + current.target = None; + break 'handle_begin; + }; + + current.target = Some(EditDrawing { drawing: *e, level }); + commands + .entity(*e) + .set_parent(current.editor) + .insert(Visibility { is_visible: true }) + .insert(ComputedVisibility::default()) + .insert(PreventDeletion::because( + "Cannot delete a drawing that is currently being edited".to_owned(), + )) + // Highlighting the drawing looks bad when the user will be + // constantly hovering over it anyway. + .insert(SuppressHighlight); + + change_camera_mode.send(ChangeProjectionMode::to_orthographic()); + + if let Ok(mut editor_tf) = local_tf.get_mut(current.editor) { + if let Ok(mut level_tf) = global_tf.get(level) { + *editor_tf = level_tf.compute_transform(); + } else { + error!("Cannot get transform of current level"); } + } else { + error!("Cannot change transform of drawing editor view"); } - } - } - for (e, mut vis) in &mut drawings { - if **selection != Some(e) { - if vis.is_visible { - vis.is_visible = false; - anchor_set.insert(e); + + if let Some(err) = app_state.set(AppState::SiteDrawingEditor).err() { + error!("Unable to switch to drawing editor mode: {err:?}"); + } + + for mut v in &mut workspace_visibility { + v.is_visible = false; } } } -} -fn restore_non_drawing_entities( - mut visibilities: Query<&mut Visibility>, - mut anchor_set: ResMut, -) { - for e in anchor_set.drain() { - visibilities - .get_mut(e) - .map(|mut vis| vis.is_visible = true) - .ok(); + for FinishEditDrawing(finish) in finish.iter() { + let c = if let Some(c) = current.target() { + if finish.is_some_and(|e| e != c.drawing) { + continue; + } + c + } else { + continue; + }; + + restore_edited_drawing(c, &mut commands); + current.target = None; + + // This camera change would not be needed if we have an edit mode stack + change_camera_mode.send(ChangeProjectionMode::to_perspective()); + + if let Some(w) = current_workspace.root { + if let Ok(mut v) = workspace_visibility.get_mut(w) { + v.is_visible = current_workspace.display; + } + + if is_site.contains(w) { + if let Some(err) = app_state.set(AppState::SiteEditor).err() { + error!("Failed to switch back to site editing mode: {err:?}"); + } + } else if is_workcell.contains(w) { + if let Some(err) = app_state.set(AppState::WorkcellEditor).err() { + error!("Failed to switch back to workcell editing mode: {err:?}"); + } + } else { + // This logic can probably be improved with an editor mode stack + error!( + "Unable to identify the type for the current workspace \ + {w:?}, so we will default to site editing mode", + ); + if let Some(err) = app_state.set(AppState::SiteEditor).err() { + error!("Failed to switch back to site editing mode: {err:?}"); + } + } + } } } -fn restore_level_entities( - mut visibilities: Query<&mut Visibility>, - mut camera_controls: ResMut, - mut cameras: Query<&mut Camera>, - headlight_toggle: Res, - mut visibility_events: VisibilityEvents, -) { - camera_controls.use_perspective(true, &mut cameras, &mut visibilities, headlight_toggle.0); - visibility_events.constraints.send(true.into()); - visibility_events.doors.send(true.into()); - visibility_events.lanes.send(true.into()); - visibility_events.lift_cabins.send(true.into()); - visibility_events.lift_cabin_doors.send(true.into()); - visibility_events.locations.send(true.into()); - visibility_events.floors.send(true.into()); - visibility_events.models.send(true.into()); - visibility_events.walls.send(true.into()); +/// Restore a drawing that was being edited back to its normal place and behavior +fn restore_edited_drawing(edit: &EditDrawing, commands: &mut Commands) { + commands + .entity(edit.drawing) + .set_parent(edit.level) + .remove::() + .remove::(); } -fn assign_drawing_parent_to_new_measurements_and_fiducials( +fn assign_drawing_parent_to_new_measurements( mut commands: Commands, - mut new_elements: Query< - (Entity, Option<&Parent>, &mut Transform), + mut changed_measurement: Query< + (Entity, &Edge, Option<&Parent>), ( Without, - Or<( - (With, Changed>), - (Changed>, With), - )>, + (With, Changed>), ), >, - drawings: Query<(Entity, &Visibility, &PixelsPerMeter), With>, + parents: Query<&Parent>, ) { - if new_elements.is_empty() { - return; - } - let (parent, ppm) = match drawings.iter().find(|(_, vis, _)| vis.is_visible == true) { - Some(parent) => (parent.0, parent.2), - None => return, - }; - for (e, old_parent, mut tf) in &mut new_elements { - if old_parent.map(|p| drawings.get(**p).ok()).is_none() { - commands.entity(parent).add_child(e); - // Set its scale to the parent's pixels per meter to make it in pixel coordinates - tf.scale = Vec3::new(ppm.0, ppm.0, 1.0); + for (e, edge, mut tf) in &mut changed_measurement { + if let (Ok(p0), Ok(p1)) = (parents.get(edge.left()), parents.get(edge.right())) { + if p0.get() != p1.get() { + commands.entity(e).set_parent(p0.get()); + } else { + warn!( + "Mismatch in parents of anchors for measurement {e:?}: {:?}, {:?}", + p0, p1 + ); + } + } else { + warn!( + "Missing parents of anchors for measurement {e:?}: {:?}, {:?}", + parents.get(edge.left()), + parents.get(edge.right()), + ); } } } fn make_drawing_default_selected( - drawings: Query<(Entity, &Visibility), With>, mut selection: ResMut, + current: Res, ) { if selection.is_changed() { if selection.0.is_none() { - if let Some(drawing) = drawings.iter().find(|(_, vis)| vis.is_visible == true) { - selection.0 = Some(drawing.0); - } + let drawing_entity = current + .target() + .expect("No drawing while spawning drawing anchor") + .drawing; + selection.0 = Some(drawing_entity); } } } impl Plugin for DrawingEditorPlugin { fn build(&self, app: &mut App) { - app.add_event::() - .add_system_set( - SystemSet::on_enter(AppState::SiteDrawingEditor) - .with_system(hide_level_entities) - .with_system(hide_non_drawing_entities), - ) - .add_system_set( - SystemSet::on_exit(AppState::SiteDrawingEditor) - .with_system(restore_level_entities) - .with_system(restore_non_drawing_entities), - ) + app.add_event::() + .add_event::() + .add_event::() + .init_resource::() + .add_system(switch_edit_drawing_mode) .add_system_set( SystemSet::on_update(AppState::SiteDrawingEditor) - .with_system(assign_drawing_parent_to_new_measurements_and_fiducials) - .with_system(scale_drawings) + .with_system(assign_drawing_parent_to_new_measurements) .with_system(make_drawing_default_selected), - ) - .init_resource::(); + ); } } diff --git a/rmf_site_editor/src/site/drawing_editor/optimizer.rs b/rmf_site_editor/src/site/drawing_editor/optimizer.rs deleted file mode 100644 index 362ab791..00000000 --- a/rmf_site_editor/src/site/drawing_editor/optimizer.rs +++ /dev/null @@ -1,375 +0,0 @@ -/* - * Copyright (C) 2023 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 bevy::ecs::system::SystemParam; -use bevy::prelude::*; - -use crate::site::{ - AlignLevelDrawings, AlignSiteDrawings, Anchor, Angle, Category, Change, ConstraintMarker, - Distance, DrawingMarker, Edge, IsPrimary, LevelProperties, MeasurementMarker, PixelsPerMeter, - Pose, Rotation, ScaleDrawing, SiteProperties, -}; -use itertools::{Either, Itertools}; -use optimization_engine::{panoc::*, *}; -use std::collections::HashSet; - -// Simple optimization purely based on measurement scale, used to transform between pixel and -// cartesian coordinates -pub fn scale_drawings( - mut drawings: Query<(&Children, &mut PixelsPerMeter), With>, - measurements: Query<(&Edge, &Distance), With>, - anchors: Query<&Anchor>, - mut events: EventReader, -) { - for e in events.iter() { - if let Ok((children, mut ppm)) = drawings.get_mut(**e) { - let mut scale_numerator = 0.0; - let mut scale_denominator = 0; - for child in children { - if let Ok((edge, distance)) = measurements.get(*child) { - if let Some(in_meters) = distance.0 { - let a0 = anchors - .get(edge.start()) - .expect("Broken measurement anchor reference"); - let d0 = a0.translation_for_category(Category::Drawing); - let a1 = anchors - .get(edge.end()) - .expect("Broken measurement anchor reference"); - let d1 = a1.translation_for_category(Category::Drawing); - let in_pixels = ((d0[0] - d1[0]) * (d0[0] - d1[0]) - + (d0[1] - d1[1]) * (d0[1] - d1[1])) - .sqrt(); - scale_numerator += in_pixels / in_meters; - scale_denominator += 1; - } - } - } - if scale_denominator > 0 { - ppm.0 = scale_numerator / (scale_denominator as f32); - } else { - warn!("No measurements found on current drawing, skipping scaling"); - } - } - } -} - -// The cost will be the sum of the square distances between pairs of points in constraints. -// Reference point pose is just world pose in meters, while the pose of the point to be optimized -// is expressed as a function of drawing translation, rotation and scale -// In matching points, first is reference second is to be optimized -// Order in u is x, y, theta, scale -fn align_level_cost( - matching_points: &Vec<([f64; 2], [f64; 2])>, - u: &[f64], - cost: &mut f64, -) -> Result<(), SolverError> { - *cost = 0.0; - let (x, y, theta, s) = (u[0], u[1], u[2], u[3]); - for (p0, p1) in matching_points { - *cost += (x + (theta.cos() * p1[0] - theta.sin() * p1[1]) / s - p0[0]).powi(2) - + (y + (theta.sin() * p1[0] + theta.cos() * p1[1]) / s - p0[1]).powi(2); - } - Ok(()) -} - -// Calculates the partial derivatives for the cost function for each variable -fn align_level_gradient( - matching_points: &Vec<([f64; 2], [f64; 2])>, - u: &[f64], - grad: &mut [f64], -) -> Result<(), SolverError> { - let (x, y, theta, s) = (u[0], u[1], u[2], u[3]); - grad[0] = 0.0; - grad[1] = 0.0; - grad[2] = 0.0; - grad[3] = 0.0; - for (p0, p1) in matching_points { - grad[0] += 2.0 * (x + (theta.cos() * p1[0] - theta.sin() * p1[1]) / s - p0[0]); - grad[1] += 2.0 * (y + (theta.sin() * p1[0] + theta.cos() * p1[1]) / s - p0[1]); - // ref https://www.wolframalpha.com/input?i=d%2Fdtheta+%28x+%2B+%28cos%28theta%29+*+p1%5B0%5D+-+sin%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B0%5D%29%5E2+%2B+%28y+%2B+%28sin%28theta%29+*+p1%5B0%5D+%2B+cos%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B1%5D%29%5E2 - grad[2] += 2.0 / s - * (theta.sin() * (p0[1] * p1[1] + p0[0] * p1[0] - p1[0] * x - p1[1] * y) - + theta.cos() * (p0[0] * p1[1] - p0[1] * p1[0] - p1[1] * x + p1[0] * y)); - // ref https://www.wolframalpha.com/input?i=d%2Fds+%28x+%2B+%28cos%28theta%29+*+p1%5B0%5D+-+sin%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B0%5D%29%5E2+%2B+%28y+%2B+%28sin%28theta%29+*+p1%5B0%5D+%2B+cos%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B1%5D%29%5E2 - grad[3] += -2.0 - * (p1[0] * theta.cos() - p1[1] * theta.sin()) - * (-p0[0] + (p1[0] * theta.cos() - p1[1] * theta.sin()) / s + x) - / (s * s) - - 2.0 - * (p1[0] * theta.sin() + p1[1] * theta.cos()) - * (-p0[1] + (p1[0] * theta.sin() + p1[1] * theta.cos()) / s + y) - / (s * s); - } - Ok(()) -} - -fn align_drawing_pair( - references: &HashSet, - target_drawing: Entity, - constraints: &Vec<&Edge>, - params: &OptimizationParams, - change: &mut OptimizationChangeParams, -) -> Option<(f64, f64, f64, f64)> { - // Function that creates a pair of reference point and target point poses, their distance to be - // minimized as part of the optimization - let make_point_pair = |reference: Entity, target: Entity| { - let reference_point = params - .global_tfs - .get(reference) - .expect("Transform for anchor not found") - .translation() - .truncate() - .to_array() - .map(|t| t as f64); - let target_point = params - .anchors - .get(target) - .expect("Broken constraint anchor reference") - .translation_for_category(Category::Drawing) - .map(|t| t as f64); - (reference_point, target_point) - }; - // Guaranteed safe since caller passes a drawing entity - let (_, _, target_pose, target_ppm, _) = params.drawings.get(target_drawing).unwrap(); - let mut matching_points = Vec::new(); - for edge in constraints.iter() { - let start_parent = params - .parents - .get(edge.start()) - .expect("Anchor in constraint without drawing parent"); - let end_parent = params - .parents - .get(edge.end()) - .expect("Anchor in constraint without drawing parent"); - if (references.contains(&*start_parent)) & (target_drawing == **end_parent) { - matching_points.push(make_point_pair(edge.start(), edge.end())); - } else if (references.contains(&*end_parent)) & (target_drawing == **start_parent) { - matching_points.push(make_point_pair(edge.end(), edge.start())); - } else { - continue; - } - } - if matching_points.is_empty() { - warn!( - "No constraints found for drawing {:?}, skipping optimization", - target_drawing - ); - return None; - } - // Optimize the transform - let min_vals = vec![ - -std::f64::INFINITY, - -std::f64::INFINITY, - -180_f64.to_radians(), - 1e-3, - ]; - let max_vals = vec![ - std::f64::INFINITY, - std::f64::INFINITY, - 180_f64.to_radians(), - 1e6, - ]; - let x = target_pose.trans[0]; - let y = target_pose.trans[1]; - let theta = match target_pose.rot.as_yaw() { - Rotation::Yaw(yaw) => yaw.radians(), - _ => unreachable!(), - }; - let s = target_ppm.0; - let mut u = vec![x as f64, y as f64, theta as f64, s as f64]; - // Now optimize it - let opt_constraints = constraints::Rectangle::new(Some(&min_vals), Some(&max_vals)); - let mut panoc_cache = PANOCCache::new(u.len(), 1e-6, 10); - let f = |u: &[f64], c: &mut f64| -> Result<(), SolverError> { - align_level_cost(&matching_points, u, c) - }; - - let df = |u: &[f64], gradient: &mut [f64]| -> Result<(), SolverError> { - align_level_gradient(&matching_points, u, gradient) - }; - let problem = Problem::new(&opt_constraints, df, f); - let mut panoc = PANOCOptimizer::new(problem, &mut panoc_cache).with_max_iter(1000); - panoc.solve(&mut u).ok(); - - // Update transform parameters with results of the optimization - let mut new_pose = target_pose.clone(); - new_pose.trans[0] = u[0] as f32; - new_pose.trans[1] = u[1] as f32; - new_pose.rot = Rotation::Yaw(Angle::Rad(u[2] as f32)); - change.pose.send(Change::new(new_pose, target_drawing)); - change - .ppm - .send(Change::new(PixelsPerMeter(u[3] as f32), target_drawing)); - Some((u[0], u[1], u[2], u[3])) -} - -#[derive(SystemParam)] -pub struct OptimizationChangeParams<'w, 's> { - pose: EventWriter<'w, 's, Change>, - ppm: EventWriter<'w, 's, Change>, -} - -#[derive(SystemParam)] -pub struct OptimizationParams<'w, 's> { - drawings: Query< - 'w, - 's, - ( - Entity, - &'static Children, - &'static Pose, - &'static PixelsPerMeter, - &'static IsPrimary, - ), - With, - >, - global_tfs: Query<'w, 's, &'static GlobalTransform>, - parents: Query<'w, 's, &'static Parent>, - anchors: Query<'w, 's, &'static Anchor>, - constraints: Query<'w, 's, &'static Edge, With>, -} - -pub fn align_level_drawings( - levels: Query<&Children, With>, - mut events: EventReader, - params: OptimizationParams, - mut change: OptimizationChangeParams, -) { - for e in events.iter() { - // Get the matching points for this entity - let level_children = levels - .get(**e) - .expect("Align level event sent to non level entity"); - let constraints = level_children - .iter() - .filter_map(|child| params.constraints.get(*child).ok()) - .collect::>(); - if constraints.is_empty() { - warn!("No constraints found for level, skipping optimization"); - continue; - } - let (references, layers): (HashSet<_>, Vec<_>) = level_children - .iter() - .filter_map(|child| params.drawings.get(*child).ok()) - .partition_map(|(e, _, _, _, primary)| { - if primary.0 == true { - Either::Left(e) - } else { - Either::Right(e) - } - }); - if layers.is_empty() { - warn!( - "No non-primary drawings found for level, at least one drawing must be set to \ - non-primary to be optimized against primary drawings.Skipping optimization" - ); - continue; - } - if references.is_empty() { - warn!( - "No primary drawings found for level. At least one drawing must be set to \ - primary to use as a reference for other drawings. Skipping optimization" - ); - continue; - } - for layer_entity in layers { - align_drawing_pair( - &references, - layer_entity, - &constraints, - ¶ms, - &mut change, - ); - } - } -} - -pub fn align_site_drawings( - levels: Query<(Entity, &Children, &Parent, &LevelProperties)>, - sites: Query<&Children, With>, - mut events: EventReader, - params: OptimizationParams, - mut change: OptimizationChangeParams, -) { - for e in events.iter() { - // Get the levels that are children of the requested site - let levels = levels - .iter() - .filter(|(_, _, p, _)| ***p == **e) - .collect::>(); - let reference_level = levels - .iter() - .min_by(|l_a, l_b| l_a.3.elevation.partial_cmp(&l_b.3.elevation).unwrap()) - .expect("Site has no levels"); - // Reference level will be the one with minimum elevation - let references = reference_level - .1 - .iter() - .filter_map(|c| { - params - .drawings - .get(*c) - .ok() - .filter(|(_, _, _, _, primary)| primary.0 == true) - }) - .map(|(e, _, _, _, _)| e) - .collect::>(); - // Layers to be optimized are primary drawings in the non reference level - let layers = levels - .iter() - .filter_map(|(e, c, _, _)| (*e != reference_level.0).then(|| c.iter())) - .flatten() - .filter_map(|child| params.drawings.get(*child).ok()) - .filter_map(|(e, _, _, _, primary)| (primary.0 == true).then(|| e)) - .collect::>(); - // Inter level constraints are children of the site - let constraints = sites - .get(**e) - .expect("Align site sent to non site entity") - .iter() - .filter_map(|child| params.constraints.get(*child).ok()) - .collect::>(); - if constraints.is_empty() { - warn!("No constraints found for site, skipping optimization"); - continue; - } - if layers.is_empty() { - warn!( - "No other levels drawings found for site, at least one other level must have a \ - primary drawing to be optimized against reference level. Skipping optimization" - ); - continue; - } - if references.is_empty() { - warn!( - "No reference level drawing found for site. At least one primary drawing must be \ - present in the lowest level to use as a reference for other levels. \ - Skipping optimization" - ); - continue; - } - for layer_entity in layers { - align_drawing_pair( - &references, - layer_entity, - &constraints, - ¶ms, - &mut change, - ); - } - } -} diff --git a/rmf_site_editor/src/site/fiducial.rs b/rmf_site_editor/src/site/fiducial.rs index 414fd026..8e143b10 100644 --- a/rmf_site_editor/src/site/fiducial.rs +++ b/rmf_site_editor/src/site/fiducial.rs @@ -18,6 +18,176 @@ use crate::interaction::VisualCue; use crate::site::*; use bevy::prelude::*; +use std::collections::HashMap; + +#[derive(Component)] +pub struct FiducialUsage { + site: Entity, + used: HashMap, + unused: HashMap, +} + +impl FiducialUsage { + pub fn used(&self) -> &HashMap { + &self.used + } + + pub fn unused(&self) -> &HashMap { + &self.unused + } + + pub fn site(&self) -> Entity { + self.site + } +} + +pub fn add_unused_fiducial_tracker( + mut commands: Commands, + new_fiducial_scope: Query, Added)>>, + sites: Query<(), With>, + parent: Query<&Parent>, + fiducials: Query<&Affiliation, With>, + fiducial_groups: Query<(Entity, &NameInSite, &Parent), (With, With)>, + children: Query<&Children>, +) { + for e in &new_fiducial_scope { + if let Some(site) = find_parent_site(e, &sites, &parent) { + let mut tracker = FiducialUsage { + site, + used: Default::default(), + unused: Default::default(), + }; + reset_fiducial_usage(e, &mut tracker, &fiducials, &fiducial_groups, &children); + commands.entity(e).insert(tracker); + } + } +} + +pub fn update_fiducial_usage_tracker( + mut unused_fiducial_trackers: Query<(Entity, &mut FiducialUsage)>, + changed_parent: Query, Changed)>, + changed_fiducial: Query<&Parent, (Changed>, With)>, + sites: Query<(), With>, + parent: Query<&Parent>, + children: Query<&Children>, + fiducials: Query<&Affiliation, With>, + changed_fiducials: Query< + (Entity, &Parent), + (Changed>, With), + >, + fiducial_groups: Query<(Entity, &NameInSite, &Parent), (With, With)>, + changed_fiducial_groups: Query< + Entity, + Or<( + (Added, With), + (With, Added), + ( + Or<(Changed, Changed)>, + With, + With, + ), + )>, + >, + removed_fiducial_groups: RemovedComponents, +) { + for e in &changed_parent { + if let Some(site) = find_parent_site(e, &sites, &parent) { + if let Ok((_, mut unused)) = unused_fiducial_trackers.get_mut(e) { + unused.site = site; + } + } + } + + for e in changed_parent + .iter() + .chain(changed_fiducial.iter().map(|p| p.get())) + { + let Ok((_, mut tracker)) = unused_fiducial_trackers.get_mut(e) else { continue }; + reset_fiducial_usage(e, &mut tracker, &fiducials, &fiducial_groups, &children); + } + + for changed_group in &changed_fiducial_groups { + let Ok((_, name, site)) = fiducial_groups.get(changed_group) else { continue }; + for (e, mut tracker) in &mut unused_fiducial_trackers { + if tracker.site == site.get() { + tracker.unused.insert(changed_group, name.0.clone()); + let Ok(scope_children) = children.get(e) else { continue }; + for child in scope_children { + let Ok(affiliation) = fiducials.get(*child) else { continue }; + if let Some(group) = affiliation.0 { + if changed_group == group { + tracker.unused.remove(&changed_group); + tracker.used.insert(changed_group, name.0.clone()); + } + } + } + } else { + // If we ever want to support moving a fiducial group between + // sites, this will take care of that. Otherwise this line will + // never be executed. + tracker.used.remove(&changed_group); + tracker.unused.remove(&changed_group); + } + } + } + + for (changed_fiducial, parent) in &changed_fiducials { + let Ok((e, mut tracker)) = unused_fiducial_trackers.get_mut(parent.get()) else { continue }; + reset_fiducial_usage(e, &mut tracker, &fiducials, &fiducial_groups, &children); + } + + for removed_group in &removed_fiducial_groups { + for (_, mut tracker) in &mut unused_fiducial_trackers { + tracker.used.remove(&removed_group); + tracker.unused.remove(&removed_group); + } + } +} + +fn find_parent_site( + mut entity: Entity, + sites: &Query<(), With>, + parents: &Query<&Parent>, +) -> Option { + loop { + if sites.contains(entity) { + return Some(entity); + } + + if let Ok(parent) = parents.get(entity) { + entity = parent.get(); + } else { + return None; + } + } +} + +fn reset_fiducial_usage( + entity: Entity, + tracker: &mut FiducialUsage, + fiducials: &Query<&Affiliation, With>, + fiducial_groups: &Query<(Entity, &NameInSite, &Parent), (With, With)>, + children: &Query<&Children>, +) { + tracker.unused.clear(); + for (group, name, site) in fiducial_groups { + if site.get() == tracker.site { + tracker.unused.insert(group, name.0.clone()); + tracker.used.remove(&group); + } + } + + let Ok(scope_children) = children.get(entity) else { return }; + for child in scope_children { + let Ok(affiliation) = fiducials.get(*child) else { continue }; + if let Some(group) = affiliation.0 { + tracker.unused.remove(&group); + if let Ok((_, name, _)) = fiducial_groups.get(group) { + tracker.used.insert(group, name.0.clone()); + } + } + } +} pub fn add_fiducial_visuals( mut commands: Commands, @@ -38,15 +208,36 @@ pub fn add_fiducial_visuals( .entity(e) .insert(assets.fiducial_mesh.clone()) .insert(assets.fiducial_material.clone()) + .insert(Visibility::default()) + .insert(ComputedVisibility::default()) .insert(Category::Fiducial) .insert(VisualCue::outline()); } } +pub fn assign_orphan_fiducials_to_parent( + mut commands: Commands, + orphans: Query< + (Entity, &Point), + (With, Without, Without), + >, + anchors: Query<&Parent, With>, + site_id: Query<&SiteID>, +) { + for (e, point) in &orphans { + if let Ok(parent) = anchors.get(point.0) { + commands.entity(e).set_parent(parent.get()); + } + } +} + pub fn update_changed_fiducial( mut fiducials: Query< (Entity, &Point, &mut Transform), - (Changed>, With), + ( + With, + Or<(Changed>, Changed)>, + ), >, anchors: AnchorParams, ) { diff --git a/rmf_site_editor/src/site/floor.rs b/rmf_site_editor/src/site/floor.rs index 972a6e58..4b679e31 100644 --- a/rmf_site_editor/src/site/floor.rs +++ b/rmf_site_editor/src/site/floor.rs @@ -15,7 +15,7 @@ * */ -use crate::{interaction::Selectable, shapes::*, site::*}; +use crate::{interaction::Selectable, shapes::*, site::*, RecencyRanking}; use bevy::{ math::Affine3A, prelude::*, @@ -28,22 +28,6 @@ use lyon::{ }; use rmf_site_format::{FloorMarker, Path}; -#[derive(Debug, Clone, Copy, Default, Deref, DerefMut, Resource)] -pub struct GlobalFloorVisibility(pub LayerVisibility); - -// Semi transparency for floors, more transparent than drawings to make them hidden -const DEFAULT_FLOOR_SEMI_TRANSPARENCY: f32 = 0.2; - -/// Resource used to set what the alpha value for partially transparent floors should be -#[derive(Clone, Resource, Deref, DerefMut)] -pub struct FloorSemiTransparency(f32); - -impl Default for FloorSemiTransparency { - fn default() -> Self { - FloorSemiTransparency(DEFAULT_FLOOR_SEMI_TRANSPARENCY) - } -} - pub const FALLBACK_FLOOR_SIZE: f32 = 0.1; pub const FLOOR_LAYER_START: f32 = DRAWING_LAYER_START + 0.001; @@ -181,11 +165,27 @@ fn floor_height(rank: Option<&RecencyRank>) -> f32 { .unwrap_or(FLOOR_LAYER_START) } +#[inline] fn floor_material( specific: Option<&LayerVisibility>, - general: &LayerVisibility, + general: Option<(&GlobalFloorVisibility, &RecencyRanking)>, ) -> StandardMaterial { - let alpha = specific.map(|s| s.alpha()).unwrap_or(general.alpha()); + let alpha = specific + .copied() + .unwrap_or_else(|| { + general + .map(|(v, r)| { + if r.is_empty() { + &v.without_drawings + } else { + &v.general + } + }) + .copied() + .unwrap_or(LayerVisibility::Opaque) + }) + .alpha(); + Color::rgba(0.3, 0.3, 0.3, alpha).into() } @@ -197,6 +197,7 @@ pub fn add_floor_visuals( &Path, Option<&RecencyRank>, Option<&LayerVisibility>, + Option<&Parent>, ), Added, >, @@ -204,25 +205,28 @@ pub fn add_floor_visuals( mut dependents: Query<&mut Dependents, With>, mut meshes: ResMut>, mut materials: ResMut>, - default_floor_visibility: Res, + default_floor_vis: Query<(&GlobalFloorVisibility, &RecencyRanking)>, ) { - for (e, new_floor, rank, vis) in &floors { + for (e, new_floor, rank, vis, parent) in &floors { let mesh = make_floor_mesh(e, new_floor, &anchors); let mut cmd = commands.entity(e); let height = floor_height(rank); - let material = materials.add(floor_material(vis, default_floor_visibility.as_ref())); + let default = parent + .map(|p| default_floor_vis.get(p.get()).ok()) + .flatten(); + let material = materials.add(floor_material(vis, default)); let mesh_entity_id = cmd .insert(SpatialBundle { transform: Transform::from_xyz(0.0, 0.0, height), - ..default() + ..Default::default() }) .add_children(|p| { p.spawn(PbrBundle { mesh: meshes.add(mesh), // TODO(MXG): load the user-specified texture when one is given material, - ..default() + ..Default::default() }) .insert(Selectable::new(e)) .id() @@ -290,16 +294,26 @@ pub fn update_floor_for_moved_anchors( } } +#[inline] fn iter_update_floor_visibility<'a>( - iter: impl Iterator, &'a FloorSegments)>, + iter: impl Iterator< + Item = ( + Option<&'a LayerVisibility>, + Option<&'a Parent>, + &'a FloorSegments, + ), + >, material_handles: &Query<&Handle>, material_assets: &mut ResMut>, - default_floor_vis: &LayerVisibility, + default_floor_vis: &Query<(&GlobalFloorVisibility, &RecencyRanking)>, ) { - for (vis, segments) in iter { + for (vis, parent, segments) in iter { if let Ok(handle) = material_handles.get(segments.mesh) { if let Some(mat) = material_assets.get_mut(handle) { - *mat = floor_material(vis, &default_floor_vis); + let default = parent + .map(|p| default_floor_vis.get(p.get()).ok()) + .flatten(); + *mat = floor_material(vis, default); } } } @@ -307,33 +321,40 @@ fn iter_update_floor_visibility<'a>( // TODO(luca) RemovedComponents is brittle, maybe wrap component in an option? pub fn update_floor_visibility( - changed_floors: Query<(Option<&LayerVisibility>, &FloorSegments), Changed>, + changed_floors: Query, Changed)>>, removed_vis: RemovedComponents, - all_floors: Query<(Option<&LayerVisibility>, &FloorSegments)>, + all_floors: Query<(Option<&LayerVisibility>, Option<&Parent>, &FloorSegments)>, material_handles: Query<&Handle>, mut material_assets: ResMut>, - default_floor_vis: Res, + default_floor_vis: Query<(&GlobalFloorVisibility, &RecencyRanking)>, + changed_default_floor_vis: Query< + &Children, + Or<( + Changed, + Changed>, + )>, + >, ) { - if default_floor_vis.is_changed() { + iter_update_floor_visibility( + changed_floors.iter().filter_map(|e| all_floors.get(e).ok()), + &material_handles, + &mut material_assets, + &default_floor_vis, + ); + + iter_update_floor_visibility( + removed_vis.iter().filter_map(|e| all_floors.get(e).ok()), + &material_handles, + &mut material_assets, + &default_floor_vis, + ); + + for children in &changed_default_floor_vis { iter_update_floor_visibility( - all_floors.iter(), + children.iter().filter_map(|e| all_floors.get(*e).ok()), &material_handles, &mut material_assets, &default_floor_vis, ); - } else { - iter_update_floor_visibility( - changed_floors.iter(), - &material_handles, - &mut material_assets, - &default_floor_vis, - ); - - iter_update_floor_visibility( - removed_vis.iter().filter_map(|e| all_floors.get(e).ok()), - &material_handles, - &mut material_assets, - &default_floor_vis, - ); - }; + } } diff --git a/rmf_site_editor/src/site/lane.rs b/rmf_site_editor/src/site/lane.rs index da9854ea..5f0fb1f7 100644 --- a/rmf_site_editor/src/site/lane.rs +++ b/rmf_site_editor/src/site/lane.rs @@ -49,7 +49,7 @@ fn should_display_lane( edge: &Edge, associated: &AssociatedGraphs, parents: &Query<&Parent>, - levels: &Query<(), With>, + levels: &Query<(), With>, current_level: &Res, graphs: &GraphSelect, ) -> bool { @@ -74,7 +74,7 @@ pub fn assign_orphan_nav_elements_to_site( ), >, current_workspace: Res, - open_sites: Query>, + open_sites: Query>, ) { if let Some(current_site) = current_workspace.to_site(&open_sites) { for e in &elements { @@ -89,7 +89,7 @@ pub fn add_lane_visuals( graphs: GraphSelect, anchors: AnchorParams, parents: Query<&Parent>, - levels: Query<(), With>, + levels: Query<(), With>, mut dependents: Query<&mut Dependents, With>, assets: Res, current_level: Res, @@ -241,7 +241,7 @@ pub fn update_changed_lane( >, anchors: AnchorParams, parents: Query<&Parent>, - levels: Query<(), With>, + levels: Query<(), With>, graphs: GraphSelect, mut transforms: Query<&mut Transform>, current_level: Res, @@ -309,7 +309,7 @@ pub fn update_visibility_for_lanes( (With, Without), >, parents: Query<&Parent>, - levels: Query<(), With>, + levels: Query<(), With>, current_level: Res, graphs: GraphSelect, lanes_with_changed_association: Query< diff --git a/rmf_site_editor/src/site/layer.rs b/rmf_site_editor/src/site/layer.rs deleted file mode 100644 index 4d6a1c73..00000000 --- a/rmf_site_editor/src/site/layer.rs +++ /dev/null @@ -1,101 +0,0 @@ -/* - * Copyright (C) 2023 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 bevy::prelude::*; - -const DEFAULT_LAYER_SEMI_TRANSPARENCY: f32 = 0.2; - -#[derive(Debug, Clone, Copy, Component)] -pub enum LayerVisibility { - /// The layer is fully opaque. This is the default for floors when no drawing is - /// present. - Opaque, - /// Make the layer semi-transparent. This is useful for allowing layers - /// to be visible undearneath them. When a drawing is added to the scene, - /// the floors will automatically change to Alpha(0.1). - Alpha(f32), - /// The layer is fully hidden. - Hidden, -} - -// TODO(MXG): Should this trait be more general? -pub trait VisibilityCycle { - type Value; - fn next(&self, transparency: f32) -> Self::Value; - fn label(&self) -> &'static str; -} - -impl LayerVisibility { - pub fn new_semi_transparent() -> Self { - LayerVisibility::Alpha(DEFAULT_LAYER_SEMI_TRANSPARENCY) - } - - pub fn alpha(&self) -> f32 { - match self { - LayerVisibility::Opaque => 1.0, - LayerVisibility::Alpha(a) => *a, - LayerVisibility::Hidden => 0.0, - } - } -} - -impl VisibilityCycle for LayerVisibility { - type Value = Self; - - /// Cycle to the next visibility option - fn next(&self, transparency: f32) -> LayerVisibility { - match self { - LayerVisibility::Opaque => LayerVisibility::Alpha(transparency), - LayerVisibility::Alpha(_) => LayerVisibility::Hidden, - LayerVisibility::Hidden => LayerVisibility::Opaque, - } - } - - fn label(&self) -> &'static str { - match self { - LayerVisibility::Opaque => "opaque", - LayerVisibility::Alpha(_) => "semi-transparent", - LayerVisibility::Hidden => "hidden", - } - } -} - -impl VisibilityCycle for Option { - type Value = Self; - fn next(&self, transparency: f32) -> Self { - match self { - Some(v) => match v { - LayerVisibility::Hidden => None, - _ => Some(v.next(transparency)), - }, - None => Some(LayerVisibility::Opaque), - } - } - - fn label(&self) -> &'static str { - match self { - Some(v) => v.label(), - None => "global default", - } - } -} - -impl Default for LayerVisibility { - fn default() -> Self { - LayerVisibility::Opaque - } -} diff --git a/rmf_site_editor/src/site/level.rs b/rmf_site_editor/src/site/level.rs index f7368d3b..b5a7cf83 100644 --- a/rmf_site_editor/src/site/level.rs +++ b/rmf_site_editor/src/site/level.rs @@ -20,7 +20,7 @@ use crate::CurrentWorkspace; use bevy::prelude::*; pub fn update_level_visibility( - mut levels: Query<(Entity, &mut Visibility), With>, + mut levels: Query<(Entity, &mut Visibility), With>, current_level: Res, ) { if current_level.is_changed() { @@ -32,8 +32,8 @@ pub fn update_level_visibility( pub fn assign_orphan_levels_to_site( mut commands: Commands, - new_levels: Query, Added)>, - open_sites: Query>, + new_levels: Query, Added)>, + open_sites: Query>, current_workspace: Res, ) { if let Some(site) = current_workspace.to_site(&open_sites) { diff --git a/rmf_site_editor/src/site/lift.rs b/rmf_site_editor/src/site/lift.rs index 6fd2d822..3a04523a 100644 --- a/rmf_site_editor/src/site/lift.rs +++ b/rmf_site_editor/src/site/lift.rs @@ -110,7 +110,7 @@ pub fn add_tags_to_lift( mut commands: Commands, new_lifts: Query<(Entity, &Edge), Added>>, orphan_lifts: Query>, Without)>, - open_sites: Query>, + open_sites: Query>, mut dependents: Query<&mut Dependents, With>, current_workspace: Res, ) { @@ -159,7 +159,7 @@ pub fn update_lift_cabin( mut anchors: Query<&mut Anchor>, assets: Res, mut meshes: ResMut>, - levels: Query<(Entity, &Parent), With>, + levels: Query<(Entity, &Parent), With>, ) { for (e, cabin, recall, child_anchor_group, child_cabin_group, site) in &lifts { // Despawn the previous cabin @@ -170,7 +170,8 @@ pub fn update_lift_cabin( let cabin_tf = match cabin { LiftCabin::Rect(params) => { let Aabb { center, .. } = params.aabb(); - let cabin_tf = Transform::from_translation(Vec3::new(center.x, center.y, 0.)); + let cabin_tf = + Transform::from_translation(Vec3::new(center.x, center.y, FLOOR_LAYER_START)); let floor_mesh: Mesh = make_flat_rect_mesh( params.depth + 2.0 * params.thickness(), params.width + 2.0 * params.thickness(), @@ -348,9 +349,9 @@ pub fn update_lift_door_availability( mut doors: Query<(Entity, &Edge, &mut LevelVisits), With>, dependents: Query<&Dependents, With>, current_level: Res, - new_levels: Query<(), Added>, - all_levels: Query<(), With>, - removed_levels: RemovedComponents, + new_levels: Query<(), Added>, + all_levels: Query<(), With>, + removed_levels: RemovedComponents, parents: Query<&Parent>, ) { for toggle in toggles.iter() { diff --git a/rmf_site_editor/src/site/light.rs b/rmf_site_editor/src/site/light.rs index 0641d338..c42d614b 100644 --- a/rmf_site_editor/src/site/light.rs +++ b/rmf_site_editor/src/site/light.rs @@ -27,7 +27,7 @@ use bevy::{ view::VisibleEntities, }, }; -use rmf_site_format::{Category, LevelProperties, Light, LightKind, Pose}; +use rmf_site_format::{Category, LevelElevation, Light, LightKind, NameInSite, Pose}; use std::collections::{BTreeMap, HashMap}; /// True/false for whether the physical lights of an environment should be @@ -143,14 +143,14 @@ pub struct ExportLights(pub std::path::PathBuf); pub fn export_lights( mut exports: EventReader, lights: Query<(&Pose, &LightKind, &Parent)>, - levels: Query<&LevelProperties>, + levels: Query<&NameInSite>, ) { for export in exports.iter() { let mut lights_per_level: BTreeMap> = BTreeMap::new(); for (pose, kind, parent) in &lights { - if let Ok(level) = levels.get(parent.get()) { + if let Ok(name) = levels.get(parent.get()) { lights_per_level - .entry(level.name.clone()) + .entry(name.0.clone()) .or_default() .push(Light { pose: pose.clone(), @@ -173,7 +173,7 @@ pub fn export_lights( let mut root: BTreeMap>> = BTreeMap::new(); for (level, lights) in lights_per_level { let mut lights_map: HashMap> = HashMap::new(); - lights_map.insert("lights".to_string(), lights); + lights_map.insert("lights".to_owned(), lights); root.insert(level, lights_map); } diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 57090cf9..f3c7938b 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -15,7 +15,7 @@ * */ -use crate::{recency::RecencyRanking, site::*, Autoload, CurrentWorkspace}; +use crate::{recency::RecencyRanking, site::*, Autoload, CurrentWorkspace, WorkspaceMarker}; use bevy::{ecs::system::SystemParam, prelude::*, tasks::AsyncComputeTaskPool}; use futures_lite::future; use rmf_site_format::legacy::building_map::BuildingMap; @@ -52,6 +52,7 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: site_cmd .insert(Category::Site) .insert(site_data.properties.clone()) + .insert(WorkspaceMarker) .with_children(|site| { for (anchor_id, anchor) in &site_data.anchors { let anchor_entity = site @@ -62,6 +63,12 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: consider_id(*anchor_id); } + for (group_id, group) in &site_data.fiducial_groups { + let group_entity = site.spawn(group.clone()).insert(SiteID(*group_id)).id(); + id_to_entity.insert(*group_id, group_entity); + consider_id(*group_id); + } + for (level_id, level_data) in &site_data.levels { let mut level_cmd = site.spawn(SiteID(*level_id)); @@ -90,7 +97,7 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: for (drawing_id, drawing) in &level_data.drawings { level - .spawn(DrawingBundle::new(drawing)) + .spawn(DrawingBundle::new(drawing.properties.clone())) .insert(SiteID(*drawing_id)) .with_children(|drawing_parent| { for (anchor_id, anchor) in &drawing.anchors { @@ -147,15 +154,6 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: .insert(SiteID(*wall_id)); consider_id(*wall_id); } - - for (constraint_id, constraint_data) in &level_data.constraints { - let constraint = level - .spawn(constraint_data.to_ecs(&id_to_entity)) - .insert(SiteID(*constraint_id)) - .id(); - id_to_entity.insert(*constraint_id, constraint); - consider_id(*constraint_id); - } }); // TODO(MXG): Log when a RecencyRanking fails to load correctly. @@ -213,6 +211,15 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: consider_id(*lift_id); } + for (fiducial_id, fiducial) in &site_data.fiducials { + let fiducial_entity = site + .spawn(fiducial.to_ecs(&id_to_entity)) + .insert(SiteID(*fiducial_id)) + .id(); + id_to_entity.insert(*fiducial_id, fiducial_entity); + consider_id(*fiducial_id); + } + for (nav_graph_id, nav_graph_data) in &site_data.navigation.guided.graphs { let nav_graph = site .spawn(SpatialBundle::default()) @@ -317,16 +324,17 @@ pub struct ImportNavGraphs { #[derive(SystemParam)] pub struct ImportNavGraphParams<'w, 's> { commands: Commands<'w, 's>, - sites: Query<'w, 's, &'static Children, With>, + sites: Query<'w, 's, &'static Children, With>, levels: Query< 'w, 's, ( Entity, - &'static LevelProperties, + &'static NameInSite, &'static Parent, &'static Children, ), + With, >, lifts: Query< 'w, @@ -354,12 +362,12 @@ fn generate_imported_nav_graphs( }; let mut level_name_to_entity = HashMap::new(); - for (e, level, parent, _) in ¶ms.levels { + for (e, name, parent, _) in ¶ms.levels { if parent.get() != into_site { continue; } - level_name_to_entity.insert(level.name.clone(), e); + level_name_to_entity.insert(name.clone().0, e); } let mut lift_name_to_entity = HashMap::new(); @@ -368,16 +376,16 @@ fn generate_imported_nav_graphs( continue; } - lift_name_to_entity.insert(name.0.clone(), e); + lift_name_to_entity.insert(name.clone().0, e); } let mut id_to_entity = HashMap::new(); for (level_id, level_data) in &from_site_data.levels { - if let Some(e) = level_name_to_entity.get(&level_data.properties.name) { + if let Some(e) = level_name_to_entity.get(&level_data.properties.name.0) { id_to_entity.insert(*level_id, *e); } else { return Err(ImportNavGraphError::MissingLevelName( - level_data.properties.name.clone(), + level_data.properties.name.0.clone(), )); } } @@ -520,7 +528,7 @@ pub fn import_nav_graph( mut import_requests: EventReader, mut autoload: Option>, current_workspace: Res, - open_sites: Query>, + open_sites: Query>, ) { for r in import_requests.iter() { if let Err(err) = generate_imported_nav_graphs(&mut params, r.into_site, &r.from_site) { diff --git a/rmf_site_editor/src/site/location.rs b/rmf_site_editor/src/site/location.rs index 535bf5a4..954ece89 100644 --- a/rmf_site_editor/src/site/location.rs +++ b/rmf_site_editor/src/site/location.rs @@ -18,16 +18,16 @@ use crate::{animate::Spinning, interaction::VisualCue, site::*}; use bevy::prelude::*; -// TODO(MXG): Consider using recency rankings for Locations so they don't +// TODO(@mxgrey): Consider using recency rankings for Locations so they don't // experience z-fighting. -const LOCATION_LAYER_HEIGHT: f32 = LANE_LAYER_LIMIT + SELECTED_LANE_OFFSET / 2.0; +pub const LOCATION_LAYER_HEIGHT: f32 = LANE_LAYER_LIMIT + SELECTED_LANE_OFFSET; -// TODO(MXG): Refactor this implementation with should_display_lane using traits and generics +// TODO(@mxgrey): Refactor this implementation with should_display_lane using traits and generics fn should_display_point( point: &Point, associated: &AssociatedGraphs, parents: &Query<&Parent>, - levels: &Query<(), With>, + levels: &Query<(), With>, current_level: &Res, graphs: &GraphSelect, ) -> bool { @@ -46,7 +46,7 @@ pub fn add_location_visuals( graphs: GraphSelect, anchors: AnchorParams, parents: Query<&Parent>, - levels: Query<(), With>, + levels: Query<(), With>, mut dependents: Query<&mut Dependents, With>, assets: Res, current_level: Res, @@ -68,8 +68,7 @@ pub fn add_location_visuals( let position = anchors .point_in_parent_frame_of(point.0, Category::Location, e) - .unwrap() - + LOCATION_LAYER_HEIGHT * Vec3::Z; + .unwrap(); // TODO(MXG): Put icons on the different visual squares based on the location tags commands .entity(e) @@ -99,7 +98,7 @@ pub fn update_changed_location( >, anchors: AnchorParams, parents: Query<&Parent>, - levels: Query<(), With>, + levels: Query<(), With>, graphs: GraphSelect, current_level: Res, ) { @@ -160,7 +159,7 @@ pub fn update_visibility_for_locations( (With, Without), >, parents: Query<&Parent>, - levels: Query<(), With>, + levels: Query<(), With>, current_level: Res, graphs: GraphSelect, locations_with_changed_association: Query< diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 99103a6f..dc51557a 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -51,9 +51,6 @@ pub use floor::*; pub mod lane; pub use lane::*; -pub mod layer; -pub use layer::*; - pub mod level; pub use level::*; @@ -151,13 +148,9 @@ impl Plugin for SitePlugin { .add_state_to_stage(SiteUpdateStage::AssignOrphans, SiteState::Off) .add_state_to_stage(CoreStage::PostUpdate, SiteState::Off) .insert_resource(ClearColor(Color::rgb(0., 0., 0.))) - .insert_resource(GlobalFloorVisibility::default()) - .insert_resource(GlobalDrawingVisibility::default()) .init_resource::() .init_resource::() .init_resource::() - .init_resource::() - .init_resource::() .add_event::() .add_event::() .add_event::() @@ -167,26 +160,24 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() - .add_event::() - .add_event::() .add_plugin(ChangePlugin::>::default()) .add_plugin(RecallPlugin::>::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(RecallPlugin::::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(RecallPlugin::::default()) + .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::::default()) - .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::>::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::