From 36bb7c4d0ac3e6c3ecb4309943e88357bcbc2e3e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 15 May 2023 16:41:19 +0800 Subject: [PATCH 01/80] Cleanup and restore drawing asset source update behavior Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/drawing.rs | 78 +++++++++-------------------- rmf_site_editor/src/site/mod.rs | 1 - 2 files changed, 24 insertions(+), 55 deletions(-) diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index 7206a29e..2ab4ae9a 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -45,7 +45,10 @@ fn drawing_layer_height(rank: Option<&RecencyRank>) -> f32 { } pub fn add_drawing_visuals( - new_drawings: Query<(Entity, &AssetSource, &Pose, &PixelsPerMeter), Added>, + new_drawings: Query< + (Entity, &AssetSource, &Pose, &PixelsPerMeter), + (With, Changed), + >, asset_server: Res, mut loading_drawings: ResMut, current_workspace: Res, @@ -86,7 +89,6 @@ pub fn handle_loaded_drawing( mut materials: ResMut>, rank: Query<&RecencyRank>, mut segments: Query<(&DrawingSegments, &mut Transform)>, - mut mesh_handles: Query<&mut Handle>, ) { for ev in ev_asset.iter() { if let AssetEvent::Created { handle } = ev { @@ -100,37 +102,19 @@ 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 pose = pose.clone(); - let transform = pose.transform().with_scale(Vec3::new( - 1.0 / pixels_per_meter.0, - 1.0 / pixels_per_meter.0, - 1., - )); - if let Ok((segment, mut tf)) = segments.get_mut(entity) { - *tf = transform; - if let Ok(mut mesh_handle) = mesh_handles.get_mut(segment.leaf) { - *mesh_handle = mesh; - } else { - println!("DEV ERROR: Partially-constructed Drawing entity detected"); - } + let leaf = if let Ok((segment, mut tf)) = segments.get_mut(entity) { + segment.leaf // We can ignore the layer height here since that update // will be handled by another system. } else { - let z = drawing_layer_height(rank.get(entity).ok()); + let transform = pose.transform().with_scale(Vec3::new( + 1.0 / pixels_per_meter.0, + 1.0 / pixels_per_meter.0, + 1., + )); let mut cmd = commands.entity(entity); - let leaf = cmd.add_children(|p| { - p.spawn(PbrBundle { - mesh, - material: materials.add(StandardMaterial { - base_color_texture: Some(handle.clone()), - ..default() - }), - transform: Transform::from_xyz(0.0, 0.0, z), - ..default() - }) - .id() - }); + let leaf = cmd.add_children(|p| p.spawn_empty().id()); cmd.insert(SpatialBundle { transform, @@ -139,37 +123,23 @@ pub fn handle_loaded_drawing( .insert(DrawingSegments { leaf }) .insert(Selectable::new(entity)) .insert(Category::Drawing); - } + leaf + }; + let z = drawing_layer_height(rank.get(entity).ok()); + commands.entity(leaf).insert(PbrBundle { + mesh, + material: materials.add(StandardMaterial { + base_color_texture: Some(handle.clone()), + ..default() + }), + transform: Transform::from_xyz(0.0, 0.0, z), + ..default() + }); } } } } -pub fn update_drawing_visuals( - changed_drawings: Query<(Entity, &AssetSource, &Pose, &PixelsPerMeter), Changed>, - asset_server: Res, - mut loading_drawings: ResMut, - current_workspace: Res, - site_files: Query<&DefaultFile>, -) { - let file_path = match get_current_workspace_path(current_workspace, site_files) { - Some(file_path) => file_path, - None => return, - }; - for (e, source, pose, pixels_per_meter) in &changed_drawings { - let asset_source = match source { - AssetSource::Local(name) => AssetSource::Local(String::from( - file_path.with_file_name(name).to_str().unwrap(), - )), - _ => source.clone(), - }; - let texture_handle: Handle = asset_server.load(&String::from(&asset_source)); - loading_drawings - .0 - .insert(texture_handle, (e, pose.clone(), pixels_per_meter.clone())); - } -} - pub fn update_drawing_rank( changed_rank: Query< (&DrawingSegments, &RecencyRank), diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index fa526f75..c9793e59 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -263,7 +263,6 @@ impl Plugin for SitePlugin { .with_system(handle_new_mesh_primitives) .with_system(add_drawing_visuals) .with_system(handle_loaded_drawing) - .with_system(update_drawing_visuals) .with_system(update_drawing_rank) .with_system(update_drawing_pixels_per_meter) .with_system(add_physical_camera_visuals) From 4170acb99bda9be02d1c6e6766238b847158924e Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 15 May 2023 17:06:16 +0800 Subject: [PATCH 02/80] Refactor loading drawing to be a component Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/drawing.rs | 42 ++++++++++++++++++----------- rmf_site_editor/src/site/mod.rs | 1 - 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index 2ab4ae9a..6dc703b4 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -24,7 +24,7 @@ use crate::{ }, CurrentWorkspace, }; -use bevy::{math::Affine3A, prelude::*, utils::HashMap}; +use bevy::{asset::LoadState, math::Affine3A, prelude::*, utils::HashMap}; use rmf_site_format::{AssetSource, DrawingMarker, PixelsPerMeter, Pose}; pub const DRAWING_LAYER_START: f32 = 0.0; @@ -36,8 +36,8 @@ pub struct DrawingSegments { // We need to keep track of the drawing data until the image is loaded // since we will need to scale the mesh according to the size of the image -#[derive(Default, Resource)] -pub struct LoadingDrawings(pub HashMap, (Entity, Pose, PixelsPerMeter)>); +#[derive(Component)] +pub struct LoadingDrawing(Handle); fn drawing_layer_height(rank: Option<&RecencyRank>) -> f32 { rank.map(|r| r.proportion() * (FLOOR_LAYER_START - DRAWING_LAYER_START) + DRAWING_LAYER_START) @@ -45,12 +45,12 @@ fn drawing_layer_height(rank: Option<&RecencyRank>) -> f32 { } pub fn add_drawing_visuals( + mut commands: Commands, new_drawings: Query< (Entity, &AssetSource, &Pose, &PixelsPerMeter), (With, Changed), >, asset_server: Res, - mut loading_drawings: ResMut, current_workspace: Res, site_files: Query<&DefaultFile>, mut default_floor_vis: ResMut, @@ -69,9 +69,7 @@ pub fn add_drawing_visuals( _ => source.clone(), }; let texture_handle: Handle = asset_server.load(&String::from(&asset_source)); - loading_drawings - .0 - .insert(texture_handle, (e, pose.clone(), pixels_per_meter.clone())); + commands.entity(e).insert(LoadingDrawing(texture_handle)); } if !new_drawings.is_empty() { @@ -82,18 +80,24 @@ pub fn add_drawing_visuals( // Asset event handler for loaded drawings pub fn handle_loaded_drawing( mut commands: Commands, - mut ev_asset: EventReader>, assets: Res>, - mut loading_drawings: ResMut, + loading_drawings: Query<( + Entity, + &AssetSource, + &Pose, + &PixelsPerMeter, + &LoadingDrawing, + )>, mut mesh_assets: ResMut>, + asset_server: Res, mut materials: ResMut>, rank: Query<&RecencyRank>, - mut segments: Query<(&DrawingSegments, &mut Transform)>, + segments: Query<&DrawingSegments>, ) { - for ev in ev_asset.iter() { - if let AssetEvent::Created { handle } = ev { - if let Some((entity, pose, pixels_per_meter)) = loading_drawings.0.remove(handle) { - let img = assets.get(handle).unwrap(); + for (entity, source, pose, pixels_per_meter, handle) in loading_drawings.iter() { + match asset_server.get_load_state(&handle.0) { + LoadState::Loaded => { + let img = assets.get(&handle.0).unwrap(); let width = img.texture_descriptor.size.width as f32; let height = img.texture_descriptor.size.height as f32; @@ -103,7 +107,7 @@ pub fn handle_loaded_drawing( ); let mesh = mesh_assets.add(mesh.into()); - let leaf = if let Ok((segment, mut tf)) = segments.get_mut(entity) { + let leaf = if let Ok(segment) = segments.get(entity) { segment.leaf // We can ignore the layer height here since that update // will be handled by another system. @@ -129,13 +133,19 @@ pub fn handle_loaded_drawing( commands.entity(leaf).insert(PbrBundle { mesh, material: materials.add(StandardMaterial { - base_color_texture: Some(handle.clone()), + base_color_texture: Some(handle.0.clone()), ..default() }), transform: Transform::from_xyz(0.0, 0.0, z), ..default() }); + commands.entity(entity).remove::(); + } + LoadState::Failed => { + error!("Failed loading drawing {:?}", String::from(source)); + commands.entity(entity).remove::(); } + _ => {} } } } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index c9793e59..da3c5f21 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -138,7 +138,6 @@ impl Plugin for SitePlugin { .insert_resource(ClearColor(Color::rgb(0., 0., 0.))) .insert_resource(FloorVisibility::default()) .init_resource::() - .init_resource::() .init_resource::() .init_resource::() .add_event::() From d9706d1e9ea60d769ba60b3a39f8e8470cdefd8c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 17 May 2023 12:48:07 +0800 Subject: [PATCH 03/80] Bare drawing editor mode with 2d camera and visibility toggling Signed-off-by: Luca Della Vedova --- .../src/interaction/category_visibility.rs | 192 ++++++++++++++++++ rmf_site_editor/src/interaction/mod.rs | 7 + rmf_site_editor/src/lib.rs | 1 + rmf_site_editor/src/save.rs | 2 +- .../src/site/drawing_editor/mod.rs | 94 +++++++++ rmf_site_editor/src/site/mod.rs | 4 + rmf_site_editor/src/site/model.rs | 1 + rmf_site_editor/src/widgets/create.rs | 22 +- rmf_site_editor/src/widgets/mod.rs | 75 ++++++- rmf_site_editor/src/workspace.rs | 2 +- rmf_site_format/src/legacy/building_map.rs | 2 +- rmf_site_format/src/legacy/level.rs | 1 + 12 files changed, 398 insertions(+), 5 deletions(-) create mode 100644 rmf_site_editor/src/interaction/category_visibility.rs create mode 100644 rmf_site_editor/src/site/drawing_editor/mod.rs diff --git a/rmf_site_editor/src/interaction/category_visibility.rs b/rmf_site_editor/src/interaction/category_visibility.rs new file mode 100644 index 00000000..05b52339 --- /dev/null +++ b/rmf_site_editor/src/interaction/category_visibility.rs @@ -0,0 +1,192 @@ +/* + * 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 bevy::utils::HashSet; + +use crate::{site::CurrentLevel, CurrentWorkspace}; + +use rmf_site_format::{ + DoorMarker, FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, + MeasurementMarker, ModelMarker, WallMarker, +}; + +#[derive(Clone, Debug, PartialEq)] +pub struct CategoryFlags { + pub doors: bool, + pub floors: bool, + pub lanes: bool, + pub lifts: bool, + pub locations: bool, + pub models: bool, + pub measurements: bool, + pub walls: bool, +} + +// In normal site editor we want to display everything except measurements +impl Default for CategoryFlags { + fn default() -> Self { + Self { + doors: true, + floors: true, + lanes: true, + lifts: true, + locations: true, + models: true, + measurements: false, + walls: true, + } + } +} + +/// Denote whether a certain category is visible or not +#[derive(Default, Debug, Deref, DerefMut, Resource)] +pub struct VisibilityCategoriesSettings(pub CategoryFlags); + +/// Stored to verify which fields were changed between iterations +#[derive(Default, Debug, Deref, DerefMut, Resource)] +pub struct RecallVisibilityCategoriesSettings(pub CategoryFlags); + +/// Keeps track of the entities that have been hidden by the user +#[derive(Default, Debug, Resource)] +pub struct VisibilitySettingsHiddenEntities { + pub current_root: Option, + pub hidden: HashSet, +} + +#[derive(SystemParam)] +pub struct FilterParams<'w, 's> { + doors: Query<'w, 's, Entity, With>, + floors: Query<'w, 's, Entity, With>, + lanes: Query<'w, 's, Entity, With>, + lifts: Query<'w, 's, Entity, Or<(With>, With)>>, + locations: Query<'w, 's, Entity, With>, + walls: Query<'w, 's, Entity, With>, + models: Query<'w, 's, Entity, With>, + measurements: Query<'w, 's, Entity, With>, + visibilities: Query<'w, 's, &'static mut Visibility>, + categories_settings: Res<'w, VisibilityCategoriesSettings>, + recall_categories_settings: ResMut<'w, RecallVisibilityCategoriesSettings>, + hidden_res: ResMut<'w, VisibilitySettingsHiddenEntities>, + current_level: Res<'w, CurrentLevel>, + current_workspace: Res<'w, CurrentWorkspace>, +} + +fn update_visibility( + enabled: bool, + mut visibilities: &mut Query<&mut Visibility>, + entities: Vec, + mut hidden_set: &mut HashSet, +) { + for e in entities.iter() { + if let Ok(mut vis) = visibilities.get_mut(*e) { + if vis.is_visible && !enabled { + vis.is_visible = false; + hidden_set.insert(*e); + } else if enabled && hidden_set.remove(e) { + vis.is_visible = true; + } + } + } +} + +pub fn update_entity_category_visibilities(mut params: FilterParams) { + // If the site or workspace was changed, reset the hidden cache + let mut update = false; + if **params.current_level != params.hidden_res.current_root + && params.current_workspace.root != params.hidden_res.current_root + { + // TODO(luca) take workspace display into account + params.hidden_res.hidden = HashSet::new(); + params.hidden_res.current_root = params.current_level.or(params.current_workspace.root); + update = true; + } + if params.categories_settings.is_changed() + && **params.categories_settings != **params.recall_categories_settings + { + update = true; + } + if update { + if params.categories_settings.doors != params.recall_categories_settings.doors { + update_visibility( + params.categories_settings.doors, + &mut params.visibilities, + params.doors.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.floors != params.recall_categories_settings.floors { + update_visibility( + params.categories_settings.floors, + &mut params.visibilities, + params.floors.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.lanes != params.recall_categories_settings.lanes { + update_visibility( + params.categories_settings.lanes, + &mut params.visibilities, + params.lanes.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.lifts != params.recall_categories_settings.lifts { + update_visibility( + params.categories_settings.lifts, + &mut params.visibilities, + params.lifts.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.locations != params.recall_categories_settings.locations { + update_visibility( + params.categories_settings.locations, + &mut params.visibilities, + params.locations.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.measurements != params.recall_categories_settings.measurements + { + update_visibility( + params.categories_settings.measurements, + &mut params.visibilities, + params.measurements.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.models != params.recall_categories_settings.models { + update_visibility( + params.categories_settings.models, + &mut params.visibilities, + params.models.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + if params.categories_settings.walls != params.recall_categories_settings.walls { + update_visibility( + params.categories_settings.walls, + &mut params.visibilities, + params.walls.iter().collect::>(), + &mut params.hidden_res.hidden, + ); + } + **params.recall_categories_settings = params.categories_settings.0.clone(); + } +} diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index ff1484dd..97b14f1f 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -26,6 +26,9 @@ pub use assets::*; pub mod camera_controls; pub use camera_controls::*; +pub mod category_visibility; +pub use category_visibility::*; + pub mod cursor; pub use cursor::*; @@ -127,6 +130,9 @@ impl Plugin for InteractionPlugin { .init_resource::() .init_resource::() .init_resource::() + .init_resource::() + .init_resource::() + .init_resource::() .add_event::() .add_event::() .add_event::() diff --git a/rmf_site_editor/src/site/assets.rs b/rmf_site_editor/src/site/assets.rs index 3fbd214a..d8d7ebcb 100644 --- a/rmf_site_editor/src/site/assets.rs +++ b/rmf_site_editor/src/site/assets.rs @@ -27,6 +27,7 @@ pub struct SiteAssets { pub lane_end_outline: Handle, pub box_mesh: Handle, pub location_mesh: Handle, + pub fiducial_mesh: Handle, pub physical_camera_mesh: Handle, pub unassigned_lane_material: Handle, pub passive_anchor_material: Handle, @@ -39,6 +40,7 @@ pub struct SiteAssets { pub select_material: Handle, pub hover_select_material: Handle, pub measurement_material: Handle, + pub fiducial_material: Handle, pub level_anchor_mesh: Handle, pub lift_anchor_mesh: Handle, pub site_anchor_mesh: Handle, @@ -72,6 +74,7 @@ impl FromWorld for SiteAssets { // let hover_select_material = materials.add(Color::rgb_u8(177, 178, 255).into()); // let hover_select_material = materials.add(Color::rgb_u8(214, 28, 78).into()); let measurement_material = materials.add(Color::rgb_u8(250, 234, 72).into()); + let fiducial_material = materials.add(Color::rgb(0.1, 0.1, 0.8).into()); let passive_anchor_material = materials.add(StandardMaterial { base_color: Color::rgb(0.4, 0.7, 0.6), // unlit: true, @@ -196,6 +199,15 @@ impl FromWorld for SiteAssets { .with_generated_outline_normals() .unwrap(), ); + // TODO(luca) have a different mesh + let fiducial_mesh = meshes.add( + Mesh::from( + make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.05, 4) + .transform_by(Affine3A::from_translation(0.00125 * Vec3::Z)), + ) + .with_generated_outline_normals() + .unwrap(), + ); let physical_camera_mesh = meshes.add( make_physical_camera_mesh() .with_generated_outline_normals() @@ -213,6 +225,7 @@ impl FromWorld for SiteAssets { lane_end_outline, box_mesh, location_mesh, + fiducial_mesh, physical_camera_mesh, unassigned_lane_material, hover_anchor_material, @@ -222,6 +235,7 @@ impl FromWorld for SiteAssets { select_material, hover_select_material, measurement_material, + fiducial_material, passive_anchor_material, unassigned_anchor_material, preview_anchor_material, diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index 6dc703b4..f0108656 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -55,6 +55,9 @@ pub fn add_drawing_visuals( site_files: Query<&DefaultFile>, mut default_floor_vis: ResMut, ) { + // 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, diff --git a/rmf_site_editor/src/site/fiducial.rs b/rmf_site_editor/src/site/fiducial.rs new file mode 100644 index 00000000..14617ec6 --- /dev/null +++ b/rmf_site_editor/src/site/fiducial.rs @@ -0,0 +1,80 @@ +/* + * 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::site::*; +use bevy::{prelude::*, render::primitives::Sphere}; + +pub fn add_fiducial_visuals( + mut commands: Commands, + fiducials: Query<(Entity, &Point), Added>, + mut dependents: Query<&mut Dependents, With>, + assets: Res, +) { + for (e, point) in fiducials.iter() { + if let Ok(mut deps) = dependents.get_mut(point.0) { + deps.insert(e); + } + + println!("Adding fiducial mesh"); + commands + .entity(e) + .insert(PbrBundle { + mesh: assets.fiducial_mesh.clone(), + material: assets.fiducial_material.clone(), + ..default() + }) + .insert(Category::Fiducial); + } +} + +pub fn update_changed_fiducial( + mut fiducials: Query< + (Entity, &Point, &mut Transform), + (Changed>, With), + >, + anchors: AnchorParams, +) { + for (e, point, mut tf) in fiducials.iter_mut() { + let position = anchors + .point_in_parent_frame_of(point.0, Category::Fiducial, e) + .unwrap(); + tf.translation = position; + } +} + +pub fn update_fiducial_for_moved_anchors( + mut fiducials: Query<(Entity, &Point, &mut Transform), With>, + anchors: AnchorParams, + changed_anchors: Query< + &Dependents, + ( + With, + Or<(Changed, Changed)>, + ), + >, +) { + for dependents in &changed_anchors { + for dependent in dependents.iter() { + if let Ok((e, point, mut tf)) = fiducials.get_mut(*dependent) { + let position = anchors + .point_in_parent_frame_of(point.0, Category::Fiducial, e) + .unwrap(); + tf.translation = position; + } + } + } +} diff --git a/rmf_site_editor/src/site/measurement.rs b/rmf_site_editor/src/site/measurement.rs index 4dd429e5..ea5d79a8 100644 --- a/rmf_site_editor/src/site/measurement.rs +++ b/rmf_site_editor/src/site/measurement.rs @@ -19,6 +19,8 @@ use crate::{interaction::Selectable, site::*}; use bevy::prelude::*; use rmf_site_format::{Edge, MeasurementMarker}; +pub const MEASUREMENT_LAYER_START: f32 = DRAWING_LAYER_START + 0.001; + pub fn add_measurement_visuals( mut commands: Commands, measurements: Query<(Entity, &Edge), Added>, @@ -27,20 +29,23 @@ pub fn add_measurement_visuals( assets: Res, ) { for (e, edge) in &measurements { + let mut transform = line_stroke_transform( + &anchors + .point_in_parent_frame_of(edge.start(), Category::Measurement, e) + .unwrap(), + &anchors + .point_in_parent_frame_of(edge.end(), Category::Measurement, e) + .unwrap(), + LANE_WIDTH, + ); + // TODO(luca) proper layering rather than hardcoded + transform.translation.z = MEASUREMENT_LAYER_START; commands .entity(e) .insert(PbrBundle { mesh: assets.lane_mid_mesh.clone(), material: assets.measurement_material.clone(), - transform: line_stroke_transform( - &anchors - .point_in_parent_frame_of(edge.start(), Category::Measurement, e) - .unwrap(), - &anchors - .point_in_parent_frame_of(edge.end(), Category::Measurement, e) - .unwrap(), - LANE_WIDTH, - ), + transform, ..default() }) .insert(Selectable::new(e)) @@ -68,6 +73,7 @@ fn update_measurement_visual( .point_in_parent_frame_of(edge.end(), Category::Measurement, entity) .unwrap(); *transform = line_stroke_transform(&start_anchor, &end_anchor, LANE_WIDTH); + transform.translation.z = MEASUREMENT_LAYER_START; } pub fn update_changed_measurement( diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index a3a405d0..7c016d29 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -39,6 +39,9 @@ pub use door::*; pub mod drawing; pub use drawing::*; +pub mod fiducial; +pub use fiducial::*; + pub mod floor; pub use floor::*; @@ -237,6 +240,7 @@ impl Plugin for SitePlugin { .with_system(update_floor_visibility) .with_system(add_lane_visuals) .with_system(add_location_visuals) + .with_system(add_fiducial_visuals) .with_system(update_level_visibility) .with_system(update_changed_lane) .with_system(update_lane_for_moved_anchor) @@ -250,6 +254,8 @@ impl Plugin for SitePlugin { ) .with_system(update_changed_location) .with_system(update_location_for_moved_anchors) + .with_system(update_changed_fiducial) + .with_system(update_fiducial_for_moved_anchors) .with_system(handle_consider_associated_graph) .with_system(handle_consider_location_tag) .with_system(update_lift_for_moved_anchors) diff --git a/rmf_site_editor/src/widgets/create.rs b/rmf_site_editor/src/widgets/create.rs index 33d7159b..936e45e3 100644 --- a/rmf_site_editor/src/widgets/create.rs +++ b/rmf_site_editor/src/widgets/create.rs @@ -88,8 +88,16 @@ impl<'a, 'w, 's> CreateWidget<'a, 'w, 's> { } } AppState::SiteDrawingEditor => { - if ui.button("Drawing").clicked() { - println!("New drawing"); + if ui.button("Measurement").clicked() { + self.events.request.change_mode.send(ChangeMode::To( + SelectAnchor::create_one_new_edge().for_measurement().into(), + )); + } + // TODO(luca) implement + if ui.button("Fiducial").clicked() { + self.events.request.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_point().for_location().into(), + )); } } AppState::WorkcellEditor => { @@ -154,6 +162,7 @@ impl<'a, 'w, 's> CreateWidget<'a, 'w, 's> { pixels_per_meter: PixelsPerMeter(100.0), marker: DrawingMarker, }; + self.events.commands.spawn(drawing); } } } diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index d03f24f9..ecd4c587 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -67,6 +67,7 @@ pub fn top_menu_bar( ui.checkbox(&mut category_settings.0.lanes, "Lanes"); ui.checkbox(&mut category_settings.0.lifts, "Lifts"); ui.checkbox(&mut category_settings.0.locations, "Locations"); + ui.checkbox(&mut category_settings.0.fiducials, "Fiducials"); ui.checkbox(&mut category_settings.0.measurements, "Measurements"); ui.checkbox(&mut category_settings.0.models, "Models"); ui.checkbox(&mut category_settings.0.walls, "Walls"); diff --git a/rmf_site_format/src/category.rs b/rmf_site_format/src/category.rs index 7fb5351f..fdb378f4 100644 --- a/rmf_site_format/src/category.rs +++ b/rmf_site_format/src/category.rs @@ -39,6 +39,7 @@ pub enum Category { Light, Location, Measurement, + Fiducial, Model, Camera, Drawing, @@ -60,6 +61,7 @@ impl Category { Self::Light => "Light", Self::Location => "Location", Self::Measurement => "Measurement", + Self::Fiducial => "Fiducial", Self::Model => "Model", Self::Camera => "Camera", Self::Drawing => "Drawing", From 88407a50194efcd1bd60ca0e83d52a805c17526f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 18 May 2023 18:22:24 +0800 Subject: [PATCH 06/80] Make fiducials selectable, cleanup Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/category_visibility.rs | 5 +---- rmf_site_editor/src/interaction/outline.rs | 5 +++-- rmf_site_editor/src/site/drawing.rs | 9 +++------ rmf_site_editor/src/site/fiducial.rs | 6 ++++-- rmf_site_editor/src/site/measurement.rs | 3 +-- rmf_site_editor/src/site/model.rs | 7 ++----- rmf_site_editor/src/site_asset_io.rs | 1 - rmf_site_editor/src/widgets/create.rs | 8 -------- rmf_site_editor/src/widgets/menu_bar.rs | 10 +++++----- rmf_site_editor/src/workspace.rs | 2 -- 10 files changed, 19 insertions(+), 37 deletions(-) diff --git a/rmf_site_editor/src/interaction/category_visibility.rs b/rmf_site_editor/src/interaction/category_visibility.rs index 8eca4a7d..66fa69ff 100644 --- a/rmf_site_editor/src/interaction/category_visibility.rs +++ b/rmf_site_editor/src/interaction/category_visibility.rs @@ -17,9 +17,6 @@ use bevy::ecs::system::SystemParam; use bevy::prelude::*; -use bevy::utils::HashSet; - -use crate::{site::CurrentLevel, CurrentWorkspace}; use rmf_site_format::{ DoorMarker, FiducialMarker, FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, @@ -82,7 +79,7 @@ pub struct FilterParams<'w, 's> { fn update_visibility( enabled: bool, - mut visibilities: &mut Query<&mut Visibility>, + visibilities: &mut Query<&mut Visibility>, entities: Vec, ) { for e in entities.iter() { diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs index 1d1a699e..47784ea6 100644 --- a/rmf_site_editor/src/interaction/outline.rs +++ b/rmf_site_editor/src/interaction/outline.rs @@ -19,8 +19,8 @@ use crate::interaction::*; use bevy::render::view::RenderLayers; use bevy_mod_outline::{OutlineBundle, OutlineRenderLayers, OutlineVolume, SetOutlineDepth}; use rmf_site_format::{ - DoorType, FloorMarker, LiftCabin, LightKind, LocationTags, MeasurementMarker, ModelMarker, - PhysicalCameraProperties, WallMarker, + DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, MeasurementMarker, + ModelMarker, PhysicalCameraProperties, WallMarker, }; use smallvec::SmallVec; @@ -114,6 +114,7 @@ pub fn add_outline_visualization( Added, Added>, Added, + Added, Added, Added, Added, diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index f0108656..6246c7d5 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -24,7 +24,7 @@ use crate::{ }, CurrentWorkspace, }; -use bevy::{asset::LoadState, math::Affine3A, prelude::*, utils::HashMap}; +use bevy::{asset::LoadState, math::Affine3A, prelude::*}; use rmf_site_format::{AssetSource, DrawingMarker, PixelsPerMeter, Pose}; pub const DRAWING_LAYER_START: f32 = 0.0; @@ -46,10 +46,7 @@ fn drawing_layer_height(rank: Option<&RecencyRank>) -> f32 { pub fn add_drawing_visuals( mut commands: Commands, - new_drawings: Query< - (Entity, &AssetSource, &Pose, &PixelsPerMeter), - (With, Changed), - >, + new_drawings: Query<(Entity, &AssetSource), (With, Changed)>, asset_server: Res, current_workspace: Res, site_files: Query<&DefaultFile>, @@ -62,7 +59,7 @@ pub fn add_drawing_visuals( Some(file_path) => file_path, None => return, }; - for (e, source, pose, pixels_per_meter) in &new_drawings { + for (e, source) in &new_drawings { // Append file name to path if it's a local file // TODO(luca) cleanup let asset_source = match source { diff --git a/rmf_site_editor/src/site/fiducial.rs b/rmf_site_editor/src/site/fiducial.rs index 14617ec6..30e5d32c 100644 --- a/rmf_site_editor/src/site/fiducial.rs +++ b/rmf_site_editor/src/site/fiducial.rs @@ -15,8 +15,9 @@ * */ +use crate::interaction::VisualCue; use crate::site::*; -use bevy::{prelude::*, render::primitives::Sphere}; +use bevy::prelude::*; pub fn add_fiducial_visuals( mut commands: Commands, @@ -37,7 +38,8 @@ pub fn add_fiducial_visuals( material: assets.fiducial_material.clone(), ..default() }) - .insert(Category::Fiducial); + .insert(Category::Fiducial) + .insert(VisualCue::outline()); } } diff --git a/rmf_site_editor/src/site/measurement.rs b/rmf_site_editor/src/site/measurement.rs index ea5d79a8..2f483075 100644 --- a/rmf_site_editor/src/site/measurement.rs +++ b/rmf_site_editor/src/site/measurement.rs @@ -15,7 +15,7 @@ * */ -use crate::{interaction::Selectable, site::*}; +use crate::site::*; use bevy::prelude::*; use rmf_site_format::{Edge, MeasurementMarker}; @@ -48,7 +48,6 @@ pub fn add_measurement_visuals( transform, ..default() }) - .insert(Selectable::new(e)) .insert(Category::Measurement) .insert(EdgeLabels::StartEnd); diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index f4c4c1f4..b7428ffc 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -84,10 +84,7 @@ pub fn update_model_scenes( (Changed, With), >, asset_server: Res, - loading_models: Query< - (Entity, &TentativeModelFormat, &PendingSpawning, &Scale), - With, - >, + loading_models: Query<(Entity, &PendingSpawning, &Scale), With>, spawned_models: Query< Entity, ( @@ -154,7 +151,7 @@ pub fn update_model_scenes( // For each model that is loading, check if its scene has finished loading // yet. If the scene has finished loading, then insert it as a child of the // model entity and make it selectable. - for (e, tentative_format, h, scale) in loading_models.iter() { + for (e, h, scale) in loading_models.iter() { if asset_server.get_load_state(&h.0) == LoadState::Loaded { let model_id = if let Some(gltf) = gltfs.get(&h.typed_weak::()) { Some(commands.entity(e).add_children(|parent| { diff --git a/rmf_site_editor/src/site_asset_io.rs b/rmf_site_editor/src/site_asset_io.rs index f50dc4f4..a934209b 100644 --- a/rmf_site_editor/src/site_asset_io.rs +++ b/rmf_site_editor/src/site_asset_io.rs @@ -116,7 +116,6 @@ impl SiteAssetIo { // Expected format: OrgName/ModelName/FileName.ext // We may need to be a bit magical here because some assets // are found in Fuel and others are not. - let name_buf = PathBuf::from(name); let binding = name.clone(); let mut tokens = binding.split("/"); let org_name = match tokens.next() { diff --git a/rmf_site_editor/src/widgets/create.rs b/rmf_site_editor/src/widgets/create.rs index 936e45e3..8d01168d 100644 --- a/rmf_site_editor/src/widgets/create.rs +++ b/rmf_site_editor/src/widgets/create.rs @@ -171,10 +171,6 @@ impl<'a, 'w, 's> CreateWidget<'a, 'w, 's> { self.events.pending_asset_sources.get_single() { if ui.button("Spawn visual").clicked() { - let model = Model { - source: source.clone(), - ..default() - }; let workcell_model = WorkcellModel { geometry: Geometry::Mesh { filename: source.into(), @@ -189,10 +185,6 @@ impl<'a, 'w, 's> CreateWidget<'a, 'w, 's> { )); } if ui.button("Spawn collision").clicked() { - let model = Model { - source: source.clone(), - ..default() - }; let workcell_model = WorkcellModel { geometry: Geometry::Mesh { filename: source.into(), diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index ecd4c587..bea68854 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -16,8 +16,8 @@ */ use crate::{ - interaction::VisibilityCategoriesSettings, AppState, CreateNewWorkspace, CurrentWorkspace, - FileEvents, LoadWorkspace, SaveWorkspace, + interaction::VisibilityCategoriesSettings, CreateNewWorkspace, FileEvents, LoadWorkspace, + SaveWorkspace, }; use bevy_egui::{ @@ -26,9 +26,9 @@ use bevy_egui::{ }; pub fn top_menu_bar( - mut egui_context: &mut EguiContext, - mut file_events: &mut FileEvents, - mut category_settings: &mut VisibilityCategoriesSettings, + egui_context: &mut EguiContext, + file_events: &mut FileEvents, + category_settings: &mut VisibilityCategoriesSettings, ) { egui::TopBottomPanel::top("top_panel").show(egui_context.ctx_mut(), |ui| { egui::menu::bar(ui, |ui| { diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index f1bdc6bd..22c38ceb 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -155,7 +155,6 @@ pub fn dispatch_new_workspace_events( } pub fn dispatch_load_workspace_events( - mut commands: Commands, mut app_state: ResMut>, mut interaction_state: ResMut>, mut load_channels: ResMut, @@ -288,7 +287,6 @@ fn handle_workspace_data( /// Handles the file opening events fn workspace_file_load_complete( - mut commands: Commands, mut app_state: ResMut>, mut interaction_state: ResMut>, mut load_site: EventWriter, From 872b0574b1ef017f50b1041ad5ae93a9c212759c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 19 May 2023 14:20:28 +0800 Subject: [PATCH 07/80] Spawn traffic editor layers Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/fiducial.rs | 1 - rmf_site_editor/src/site/save.rs | 12 ++++++-- rmf_site_editor/src/widgets/create.rs | 1 + rmf_site_format/src/drawing.rs | 2 +- rmf_site_format/src/legacy/building_map.rs | 34 ++++++++++++++++++++++ rmf_site_format/src/legacy/level.rs | 21 ++++++++++++- 6 files changed, 66 insertions(+), 5 deletions(-) diff --git a/rmf_site_editor/src/site/fiducial.rs b/rmf_site_editor/src/site/fiducial.rs index 30e5d32c..6fa566d9 100644 --- a/rmf_site_editor/src/site/fiducial.rs +++ b/rmf_site_editor/src/site/fiducial.rs @@ -30,7 +30,6 @@ pub fn add_fiducial_visuals( deps.insert(e); } - println!("Adding fiducial mesh"); commands .entity(e) .insert(PbrBundle { diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 42972aa7..acda748d 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -203,7 +203,14 @@ fn generate_levels( Without, >, Query< - (&AssetSource, &Pose, &PixelsPerMeter, &SiteID, &Parent), + ( + &NameInSite, + &AssetSource, + &Pose, + &PixelsPerMeter, + &SiteID, + &Parent, + ), (With, Without), >, Query< @@ -368,12 +375,13 @@ fn generate_levels( } } - for (source, pose, pixels_per_meter, id, parent) in &q_drawings { + for (name, source, pose, pixels_per_meter, id, parent) in &q_drawings { if let Ok((_, level_id, _, _, _)) = q_levels.get(parent.get()) { if let Some(level) = levels.get_mut(&level_id.0) { level.drawings.insert( id.0, Drawing { + name: name.clone(), source: source.clone(), pose: pose.clone(), pixels_per_meter: pixels_per_meter.clone(), diff --git a/rmf_site_editor/src/widgets/create.rs b/rmf_site_editor/src/widgets/create.rs index 8d01168d..a3d94f0f 100644 --- a/rmf_site_editor/src/widgets/create.rs +++ b/rmf_site_editor/src/widgets/create.rs @@ -157,6 +157,7 @@ impl<'a, 'w, 's> CreateWidget<'a, 'w, 's> { { if ui.button("Add Drawing").clicked() { let drawing = Drawing { + name: Default::default(), source: source.clone(), pose: Pose::default(), pixels_per_meter: PixelsPerMeter(100.0), diff --git a/rmf_site_format/src/drawing.rs b/rmf_site_format/src/drawing.rs index 4662abc4..d6fea540 100644 --- a/rmf_site_format/src/drawing.rs +++ b/rmf_site_format/src/drawing.rs @@ -30,10 +30,10 @@ impl Default for PixelsPerMeter { } } -// TODO(MXG): Consider adding NameInSite field #[derive(Serialize, Deserialize, Debug, Clone)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct Drawing { + pub name: NameInSite, pub source: AssetSource, pub pose: Pose, pub pixels_per_meter: PixelsPerMeter, diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index b366a000..26393df2 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -10,6 +10,7 @@ use crate::{ use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; use std::collections::{BTreeMap, HashMap}; +use std::path::Path; #[derive(Deserialize, Serialize, Clone)] #[serde(rename_all = "snake_case")] @@ -194,6 +195,14 @@ impl BuildingMap { drawings.insert( id, SiteDrawing { + name: NameInSite( + Path::new(&level.drawing.filename) + .file_stem() + .unwrap_or_default() + .to_str() + .unwrap() + .to_string(), + ), source: AssetSource::Local(level.drawing.filename.clone()), pose, pixels_per_meter, @@ -224,6 +233,31 @@ impl BuildingMap { ); } + for (name, layer) in &level.layers { + // TODO(luca) coordinates in site and traffic editor might be different, use + // optimization engine instead of parsing + let id = site_id.next().unwrap(); + let pose = Pose { + trans: [ + layer.transform.translation_x as f32, + layer.transform.translation_y as f32, + 0.0 as f32, + ], + rot: Rotation::Yaw(Angle::Rad(layer.transform.yaw as f32)), + }; + drawings.insert( + id, + SiteDrawing { + name: NameInSite(name.clone()), + source: AssetSource::Local(layer.filename.clone()), + pose, + pixels_per_meter: PixelsPerMeter((1.0 / layer.transform.scale) as f32), + marker: DrawingMarker, + }, + ); + rankings.drawings.insert(0, id); + } + let mut floors = BTreeMap::new(); for floor in &level.floors { let site_floor = floor.to_site(&vertex_to_anchor_id)?; diff --git a/rmf_site_format/src/legacy/level.rs b/rmf_site_format/src/legacy/level.rs index 5beb68a7..1d29bc2c 100644 --- a/rmf_site_format/src/legacy/level.rs +++ b/rmf_site_format/src/legacy/level.rs @@ -5,13 +5,14 @@ use super::{ }; use glam::{DAffine2, DVec2}; use serde::{Deserialize, Serialize}; +use std::collections::BTreeMap; #[derive(Deserialize, Serialize, Clone, Default)] pub struct LevelDrawing { pub filename: String, } -#[derive(Clone, Copy)] +#[derive(Debug, Clone, Copy)] pub struct Alignment { pub translation: DVec2, pub rotation: f64, @@ -28,6 +29,22 @@ impl Alignment { } } +#[derive(Deserialize, Serialize, Clone, Debug)] +pub struct LayerTransform { + pub scale: f64, + pub translation_x: f64, + pub translation_y: f64, + pub yaw: f64, +} + +#[derive(Deserialize, Serialize, Clone, Debug)] +pub struct Layer { + // TODO(luca) add color and features + pub filename: String, + pub transform: LayerTransform, + pub visible: bool, +} + // TODO(luca) add layers vector for robot maps #[derive(Deserialize, Serialize, Clone, Default)] pub struct Level { @@ -49,6 +66,8 @@ pub struct Level { #[serde(default)] pub floors: Vec, #[serde(default)] + pub layers: BTreeMap, + #[serde(default)] pub physical_cameras: Vec, #[serde(default)] pub fiducials: Vec, From 47e39f4eb7c5645dd57ed073623751f8576d36e4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 24 May 2023 10:51:10 +0800 Subject: [PATCH 08/80] WIP fiducials and features as children of drawings Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/drawing.rs | 70 +++++++++---- rmf_site_editor/src/site/load.rs | 46 ++++++--- rmf_site_editor/src/site/mod.rs | 1 + rmf_site_editor/src/site/save.rs | 40 +++++--- rmf_site_editor/src/widgets/create.rs | 4 +- rmf_site_format/src/drawing.rs | 16 ++- rmf_site_format/src/legacy/building_map.rs | 110 +++++++++++++++------ rmf_site_format/src/legacy/level.rs | 24 +++++ rmf_site_format/src/level.rs | 3 - 9 files changed, 233 insertions(+), 81 deletions(-) diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index 6246c7d5..832ead83 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -19,8 +19,8 @@ use crate::{ interaction::Selectable, shapes::make_flat_rect_mesh, site::{ - get_current_workspace_path, Category, DefaultFile, FloorVisibility, RecencyRank, - FLOOR_LAYER_START, + get_current_workspace_path, Anchor, Category, DefaultFile, FiducialMarker, FloorVisibility, + RecencyRank, FLOOR_LAYER_START, }, CurrentWorkspace, }; @@ -89,6 +89,8 @@ pub fn handle_loaded_drawing( &LoadingDrawing, )>, mut mesh_assets: ResMut>, + children: Query<&Children>, + mut anchors: Query<&mut Anchor>, asset_server: Res, mut materials: ResMut>, rank: Query<&RecencyRank>, @@ -101,32 +103,41 @@ pub fn handle_loaded_drawing( let width = img.texture_descriptor.size.width as f32; let height = img.texture_descriptor.size.height as f32; + //let centering_vec = Vec3::new(width / 2.0, -height / 2.0, 0.0); + let mut centering_vec = Vec3::new(0.0, 0.0, 0.0); + // We set this up so that the origin of the drawing is in - let mesh = make_flat_rect_mesh(width, height).transform_by( - Affine3A::from_translation(Vec3::new(width / 2.0, -height / 2.0, 0.0)), - ); + let mesh = make_flat_rect_mesh(width, height) + .transform_by(Affine3A::from_translation(centering_vec)); let mesh = mesh_assets.add(mesh.into()); + centering_vec = Vec3::new(-width / 2.0, height / 2.0, 0.0); + + // Also translate the child anchors to center them + if let Ok(children) = children.get(entity) { + for child in children.iter() { + if let Ok(mut anchor) = anchors.get_mut(*child) { + let original_tf = anchor.local_transform(Category::General); + anchor.move_to( + &(Transform::from_translation(centering_vec) * original_tf), + ); + } + } + } + let leaf = if let Ok(segment) = segments.get(entity) { segment.leaf // We can ignore the layer height here since that update // will be handled by another system. } else { - let transform = pose.transform().with_scale(Vec3::new( - 1.0 / pixels_per_meter.0, - 1.0 / pixels_per_meter.0, - 1., - )); let mut cmd = commands.entity(entity); let leaf = cmd.add_children(|p| p.spawn_empty().id()); - cmd.insert(SpatialBundle { - transform, - ..default() - }) - .insert(DrawingSegments { leaf }) - .insert(Selectable::new(entity)) - .insert(Category::Drawing); + cmd.insert(DrawingSegments { leaf }) + .insert(SpatialBundle::from_transform(pose.transform().with_scale( + Vec3::new(1.0 / pixels_per_meter.0, 1.0 / pixels_per_meter.0, 1.), + ))) + .insert(Selectable::new(entity)); leaf }; let z = drawing_layer_height(rank.get(entity).ok()); @@ -168,7 +179,30 @@ pub fn update_drawing_rank( pub fn update_drawing_pixels_per_meter( mut changed_drawings: Query<(&mut Transform, &PixelsPerMeter), Changed>, ) { - for (mut tf, pixels_per_meter) in &mut changed_drawings { + for (mut tf, pixels_per_meter) in changed_drawings.iter_mut() { tf.scale = Vec3::new(1.0 / pixels_per_meter.0, 1.0 / pixels_per_meter.0, 1.); } } + +pub fn update_anchor_and_fiducial_visuals_for_changed_pixels_per_meter( + changed_drawings: Query<(Entity, &PixelsPerMeter), Changed>, + visuals: Query< + Entity, + ( + Without, + Or<(With, With)>, + ), + >, + children: Query<&Children>, + mut transforms: Query<&mut Transform>, +) { + for (e, pixels_per_meter) in changed_drawings.iter() { + for child in DescendantIter::new(&children, e) { + if visuals.get(child).is_ok() { + if let Ok(mut tf) = transforms.get_mut(child) { + tf.scale = Vec3::new(pixels_per_meter.0, pixels_per_meter.0, 1.0); + } + } + } + } +} diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 8c50fdc8..3969a149 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -48,10 +48,7 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: } }; - let mut site_cmd = commands.spawn(SpatialBundle { - visibility: Visibility { is_visible: false }, - ..default() - }); + let mut site_cmd = commands.spawn(SpatialBundle::INVISIBLE_IDENTITY); site_cmd .insert(Category::Site) .insert(site_data.properties.clone()) @@ -69,10 +66,7 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: let mut level_cmd = site.spawn(SiteID(*level_id)); level_cmd - .insert(SpatialBundle { - visibility: Visibility { is_visible: false }, - ..default() - }) + .insert(SpatialBundle::INVISIBLE_IDENTITY) .insert(level_data.properties.clone()) .insert(Category::Level) .with_children(|level| { @@ -95,17 +89,37 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: } for (drawing_id, drawing) in &level_data.drawings { - level.spawn(drawing.clone()).insert(SiteID(*drawing_id)); + let drawing_entity = level + .spawn(DrawingBundle { + name: drawing.name.clone(), + source: drawing.source.clone(), + pose: drawing.pose.clone(), + pixels_per_meter: drawing.pixels_per_meter.clone(), + marker: DrawingMarker, + }) + .insert(SiteID(*drawing_id)) + .insert(Category::Drawing) + .with_children(|drawing_parent| { + for (anchor_id, anchor) in &drawing.anchors { + let anchor_entity = drawing_parent + .spawn(AnchorBundle::new(anchor.clone())) + .insert(SiteID(*anchor_id)) + .id(); + id_to_entity.insert(*anchor_id, anchor_entity); + consider_id(*anchor_id); + } + for (fiducial_id, fiducial) in &drawing.fiducials { + drawing_parent + .spawn(fiducial.to_ecs(&id_to_entity)) + .insert(SiteID(*fiducial_id)); + //id_to_entity.insert(**fiducial_id, anchor_entity); + consider_id(*fiducial_id); + } + }) + .id(); consider_id(*drawing_id); } - for (fiducial_id, fiducial) in &level_data.fiducials { - level - .spawn(fiducial.to_ecs(&id_to_entity)) - .insert(SiteID(*fiducial_id)); - consider_id(*fiducial_id); - } - for (floor_id, floor) in &level_data.floors { level .spawn(floor.to_ecs(&id_to_entity)) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 7c016d29..c7194065 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -271,6 +271,7 @@ impl Plugin for SitePlugin { .with_system(make_models_selectable) .with_system(handle_new_mesh_primitives) .with_system(add_drawing_visuals) + .with_system(update_anchor_and_fiducial_visuals_for_changed_pixels_per_meter) .with_system(handle_loaded_drawing) .with_system(update_drawing_rank) .with_system(update_drawing_pixels_per_meter) diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index acda748d..cc460b32 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -382,10 +382,11 @@ fn generate_levels( id.0, Drawing { name: name.clone(), + anchors: BTreeMap::default(), + fiducials: BTreeMap::default(), source: source.clone(), pose: pose.clone(), pixels_per_meter: pixels_per_meter.clone(), - marker: DrawingMarker, }, ); } @@ -394,17 +395,31 @@ fn generate_levels( for (point, o_point, label, id, parent) in &q_fiducials { let point = o_point.map(|x| &x.0).unwrap_or(point); - if let Ok((_, level_id, _, _, _)) = q_levels.get(parent.get()) { - if let Some(level) = levels.get_mut(&level_id.0) { - let anchor = Point(get_anchor_id(point.0)?); - level.fiducials.insert( - id.0, - Fiducial { - anchor, - label: label.clone(), - marker: FiducialMarker, - }, - ); + // Fiducials have a drawing, then a level parent + if let Ok((_, _, _, _, drawing_id, drawing_parent)) = q_drawings.get(parent.get()) { + if let Ok((_, level_id, _, _, _)) = q_levels.get(drawing_parent.get()) { + if let Some(level) = levels.get_mut(&level_id.0) { + if let Some(drawing) = level.drawings.get_mut(&drawing_id.0) { + let anchor = Point(get_anchor_id(point.0)?); + if let Ok((anchor, anchor_id, anchor_parent)) = q_anchors.get(point.0) { + if let Ok((_, _, _, _, anchor_drawing_id, _)) = + q_drawings.get(parent.get()) + { + if anchor_drawing_id.0 == drawing_id.0 { + drawing.anchors.insert(anchor_id.0, anchor.clone()); + } + } + } + drawing.fiducials.insert( + id.0, + Fiducial { + anchor, + label: label.clone(), + marker: FiducialMarker, + }, + ); + } + } } } } @@ -986,7 +1001,6 @@ pub fn save_nav_graphs(world: &mut World) { for (_, level) in &mut site.levels { level.doors.clear(); level.drawings.clear(); - level.fiducials.clear(); level.floors.clear(); level.lights.clear(); level.measurements.clear(); diff --git a/rmf_site_editor/src/widgets/create.rs b/rmf_site_editor/src/widgets/create.rs index a3d94f0f..40a48ec0 100644 --- a/rmf_site_editor/src/widgets/create.rs +++ b/rmf_site_editor/src/widgets/create.rs @@ -25,7 +25,7 @@ use bevy::prelude::*; use bevy_egui::egui::{CollapsingHeader, Ui}; use rmf_site_format::{ - AssetSource, Drawing, DrawingMarker, Geometry, Model, Pending, PixelsPerMeter, Pose, + AssetSource, DrawingBundle, DrawingMarker, Geometry, Model, Pending, PixelsPerMeter, Pose, RecallAssetSource, Scale, WorkcellModel, }; @@ -156,7 +156,7 @@ impl<'a, 'w, 's> CreateWidget<'a, 'w, 's> { self.events.pending_asset_sources.get_single() { if ui.button("Add Drawing").clicked() { - let drawing = Drawing { + let drawing = DrawingBundle { name: Default::default(), source: source.clone(), pose: Pose::default(), diff --git a/rmf_site_format/src/drawing.rs b/rmf_site_format/src/drawing.rs index d6fea540..2a7f7ce8 100644 --- a/rmf_site_format/src/drawing.rs +++ b/rmf_site_format/src/drawing.rs @@ -19,6 +19,7 @@ use crate::*; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component}; use serde::{Deserialize, Serialize}; +use std::collections::BTreeMap; #[derive(Serialize, Deserialize, Debug, Clone, Copy)] #[cfg_attr(feature = "bevy", derive(Component))] @@ -31,8 +32,21 @@ impl Default for PixelsPerMeter { } #[derive(Serialize, Deserialize, Debug, Clone)] -#[cfg_attr(feature = "bevy", derive(Bundle))] pub struct Drawing { + pub name: NameInSite, + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub anchors: BTreeMap, + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub fiducials: BTreeMap>, + pub source: AssetSource, + pub pose: Pose, + pub pixels_per_meter: PixelsPerMeter, +} + +// TODO(luca) move this to rmf_site_editor? +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct DrawingBundle { pub name: NameInSite, pub source: AssetSource, pub pose: Pose, diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 26393df2..83066b62 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -99,9 +99,17 @@ impl BuildingMap { } for fiducial in &mut level.fiducials { - let p = tf.transform_point2(fiducial.to_vec()); - fiducial.0 = p.x; - fiducial.1 = -p.y; + fiducial.1 = -fiducial.1; + } + + for feature in &mut level.features { + feature.y = -feature.y; + } + + for (_, layer) in &mut level.layers { + for feature in &mut layer.features { + feature.y = -feature.y; + } } } @@ -179,6 +187,7 @@ impl BuildingMap { let mut rankings = RankingsInLevel::default(); let mut drawings = BTreeMap::new(); + let mut feature_id_map = HashMap::new(); if !level.drawing.filename.is_empty() { let (pose, pixels_per_meter) = if let Some(a) = level.alignment { let p = a.translation; @@ -191,6 +200,49 @@ impl BuildingMap { (Pose::default(), PixelsPerMeter::default()) }; + let mut anchors = BTreeMap::new(); + let mut fiducials = BTreeMap::new(); + for fiducial in &level.fiducials { + let anchor_id = site_id.next().unwrap(); + anchors.insert(anchor_id, [fiducial.0 as f32, fiducial.1 as f32].into()); + // Do not add this anchor to the vertex_to_anchor_id map because + // this fiducial is not really recognized as a vertex to the + // building format. + fiducials.insert( + site_id.next().unwrap(), + SiteFiducial { + label: if fiducial.2.is_empty() { + Label(None) + } else { + Label(Some(fiducial.2.clone())) + }, + anchor: anchor_id.into(), + marker: FiducialMarker, + }, + ); + } + + for feature in &level.features { + let anchor_id = site_id.next().unwrap(); + anchors.insert(anchor_id, [feature.x as f32, feature.y as f32].into()); + feature_id_map.insert(feature.id.clone(), anchor_id); + // Do not add this anchor to the vertex_to_anchor_id map because + // this fiducial is not really recognized as a vertex to the + // building format. + fiducials.insert( + site_id.next().unwrap(), + SiteFiducial { + label: if feature.name.is_empty() { + Label(None) + } else { + Label(Some(feature.name.clone())) + }, + anchor: anchor_id.into(), + marker: FiducialMarker, + }, + ); + } + let id = site_id.next().unwrap(); drawings.insert( id, @@ -203,36 +255,16 @@ impl BuildingMap { .unwrap() .to_string(), ), + anchors, + fiducials, source: AssetSource::Local(level.drawing.filename.clone()), pose, pixels_per_meter, - marker: DrawingMarker, }, ); rankings.drawings.insert(0, id); } - let mut fiducials = BTreeMap::new(); - for fiducial in &level.fiducials { - let anchor_id = site_id.next().unwrap(); - anchors.insert(anchor_id, [fiducial.0 as f32, fiducial.1 as f32].into()); - // Do not add this anchor to the vertex_to_anchor_id map because - // this fiducial is not really recognized as a vertex to the - // building format. - fiducials.insert( - site_id.next().unwrap(), - SiteFiducial { - label: if fiducial.2.is_empty() { - Label(None) - } else { - Label(Some(fiducial.2.clone())) - }, - anchor: anchor_id.into(), - marker: FiducialMarker, - }, - ); - } - for (name, layer) in &level.layers { // TODO(luca) coordinates in site and traffic editor might be different, use // optimization engine instead of parsing @@ -245,17 +277,40 @@ impl BuildingMap { ], rot: Rotation::Yaw(Angle::Rad(layer.transform.yaw as f32)), }; + rankings.drawings.insert(0, id); + let mut anchors = BTreeMap::new(); + let mut fiducials = BTreeMap::new(); + for feature in &layer.features { + let anchor_id = site_id.next().unwrap(); + anchors.insert(anchor_id, [feature.x as f32, feature.y as f32].into()); + feature_id_map.insert(feature.id.clone(), anchor_id); + // Do not add this anchor to the vertex_to_anchor_id map because + // this fiducial is not really recognized as a vertex to the + // building format. + fiducials.insert( + site_id.next().unwrap(), + SiteFiducial { + label: if feature.name.is_empty() { + Label(None) + } else { + Label(Some(feature.name.clone())) + }, + anchor: anchor_id.into(), + marker: FiducialMarker, + }, + ); + } drawings.insert( id, SiteDrawing { name: NameInSite(name.clone()), + anchors, + fiducials, source: AssetSource::Local(layer.filename.clone()), pose, pixels_per_meter: PixelsPerMeter((1.0 / layer.transform.scale) as f32), - marker: DrawingMarker, }, ); - rankings.drawings.insert(0, id); } let mut floors = BTreeMap::new(); @@ -308,7 +363,6 @@ impl BuildingMap { anchors, doors, drawings, - fiducials, floors, lights, measurements, diff --git a/rmf_site_format/src/legacy/level.rs b/rmf_site_format/src/legacy/level.rs index 1d29bc2c..bf552e2c 100644 --- a/rmf_site_format/src/legacy/level.rs +++ b/rmf_site_format/src/legacy/level.rs @@ -42,9 +42,29 @@ pub struct Layer { // TODO(luca) add color and features pub filename: String, pub transform: LayerTransform, + pub features: Vec, pub visible: bool, } +#[derive(Deserialize, Serialize, Clone, Debug)] +pub struct Feature { + pub id: String, + pub name: String, + pub x: f64, + pub y: f64, +} + +impl Feature { + pub fn to_vec(&self) -> DVec2 { + DVec2::new(self.x, self.y) + } +} + +#[derive(Deserialize, Serialize, Clone, Debug)] +pub struct Constraint { + pub ids: [String; 2], +} + // TODO(luca) add layers vector for robot maps #[derive(Deserialize, Serialize, Clone, Default)] pub struct Level { @@ -68,6 +88,10 @@ pub struct Level { #[serde(default)] pub layers: BTreeMap, #[serde(default)] + pub features: Vec, + #[serde(default)] + pub constraints: Vec, + #[serde(default)] pub physical_cameras: Vec, #[serde(default)] pub fiducials: Vec, diff --git a/rmf_site_format/src/level.rs b/rmf_site_format/src/level.rs index 39de1b5f..d8c12560 100644 --- a/rmf_site_format/src/level.rs +++ b/rmf_site_format/src/level.rs @@ -47,8 +47,6 @@ pub struct Level { #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub drawings: BTreeMap, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - pub fiducials: BTreeMap>, - #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub floors: BTreeMap>, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub lights: BTreeMap, @@ -72,7 +70,6 @@ impl Level { anchors: Default::default(), doors: Default::default(), drawings: Default::default(), - fiducials: Default::default(), floors: Default::default(), lights: Default::default(), measurements: Default::default(), From ce8f0cc1c8279e4afb07fd2c6e9d49c6b35915c0 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 24 May 2023 11:20:44 +0800 Subject: [PATCH 09/80] Cleanup and fix system ordering Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/drawing.rs | 19 +------------------ rmf_site_editor/src/site/mod.rs | 2 +- rmf_site_format/src/legacy/building_map.rs | 10 +++++----- 3 files changed, 7 insertions(+), 24 deletions(-) diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index 832ead83..fad8bb31 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -89,8 +89,6 @@ pub fn handle_loaded_drawing( &LoadingDrawing, )>, mut mesh_assets: ResMut>, - children: Query<&Children>, - mut anchors: Query<&mut Anchor>, asset_server: Res, mut materials: ResMut>, rank: Query<&RecencyRank>, @@ -103,28 +101,13 @@ pub fn handle_loaded_drawing( let width = img.texture_descriptor.size.width as f32; let height = img.texture_descriptor.size.height as f32; - //let centering_vec = Vec3::new(width / 2.0, -height / 2.0, 0.0); - let mut centering_vec = Vec3::new(0.0, 0.0, 0.0); + let centering_vec = Vec3::new(width / 2.0, -height / 2.0, 0.0); // We set this up so that the origin of the drawing is in let mesh = make_flat_rect_mesh(width, height) .transform_by(Affine3A::from_translation(centering_vec)); let mesh = mesh_assets.add(mesh.into()); - centering_vec = Vec3::new(-width / 2.0, height / 2.0, 0.0); - - // Also translate the child anchors to center them - if let Ok(children) = children.get(entity) { - for child in children.iter() { - if let Ok(mut anchor) = anchors.get_mut(*child) { - let original_tf = anchor.local_transform(Category::General); - anchor.move_to( - &(Transform::from_translation(centering_vec) * original_tf), - ); - } - } - } - let leaf = if let Ok(segment) = segments.get(entity) { segment.leaf // We can ignore the layer height here since that update diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index c7194065..d3063879 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -200,6 +200,7 @@ impl Plugin for SitePlugin { .with_system(update_lift_cabin) .with_system(update_lift_edge) .with_system(update_model_tentative_formats) + .with_system(update_anchor_and_fiducial_visuals_for_changed_pixels_per_meter) .with_system(update_material_for_display_color), ) .add_system_set( @@ -271,7 +272,6 @@ impl Plugin for SitePlugin { .with_system(make_models_selectable) .with_system(handle_new_mesh_primitives) .with_system(add_drawing_visuals) - .with_system(update_anchor_and_fiducial_visuals_for_changed_pixels_per_meter) .with_system(handle_loaded_drawing) .with_system(update_drawing_rank) .with_system(update_drawing_pixels_per_meter) diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 83066b62..5a6b9a36 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -1,11 +1,11 @@ use super::{level::Level, lift::Lift, PortingError, Result}; use crate::{ legacy::optimization::align_building, Anchor, Angle, AssetSource, AssociatedGraphs, - DisplayColor, Dock as SiteDock, Drawing as SiteDrawing, DrawingMarker, - Fiducial as SiteFiducial, FiducialMarker, Guided, Label, Lane as SiteLane, LaneMarker, - Level as SiteLevel, LevelProperties as SiteLevelProperties, Motion, NameInSite, NavGraph, - Navigation, OrientationConstraint, PixelsPerMeter, Pose, RankingsInLevel, ReverseLane, - Rotation, Site, SiteProperties, DEFAULT_NAV_GRAPH_COLORS, + DisplayColor, Dock as SiteDock, Drawing as SiteDrawing, Fiducial as SiteFiducial, + FiducialMarker, Guided, Label, Lane as SiteLane, LaneMarker, Level as SiteLevel, + LevelProperties as SiteLevelProperties, Motion, NameInSite, NavGraph, Navigation, + OrientationConstraint, PixelsPerMeter, Pose, RankingsInLevel, ReverseLane, Rotation, Site, + SiteProperties, DEFAULT_NAV_GRAPH_COLORS, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; From 4911a89f947760624d65808a8081ddb601a90103 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 25 May 2023 09:30:18 +0800 Subject: [PATCH 10/80] Fiducial and anchor scaling working Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/anchor.rs | 16 +++- rmf_site_editor/src/site/anchor.rs | 2 +- rmf_site_editor/src/site/assets.rs | 9 ++- rmf_site_editor/src/site/drawing.rs | 32 ++++---- rmf_site_editor/src/site/load.rs | 13 ++-- rmf_site_editor/src/site/mod.rs | 4 +- rmf_site_editor/src/site/save.rs | 91 ++++++++++------------ rmf_site_format/src/drawing.rs | 2 + rmf_site_format/src/legacy/building_map.rs | 60 ++++++++++---- rmf_site_format/src/level.rs | 3 - 10 files changed, 130 insertions(+), 102 deletions(-) diff --git a/rmf_site_editor/src/interaction/anchor.rs b/rmf_site_editor/src/interaction/anchor.rs index 5c8d9a9c..3e334399 100644 --- a/rmf_site_editor/src/interaction/anchor.rs +++ b/rmf_site_editor/src/interaction/anchor.rs @@ -32,16 +32,24 @@ pub struct AnchorVisualization { pub fn add_anchor_visual_cues( mut commands: Commands, - new_anchors: Query< - (Entity, &Parent, Option<&Subordinate>, &Anchor), + mut new_anchors: Query< + ( + Entity, + &Parent, + Option<&Subordinate>, + &Anchor, + Option<&mut Transform>, + ), (Added, Without), >, categories: Query<&Category>, site_assets: Res, interaction_assets: Res, ) { - for (e, parent, subordinate, anchor) in &new_anchors { - let body_mesh = match categories.get(parent.get()).unwrap() { + for (e, parent, subordinate, anchor, mut tf) in new_anchors.iter_mut() { + let category = categories.get(parent.get()).unwrap(); + let body_mesh = match &category { + Category::Drawing => site_assets.drawing_anchor_mesh.clone(), Category::Level => site_assets.level_anchor_mesh.clone(), Category::Lift => site_assets.lift_anchor_mesh.clone(), _ => site_assets.site_anchor_mesh.clone(), diff --git a/rmf_site_editor/src/site/anchor.rs b/rmf_site_editor/src/site/anchor.rs index 0265e560..3ec59880 100644 --- a/rmf_site_editor/src/site/anchor.rs +++ b/rmf_site_editor/src/site/anchor.rs @@ -96,7 +96,7 @@ pub fn update_anchor_transforms( mut changed_anchors: Query<(&Anchor, &mut Transform), Changed>, ) { for (anchor, mut tf) in &mut changed_anchors { - *tf = anchor.local_transform(Category::General); + tf.translation = anchor.local_transform(Category::General).translation; } } diff --git a/rmf_site_editor/src/site/assets.rs b/rmf_site_editor/src/site/assets.rs index d8d7ebcb..76f651b7 100644 --- a/rmf_site_editor/src/site/assets.rs +++ b/rmf_site_editor/src/site/assets.rs @@ -41,6 +41,7 @@ pub struct SiteAssets { pub hover_select_material: Handle, pub measurement_material: Handle, pub fiducial_material: Handle, + pub drawing_anchor_mesh: Handle, pub level_anchor_mesh: Handle, pub lift_anchor_mesh: Handle, pub site_anchor_mesh: Handle, @@ -148,6 +149,11 @@ impl FromWorld for SiteAssets { let default_mesh_grey_material = materials.add(Color::rgb(0.7, 0.7, 0.7).into()); let mut meshes = world.get_resource_mut::>().unwrap(); + let drawing_anchor_mesh = meshes.add( + Mesh::from(make_cylinder(0.005, 0.05)) + .with_generated_outline_normals() + .unwrap(), + ); let level_anchor_mesh = meshes.add( Mesh::from(shape::UVSphere { radius: 0.05, // TODO(MXG): Make the vertex radius configurable @@ -202,7 +208,7 @@ impl FromWorld for SiteAssets { // TODO(luca) have a different mesh let fiducial_mesh = meshes.add( Mesh::from( - make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.05, 4) + make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 4) .transform_by(Affine3A::from_translation(0.00125 * Vec3::Z)), ) .with_generated_outline_normals() @@ -215,6 +221,7 @@ impl FromWorld for SiteAssets { ); Self { + drawing_anchor_mesh, level_anchor_mesh, lift_anchor_mesh, site_anchor_mesh, diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index fad8bb31..b4af8821 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -20,7 +20,7 @@ use crate::{ shapes::make_flat_rect_mesh, site::{ get_current_workspace_path, Anchor, Category, DefaultFile, FiducialMarker, FloorVisibility, - RecencyRank, FLOOR_LAYER_START, + MeasurementMarker, RecencyRank, FLOOR_LAYER_START, }, CurrentWorkspace, }; @@ -167,22 +167,24 @@ pub fn update_drawing_pixels_per_meter( } } -pub fn update_anchor_and_fiducial_visuals_for_changed_pixels_per_meter( - changed_drawings: Query<(Entity, &PixelsPerMeter), Changed>, - visuals: Query< - Entity, - ( - Without, - Or<(With, With)>, - ), - >, - children: Query<&Children>, +pub fn update_meshes_for_changed_pixels_per_meter( + changed_drawings: Query<(Entity, &PixelsPerMeter, &Children), Changed>, + points: Query, With)>>, + measurements: Query>, mut transforms: Query<&mut Transform>, ) { - for (e, pixels_per_meter) in changed_drawings.iter() { - for child in DescendantIter::new(&children, e) { - if visuals.get(child).is_ok() { - if let Ok(mut tf) = transforms.get_mut(child) { + for (e, pixels_per_meter, children) in changed_drawings.iter() { + for child in children { + if points.get(*child).is_ok() { + if let Ok(mut tf) = transforms.get_mut(*child) { + tf.scale = Vec3::new(pixels_per_meter.0, pixels_per_meter.0, 1.0); + } + } else if measurements.get(*child).is_ok() { + // Measurements are only scaled in width + // TODO(luca) This does nothing since measurements scale is overwritten when they + // are updated, fix it at the measurement level + // TODO(luca) make this actually only scale width + if let Ok(mut tf) = transforms.get_mut(*child) { tf.scale = Vec3::new(pixels_per_meter.0, pixels_per_meter.0, 1.0); } } diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 3969a149..7723a825 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -115,6 +115,12 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: //id_to_entity.insert(**fiducial_id, anchor_entity); consider_id(*fiducial_id); } + for (measurement_id, measurement) in &drawing.measurements { + drawing_parent + .spawn(measurement.to_ecs(&id_to_entity)) + .insert(SiteID(*measurement_id)); + consider_id(*measurement_id); + } }) .id(); consider_id(*drawing_id); @@ -132,13 +138,6 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: consider_id(*light_id); } - for (measurement_id, measurement) in &level_data.measurements { - level - .spawn(measurement.to_ecs(&id_to_entity)) - .insert(SiteID(*measurement_id)); - consider_id(*measurement_id); - } - for (model_id, model) in &level_data.models { level.spawn(model.clone()).insert(SiteID(*model_id)); consider_id(*model_id); diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index d3063879..168991ba 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -200,7 +200,8 @@ impl Plugin for SitePlugin { .with_system(update_lift_cabin) .with_system(update_lift_edge) .with_system(update_model_tentative_formats) - .with_system(update_anchor_and_fiducial_visuals_for_changed_pixels_per_meter) + .with_system(add_measurement_visuals) + .with_system(update_meshes_for_changed_pixels_per_meter) .with_system(update_material_for_display_color), ) .add_system_set( @@ -263,7 +264,6 @@ impl Plugin for SitePlugin { .with_system(update_lift_door_availability) .with_system(update_physical_lights) .with_system(toggle_physical_lights) - .with_system(add_measurement_visuals) .with_system(update_changed_measurement) .with_system(update_measurement_for_moved_anchors) .with_system(update_model_scenes) diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index cc460b32..68caadde 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -204,6 +204,7 @@ fn generate_levels( >, Query< ( + Entity, &NameInSite, &AssetSource, &Pose, @@ -289,6 +290,7 @@ fn generate_levels( Without, >, Query<&SiteID>, + Query<&Children>, )> = SystemState::new(world); let ( @@ -304,6 +306,7 @@ fn generate_levels( q_walls, q_levels, q_site_ids, + q_children, ) = state.get(world); let mut levels = BTreeMap::new(); @@ -375,42 +378,33 @@ fn generate_levels( } } - for (name, source, pose, pixels_per_meter, id, parent) in &q_drawings { + for (entity, name, source, pose, pixels_per_meter, id, parent) in &q_drawings { if let Ok((_, level_id, _, _, _)) = q_levels.get(parent.get()) { if let Some(level) = levels.get_mut(&level_id.0) { - level.drawings.insert( - id.0, - Drawing { - name: name.clone(), - anchors: BTreeMap::default(), - fiducials: BTreeMap::default(), - source: source.clone(), - pose: pose.clone(), - pixels_per_meter: pixels_per_meter.clone(), - }, - ); - } - } - } - - for (point, o_point, label, id, parent) in &q_fiducials { - let point = o_point.map(|x| &x.0).unwrap_or(point); - // Fiducials have a drawing, then a level parent - if let Ok((_, _, _, _, drawing_id, drawing_parent)) = q_drawings.get(parent.get()) { - if let Ok((_, level_id, _, _, _)) = q_levels.get(drawing_parent.get()) { - if let Some(level) = levels.get_mut(&level_id.0) { - if let Some(drawing) = level.drawings.get_mut(&drawing_id.0) { + let mut measurements = BTreeMap::new(); + let mut fiducials = BTreeMap::new(); + let mut anchors = BTreeMap::new(); + for e in DescendantIter::new(&q_children, entity) { + if let Ok((anchor, anchor_id, _)) = q_anchors.get(e) { + anchors.insert(anchor_id.0, anchor.clone()); + } + if let Ok((edge, o_edge, distance, label, id, parent)) = q_measurements.get(e) { + let edge = o_edge.map(|x| &x.0).unwrap_or(edge); + let anchors = get_anchor_id_edge(edge)?; + measurements.insert( + id.0, + Measurement { + anchors, + distance: distance.clone(), + label: label.clone(), + marker: MeasurementMarker, + }, + ); + } + if let Ok((point, o_point, label, id, parent)) = q_fiducials.get(e) { + let point = o_point.map(|x| &x.0).unwrap_or(point); let anchor = Point(get_anchor_id(point.0)?); - if let Ok((anchor, anchor_id, anchor_parent)) = q_anchors.get(point.0) { - if let Ok((_, _, _, _, anchor_drawing_id, _)) = - q_drawings.get(parent.get()) - { - if anchor_drawing_id.0 == drawing_id.0 { - drawing.anchors.insert(anchor_id.0, anchor.clone()); - } - } - } - drawing.fiducials.insert( + fiducials.insert( id.0, Fiducial { anchor, @@ -420,6 +414,18 @@ fn generate_levels( ); } } + level.drawings.insert( + id.0, + Drawing { + name: name.clone(), + anchors, + fiducials, + measurements, + source: source.clone(), + pose: pose.clone(), + pixels_per_meter: pixels_per_meter.clone(), + }, + ); } } } @@ -455,24 +461,6 @@ fn generate_levels( } } - for (edge, o_edge, distance, label, id, parent) in &q_measurements { - let edge = o_edge.map(|x| &x.0).unwrap_or(edge); - if let Ok((_, level_id, _, _, _)) = q_levels.get(parent.get()) { - if let Some(level) = levels.get_mut(&level_id.0) { - let anchors = get_anchor_id_edge(edge)?; - level.measurements.insert( - id.0, - Measurement { - anchors, - distance: distance.clone(), - label: label.clone(), - marker: MeasurementMarker, - }, - ); - } - } - } - for (name, source, pose, is_static, constraint_dependents, scale, id, parent) in &q_models { if let Ok((_, level_id, _, _, _)) = q_levels.get(parent.get()) { if let Some(level) = levels.get_mut(&level_id.0) { @@ -1003,7 +991,6 @@ pub fn save_nav_graphs(world: &mut World) { level.drawings.clear(); level.floors.clear(); level.lights.clear(); - level.measurements.clear(); level.models.clear(); level.walls.clear(); } diff --git a/rmf_site_format/src/drawing.rs b/rmf_site_format/src/drawing.rs index 2a7f7ce8..4994803a 100644 --- a/rmf_site_format/src/drawing.rs +++ b/rmf_site_format/src/drawing.rs @@ -38,6 +38,8 @@ pub struct Drawing { pub anchors: BTreeMap, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub fiducials: BTreeMap>, + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub measurements: BTreeMap>, pub source: AssetSource, pub pose: Pose, pub pixels_per_meter: PixelsPerMeter, diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 5a6b9a36..65571cd0 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -9,7 +9,7 @@ use crate::{ }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; -use std::collections::{BTreeMap, HashMap}; +use std::collections::{BTreeMap, HashMap, HashSet}; use std::path::Path; #[derive(Deserialize, Serialize, Clone)] @@ -66,10 +66,20 @@ impl BuildingMap { let alignment = alignments.get(level_name).unwrap(); let tf = alignment.to_affine(); level.alignment = Some(alignment.clone()); - for v in &mut level.vertices { - let p = tf.transform_point2(v.to_vec()); - v.0 = p.x as f64; - v.1 = -p.y as f64; + // We need to keep the vertices associated with measurements in image coordinates + let mut drawing_vertices = HashSet::new(); + for measurement in &level.measurements { + drawing_vertices.insert(measurement.0); + drawing_vertices.insert(measurement.1); + } + for (idx, mut v) in level.vertices.iter_mut().enumerate() { + if drawing_vertices.contains(&idx) { + v.1 = -v.1; + } else { + let p = tf.transform_point2(v.to_vec()); + v.0 = p.x as f64; + v.1 = -p.y as f64; + } } let delta_yaw = get_delta_yaw(&tf); @@ -200,11 +210,12 @@ impl BuildingMap { (Pose::default(), PixelsPerMeter::default()) }; - let mut anchors = BTreeMap::new(); + let mut drawing_anchors = BTreeMap::new(); let mut fiducials = BTreeMap::new(); for fiducial in &level.fiducials { let anchor_id = site_id.next().unwrap(); - anchors.insert(anchor_id, [fiducial.0 as f32, fiducial.1 as f32].into()); + drawing_anchors + .insert(anchor_id, [fiducial.0 as f32, fiducial.1 as f32].into()); // Do not add this anchor to the vertex_to_anchor_id map because // this fiducial is not really recognized as a vertex to the // building format. @@ -224,7 +235,7 @@ impl BuildingMap { for feature in &level.features { let anchor_id = site_id.next().unwrap(); - anchors.insert(anchor_id, [feature.x as f32, feature.y as f32].into()); + drawing_anchors.insert(anchor_id, [feature.x as f32, feature.y as f32].into()); feature_id_map.insert(feature.id.clone(), anchor_id); // Do not add this anchor to the vertex_to_anchor_id map because // this fiducial is not really recognized as a vertex to the @@ -243,6 +254,27 @@ impl BuildingMap { ); } + let mut measurements = BTreeMap::new(); + for measurement in &level.measurements { + let mut site_measurement = measurement.to_site(&vertex_to_anchor_id)?; + let edge = &mut site_measurement.anchors; + let (start_anchor, end_anchor) = ( + anchors.get(&edge.left()).unwrap(), + anchors.get(&edge.right()).unwrap(), + ); + // Now get the anchors and duplicate them in the drawing + let anchor_id = site_id.next().unwrap(); + drawing_anchors.insert(anchor_id, start_anchor.clone()); + *edge.left_mut() = anchor_id; + let anchor_id = site_id.next().unwrap(); + drawing_anchors.insert(anchor_id, end_anchor.clone()); + *edge.right_mut() = anchor_id; + //println!("Measurement edge is {:?}", edge); + measurements.insert(site_id.next().unwrap(), site_measurement); + // TODO(luca) remove original anchors if they have no other dependents + // TODO(MXG): Have rankings for measurements + } + let id = site_id.next().unwrap(); drawings.insert( id, @@ -255,8 +287,9 @@ impl BuildingMap { .unwrap() .to_string(), ), - anchors, + anchors: drawing_anchors, fiducials, + measurements, source: AssetSource::Local(level.drawing.filename.clone()), pose, pixels_per_meter, @@ -306,6 +339,7 @@ impl BuildingMap { name: NameInSite(name.clone()), anchors, fiducials, + measurements: Default::default(), source: AssetSource::Local(layer.filename.clone()), pose, pixels_per_meter: PixelsPerMeter((1.0 / layer.transform.scale) as f32), @@ -321,13 +355,6 @@ impl BuildingMap { rankings.floors.insert(0, id); } - let mut measurements = BTreeMap::new(); - for measurement in &level.measurements { - let site_measurement = measurement.to_site(&vertex_to_anchor_id)?; - measurements.insert(site_id.next().unwrap(), site_measurement); - // TODO(MXG): Have rankings for measurements - } - let mut models = BTreeMap::new(); for model in &level.models { models.insert(site_id.next().unwrap(), model.to_site()); @@ -365,7 +392,6 @@ impl BuildingMap { drawings, floors, lights, - measurements, models, physical_cameras, walls, diff --git a/rmf_site_format/src/level.rs b/rmf_site_format/src/level.rs index d8c12560..a2f58d94 100644 --- a/rmf_site_format/src/level.rs +++ b/rmf_site_format/src/level.rs @@ -51,8 +51,6 @@ pub struct Level { #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub lights: BTreeMap, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - pub measurements: BTreeMap>, - #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub models: BTreeMap, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub physical_cameras: BTreeMap, @@ -72,7 +70,6 @@ impl Level { drawings: Default::default(), floors: Default::default(), lights: Default::default(), - measurements: Default::default(), models: Default::default(), physical_cameras: Default::default(), walls: Default::default(), From 13e9a03c37ff982f1ebd213aa1e0e291486c93d9 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 25 May 2023 09:49:53 +0800 Subject: [PATCH 11/80] Minor cleanup Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/anchor.rs | 15 ++++----------- rmf_site_editor/src/site/assets.rs | 1 - rmf_site_editor/src/site/drawing.rs | 11 +++++------ 3 files changed, 9 insertions(+), 18 deletions(-) diff --git a/rmf_site_editor/src/interaction/anchor.rs b/rmf_site_editor/src/interaction/anchor.rs index 3e334399..573b29f8 100644 --- a/rmf_site_editor/src/interaction/anchor.rs +++ b/rmf_site_editor/src/interaction/anchor.rs @@ -32,23 +32,16 @@ pub struct AnchorVisualization { pub fn add_anchor_visual_cues( mut commands: Commands, - mut new_anchors: Query< - ( - Entity, - &Parent, - Option<&Subordinate>, - &Anchor, - Option<&mut Transform>, - ), + new_anchors: Query< + (Entity, &Parent, Option<&Subordinate>, &Anchor), (Added, Without), >, categories: Query<&Category>, site_assets: Res, interaction_assets: Res, ) { - for (e, parent, subordinate, anchor, mut tf) in new_anchors.iter_mut() { - let category = categories.get(parent.get()).unwrap(); - let body_mesh = match &category { + for (e, parent, subordinate, anchor) in &new_anchors { + let body_mesh = match categories.get(parent.get()).unwrap() { Category::Drawing => site_assets.drawing_anchor_mesh.clone(), Category::Level => site_assets.level_anchor_mesh.clone(), Category::Lift => site_assets.lift_anchor_mesh.clone(), diff --git a/rmf_site_editor/src/site/assets.rs b/rmf_site_editor/src/site/assets.rs index 76f651b7..c55860c4 100644 --- a/rmf_site_editor/src/site/assets.rs +++ b/rmf_site_editor/src/site/assets.rs @@ -205,7 +205,6 @@ impl FromWorld for SiteAssets { .with_generated_outline_normals() .unwrap(), ); - // TODO(luca) have a different mesh let fiducial_mesh = meshes.add( Mesh::from( make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 4) diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index b4af8821..f989dba1 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -101,11 +101,10 @@ pub fn handle_loaded_drawing( let width = img.texture_descriptor.size.width as f32; let height = img.texture_descriptor.size.height as f32; - let centering_vec = Vec3::new(width / 2.0, -height / 2.0, 0.0); - - // We set this up so that the origin of the drawing is in - let mesh = make_flat_rect_mesh(width, height) - .transform_by(Affine3A::from_translation(centering_vec)); + // We set this up so that the origin of the drawing is in the top left corner + let mesh = make_flat_rect_mesh(width, height).transform_by( + Affine3A::from_translation(Vec3::new(width / 2.0, -height / 2.0, 0.0)), + ); let mesh = mesh_assets.add(mesh.into()); let leaf = if let Ok(segment) = segments.get(entity) { @@ -162,7 +161,7 @@ pub fn update_drawing_rank( pub fn update_drawing_pixels_per_meter( mut changed_drawings: Query<(&mut Transform, &PixelsPerMeter), Changed>, ) { - for (mut tf, pixels_per_meter) in changed_drawings.iter_mut() { + for (mut tf, pixels_per_meter) in &mut changed_drawings { tf.scale = Vec3::new(1.0 / pixels_per_meter.0, 1.0 / pixels_per_meter.0, 1.); } } From 607d0bdc0b1341c385deeee160045f41bd5fa175 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 25 May 2023 11:27:15 +0800 Subject: [PATCH 12/80] Fix measurement scale Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/drawing.rs | 22 +++++++------- rmf_site_editor/src/site/measurement.rs | 40 ++++++++++++++++++------- rmf_site_editor/src/site/mod.rs | 4 ++- 3 files changed, 43 insertions(+), 23 deletions(-) diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs index f989dba1..ac1e4132 100644 --- a/rmf_site_editor/src/site/drawing.rs +++ b/rmf_site_editor/src/site/drawing.rs @@ -166,25 +166,23 @@ pub fn update_drawing_pixels_per_meter( } } -pub fn update_meshes_for_changed_pixels_per_meter( +pub fn update_drawing_children_to_pixel_coordinates( + mut commands: Commands, changed_drawings: Query<(Entity, &PixelsPerMeter, &Children), Changed>, - points: Query, With)>>, - measurements: Query>, + meshes: Query, With, With)>>, mut transforms: Query<&mut Transform>, ) { for (e, pixels_per_meter, children) in changed_drawings.iter() { for child in children { - if points.get(*child).is_ok() { - if let Ok(mut tf) = transforms.get_mut(*child) { - tf.scale = Vec3::new(pixels_per_meter.0, pixels_per_meter.0, 1.0); - } - } else if measurements.get(*child).is_ok() { - // Measurements are only scaled in width - // TODO(luca) This does nothing since measurements scale is overwritten when they - // are updated, fix it at the measurement level - // TODO(luca) make this actually only scale width + if meshes.get(*child).is_ok() { if let Ok(mut tf) = transforms.get_mut(*child) { tf.scale = Vec3::new(pixels_per_meter.0, pixels_per_meter.0, 1.0); + } else { + commands + .entity(*child) + .insert(SpatialBundle::from_transform(Transform::from_scale( + Vec3::new(pixels_per_meter.0, pixels_per_meter.0, 1.0), + ))); } } } diff --git a/rmf_site_editor/src/site/measurement.rs b/rmf_site_editor/src/site/measurement.rs index 2f483075..fded16fb 100644 --- a/rmf_site_editor/src/site/measurement.rs +++ b/rmf_site_editor/src/site/measurement.rs @@ -15,12 +15,17 @@ * */ +use crate::interaction::Selectable; use crate::site::*; use bevy::prelude::*; use rmf_site_format::{Edge, MeasurementMarker}; pub const MEASUREMENT_LAYER_START: f32 = DRAWING_LAYER_START + 0.001; +/// Stores which (child) entity contains the measurement mesh +#[derive(Component, Debug, Clone, Deref, DerefMut)] +pub struct MeasurementSegment(pub Entity); + pub fn add_measurement_visuals( mut commands: Commands, measurements: Query<(Entity, &Edge), Added>, @@ -40,16 +45,23 @@ pub fn add_measurement_visuals( ); // TODO(luca) proper layering rather than hardcoded transform.translation.z = MEASUREMENT_LAYER_START; - commands - .entity(e) - .insert(PbrBundle { + + let child_id = commands + .spawn(PbrBundle { mesh: assets.lane_mid_mesh.clone(), material: assets.measurement_material.clone(), transform, ..default() }) + .insert(Selectable::new(e)) + .id(); + + commands + .entity(e) .insert(Category::Measurement) - .insert(EdgeLabels::StartEnd); + .insert(MeasurementSegment(child_id)) + .insert(EdgeLabels::StartEnd) + .push_children(&[child_id]); for anchor in &edge.array() { if let Ok(mut deps) = dependents.get_mut(*anchor) { @@ -77,18 +89,21 @@ fn update_measurement_visual( pub fn update_changed_measurement( mut measurements: Query< - (Entity, &Edge, &mut Transform), + (Entity, &Edge, &MeasurementSegment), (Changed>, With), >, anchors: AnchorParams, + mut transforms: Query<&mut Transform>, ) { - for (e, edge, mut tf) in &mut measurements { - update_measurement_visual(e, edge, &anchors, tf.as_mut()); + for (e, edge, segment) in &measurements { + if let Ok(mut tf) = transforms.get_mut(**segment) { + update_measurement_visual(**segment, edge, &anchors, tf.as_mut()); + } } } pub fn update_measurement_for_moved_anchors( - mut measurements: Query<(Entity, &Edge, &mut Transform), With>, + mut measurements: Query<(Entity, &Edge, &MeasurementSegment), With>, anchors: AnchorParams, changed_anchors: Query< &Dependents, @@ -97,11 +112,16 @@ pub fn update_measurement_for_moved_anchors( Or<(Changed, Changed)>, ), >, + mut transforms: Query<&mut Transform>, + global_tf: Query<&GlobalTransform>, + vis: Query<&ComputedVisibility>, ) { for changed_anchor in &changed_anchors { for dependent in changed_anchor.iter() { - if let Some((e, measurement, mut tf)) = measurements.get_mut(*dependent).ok() { - update_measurement_visual(e, measurement, &anchors, tf.as_mut()); + if let Some((e, measurement, segment)) = measurements.get_mut(*dependent).ok() { + if let Ok(mut tf) = transforms.get_mut(**segment) { + update_measurement_visual(**segment, measurement, &anchors, tf.as_mut()); + } } } } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 168991ba..4716ac72 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -200,8 +200,10 @@ impl Plugin for SitePlugin { .with_system(update_lift_cabin) .with_system(update_lift_edge) .with_system(update_model_tentative_formats) + // TODO(luca) getting query not match panics when this is moved back with the + // others, refactor stages .with_system(add_measurement_visuals) - .with_system(update_meshes_for_changed_pixels_per_meter) + .with_system(update_drawing_children_to_pixel_coordinates) .with_system(update_material_for_display_color), ) .add_system_set( From cdce56b192ee9723b57adf9ecd1eff0999aa2f36 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 25 May 2023 18:34:02 +0800 Subject: [PATCH 13/80] Implement constraints Signed-off-by: Luca Della Vedova --- .../src/interaction/category_visibility.rs | 14 +- rmf_site_editor/src/site/constraint.rs | 151 ++++++++++++++++++ .../src/site/drawing_editor/mod.rs | 39 +---- rmf_site_editor/src/site/load.rs | 9 ++ rmf_site_editor/src/site/measurement.rs | 8 +- rmf_site_editor/src/site/mod.rs | 7 + rmf_site_editor/src/site/save.rs | 64 +++++++- rmf_site_editor/src/widgets/menu_bar.rs | 1 + rmf_site_format/src/category.rs | 2 + rmf_site_format/src/legacy/building_map.rs | 48 ++++-- rmf_site_format/src/site.rs | 31 +++- 11 files changed, 317 insertions(+), 57 deletions(-) create mode 100644 rmf_site_editor/src/site/constraint.rs diff --git a/rmf_site_editor/src/interaction/category_visibility.rs b/rmf_site_editor/src/interaction/category_visibility.rs index 66fa69ff..f0884b9c 100644 --- a/rmf_site_editor/src/interaction/category_visibility.rs +++ b/rmf_site_editor/src/interaction/category_visibility.rs @@ -19,8 +19,8 @@ use bevy::ecs::system::SystemParam; use bevy::prelude::*; use rmf_site_format::{ - DoorMarker, FiducialMarker, FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, - LocationTags, MeasurementMarker, ModelMarker, WallMarker, + ConstraintMarker, DoorMarker, FiducialMarker, FloorMarker, LaneMarker, LiftCabin, + LiftCabinDoorMarker, LocationTags, MeasurementMarker, ModelMarker, WallMarker, }; #[derive(Clone, Debug, PartialEq)] @@ -31,6 +31,7 @@ pub struct CategoryFlags { pub lifts: bool, pub locations: bool, pub fiducials: bool, + pub constraints: bool, pub models: bool, pub measurements: bool, pub walls: bool, @@ -46,6 +47,7 @@ impl Default for CategoryFlags { lifts: true, locations: true, fiducials: true, + constraints: true, models: true, measurements: true, walls: true, @@ -69,6 +71,7 @@ pub struct FilterParams<'w, 's> { lifts: Query<'w, 's, Entity, Or<(With>, With)>>, locations: Query<'w, 's, Entity, With>, fiducials: Query<'w, 's, Entity, With>, + constraints: Query<'w, 's, Entity, With>, walls: Query<'w, 's, Entity, With>, models: Query<'w, 's, Entity, With>, measurements: Query<'w, 's, Entity, With>, @@ -134,6 +137,13 @@ pub fn update_entity_category_visibilities(mut params: FilterParams) { params.fiducials.iter().collect::>(), ); } + if params.categories_settings.constraints != params.recall_categories_settings.constraints { + update_visibility( + params.categories_settings.constraints, + &mut params.visibilities, + params.constraints.iter().collect::>(), + ); + } if params.categories_settings.measurements != params.recall_categories_settings.measurements { update_visibility( params.categories_settings.measurements, diff --git a/rmf_site_editor/src/site/constraint.rs b/rmf_site_editor/src/site/constraint.rs new file mode 100644 index 00000000..580778b6 --- /dev/null +++ b/rmf_site_editor/src/site/constraint.rs @@ -0,0 +1,151 @@ +/* + * 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::Selectable; +use crate::site::*; +use crate::CurrentWorkspace; +use bevy::prelude::*; +use rmf_site_format::{Edge, LaneMarker}; + +const CONSTRAINT_WIDTH: f32 = 0.2 * LANE_WIDTH; + +/// Stores which (child) entity contains the measurement mesh +#[derive(Component, Debug, Clone, Deref, DerefMut)] +pub struct ConstraintSegment(pub Entity); + +// TODO(luca) Figure out whether all constraints in site are OK or if we should have some in levels +// instead +pub fn assign_orphan_constraints_to_site( + mut commands: Commands, + elements: Query, With)>, + current_workspace: Res, + open_sites: Query>, +) { + if let Some(current_site) = current_workspace.to_site(&open_sites) { + for e in &elements { + commands.entity(current_site).add_child(e); + } + } +} + +pub fn add_constraint_visuals( + mut commands: Commands, + constraints: Query<(Entity, &Edge), Added>, + anchors: AnchorParams, + assets: Res, + global_tfs: Query<&GlobalTransform>, + mut dependents: Query<&mut Dependents, With>, +) { + for (e, edge) in &constraints { + // TODO(luca) calculate this based on current level, site and anchor parent + let is_visible = true; + + let mut transform = line_stroke_transform( + &anchors + .point_in_parent_frame_of(edge.start(), Category::Constraint, e) + .unwrap(), + &anchors + .point_in_parent_frame_of(edge.end(), Category::Constraint, e) + .unwrap(), + CONSTRAINT_WIDTH, + ); + // TODO(luca) proper layering rather than hardcoded + transform.translation.z = MEASUREMENT_LAYER_START; + + let child_id = commands + .spawn(PbrBundle { + mesh: assets.lane_mid_mesh.clone(), + material: assets.fiducial_material.clone(), + transform, + ..default() + }) + .insert(Selectable::new(e)) + .id(); + + commands + .entity(e) + .insert(SpatialBundle { + transform: Transform::from_translation([0., 0., MEASUREMENT_LAYER_START].into()), + visibility: Visibility { is_visible }, + ..default() + }) + .insert(Category::Constraint) + .insert(ConstraintSegment(child_id)) + .add_child(child_id) + .insert(EdgeLabels::StartEnd); + + for anchor in &edge.array() { + if let Ok(mut deps) = dependents.get_mut(*anchor) { + deps.insert(e); + } + } + } +} + +fn update_constraint_visual( + entity: Entity, + edge: &Edge, + anchors: &AnchorParams, + transform: &mut Transform, +) { + let start_anchor = anchors + .point_in_parent_frame_of(edge.start(), Category::Measurement, entity) + .unwrap(); + let end_anchor = anchors + .point_in_parent_frame_of(edge.end(), Category::Measurement, entity) + .unwrap(); + *transform = line_stroke_transform(&start_anchor, &end_anchor, CONSTRAINT_WIDTH); + transform.translation.z = MEASUREMENT_LAYER_START; +} + +pub fn update_changed_constraint( + constraints: Query< + (Entity, &Edge, &ConstraintSegment), + (Changed>, With), + >, + anchors: AnchorParams, + mut transforms: Query<&mut Transform>, +) { + for (e, edge, segment) in &constraints { + if let Ok(mut tf) = transforms.get_mut(**segment) { + update_constraint_visual(**segment, edge, &anchors, tf.as_mut()); + } + } +} + +pub fn update_constraint_for_moved_anchors( + constraints: Query<(Entity, &Edge, &ConstraintSegment), With>, + anchors: AnchorParams, + changed_anchors: Query< + &Dependents, + ( + With, + Or<(Changed, Changed)>, + ), + >, + mut transforms: Query<&mut Transform>, +) { + for changed_anchor in &changed_anchors { + for dependent in changed_anchor.iter() { + if let Some((e, edge, segment)) = constraints.get(*dependent).ok() { + if let Ok(mut tf) = transforms.get_mut(**segment) { + update_constraint_visual(**segment, edge, &anchors, tf.as_mut()); + } + } + } + } +} diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs index 198f9944..0733fec5 100644 --- a/rmf_site_editor/src/site/drawing_editor/mod.rs +++ b/rmf_site_editor/src/site/drawing_editor/mod.rs @@ -31,10 +31,13 @@ fn hide_level_entities( mut category_settings: ResMut, ) { camera_controls.use_orthographic(true, &mut cameras, &mut visibilities, headlight_toggle.0); + category_settings.0.doors = false; + category_settings.0.lanes = false; category_settings.0.lifts = false; + category_settings.0.locations = false; category_settings.0.floors = false; category_settings.0.models = false; - category_settings.0.measurements = true; + category_settings.0.walls = false; } fn restore_level_entities( @@ -56,39 +59,5 @@ impl Plugin for DrawingEditorPlugin { .add_system_set( SystemSet::on_exit(AppState::SiteDrawingEditor).with_system(restore_level_entities), ); - /* - app.add_plugin(InfiniteGridPlugin) - .insert_resource(WgpuSettings { - features: WgpuFeatures::POLYGON_MODE_LINE, - ..default() - }) - .add_system_set(SystemSet::on_enter(AppState::WorkcellEditor).with_system(spawn_grid)) - .add_system_set(SystemSet::on_exit(AppState::WorkcellEditor).with_system(delete_grid)) - .add_system_set( - SystemSet::on_update(AppState::WorkcellEditor) - .with_system(add_wireframe_to_meshes) - .with_system(update_constraint_dependents) - .with_system(update_model_scenes) - .with_system(update_model_tentative_formats) - .with_system(make_models_selectable) - .with_system(handle_workcell_keyboard_input) - .with_system(handle_new_mesh_primitives) - .with_system(change_workcell.before(load_workcell)) - .with_system(handle_new_urdf_roots), - ) - .add_system(load_workcell) - .add_system(save_workcell) - .add_system(add_workcell_visualization) - .add_system_set( - SystemSet::on_update(AppState::WorkcellEditor) - .before(TransformSystem::TransformPropagate) - .after(VisibilitySystems::VisibilityPropagate) - .with_system(update_anchor_transforms) - .with_system( - add_anchors_for_new_mesh_constraints.before(update_anchor_transforms), - ) - .with_system(update_transforms_for_changed_poses), - ); - */ } } diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 7723a825..3b94087e 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -240,6 +240,15 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: id_to_entity.insert(*location_id, location); consider_id(*location_id); } + + for (constraint_id, constraint_data) in &site_data.constraints { + let constraint = site + .spawn(constraint_data.to_ecs(&id_to_entity)) + .insert(SiteID(*constraint_id)) + .id(); + id_to_entity.insert(*constraint_id, constraint); + consider_id(*constraint_id); + } }); let nav_graph_rankings = match RecencyRanking::::from_u32( diff --git a/rmf_site_editor/src/site/measurement.rs b/rmf_site_editor/src/site/measurement.rs index fded16fb..a2c4821f 100644 --- a/rmf_site_editor/src/site/measurement.rs +++ b/rmf_site_editor/src/site/measurement.rs @@ -88,7 +88,7 @@ fn update_measurement_visual( } pub fn update_changed_measurement( - mut measurements: Query< + measurements: Query< (Entity, &Edge, &MeasurementSegment), (Changed>, With), >, @@ -103,7 +103,7 @@ pub fn update_changed_measurement( } pub fn update_measurement_for_moved_anchors( - mut measurements: Query<(Entity, &Edge, &MeasurementSegment), With>, + measurements: Query<(Entity, &Edge, &MeasurementSegment), With>, anchors: AnchorParams, changed_anchors: Query< &Dependents, @@ -113,12 +113,10 @@ pub fn update_measurement_for_moved_anchors( ), >, mut transforms: Query<&mut Transform>, - global_tf: Query<&GlobalTransform>, - vis: Query<&ComputedVisibility>, ) { for changed_anchor in &changed_anchors { for dependent in changed_anchor.iter() { - if let Some((e, measurement, segment)) = measurements.get_mut(*dependent).ok() { + if let Some((e, measurement, segment)) = measurements.get(*dependent).ok() { if let Ok(mut tf) = transforms.get_mut(**segment) { update_measurement_visual(**segment, measurement, &anchors, tf.as_mut()); } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 4716ac72..e7cd4d41 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -24,6 +24,9 @@ pub use assets::*; pub mod change_plugin; pub use change_plugin::*; +pub mod constraint; +pub use constraint::*; + pub mod deletion; pub use deletion::*; @@ -216,6 +219,7 @@ impl Plugin for SitePlugin { SiteUpdateStage::AssignOrphans, SystemSet::on_update(SiteState::Display) .with_system(assign_orphan_anchors_to_parent) + .with_system(assign_orphan_constraints_to_site) .with_system(assign_orphan_levels_to_site) .with_system(assign_orphan_nav_elements_to_site) .with_system(assign_orphan_elements_to_level::) @@ -245,6 +249,7 @@ impl Plugin for SitePlugin { .with_system(add_lane_visuals) .with_system(add_location_visuals) .with_system(add_fiducial_visuals) + .with_system(add_constraint_visuals) .with_system(update_level_visibility) .with_system(update_changed_lane) .with_system(update_lane_for_moved_anchor) @@ -268,6 +273,8 @@ impl Plugin for SitePlugin { .with_system(toggle_physical_lights) .with_system(update_changed_measurement) .with_system(update_measurement_for_moved_anchors) + .with_system(update_constraint_for_moved_anchors) + .with_system(update_changed_constraint) .with_system(update_model_scenes) .with_system(handle_new_sdf_roots) .with_system(update_model_scales) diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 68caadde..595ce280 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -89,12 +89,13 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration >, Query, Without)>, Query>, Without)>, + Query, Without)>, Query<&NextSiteID>, Query<&SiteID>, Query<&Children>, )> = SystemState::new(world); - let (level_children, nav_graph_elements, levels, lifts, sites, site_ids, children) = + let (level_children, nav_graph_elements, levels, lifts, constraints, sites, site_ids, children) = state.get_mut(world); let mut new_entities = Vec::new(); @@ -147,6 +148,12 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration } } } + + if let Ok(constraint) = constraints.get(*site_child) { + if !site_ids.contains(constraint) { + new_entities.push(constraint); + } + } } let mut next_site_id = sites @@ -851,6 +858,59 @@ fn generate_locations( Ok(locations) } +fn generate_constraints( + world: &mut World, + site: Entity, +) -> Result>, SiteGenerationError> { + let mut state: SystemState<( + Query< + ( + &Edge, + Option<&Original>>, + &SiteID, + &Parent, + ), + (With, Without), + >, + Query<&SiteID, With>, + )> = SystemState::new(world); + + let (q_constraints, q_anchors) = state.get(world); + + let get_anchor_id = |entity| { + let site_id = q_anchors + .get(entity) + .map_err(|_| SiteGenerationError::BrokenAnchorReference(entity))?; + Ok(site_id.0) + }; + + let get_anchor_id_edge = |edge: &Edge| { + let left = get_anchor_id(edge.left())?; + let right = get_anchor_id(edge.right())?; + Ok(Edge::new(left, right)) + }; + + let mut constraints = BTreeMap::new(); + for (edge, o_edge, constraint_id, parent) in &q_constraints { + if parent.get() != site { + continue; + } + + let edge = o_edge.map(|x| &x.0).unwrap_or(edge); + let edge = get_anchor_id_edge(edge)?; + + constraints.insert( + constraint_id.0, + Constraint { + edge: edge.clone(), + marker: ConstraintMarker, + }, + ); + } + + Ok(constraints) +} + fn generate_graph_rankings( world: &mut World, site: Entity, @@ -887,6 +947,7 @@ pub fn generate_site( let nav_graphs = generate_nav_graphs(world, site)?; let lanes = generate_lanes(world, site)?; let locations = generate_locations(world, site)?; + let constraints = generate_constraints(world, site)?; let graph_ranking = generate_graph_rankings(world, site)?; let props = match world.get::(site) { @@ -899,6 +960,7 @@ pub fn generate_site( return Ok(Site { format_version: rmf_site_format::SemVer::default(), anchors, + constraints, properties: props.clone(), levels, lifts, diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index bea68854..93a4ed09 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -68,6 +68,7 @@ pub fn top_menu_bar( ui.checkbox(&mut category_settings.0.lifts, "Lifts"); ui.checkbox(&mut category_settings.0.locations, "Locations"); ui.checkbox(&mut category_settings.0.fiducials, "Fiducials"); + ui.checkbox(&mut category_settings.0.constraints, "Constraints"); ui.checkbox(&mut category_settings.0.measurements, "Measurements"); ui.checkbox(&mut category_settings.0.models, "Models"); ui.checkbox(&mut category_settings.0.walls, "Walls"); diff --git a/rmf_site_format/src/category.rs b/rmf_site_format/src/category.rs index fdb378f4..996f01ea 100644 --- a/rmf_site_format/src/category.rs +++ b/rmf_site_format/src/category.rs @@ -43,6 +43,7 @@ pub enum Category { Model, Camera, Drawing, + Constraint, Workcell, } @@ -65,6 +66,7 @@ impl Category { Self::Model => "Model", Self::Camera => "Camera", Self::Drawing => "Drawing", + Self::Constraint => "Constraint", Self::Workcell => "Workcell", } } diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 65571cd0..9fe87bc4 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -1,11 +1,11 @@ use super::{level::Level, lift::Lift, PortingError, Result}; use crate::{ legacy::optimization::align_building, Anchor, Angle, AssetSource, AssociatedGraphs, - DisplayColor, Dock as SiteDock, Drawing as SiteDrawing, Fiducial as SiteFiducial, - FiducialMarker, Guided, Label, Lane as SiteLane, LaneMarker, Level as SiteLevel, - LevelProperties as SiteLevelProperties, Motion, NameInSite, NavGraph, Navigation, - OrientationConstraint, PixelsPerMeter, Pose, RankingsInLevel, ReverseLane, Rotation, Site, - SiteProperties, DEFAULT_NAV_GRAPH_COLORS, + Constraint as SiteConstraint, ConstraintMarker, DisplayColor, Dock as SiteDock, + Drawing as SiteDrawing, Fiducial as SiteFiducial, FiducialMarker, Guided, Label, + Lane as SiteLane, LaneMarker, Level as SiteLevel, LevelProperties as SiteLevelProperties, + Motion, NameInSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose, + RankingsInLevel, ReverseLane, Rotation, Site, SiteProperties, DEFAULT_NAV_GRAPH_COLORS, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; @@ -144,6 +144,7 @@ impl BuildingMap { let mut levels = BTreeMap::new(); let mut level_name_to_id = BTreeMap::new(); let mut lanes = BTreeMap::>::new(); + let mut constraints = BTreeMap::new(); let mut locations = BTreeMap::new(); let mut lift_cabin_anchors: BTreeMap> = BTreeMap::new(); @@ -197,7 +198,7 @@ impl BuildingMap { let mut rankings = RankingsInLevel::default(); let mut drawings = BTreeMap::new(); - let mut feature_id_map = HashMap::new(); + let mut feature_id_to_anchor_id = HashMap::new(); if !level.drawing.filename.is_empty() { let (pose, pixels_per_meter) = if let Some(a) = level.alignment { let p = a.translation; @@ -216,17 +217,19 @@ impl BuildingMap { let anchor_id = site_id.next().unwrap(); drawing_anchors .insert(anchor_id, [fiducial.0 as f32, fiducial.1 as f32].into()); + let label = if fiducial.2.is_empty() { + Label(None) + } else { + feature_id_to_anchor_id.insert(fiducial.2.clone(), anchor_id); + Label(Some(fiducial.2.clone())) + }; // Do not add this anchor to the vertex_to_anchor_id map because // this fiducial is not really recognized as a vertex to the // building format. fiducials.insert( site_id.next().unwrap(), SiteFiducial { - label: if fiducial.2.is_empty() { - Label(None) - } else { - Label(Some(fiducial.2.clone())) - }, + label, anchor: anchor_id.into(), marker: FiducialMarker, }, @@ -236,7 +239,7 @@ impl BuildingMap { for feature in &level.features { let anchor_id = site_id.next().unwrap(); drawing_anchors.insert(anchor_id, [feature.x as f32, feature.y as f32].into()); - feature_id_map.insert(feature.id.clone(), anchor_id); + feature_id_to_anchor_id.insert(feature.id.clone(), anchor_id); // Do not add this anchor to the vertex_to_anchor_id map because // this fiducial is not really recognized as a vertex to the // building format. @@ -316,7 +319,7 @@ impl BuildingMap { for feature in &layer.features { let anchor_id = site_id.next().unwrap(); anchors.insert(anchor_id, [feature.x as f32, feature.y as f32].into()); - feature_id_map.insert(feature.id.clone(), anchor_id); + feature_id_to_anchor_id.insert(feature.id.clone(), anchor_id); // Do not add this anchor to the vertex_to_anchor_id map because // this fiducial is not really recognized as a vertex to the // building format. @@ -347,6 +350,24 @@ impl BuildingMap { ); } + // Now set the constraints + // TODO(luca) multilevel constraints with equally named fiducials + // Require generating unique key for each fiducial based on level and name + for constraint in &level.constraints { + if let Some(id_0) = feature_id_to_anchor_id.get(&constraint.ids[0]) { + if let Some(id_1) = feature_id_to_anchor_id.get(&constraint.ids[1]) { + let id = site_id.next().unwrap(); + constraints.insert( + id, + SiteConstraint { + edge: [*id_0, *id_1].into(), + marker: ConstraintMarker, + }, + ); + } + } + } + let mut floors = BTreeMap::new(); for floor in &level.floors { let site_floor = floor.to_site(&vertex_to_anchor_id)?; @@ -492,6 +513,7 @@ impl BuildingMap { Ok(Site { format_version: Default::default(), anchors: site_anchors, + constraints, properties: SiteProperties { name: self.name.clone(), }, diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 21ad67fb..d51c7d7a 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -17,7 +17,7 @@ use crate::*; #[cfg(feature = "bevy")] -use bevy::prelude::{Component, Entity}; +use bevy::prelude::{Bundle, Component, Entity}; use serde::{Deserialize, Serialize}; use std::{collections::BTreeMap, io}; @@ -37,6 +37,19 @@ impl Default for SiteProperties { } } +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct Constraint { + pub edge: Edge, + /// Marker that tells bevy the entity is a Constraint-type + #[serde(skip)] + pub marker: ConstraintMarker, +} + +#[derive(Clone, Debug, Default)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct ConstraintMarker; + #[derive(Serialize, Deserialize, Debug, Clone, Default)] pub struct Site { /// The site data format that is being used @@ -46,6 +59,9 @@ pub struct Site { // from level anchors, or does the grouping make the intent obvious enough? #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub anchors: BTreeMap, + /// Constraints to be used for drawing scaling and floor alignment + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub constraints: BTreeMap>, /// Properties that are tied to the whole site pub properties: SiteProperties, /// Properties of each level @@ -108,3 +124,16 @@ impl RefTrait for u32 {} #[cfg(feature = "bevy")] impl RefTrait for Entity {} + +#[cfg(feature = "bevy")] +impl Constraint { + pub fn to_ecs( + &self, + id_to_entity: &std::collections::HashMap, + ) -> Constraint { + Constraint { + edge: self.edge.to_ecs(id_to_entity), + marker: Default::default(), + } + } +} From 2e3dfa2fbc1dd6fd761da08d1458891099f520e2 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 26 May 2023 11:00:46 +0800 Subject: [PATCH 14/80] Minor fix for layering Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/outline.rs | 5 +++-- rmf_site_editor/src/site/constraint.rs | 9 ++++++--- rmf_site_editor/src/site/measurement.rs | 4 +++- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs index 47784ea6..4a3338f6 100644 --- a/rmf_site_editor/src/interaction/outline.rs +++ b/rmf_site_editor/src/interaction/outline.rs @@ -19,8 +19,8 @@ use crate::interaction::*; use bevy::render::view::RenderLayers; use bevy_mod_outline::{OutlineBundle, OutlineRenderLayers, OutlineVolume, SetOutlineDepth}; use rmf_site_format::{ - DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, MeasurementMarker, - ModelMarker, PhysicalCameraProperties, WallMarker, + ConstraintMarker, DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, + MeasurementMarker, ModelMarker, PhysicalCameraProperties, WallMarker, }; use smallvec::SmallVec; @@ -114,6 +114,7 @@ pub fn add_outline_visualization( Added, Added>, Added, + Added, Added, Added, Added, diff --git a/rmf_site_editor/src/site/constraint.rs b/rmf_site_editor/src/site/constraint.rs index 580778b6..233580f8 100644 --- a/rmf_site_editor/src/site/constraint.rs +++ b/rmf_site_editor/src/site/constraint.rs @@ -21,6 +21,9 @@ use crate::CurrentWorkspace; use bevy::prelude::*; use rmf_site_format::{Edge, LaneMarker}; +// TODO(luca) proper recency ranking, this will break for > 10 drawings +pub const CONSTRAINT_LAYER_START: f32 = + FLOOR_LAYER_START - (FLOOR_LAYER_START - DRAWING_LAYER_START) / 10.0; const CONSTRAINT_WIDTH: f32 = 0.2 * LANE_WIDTH; /// Stores which (child) entity contains the measurement mesh @@ -64,7 +67,7 @@ pub fn add_constraint_visuals( CONSTRAINT_WIDTH, ); // TODO(luca) proper layering rather than hardcoded - transform.translation.z = MEASUREMENT_LAYER_START; + transform.translation.z = CONSTRAINT_LAYER_START; let child_id = commands .spawn(PbrBundle { @@ -79,7 +82,7 @@ pub fn add_constraint_visuals( commands .entity(e) .insert(SpatialBundle { - transform: Transform::from_translation([0., 0., MEASUREMENT_LAYER_START].into()), + transform: Transform::from_translation([0., 0., CONSTRAINT_LAYER_START].into()), visibility: Visibility { is_visible }, ..default() }) @@ -109,7 +112,7 @@ fn update_constraint_visual( .point_in_parent_frame_of(edge.end(), Category::Measurement, entity) .unwrap(); *transform = line_stroke_transform(&start_anchor, &end_anchor, CONSTRAINT_WIDTH); - transform.translation.z = MEASUREMENT_LAYER_START; + transform.translation.z = CONSTRAINT_LAYER_START; } pub fn update_changed_constraint( diff --git a/rmf_site_editor/src/site/measurement.rs b/rmf_site_editor/src/site/measurement.rs index a2c4821f..9b1dbf3f 100644 --- a/rmf_site_editor/src/site/measurement.rs +++ b/rmf_site_editor/src/site/measurement.rs @@ -20,7 +20,9 @@ use crate::site::*; use bevy::prelude::*; use rmf_site_format::{Edge, MeasurementMarker}; -pub const MEASUREMENT_LAYER_START: f32 = DRAWING_LAYER_START + 0.001; +// TODO(luca) proper recency ranking, this will break for > 10 drawings +pub const MEASUREMENT_LAYER_START: f32 = + DRAWING_LAYER_START + (FLOOR_LAYER_START - DRAWING_LAYER_START) / 10.0; /// Stores which (child) entity contains the measurement mesh #[derive(Component, Debug, Clone, Deref, DerefMut)] From 1859621d2c1393b0d54c28338aca27f792af6f1d Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 26 May 2023 12:12:47 +0800 Subject: [PATCH 15/80] Add a component for ground truth drawings Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/load.rs | 1 + rmf_site_editor/src/site/mod.rs | 1 + rmf_site_editor/src/site/save.rs | 4 +++- rmf_site_editor/src/widgets/create.rs | 3 ++- rmf_site_editor/src/widgets/inspector/mod.rs | 13 +++++++++++++ rmf_site_editor/src/widgets/mod.rs | 2 ++ rmf_site_format/src/drawing.rs | 8 ++++++++ rmf_site_format/src/legacy/building_map.rs | 4 +++- 8 files changed, 33 insertions(+), 3 deletions(-) diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 3b94087e..a7c53801 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -94,6 +94,7 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: name: drawing.name.clone(), source: drawing.source.clone(), pose: drawing.pose.clone(), + is_primary: drawing.is_primary.clone(), pixels_per_meter: drawing.pixels_per_meter.clone(), marker: DrawingMarker, }) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index e7cd4d41..f40fb273 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -168,6 +168,7 @@ impl Plugin for SitePlugin { .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::::default()) + .add_plugin(ChangePlugin::::default()) .add_plugin(ChangePlugin::>::default()) .add_plugin(ChangePlugin::