diff --git a/rmf_site_editor/src/interaction/assets.rs b/rmf_site_editor/src/interaction/assets.rs index 32250766..bdc6fb51 100644 --- a/rmf_site_editor/src/interaction/assets.rs +++ b/rmf_site_editor/src/interaction/assets.rs @@ -55,7 +55,7 @@ impl InteractionAssets { // The arrows should originate in the mesh origin let pos = Vec3::splat(0.0); let rot_x = Quat::from_rotation_y(90_f32.to_radians()); - let rot_y = Quat::from_rotation_x(-90_f32.to_radians()); + let rot_y = Quat::from_rotation_x((-90_f32).to_radians()); let rot_z = Quat::default(); let x_mat = self.x_axis_materials.clone(); let y_mat = self.y_axis_materials.clone(); @@ -145,12 +145,12 @@ impl InteractionAssets { ( self.x_axis_materials.clone(), Vec3::new(-offset, 0., height), - Quat::from_rotation_y(-90_f32.to_radians()), + Quat::from_rotation_y(-(90_f32).to_radians()), ), ( self.y_axis_materials.clone(), Vec3::new(0., offset, height), - Quat::from_rotation_x(-90_f32.to_radians()), + Quat::from_rotation_x(-(90_f32).to_radians()), ), ( self.y_axis_materials.clone(), @@ -190,7 +190,7 @@ impl InteractionAssets { ( self.y_axis_materials.clone(), Vec3::new(0., offset, 0.), - Quat::from_rotation_x(-90_f32.to_radians()), + Quat::from_rotation_x(-(90_f32).to_radians()), ), ( self.z_axis_materials.clone(), diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 375845fe..78df82c4 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -22,7 +22,9 @@ use crate::{ }; use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow}; use bevy_mod_raycast::{deferred::RaycastMesh, deferred::RaycastSource, primitives::rays::Ray3d}; -use rmf_site_format::{FloorMarker, Model, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel}; +use rmf_site_format::{ + FloorMarker, ModelInstance, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel, +}; use std::collections::HashSet; /// A resource that keeps track of the unique entities that play a role in @@ -113,7 +115,11 @@ impl Cursor { } // TODO(luca) reduce duplication here - pub fn set_model_preview(&mut self, commands: &mut Commands, model: Option) { + pub fn set_model_instance_preview( + &mut self, + commands: &mut Commands, + model: Option>, + ) { self.remove_preview(commands); self.preview_model = if let Some(model) = model { let e = commands.spawn(model).insert(Pending).id(); diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 16290689..b95fe736 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -58,6 +58,9 @@ pub use light::*; pub mod mode; pub use mode::*; +pub mod model; +pub use model::*; + pub mod model_preview; pub use model_preview::*; @@ -215,6 +218,7 @@ impl Plugin for InteractionPlugin { Update, ( make_model_previews_not_selectable, + update_model_instance_visual_cues.after(maintain_hovered_entities), update_lane_visual_cues.after(maintain_selected_entities), update_edge_visual_cues.after(maintain_selected_entities), update_point_visual_cues.after(maintain_selected_entities), diff --git a/rmf_site_editor/src/interaction/model.rs b/rmf_site_editor/src/interaction/model.rs new file mode 100644 index 00000000..e720396d --- /dev/null +++ b/rmf_site_editor/src/interaction/model.rs @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{interaction::*, site::*}; +use bevy::prelude::*; + +pub fn update_model_instance_visual_cues( + model_descriptions: Query< + (Entity, &Selected, &Hovered), + ( + With, + With, + Or<(Changed, Changed)>, + ), + >, + mut model_instances: Query< + ( + Entity, + &mut Selected, + &mut Hovered, + &mut Affiliation, + Option>>, + ), + (With, Without), + >, + mut locations: Query<&mut Selected, (With, Without)>, + mut removed_components: RemovedComponents>, +) { + for (instance_entity, mut instance_selected, mut instance_hovered, affiliation, tasks) in + &mut model_instances + { + // By clearing instance supports for selected / hovered, we assume instances will never + // be supporting select / hovering for anything else + let mut is_description_selected = false; + if let Some(description_entity) = affiliation.0 { + if let Ok((_, description_selected, description_hovered)) = + model_descriptions.get(description_entity) + { + if description_selected.cue() { + instance_selected + .support_selected + .insert(description_entity); + is_description_selected = true; + } else { + instance_selected.support_selected.clear(); + } + if description_hovered.cue() { + instance_hovered.support_hovering.insert(description_entity); + } else { + instance_hovered.support_hovering.clear(); + } + } + } + + // When an instance is selected, select all locations supporting it + if let Some(tasks) = tasks { + // When tasks for an instance have changed, reset all locations from supporting this instance + if tasks.is_changed() { + for mut location_selected in locations.iter_mut() { + location_selected.support_selected.remove(&instance_entity); + } + } + + if let Some(task_location) = tasks.0.first().and_then(|t| t.location()) { + if let Ok(mut location_selected) = locations.get_mut(task_location.0) { + if instance_selected.cue() && !is_description_selected { + location_selected.support_selected.insert(instance_entity); + } else { + location_selected.support_selected.remove(&instance_entity); + } + } + } + } + } + + // When instances are removed, prevent any location from supporting them + for removed in removed_components.read() { + for mut location_selected in locations.iter_mut() { + location_selected.support_selected.remove(&removed); + } + } +} diff --git a/rmf_site_editor/src/interaction/select_anchor.rs b/rmf_site_editor/src/interaction/select_anchor.rs index 87a24776..2f950ab3 100644 --- a/rmf_site_editor/src/interaction/select_anchor.rs +++ b/rmf_site_editor/src/interaction/select_anchor.rs @@ -26,8 +26,9 @@ use crate::{ }; use bevy::{ecs::system::SystemParam, prelude::*}; use rmf_site_format::{ - Door, Edge, Fiducial, Floor, FrameMarker, Lane, LiftProperties, Location, Measurement, Model, - NameInWorkcell, NameOfSite, Path, PixelsPerMeter, Point, Pose, Side, Wall, WorkcellModel, + Door, Edge, Fiducial, Floor, FrameMarker, Lane, LiftProperties, Location, Measurement, + ModelInstance, NameInWorkcell, NameOfSite, Path, PixelsPerMeter, Point, Pose, Side, Wall, + WorkcellModel, }; use std::collections::HashSet; use std::sync::Arc; @@ -1186,7 +1187,8 @@ impl<'w, 's> SelectAnchorPlacementParams<'w, 's> { false, ); set_visibility(self.cursor.frame_placement, &mut self.visibility, false); - self.cursor.set_model_preview(&mut self.commands, None); + self.cursor + .set_model_instance_preview(&mut self.commands, None); for e in self.hidden_entities.drawing_anchors.drain() { set_visibility(e, &mut self.visibility, true); } @@ -1297,9 +1299,9 @@ impl SelectAnchorPointBuilder { } } - pub fn for_model(self, model: Model) -> SelectAnchor3D { + pub fn for_model_instance(self, model: ModelInstance) -> SelectAnchor3D { SelectAnchor3D { - bundle: PlaceableObject::Model(model), + bundle: PlaceableObject::ModelInstance(model), parent: None, target: self.for_element, continuity: self.continuity, @@ -1686,7 +1688,7 @@ impl SelectAnchor { #[derive(Clone)] enum PlaceableObject { - Model(Model), + ModelInstance(ModelInstance), Anchor, VisualMesh(WorkcellModel), CollisionMesh(WorkcellModel), @@ -2151,11 +2153,11 @@ pub fn handle_select_anchor_3d_mode( PlaceableObject::Anchor => { set_visibility(params.cursor.frame_placement, &mut params.visibility, true); } - PlaceableObject::Model(ref m) => { + PlaceableObject::ModelInstance(ref m) => { // Spawn the model as a child of the cursor params .cursor - .set_model_preview(&mut params.commands, Some(m.clone())); + .set_model_instance_preview(&mut params.commands, Some(m.clone())); } PlaceableObject::VisualMesh(ref m) | PlaceableObject::CollisionMesh(ref m) => { // Spawn the model as a child of the cursor @@ -2200,7 +2202,7 @@ pub fn handle_select_anchor_3d_mode( NameInWorkcell("Unnamed".to_string()), )) .id(), - PlaceableObject::Model(ref a) => { + PlaceableObject::ModelInstance(ref a) => { let mut model = a.clone(); // If we are in workcell mode, add a "base link" frame to the model if matches!(**app_state, AppState::WorkcellEditor) { diff --git a/rmf_site_editor/src/occupancy.rs b/rmf_site_editor/src/occupancy.rs index 7dea7f08..945b1031 100644 --- a/rmf_site_editor/src/occupancy.rs +++ b/rmf_site_editor/src/occupancy.rs @@ -140,6 +140,8 @@ pub struct CalculateGrid { pub floor: f32, /// Ignore meshes above this height pub ceiling: f32, + // Ignore these entities + pub ignore: Option>, } enum Group { @@ -180,6 +182,14 @@ fn calculate_grid( let physical_entities = collect_physical_entities(&bodies, &meta); info!("Checking {:?} physical entities", physical_entities.len()); for e in &physical_entities { + if request + .ignore + .as_ref() + .is_some_and(|ignored_set| ignored_set.contains(e)) + { + continue; + } + let (_, mesh, aabb, tf) = match bodies.get(*e) { Ok(body) => body, Err(_) => continue, diff --git a/rmf_site_editor/src/shapes.rs b/rmf_site_editor/src/shapes.rs index 1d173766..4a3d51f0 100644 --- a/rmf_site_editor/src/shapes.rs +++ b/rmf_site_editor/src/shapes.rs @@ -876,7 +876,7 @@ pub(crate) fn make_physical_camera_mesh() -> Mesh { ) .transform_by( Affine3A::from_translation([lens_hood_protrusion * scale, 0., 0.].into()) - * Affine3A::from_rotation_y(-90_f32.to_radians()) + * Affine3A::from_rotation_y(-(90_f32).to_radians()) * Affine3A::from_rotation_z(45_f32.to_radians()), ) .merge_into(&mut mesh); diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index d57f9af2..3f1546aa 100644 --- a/rmf_site_editor/src/site/deletion.rs +++ b/rmf_site_editor/src/site/deletion.rs @@ -20,12 +20,14 @@ use crate::{ log::Log, site::{ Category, CurrentLevel, Dependents, LevelElevation, LevelProperties, NameInSite, - SiteUpdateSet, + RemoveInstance, SiteUpdateSet, }, AppState, Issue, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use rmf_site_format::{ConstraintDependents, Edge, MeshConstraint, Path, Point}; +use rmf_site_format::{ + Affiliation, ConstraintDependents, Edge, Group, InstanceMarker, MeshConstraint, Path, Point, +}; use std::collections::HashSet; // TODO(MXG): Use this module to implement the deletion buffer. The role of the @@ -82,6 +84,7 @@ struct DeletionParams<'w, 's> { edges: Query<'w, 's, &'static Edge>, points: Query<'w, 's, &'static Point>, paths: Query<'w, 's, &'static Path>, + instances: Query<'w, 's, &'static Affiliation, (With, Without)>, parents: Query<'w, 's, &'static mut Parent>, dependents: Query<'w, 's, &'static mut Dependents>, constraint_dependents: Query<'w, 's, &'static mut ConstraintDependents>, @@ -90,6 +93,7 @@ struct DeletionParams<'w, 's> { selection: Res<'w, Selection>, current_level: ResMut<'w, CurrentLevel>, levels: Query<'w, 's, Entity, With>, + remove_instance: EventWriter<'w, RemoveInstance>, select: EventWriter<'w, Select>, log: EventWriter<'w, Log>, issues: Query<'w, 's, (Entity, &'static mut Issue)>, @@ -137,6 +141,14 @@ fn cautious_delete(element: Entity, params: &mut DeletionParams) { } } + if let Ok(_) = params.instances.get(element) { + params.remove_instance.send(RemoveInstance(element)); + if **params.selection == Some(element) { + params.select.send(Select(None)); + } + return; + } + for descendent in &all_descendents { if let Ok(prevent) = params.preventions.get(*descendent) { if *descendent == element { diff --git a/rmf_site_editor/src/site/group.rs b/rmf_site_editor/src/site/group.rs index 6d28d026..bba98218 100644 --- a/rmf_site_editor/src/site/group.rs +++ b/rmf_site_editor/src/site/group.rs @@ -27,15 +27,9 @@ pub struct MergeGroups { pub into_group: Entity, } -#[derive(Component, Deref)] +#[derive(Component, Deref, DerefMut)] pub struct Members(Vec); -impl Members { - pub fn iter(&self) -> impl Iterator { - self.0.iter() - } -} - #[derive(Component, Clone, Copy)] struct LastAffiliation(Option); diff --git a/rmf_site_editor/src/site/level.rs b/rmf_site_editor/src/site/level.rs index 25610cea..fda3ac9a 100644 --- a/rmf_site_editor/src/site/level.rs +++ b/rmf_site_editor/src/site/level.rs @@ -16,7 +16,7 @@ */ use crate::site::*; -use crate::{interaction::CameraControls, CurrentWorkspace}; +use crate::CurrentWorkspace; use bevy::prelude::*; pub fn update_level_visibility( diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 7bb2f73d..c47ff40c 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -17,7 +17,10 @@ use crate::{recency::RecencyRanking, site::*, WorkspaceMarker}; use bevy::{ecs::system::SystemParam, prelude::*}; -use std::{collections::HashMap, path::PathBuf}; +use std::{ + collections::{HashMap, HashSet}, + path::PathBuf, +}; use thiserror::Error as ThisError; /// This component is given to the site to keep track of what file it should be @@ -210,11 +213,6 @@ fn generate_site_entities( consider_id(*light_id); } - for (model_id, model) in &level_data.models { - level.spawn(model.clone()).insert(SiteID(*model_id)); - consider_id(*model_id); - } - for (physical_camera_id, physical_camera) in &level_data.physical_cameras { level .spawn(physical_camera.clone()) @@ -248,6 +246,60 @@ fn generate_site_entities( consider_id(*level_id); } + let mut model_description_dependents = HashMap::>::new(); + for (model_description_id, model_description) in &site_data.model_descriptions { + let model_description = commands + .spawn(model_description.clone()) + .insert(SiteID(*model_description_id)) + .insert(Category::ModelDescription) + .set_parent(site_id) + .id(); + id_to_entity.insert(*model_description_id, model_description); + consider_id(*model_description_id); + model_description_dependents.insert(model_description, HashSet::new()); + } + + for (model_instance_id, model_instance) in &site_data.model_instances { + let model_instance = model_instance.convert(&id_to_entity).for_site(site_id)?; + let model_instance_entity = commands + .spawn(model_instance.clone()) + .insert(SiteID(*model_instance_id)) + .set_parent(site_id) + .id(); + id_to_entity.insert(*model_instance_id, model_instance_entity); + consider_id(*model_instance_id); + + if let Some(model_description) = model_instance.description.0 { + model_description_dependents + .entry(model_description) + .and_modify(|hset| { + hset.insert(model_instance_entity); + }); + } + } + + for (model_description_entity, dependents) in model_description_dependents { + commands + .entity(model_description_entity) + .insert(Dependents(dependents)); + } + + // let mut model_description_dependents = HashMap::>::new(); + for (scenario_id, scenario_bundle) in &site_data.scenarios { + let parent = match scenario_bundle.scenario.parent_scenario.0 { + Some(parent_id) => *id_to_entity.get(&parent_id).unwrap_or(&site_id), + None => site_id, + }; + let scenario_bundle = scenario_bundle.convert(&id_to_entity).for_site(site_id)?; + let scenario_entity = commands + .spawn(scenario_bundle.clone()) + .insert(SiteID(*scenario_id)) + .set_parent(parent) + .id(); + id_to_entity.insert(*scenario_id, scenario_entity); + consider_id(*scenario_id); + } + for (lift_id, lift_data) in &site_data.lifts { let lift_entity = commands.spawn(SiteID(*lift_id)).set_parent(site_id).id(); @@ -265,7 +317,6 @@ fn generate_site_entities( } }); }); - for (door_id, door) in &lift_data.cabin_doors { let door_entity = commands .spawn(door.convert(&id_to_entity).for_site(site_id)?) @@ -396,7 +447,11 @@ pub fn load_site( } if cmd.focus { - change_current_site.send(ChangeCurrentSite { site, level: None }); + change_current_site.send(ChangeCurrentSite { + site, + level: None, + scenario: None, + }); } } } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 3dd7de9c..9bcaa652 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -99,6 +99,9 @@ pub use sdf::*; pub mod save; pub use save::*; +pub mod scenario; +pub use scenario::*; + pub mod sdf_exporter; pub use sdf_exporter::*; @@ -118,7 +121,7 @@ pub mod wall; pub use wall::*; use crate::recency::{RecencyRank, RecencyRankingPlugin}; -use crate::{clear_old_issues_on_new_validate_event, AppState, RegisterIssueType}; +use crate::{AppState, RegisterIssueType}; pub use rmf_site_format::*; use bevy::{prelude::*, render::view::visibility::VisibilitySystems, transform::TransformSystem}; @@ -188,6 +191,7 @@ impl Plugin for SitePlugin { .init_resource::() .init_resource::() .init_resource::() + .init_resource::() .init_resource::() .init_resource::() .init_resource::() @@ -205,6 +209,9 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() + .add_event::() + .add_event::() + .add_event::() .add_event::() .add_event::() .add_event::() @@ -264,6 +271,11 @@ impl Plugin for SitePlugin { DrawingEditorPlugin, SiteVisualizerPlugin, )) + .add_plugins(( + ChangePlugin::>::default(), + ChangePlugin::>::default(), + ChangePlugin::>::default(), + )) .add_issue_type(&DUPLICATED_DOOR_NAME_ISSUE_UUID, "Duplicate door name") .add_issue_type(&DUPLICATED_LIFT_NAME_ISSUE_UUID, "Duplicate lift name") .add_issue_type( @@ -310,6 +322,7 @@ impl Plugin for SitePlugin { assign_orphan_levels_to_site, assign_orphan_nav_elements_to_site, assign_orphan_fiducials_to_parent, + assign_orphan_model_instances_to_level, assign_orphan_elements_to_level::, assign_orphan_elements_to_level::, assign_orphan_elements_to_level::, @@ -360,6 +373,10 @@ impl Plugin for SitePlugin { add_location_visuals, add_fiducial_visuals, update_level_visibility, + update_scenario_properties, + handle_remove_scenarios.before(update_current_scenario), + update_current_scenario.before(update_scenario_properties), + handle_remove_instances, update_changed_lane, update_lane_for_moved_anchor, ) @@ -397,6 +414,9 @@ impl Plugin for SitePlugin { update_measurement_for_moved_anchors, handle_model_loaded_events, update_model_scenes, + update_model_instances::, + update_model_instances::, + update_model_instances::, update_affiliations, update_members_of_groups.after(update_affiliations), update_model_scales, diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 01bed237..d4f274b3 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -17,7 +17,10 @@ use crate::{ interaction::{DragPlaneBundle, Selectable, MODEL_PREVIEW_LAYER}, - site::{Category, PreventDeletion, SiteAssets}, + site::{ + Affiliation, AssetSource, Category, CurrentLevel, Group, ModelMarker, ModelProperty, + NameInSite, Pending, Pose, PreventDeletion, Scale, SiteAssets, SiteParent, + }, site_asset_io::MODEL_ENVIRONMENT_VARIABLE, }; use bevy::{ @@ -27,7 +30,6 @@ use bevy::{ render::view::RenderLayers, }; use bevy_mod_outline::OutlineMeshExt; -use rmf_site_format::{AssetSource, ModelMarker, Pending, Pose, Scale}; use smallvec::SmallVec; use std::any::TypeId; @@ -480,3 +482,61 @@ pub fn propagate_model_render_layers( } } } + +/// This system keeps model instances up to date with the properties of their affiliated descriptions +pub fn update_model_instances( + mut commands: Commands, + model_properties: Query< + (Entity, &NameInSite, Ref>), + (With, With), + >, + model_instances: Query<(Entity, Ref>), (With, Without)>, + mut removals: RemovedComponents>, +) { + // Removals + if !removals.is_empty() { + for description_entity in removals.read() { + for (instance_entity, affiliation) in model_instances.iter() { + if affiliation.0 == Some(description_entity) { + commands.entity(instance_entity).remove::(); + } + } + } + } + + // Changes + for (instance_entity, affiliation) in model_instances.iter() { + if let Some(description_entity) = affiliation.0 { + if let Ok((_, _, property)) = model_properties.get(description_entity) { + if property.is_changed() || affiliation.is_changed() { + let mut cmd = commands.entity(instance_entity); + cmd.remove::>(); + cmd.insert(property.0.clone()); + } + } + } + } +} + +pub fn assign_orphan_model_instances_to_level( + mut commands: Commands, + mut orphan_instances: Query< + (Entity, Option<&Parent>, &mut SiteParent), + (With, Without), + >, + current_level: Res, +) { + let current_level = match current_level.0 { + Some(c) => c, + None => return, + }; + + for (instance_entity, parent, mut site_parent) in orphan_instances.iter_mut() { + if parent.is_none() { + commands.entity(current_level).add_child(instance_entity); + } + if site_parent.0.is_none() { + site_parent.0 = Some(current_level); + } + } +} diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 458d8319..7fe49d6f 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -107,14 +107,18 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration With, With, With, - With, With, + With, With, With, + With, )>, Without, ), >, + Query, With)>, + Query, Without)>, + Query>, Query< Entity, ( @@ -146,6 +150,9 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration let ( level_children, + model_descriptions, + model_instances, + scenarios, nav_graph_elements, levels, lifts, @@ -197,6 +204,36 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration } } + if let Ok(model_description) = model_descriptions.get(*site_child) { + if !site_ids.contains(model_description) { + new_entities.push(model_description); + println!("NEW DESCRIPTION"); + } + } + + if let Ok(model_instance) = model_instances.get(*site_child) { + if !site_ids.contains(model_instance) { + new_entities.push(model_instance); + println!("NEW INSTANCE"); + } + } + + // Ensure root scenarios have the smallest Site_ID, since when deserializing, child scenarios would + // require parent scenarios to already be spawned and have its parent entity + if let Ok(scenario) = scenarios.get(*site_child) { + let mut queue = vec![scenario]; + while let Some(scenario) = queue.pop() { + if !site_ids.contains(scenario) { + new_entities.push(scenario); + } + if let Ok(scenario_children) = children.get(scenario) { + for child in scenario_children { + queue.push(*child); + } + } + } + } + if let Ok(e) = drawing_children.get(*site_child) { // Sites can contain anchors and fiducials but should not contain // measurements, so this query doesn't make perfect sense to use @@ -336,10 +373,6 @@ fn generate_levels( ), (With, Without), >, - Query< - (&NameInSite, &AssetSource, &Pose, &IsStatic, &Scale, &SiteID), - (With, Without), - >, Query<(&NameInSite, &Pose, &PhysicalCameraProperties, &SiteID), Without>, Query< ( @@ -377,7 +410,6 @@ fn generate_levels( q_floors, q_lights, q_measurements, - q_models, q_physical_cameras, q_walls, q_levels, @@ -553,19 +585,6 @@ fn generate_levels( }, ); } - if let Ok((name, source, pose, is_static, scale, id)) = q_models.get(*c) { - level.models.insert( - id.0, - Model { - name: name.clone(), - source: source.clone(), - pose: pose.clone(), - is_static: is_static.clone(), - scale: scale.clone(), - marker: ModelMarker, - }, - ); - } if let Ok((name, pose, properties, id)) = q_physical_cameras.get(*c) { level.physical_cameras.insert( id.0, @@ -1181,6 +1200,181 @@ fn migrate_relative_paths( } } +fn generate_model_descriptions( + site: Entity, + world: &mut World, +) -> Result, SiteGenerationError> { + let mut state: SystemState<( + Query< + ( + &SiteID, + &NameInSite, + &ModelProperty, + &ModelProperty, + &ModelProperty, + ), + (With, With, Without), + >, + Query<&Children>, + )> = SystemState::new(world); + let (model_descriptions, children) = state.get(world); + + let mut res = BTreeMap::::new(); + if let Ok(children) = children.get(site) { + for child in children.iter() { + if let Ok((site_id, name, source, is_static, scale)) = model_descriptions.get(*child) { + res.insert( + site_id.0, + ModelDescriptionBundle { + name: name.clone(), + source: source.clone(), + is_static: is_static.clone(), + scale: scale.clone(), + ..Default::default() + }, + ); + } + } + } + Ok(res) +} + +fn generate_model_instances( + site: Entity, + world: &mut World, +) -> Result>, SiteGenerationError> { + let mut state: SystemState<( + Query<&SiteID, (With, With, Without)>, + Query< + ( + Entity, + &SiteID, + &NameInSite, + &Pose, + &SiteParent, + &Affiliation, + ), + (With, Without, Without), + >, + Query<(Entity, &SiteID), With>, + Query<&Children>, + Query<&Parent>, + )> = SystemState::new(world); + let (model_descriptions, model_instances, levels, _, parents) = state.get(world); + + let mut site_levels_ids = std::collections::HashMap::::new(); + for (level_entity, site_id) in levels.iter() { + if parents.get(level_entity).is_ok_and(|p| p.get() == site) { + site_levels_ids.insert(level_entity, site_id.0); + } + } + let mut res = BTreeMap::>::new(); + for ( + _instance_entity, + instance_id, + instance_name, + instance_pose, + instance_parent, + instance_affiliation, + ) in model_instances.iter() + { + if instance_parent + .0 + .is_some_and(|p| !site_levels_ids.contains_key(&p)) + { + continue; + } + res.insert( + instance_id.0, + ModelInstance:: { + name: instance_name.clone(), + pose: instance_pose.clone(), + parent: SiteParent(instance_parent.0.map(|p| site_levels_ids[&p])), + description: Affiliation( + instance_affiliation + .0 + .map(|e| model_descriptions.get(e).ok().map(|d| d.0)) + .flatten(), + ), + ..Default::default() + }, + ); + } + Ok(res) +} + +fn generate_scenarios( + site: Entity, + world: &mut World, +) -> Result>, SiteGenerationError> { + let mut state: SystemState<( + Query<(Entity, &NameInSite, &SiteID, &Scenario), With>, + Query<&SiteID, With>, + Query<&Children>, + Query<&Parent>, + )> = SystemState::new(world); + let (scenarios, instances, children, parent) = state.get(world); + let mut res = BTreeMap::>::new(); + + if let Ok(site_children) = children.get(site) { + for site_child in site_children.iter() { + if let Ok((entity, ..)) = scenarios.get(*site_child) { + let mut queue = vec![entity]; + + while let Some(scenario) = queue.pop() { + if let Ok(scenario_children) = children.get(scenario) { + for scenario_child in scenario_children.iter() { + queue.push(*scenario_child); + } + } + + if let Ok((_, name, site_id, scenario)) = scenarios.get(scenario) { + res.insert( + site_id.0, + ScenarioBundle { + name: name.clone(), + scenario: Scenario { + parent_scenario: match scenario.parent_scenario.0 { + Some(parent) => Affiliation( + scenarios + .get(parent) + .map(|(_, _, site_id, _)| site_id.0) + .ok(), + ), + None => Affiliation(None), + }, + added_instances: scenario + .added_instances + .iter() + .map(|(entity, pose)| { + (instances.get(*entity).unwrap().0, pose.clone()) + }) + .collect(), + moved_instances: scenario + .moved_instances + .iter() + .map(|(entity, pose)| { + (instances.get(*entity).unwrap().0, pose.clone()) + }) + .collect(), + removed_instances: scenario + .removed_instances + .iter() + .map(|entity| instances.get(*entity).unwrap().0) + .collect(), + }, + marker: ScenarioMarker, + }, + ); + } + } + } + } + } + println!("ADDED SCENARIOS: {:?}", res.len()); + Ok(res) +} + pub fn generate_site( world: &mut World, site: Entity, @@ -1199,6 +1393,9 @@ pub fn generate_site( let locations = generate_locations(world, site)?; let graph_ranking = generate_graph_rankings(world, site)?; let properties = generate_site_properties(world, site)?; + let model_descriptions = generate_model_descriptions(site, world)?; + let model_instances = generate_model_instances(site, world)?; + let scenarios = generate_scenarios(site, world)?; disassemble_edited_drawing(world); return Ok(Site { @@ -1220,6 +1417,9 @@ pub fn generate_site( }, // TODO(MXG): Parse agent information once the spec is figured out agents: Default::default(), + model_descriptions, + model_instances, + scenarios, }); } @@ -1428,7 +1628,6 @@ pub fn save_nav_graphs(world: &mut World) { level.drawings.clear(); level.floors.clear(); level.lights.clear(); - level.models.clear(); level.walls.clear(); } diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs new file mode 100644 index 00000000..4d326b7d --- /dev/null +++ b/rmf_site_editor/src/site/scenario.rs @@ -0,0 +1,295 @@ +/* + * Copyright (C) 2024 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::Selection, + site::{ + CurrentScenario, Delete, Dependents, InstanceMarker, Pending, Pose, Scenario, + ScenarioBundle, ScenarioMarker, SiteParent, + }, + CurrentWorkspace, +}; +use bevy::prelude::*; +use std::collections::HashMap; + +#[derive(Clone, Copy, Debug, Event)] +pub struct ChangeCurrentScenario(pub Entity); + +/// Handles changes to the current scenario +pub fn update_current_scenario( + mut commands: Commands, + mut selection: ResMut, + mut change_current_scenario: EventReader, + mut current_scenario: ResMut, + current_workspace: Res, + scenarios: Query<&Scenario>, + mut instances: Query< + (Entity, &mut Pose, &SiteParent, &mut Visibility), + With, + >, +) { + for ChangeCurrentScenario(scenario_entity) in change_current_scenario.read() { + // Used to build a scenario from root + let mut scenario_stack = Vec::<&Scenario>::new(); + let mut scenario = scenarios + .get(*scenario_entity) + .expect("Failed to get scenario entity"); + loop { + scenario_stack.push(scenario); + if let Some(scenario_parent) = scenario.parent_scenario.0 { + scenario = scenarios + .get(scenario_parent) + .expect("Scenario parent doesn't exist"); + } else { + break; + } + } + + // Iterate stack to identify instances in this model + let mut active_instances = HashMap::::new(); + for scenario in scenario_stack.iter().rev() { + for (e, pose) in scenario.added_instances.iter() { + active_instances.insert(*e, pose.clone()); + } + for (e, pose) in scenario.moved_instances.iter() { + active_instances.insert(*e, pose.clone()); + } + for e in scenario.removed_instances.iter() { + active_instances.remove(e); + } + } + + let current_site_entity = match current_workspace.root { + Some(current_site) => current_site, + None => return, + }; + + // If active, assign parent to level, otherwise assign parent to site + for (entity, mut pose, parent, mut visibility) in instances.iter_mut() { + if let Some(new_pose) = active_instances.get(&entity) { + if let Some(parent_entity) = parent.0 { + commands.entity(entity).set_parent(parent_entity); + } else { + commands.entity(entity).set_parent(current_site_entity); + warn!("Model instance {:?} has no valid site parent", entity); + } + *pose = new_pose.clone(); + *visibility = Visibility::Inherited; + } else { + commands.entity(entity).set_parent(current_site_entity); + *visibility = Visibility::Hidden; + } + } + + // Deselect if not in current scenario + if let Some(selected_entity) = selection.0.clone() { + if let Ok((instance_entity, ..)) = instances.get(selected_entity) { + if active_instances.get(&instance_entity).is_none() { + selection.0 = None; + } + } + } + + *current_scenario = CurrentScenario(Some(*scenario_entity)); + } +} + +/// Tracks pose changes for instances in the current scenario to update its properties +pub fn update_scenario_properties( + current_scenario: Res, + mut scenarios: Query<&mut Scenario>, + mut change_current_scenario: EventReader, + changed_instances: Query<(Entity, Ref), (With, Without)>, +) { + // Do nothing if scenario has changed, as we rely on pose changes by the user and not the system updating instances + for ChangeCurrentScenario(_) in change_current_scenario.read() { + return; + } + + if let Some(mut current_scenario) = current_scenario + .0 + .and_then(|entity| scenarios.get_mut(entity).ok()) + { + for (entity, pose) in changed_instances.iter() { + if pose.is_changed() { + let existing_removed_instance = current_scenario + .removed_instances + .iter_mut() + .find(|e| **e == entity) + .map(|e| e.clone()); + if let Some(existing_removed_instance) = existing_removed_instance { + current_scenario + .moved_instances + .retain(|(e, _)| *e != existing_removed_instance); + current_scenario + .added_instances + .retain(|(e, _)| *e != existing_removed_instance); + return; + } + + let existing_added_instance: Option<&mut (Entity, Pose)> = current_scenario + .added_instances + .iter_mut() + .find(|(e, _)| *e == entity); + if let Some(existing_added_instance) = existing_added_instance { + existing_added_instance.1 = pose.clone(); + return; + } else if pose.is_added() { + current_scenario + .added_instances + .push((entity, pose.clone())); + return; + } + + let existing_moved_instance = current_scenario + .moved_instances + .iter_mut() + .find(|(e, _)| *e == entity); + if let Some(existing_moved_instance) = existing_moved_instance { + existing_moved_instance.1 = pose.clone(); + return; + } else { + current_scenario + .moved_instances + .push((entity, pose.clone())); + return; + } + } + } + } +} + +#[derive(Debug, Clone, Copy, Event)] +pub struct RemoveScenario(pub Entity); + +/// When a scenario is removed, all child scenarios are removed as well +pub fn handle_remove_scenarios( + mut commands: Commands, + mut remove_scenario_requests: EventReader, + mut change_current_scenario: EventWriter, + mut delete: EventWriter, + mut current_scenario: ResMut, + current_workspace: Res, + mut scenarios: Query< + (Entity, &Scenario, Option<&mut Dependents>), + With, + >, + children: Query<&Children>, +) { + for request in remove_scenario_requests.read() { + // Any child scenarios or instances added within the subtree are considered dependents + // to be deleted + let mut subtree_dependents = std::collections::HashSet::::new(); + let mut queue = vec![request.0]; + while let Some(scenario_entity) = queue.pop() { + if let Ok((_, scenario, _)) = scenarios.get(scenario_entity) { + scenario.added_instances.iter().for_each(|(e, _)| { + subtree_dependents.insert(*e); + }); + } + if let Ok(children) = children.get(scenario_entity) { + children.iter().for_each(|e| { + subtree_dependents.insert(*e); + queue.push(*e); + }); + } + } + + // Change to parent scenario, else root, else create an empty scenario and switch to it + if let Some(parent_scenario_entity) = scenarios + .get(request.0) + .map(|(_, s, _)| s.parent_scenario.0) + .ok() + .flatten() + { + change_current_scenario.send(ChangeCurrentScenario(parent_scenario_entity)); + } else if let Some((root_scenario_entity, _, _)) = scenarios + .iter() + .filter(|(e, s, _)| request.0 != *e && s.parent_scenario.0.is_none()) + .next() + { + change_current_scenario.send(ChangeCurrentScenario(root_scenario_entity)); + } else { + let new_scenario_entity = commands + .spawn(ScenarioBundle::::default()) + .set_parent(current_workspace.root.expect("No current site")) + .id(); + *current_scenario = CurrentScenario(Some(new_scenario_entity)); + } + + // Delete with dependents + if let Ok((_, _, Some(mut depenedents))) = scenarios.get_mut(request.0) { + depenedents.extend(subtree_dependents.iter()); + } else { + commands + .entity(request.0) + .insert(Dependents(subtree_dependents)); + } + delete.send(Delete::new(request.0).and_dependents()); + } +} + +#[derive(Debug, Clone, Copy, Event)] +pub struct RemoveInstance(pub Entity); + +/// Handle requests to remove model instances. If an instance was added in this scenario, or if +/// the scenario is root, the InstanceMarker is removed, allowing it to be permanently deleted. +/// Otherwise, it is only temporarily removed. +pub fn handle_remove_instances( + mut commands: Commands, + mut scenarios: Query<&mut Scenario>, + current_scenario: ResMut, + mut change_current_scenario: EventWriter, + mut remove_requests: EventReader, + mut delete: EventWriter, +) { + for removal in remove_requests.read() { + let Some(current_scenario_entity) = current_scenario.0 else { + delete.send(Delete::new(removal.0)); + return; + }; + + if let Ok(mut current_scenario) = scenarios.get_mut(current_scenario_entity) { + // Delete if root scenario + if current_scenario.parent_scenario.0.is_none() { + current_scenario + .added_instances + .retain(|(e, _)| *e != removal.0); + commands.entity(removal.0).remove::(); + delete.send(Delete::new(removal.0)); + return; + } + // Delete if added in this scenario + if let Some(added_id) = current_scenario + .added_instances + .iter() + .position(|(e, _)| *e == removal.0) + { + current_scenario.added_instances.remove(added_id); + commands.entity(removal.0).remove::(); + delete.send(Delete::new(removal.0)); + return; + } + // Otherwise, remove + current_scenario + .moved_instances + .retain(|(e, _)| *e != removal.0); + current_scenario.removed_instances.push(removal.0); + change_current_scenario.send(ChangeCurrentScenario(current_scenario_entity)); + } + } +} diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs index b3cd666a..1fd35de4 100644 --- a/rmf_site_editor/src/site/site.rs +++ b/rmf_site_editor/src/site/site.rs @@ -17,10 +17,14 @@ use crate::{interaction::CameraControls, CurrentWorkspace}; use bevy::prelude::*; +use itertools::Itertools; use rmf_site_format::{ - LevelElevation, LevelProperties, NameInSite, NameOfSite, Pose, UserCameraPoseMarker, + LevelElevation, LevelProperties, NameInSite, NameOfSite, Pose, ScenarioBundle, ScenarioMarker, + UserCameraPoseMarker, }; +use super::ChangeCurrentScenario; + /// Used as an event to command that a new site should be made the current one #[derive(Clone, Copy, Debug, Event)] pub struct ChangeCurrentSite { @@ -28,12 +32,18 @@ pub struct ChangeCurrentSite { pub site: Entity, /// What should its current level be pub level: Option, + /// What should its current scenario be + pub scenario: Option, } /// Used as a resource that keeps track of the current level entity #[derive(Clone, Copy, Debug, Default, Deref, DerefMut, Resource)] pub struct CurrentLevel(pub Option); +/// Used as a resource that keeps track of the current scenario entity +#[derive(Clone, Copy, Debug, Default, Deref, DerefMut, Resource)] +pub struct CurrentScenario(pub Option); + /// Used as a component that maps from the site entity to the level entity which /// was most recently selected for it. #[derive(Component, Clone, Deref, DerefMut, Debug)] @@ -47,14 +57,17 @@ pub struct NextSiteID(pub u32); pub fn change_site( mut commands: Commands, mut change_current_site: EventReader, + mut change_current_scenario: EventWriter, mut current_workspace: ResMut, mut current_level: ResMut, + current_scenario: ResMut, cached_levels: Query<&CachedLevel>, mut visibility: Query<&mut Visibility>, open_sites: Query>, children: Query<&Children>, parents: Query<&Parent>, levels: Query>, + scenarios: Query>, ) { let mut set_visibility = |entity, value| { if let Ok(mut v) = visibility.get_mut(entity) { @@ -135,6 +148,30 @@ pub fn change_site( } } } + + if let Some(new_scenario) = cmd.scenario { + if let Some(previous_scenario) = current_scenario.0 { + if previous_scenario != new_scenario { + change_current_scenario.send(ChangeCurrentScenario(new_scenario)); + } + } + } else { + if let Ok(children) = children.get(cmd.site) { + let any_scenario = children + .iter() + .filter(|child| scenarios.get(**child).is_ok()) + .find_or_first(|_| true); + if let Some(new_scenario) = any_scenario { + change_current_scenario.send(ChangeCurrentScenario(*new_scenario)); + } else { + let new_scenario = commands + .spawn(ScenarioBundle::::default()) + .set_parent(cmd.site) + .id(); + change_current_scenario.send(ChangeCurrentScenario(new_scenario)); + } + } + } } } diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 9166a137..a45b1251 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -18,14 +18,17 @@ use crate::{ inspector::{InspectAssetSourceComponent, InspectScaleComponent}, interaction::{ChangeMode, SelectAnchor, SelectAnchor3D}, - site::{AssetSource, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale}, + site::{AssetSource, Category, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale}, widgets::{prelude::*, AssetGalleryStatus}, AppState, CurrentWorkspace, }; -use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{CollapsingHeader, Ui}; +use bevy::{audio::Source, ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{CollapsingHeader, ComboBox, Grid, Ui}; -use rmf_site_format::{DrawingProperties, Geometry, Model, WorkcellModel}; +use rmf_site_format::{ + Affiliation, DrawingProperties, Geometry, Group, IsStatic, ModelDescriptionBundle, + ModelInstance, ModelMarker, ModelProperty, NameInSite, SiteID, WorkcellModel, +}; /// This widget provides a widget with buttons for creating new site elements. #[derive(Default)] @@ -33,8 +36,7 @@ pub struct CreationPlugin {} impl Plugin for CreationPlugin { fn build(&self, app: &mut App) { - app.init_resource::() - .init_resource::() + app.init_resource::() .add_plugins(PropertiesTilePlugin::::new()); } } @@ -45,8 +47,13 @@ struct Creation<'w, 's> { app_state: Res<'w, State>, change_mode: EventWriter<'w, ChangeMode>, current_workspace: Res<'w, CurrentWorkspace>, - pending_drawings: ResMut<'w, PendingDrawing>, - pending_model: ResMut<'w, PendingModel>, + model_descriptions: Query< + 'w, + 's, + (Entity, &'static NameInSite, Option<&'static SiteID>), + (With, With), + >, + creation_data: ResMut<'w, CreationData>, asset_gallery: Option>, commands: Commands<'w, 's>, } @@ -68,208 +75,476 @@ impl<'w, 's> WidgetSystem for Creation<'w, 's> { impl<'w, 's> Creation<'w, 's> { pub fn show_widget(&mut self, ui: &mut Ui) { - ui.vertical(|ui| { - match self.app_state.get() { - AppState::MainMenu | AppState::SiteVisualizer => { - return; - } - AppState::SiteEditor => { - if ui.button("Lane").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_new_edge_sequence().for_lane().into(), - )); + ui.horizontal(|ui| { + ui.label("New"); + ComboBox::from_id_source("creation_mode") + .selected_text(self.creation_data.to_string()) + .show_ui(ui, |ui| { + for mode_name in CreationData::string_values() { + if ui + .selectable_value( + &mut self.creation_data.to_string(), + mode_name, + mode_name, + ) + .clicked() + { + *self.creation_data = CreationData::from_string(mode_name); + } } + }); + }); + ui.separator(); - if ui.button("Location").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_new_point().for_location().into(), - )); - } + match *self.creation_data { + CreationData::SiteObject => { + self.show_create_site_objects(ui); + } + CreationData::Drawing(_) => { + self.show_create_drawing(ui); + } + CreationData::ModelDescription(_) => { + self.show_create_model_description(ui); + } + CreationData::ModelInstance(_) => self.show_create_model_instance(ui), + } + } - if ui.button("Wall").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_new_edge_sequence().for_wall().into(), - )); - } + pub fn show_create_site_objects(&mut self, ui: &mut Ui) { + match self.app_state.get() { + AppState::SiteEditor => { + Grid::new("create_site_objects") + .num_columns(3) + .show(ui, |ui| { + if ui.button("↔ Lane").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_edge_sequence().for_lane().into(), + )); + } - if ui.button("Door").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_one_new_edge().for_door().into(), - )); - } + if ui.button("📌 Location").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_point().for_location().into(), + )); + } - if ui.button("Lift").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_one_new_edge().for_lift().into(), - )); - } + if ui.button("■ Wall").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_edge_sequence().for_wall().into(), + )); + } - if ui.button("Floor").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_new_path().for_floor().into(), - )); - } - if ui.button("Fiducial").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_new_point().for_site_fiducial().into(), - )); - } + ui.end_row(); - ui.add_space(10.0); - CollapsingHeader::new("New drawing") - .default_open(false) - .show(ui, |ui| { - let default_file = self - .current_workspace - .root - .map(|e| self.default_file.get(e).ok()) - .flatten(); - if let Some(new_asset_source) = InspectAssetSourceComponent::new( - &self.pending_drawings.source, - &self.pending_drawings.recall_source, - default_file, - ) - .show(ui) - { - self.pending_drawings - .recall_source - .remember(&new_asset_source); - self.pending_drawings.source = new_asset_source; - } - ui.add_space(5.0); - if ui.button("Add Drawing").clicked() { - self.commands.spawn(DrawingBundle::new(DrawingProperties { - source: self.pending_drawings.source.clone(), - ..default() - })); - } - }); + if ui.button("🚪 Door").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_one_new_edge().for_door().into(), + )); + } + + if ui.button("⬍ Lift").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_one_new_edge().for_lift().into(), + )); + } + + if ui.button("✏ Floor").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_path().for_floor().into(), + )); + } + + ui.end_row(); + + if ui.button("☉ Fiducial").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_point().for_site_fiducial().into(), + )); + } + }); + } + AppState::SiteDrawingEditor => { + if ui.button("Fiducial").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_new_point() + .for_drawing_fiducial() + .into(), + )); } - AppState::SiteDrawingEditor => { - if ui.button("Fiducial").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_new_point() - .for_drawing_fiducial() - .into(), - )); - } - if ui.button("Measurement").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor::create_one_new_edge().for_measurement().into(), - )); - } + if ui.button("Measurement").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor::create_one_new_edge().for_measurement().into(), + )); } - AppState::WorkcellEditor => { - if ui.button("Frame").clicked() { - self.change_mode.send(ChangeMode::To( - SelectAnchor3D::create_new_point().for_anchor(None).into(), - )); - } + } + AppState::WorkcellEditor => { + if ui.button("Frame").clicked() { + self.change_mode.send(ChangeMode::To( + SelectAnchor3D::create_new_point().for_anchor(None).into(), + )); } } - match self.app_state.get() { - AppState::MainMenu | AppState::SiteDrawingEditor | AppState::SiteVisualizer => {} - AppState::SiteEditor | AppState::WorkcellEditor => { - ui.add_space(10.0); - CollapsingHeader::new("New model") - .default_open(false) - .show(ui, |ui| { - let default_file = self - .current_workspace - .root - .map(|e| self.default_file.get(e).ok()) - .flatten(); - if let Some(new_asset_source) = InspectAssetSourceComponent::new( - &self.pending_model.source, - &self.pending_model.recall_source, - default_file, - ) - .show(ui) - { - self.pending_model.recall_source.remember(&new_asset_source); - self.pending_model.source = new_asset_source; - } - ui.add_space(5.0); - if let Some(new_scale) = - InspectScaleComponent::new(&self.pending_model.scale).show(ui) - { - self.pending_model.scale = new_scale; - } + _ => { + return; + } + } + } + + pub fn show_create_drawing(&mut self, ui: &mut Ui) { + match self.app_state.get() { + AppState::SiteEditor => { + let pending_drawings = match *self.creation_data { + CreationData::Drawing(ref mut pending_drawings) => pending_drawings, + _ => return, + }; + + let default_file = self + .current_workspace + .root + .map(|e| self.default_file.get(e).ok()) + .flatten(); + if let Some(new_asset_source) = InspectAssetSourceComponent::new( + &pending_drawings.source, + &pending_drawings.recall_source, + default_file, + ) + .show(ui) + { + pending_drawings.recall_source.remember(&new_asset_source); + pending_drawings.source = new_asset_source; + } + ui.add_space(5.0); + if ui.button("Add Drawing").clicked() { + self.commands.spawn(DrawingBundle::new(DrawingProperties { + source: pending_drawings.source.clone(), + ..default() + })); + } + } + _ => { + return; + } + } + } + + pub fn show_create_model_description(&mut self, ui: &mut Ui) { + match self.app_state.get() { + AppState::MainMenu | AppState::SiteDrawingEditor | AppState::SiteVisualizer => {} + AppState::SiteEditor | AppState::WorkcellEditor => { + let pending_model = match *self.creation_data { + CreationData::ModelDescription(ref mut pending_model) => pending_model, + _ => return, + }; + + ui.horizontal(|ui| { + ui.label("Description Name"); + let mut new_name = pending_model.name.clone(); + ui.text_edit_singleline(&mut new_name); + pending_model.name = new_name; + }); + + ui.add_space(10.0); + let default_file = self + .current_workspace + .root + .map(|e| self.default_file.get(e).ok()) + .flatten(); + if let Some(new_asset_source) = InspectAssetSourceComponent::new( + &pending_model.source, + &pending_model.recall_source, + default_file, + ) + .show(ui) + { + pending_model.recall_source.remember(&new_asset_source); + pending_model.source = new_asset_source; + } + + ui.add_space(5.0); + if let Some(new_scale) = InspectScaleComponent::new(&pending_model.scale).show(ui) { + pending_model.scale = new_scale; + } + + ui.separator(); + if let Some(asset_gallery) = &mut self.asset_gallery { + match self.app_state.get() { + AppState::MainMenu + | AppState::SiteDrawingEditor + | AppState::SiteVisualizer => {} + AppState::SiteEditor => { ui.add_space(5.0); - if let Some(asset_gallery) = &mut self.asset_gallery { - match self.app_state.get() { - AppState::MainMenu - | AppState::SiteDrawingEditor - | AppState::SiteVisualizer => {} - AppState::SiteEditor => { - if ui.button("Browse fuel").clicked() { - asset_gallery.show = true; - } - if ui.button("Spawn model").clicked() { - let model = Model { - source: self.pending_model.source.clone(), - scale: self.pending_model.scale, - ..default() - }; + + ui.horizontal(|ui| { + let add_icon = match pending_model.spawn_instance { + true => "✚", + false => "⬆", + }; + if ui.button(add_icon).clicked() { + if let Some(site_entity) = self.current_workspace.root { + let model_description_bundle = ModelDescriptionBundle { + name: NameInSite(pending_model.name.clone()), + source: ModelProperty(pending_model.source.clone()), + is_static: ModelProperty(IsStatic::default()), + scale: ModelProperty(pending_model.scale.clone()), + group: Group, + marker: ModelMarker, + }; + let description_entity = self + .commands + .spawn(model_description_bundle) + .insert(Category::ModelDescription) + .set_parent(site_entity) + .id(); + + if pending_model.spawn_instance { + let model_instance: ModelInstance = + ModelInstance { + name: NameInSite( + pending_model.instance_name.clone(), + ), + description: Affiliation(Some( + description_entity, + )), + ..Default::default() + }; self.change_mode.send(ChangeMode::To( SelectAnchor3D::create_new_point() - .for_model(model) + .for_model_instance(model_instance) .into(), )); } } - AppState::WorkcellEditor => { - if ui.button("Browse fuel").clicked() { - asset_gallery.show = true; - } - if ui.button("Spawn visual").clicked() { - let workcell_model = WorkcellModel { - geometry: Geometry::Mesh { - source: self.pending_model.source.clone(), - scale: Some(*self.pending_model.scale), - }, - ..default() - }; - self.change_mode.send(ChangeMode::To( - SelectAnchor3D::create_new_point() - .for_visual(workcell_model) - .into(), - )); + } + ComboBox::from_id_source("load_or_load_and_spawn") + .selected_text(if pending_model.spawn_instance { + "Load and Spawn" + } else { + "Load Description" + }) + .show_ui(ui, |ui| { + if ui + .selectable_label( + pending_model.spawn_instance, + "Load and Spawn", + ) + .clicked() + { + pending_model.spawn_instance = true; } - if ui.button("Spawn collision").clicked() { - let workcell_model = WorkcellModel { - geometry: Geometry::Mesh { - source: self.pending_model.source.clone(), - scale: Some(*self.pending_model.scale), - }, - ..default() - }; - self.change_mode.send(ChangeMode::To( - SelectAnchor3D::create_new_point() - .for_collision(workcell_model) - .into(), - )); + if ui + .selectable_label( + !pending_model.spawn_instance, + "Load Description", + ) + .clicked() + { + pending_model.spawn_instance = false; } - ui.add_space(10.0); - } + }); + if pending_model.spawn_instance { + ui.text_edit_singleline(&mut pending_model.instance_name); } + }); + ui.add_space(3.0); + if ui + .selectable_label(asset_gallery.show, "Browse Fuel") + .clicked() + { + asset_gallery.show = !asset_gallery.show; } - }); + } + AppState::WorkcellEditor => { + if ui + .selectable_label(asset_gallery.show, "Browse Fuel") + .clicked() + { + asset_gallery.show = !asset_gallery.show; + } + if ui.button("Spawn visual").clicked() { + let workcell_model = WorkcellModel { + geometry: Geometry::Mesh { + source: pending_model.source.clone(), + scale: Some(*pending_model.scale), + }, + ..default() + }; + self.change_mode.send(ChangeMode::To( + SelectAnchor3D::create_new_point() + .for_visual(workcell_model) + .into(), + )); + } + if ui.button("Spawn collision").clicked() { + let workcell_model = WorkcellModel { + geometry: Geometry::Mesh { + source: pending_model.source.clone(), + scale: Some(*pending_model.scale), + }, + ..default() + }; + self.change_mode.send(ChangeMode::To( + SelectAnchor3D::create_new_point() + .for_collision(workcell_model) + .into(), + )); + } + ui.add_space(10.0); + } + } } } - }); + } + } + + pub fn show_create_model_instance(&mut self, ui: &mut Ui) { + match self.app_state.get() { + AppState::MainMenu | AppState::SiteDrawingEditor | AppState::SiteVisualizer => {} + AppState::SiteEditor | AppState::WorkcellEditor => { + let pending_model_instance = match *self.creation_data { + CreationData::ModelInstance(ref mut pending_model) => pending_model, + _ => return, + }; + + let mut style = (*ui.ctx().style()).clone(); + style.wrap = Some(false); + ui.horizontal(|ui| { + ui.ctx().set_style(style); + ui.label("Description"); + let selected_text = pending_model_instance + .description_entity + .and_then(|description_entity| { + self.model_descriptions + .get(description_entity) + .ok() + .map(|(_, name, _)| name.0.clone()) + }) + .unwrap_or_else(|| "Select Description".to_string()); + ComboBox::from_id_source("select_instance_to_spawn") + .selected_text(selected_text) + .show_ui(ui, |ui| { + for (entity, name, site_id) in self.model_descriptions.iter() { + if ui + .selectable_label( + pending_model_instance.description_entity == Some(entity), + format!( + "#{} {}", + site_id + .map(|s| s.to_string()) + .unwrap_or("*".to_string()), + name.0 + ), + ) + .clicked() + { + pending_model_instance.description_entity = Some(entity); + } + } + }); + }); + + ui.horizontal(|ui| { + ui.add_enabled_ui(pending_model_instance.description_entity.is_some(), |ui| { + if ui.button("➕ Spawn").clicked() { + let model_instance: ModelInstance = ModelInstance { + name: NameInSite(pending_model_instance.instance_name.clone()), + description: Affiliation(Some( + pending_model_instance.description_entity.unwrap(), + )), + ..Default::default() + }; + self.change_mode.send(ChangeMode::To( + SelectAnchor3D::create_new_point() + .for_model_instance(model_instance) + .into(), + )); + } + }); + ui.text_edit_singleline(&mut pending_model_instance.instance_name); + }); + } + } } } #[derive(Resource, Clone, Default)] +enum CreationData { + #[default] + SiteObject, + Drawing(PendingDrawing), + ModelDescription(PendingModelDescription), + ModelInstance(PendingModelInstance), +} + +impl CreationData { + fn to_string(&self) -> &str { + match self { + Self::SiteObject => "Site Object", + Self::Drawing(_) => "Drawing", + Self::ModelDescription(_) => "Model Description", + Self::ModelInstance(_) => "Model Instance", + } + } + + fn from_string(s: &str) -> Self { + match s { + "Site Object" => Self::SiteObject, + "Drawing" => Self::Drawing(PendingDrawing::default()), + "Model Description" => Self::ModelDescription(PendingModelDescription::default()), + "Model Instance" => Self::ModelInstance(PendingModelInstance::default()), + _ => Self::SiteObject, + } + } + + fn string_values() -> Vec<&'static str> { + vec![ + "Site Object", + "Drawing", + "Model Description", + "Model Instance", + ] + } +} + +#[derive(Clone, Default)] struct PendingDrawing { pub source: AssetSource, pub recall_source: RecallAssetSource, } -#[derive(Resource, Clone, Default)] -struct PendingModel { +#[derive(Clone)] +struct PendingModelInstance { + pub description_entity: Option, + pub instance_name: String, +} + +impl Default for PendingModelInstance { + fn default() -> Self { + Self { + description_entity: None, + instance_name: "".to_string(), + } + } +} + +#[derive(Clone)] +struct PendingModelDescription { + pub name: String, pub source: AssetSource, pub recall_source: RecallAssetSource, pub scale: Scale, + pub spawn_instance: bool, + pub instance_name: String, +} + +impl Default for PendingModelDescription { + fn default() -> Self { + Self { + name: "".to_string(), + source: AssetSource::default(), + recall_source: RecallAssetSource::default(), + scale: Scale::default(), + spawn_instance: true, + instance_name: " ".to_string(), + } + } } diff --git a/rmf_site_editor/src/widgets/fuel_asset_browser.rs b/rmf_site_editor/src/widgets/fuel_asset_browser.rs index 0c7a2885..a5c349bb 100644 --- a/rmf_site_editor/src/widgets/fuel_asset_browser.rs +++ b/rmf_site_editor/src/widgets/fuel_asset_browser.rs @@ -16,9 +16,13 @@ */ use crate::{ - interaction::{ChangeMode, ModelPreviewCamera, SelectAnchor3D}, - site::{AssetSource, FuelClient, Model, SetFuelApiKey, UpdateFuelCache}, + interaction::ModelPreviewCamera, + site::{ + AssetSource, FuelClient, Model, ModelDescriptionBundle, ModelProperty, NameInSite, Scale, + SetFuelApiKey, UpdateFuelCache, + }, widgets::prelude::*, + CurrentWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{self, Button, ComboBox, ImageSource, RichText, ScrollArea, Ui, Window}; @@ -72,12 +76,12 @@ pub struct AssetGalleryStatus { pub struct FuelAssetBrowser<'w, 's> { fuel_client: ResMut<'w, FuelClient>, // TODO(luca) refactor to see whether we need + current_workspace: Res<'w, CurrentWorkspace>, asset_gallery_status: ResMut<'w, AssetGalleryStatus>, model_preview_camera: Res<'w, ModelPreviewCamera>, update_cache: EventWriter<'w, UpdateFuelCache>, set_api_key: EventWriter<'w, SetFuelApiKey>, commands: Commands<'w, 's>, - change_mode: EventWriter<'w, ChangeMode>, } fn fuel_asset_browser_panel(In(input): In, world: &mut World) { @@ -268,16 +272,22 @@ impl<'w, 's> FuelAssetBrowser<'w, 's> { } if let Some(selected) = &gallery_status.selected { - if ui.button("Spawn model").clicked() { - let model = Model { - source: AssetSource::Remote( + if ui.button("Load as Description").clicked() { + let model_description = ModelDescriptionBundle { + name: NameInSite(selected.owner.clone() + "/" + &selected.name), + source: ModelProperty(AssetSource::Remote( selected.owner.clone() + "/" + &selected.name + "/model.sdf", - ), - ..default() + )), + scale: ModelProperty(Scale::default()), + ..Default::default() }; - self.change_mode.send(ChangeMode::To( - SelectAnchor3D::create_new_point().for_model(model).into(), - )); + if let Some(site_entity) = self.current_workspace.root { + self.commands + .spawn(model_description) + .set_parent(site_entity); + } else { + warn!("No current site found to load model description"); + } } } } diff --git a/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs b/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs index f85298b2..61bdcd6f 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs @@ -23,15 +23,19 @@ use crate::{ use bevy::prelude::*; use bevy_egui::egui::{ComboBox, Ui}; use pathdiff::diff_paths; -use rmf_site_format::{AssetSource, RecallAssetSource}; +use rmf_site_format::{Affiliation, AssetSource, RecallAssetSource}; #[cfg(not(target_arch = "wasm32"))] use rfd::FileDialog; #[derive(SystemParam)] pub struct InspectAssetSource<'w, 's> { - asset_sources: - Query<'w, 's, (&'static AssetSource, &'static RecallAssetSource), Without>, + asset_sources: Query< + 'w, + 's, + (&'static AssetSource, &'static RecallAssetSource), + (Without, Without>), + >, change_asset_source: EventWriter<'w, Change>, current_workspace: Res<'w, CurrentWorkspace>, default_file: Query<'w, 's, &'static DefaultFile>, diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index f7b0df47..092c066b 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -16,7 +16,7 @@ */ use crate::{ - site::{Affiliation, Change, DefaultFile, Group, Members, NameInSite, Texture}, + site::{Affiliation, Change, DefaultFile, Group, Members, ModelMarker, NameInSite, Texture}, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, CurrentWorkspace, }; @@ -26,7 +26,7 @@ use bevy_egui::egui::{CollapsingHeader, RichText, Ui}; #[derive(SystemParam)] pub struct InspectGroup<'w, 's> { is_group: Query<'w, 's, (), With>, - affiliation: Query<'w, 's, &'static Affiliation>, + affiliation: Query<'w, 's, &'static Affiliation, Without>, names: Query<'w, 's, &'static NameInSite>, textures: Query<'w, 's, &'static Texture>, members: Query<'w, 's, &'static Members>, diff --git a/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_differential_drive.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_differential_drive.rs new file mode 100644 index 00000000..b1bfe042 --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_differential_drive.rs @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2024 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 super::get_selected_description_entity; +use crate::{ + site::{Affiliation, Change, DifferentialDrive, Group, ModelMarker, ModelProperty, Pose}, + widgets::{prelude::*, Inspect}, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{DragValue, Grid}; + +#[derive(SystemParam)] +pub struct InspectModelDifferentialDrive<'w, 's> { + model_instances: Query< + 'w, + 's, + &'static Affiliation, + (With, Without, With), + >, + model_descriptions: + Query<'w, 's, &'static ModelProperty, (With, With)>, + change_differential_drive: EventWriter<'w, Change>>, + poses: Query<'w, 's, &'static Pose>, + gizmos: Gizmos<'s>, +} + +impl<'w, 's> WidgetSystem for InspectModelDifferentialDrive<'w, 's> { + fn show( + Inspect { selection, .. }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + let mut params = state.get_mut(world); + let Some(description_entity) = get_selected_description_entity( + selection, + ¶ms.model_instances, + ¶ms.model_descriptions, + ) else { + return; + }; + let Ok(ModelProperty(differential_drive)) = + params.model_descriptions.get(description_entity) + else { + return; + }; + + let mut new_differential_drive = differential_drive.clone(); + + ui.label("Differential Drive"); + ui.indent("inspect_differential_drive_properties", |ui| { + Grid::new("inspect_diferential_drive_1") + .num_columns(2) + .show(ui, |ui| { + ui.label("max velocity"); + ui.add( + DragValue::new(&mut new_differential_drive.translational_speed) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.label("m/s"); + ui.end_row(); + + ui.label("max angular"); + ui.add( + DragValue::new(&mut new_differential_drive.rotational_speed) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.label("rad/s"); + ui.end_row(); + + ui.label("collision radius"); + if ui + .add( + DragValue::new(&mut new_differential_drive.collision_radius) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ) + .is_pointer_button_down_on() + { + if let Ok(pose) = params.poses.get(selection) { + params.gizmos.circle( + Vec3::new(pose.trans[0], pose.trans[1], pose.trans[2] + 0.01), + Vec3::Z, + new_differential_drive.collision_radius, + Color::RED, + ); + } + }; + ui.label("m") + }); + + Grid::new("inspect_differential_drive_2") + .num_columns(3) + .show(ui, |ui| { + ui.label("center offset"); + ui.label("x"); + ui.label("y"); + ui.end_row(); + + ui.label(""); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[0]) + .clamp_range(std::f32::NEG_INFINITY..=std::f32::INFINITY) + .speed(0.01), + ); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[1]) + .clamp_range(std::f32::NEG_INFINITY..=std::f32::INFINITY) + .speed(0.01), + ); + }); + + ui.horizontal(|ui| { + ui.label("bidirectional"); + ui.checkbox(&mut new_differential_drive.bidirectional, ""); + }); + }); + + if new_differential_drive != *differential_drive { + params.change_differential_drive.send(Change::new( + ModelProperty(new_differential_drive), + description_entity, + )); + } + } +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_required_properties.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_required_properties.rs new file mode 100644 index 00000000..d631588f --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_required_properties.rs @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2024 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 super::get_selected_description_entity; +use crate::{ + site::{ + Affiliation, AssetSource, Change, DefaultFile, Group, ModelMarker, ModelProperty, + RecallAssetSource, Scale, + }, + widgets::{prelude::*, Inspect, InspectAssetSourceComponent, InspectScaleComponent}, + CurrentWorkspace, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; + +#[derive(SystemParam)] +pub struct InspectModelScale<'w, 's> { + model_instances: Query< + 'w, + 's, + &'static Affiliation, + (With, Without, With), + >, + model_descriptions: + Query<'w, 's, &'static ModelProperty, (With, With)>, + change_scale: EventWriter<'w, Change>>, +} + +impl<'w, 's> WidgetSystem for InspectModelScale<'w, 's> { + fn show( + Inspect { selection, .. }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + let mut params = state.get_mut(world); + let Some(description_entity) = get_selected_description_entity( + selection, + ¶ms.model_instances, + ¶ms.model_descriptions, + ) else { + return; + }; + + let Ok(ModelProperty(scale)) = params.model_descriptions.get(description_entity) else { + return; + }; + if let Some(new_scale) = InspectScaleComponent::new(scale).show(ui) { + params + .change_scale + .send(Change::new(ModelProperty(new_scale), description_entity)); + } + } +} + +#[derive(SystemParam)] +pub struct InspectModelAssetSource<'w, 's> { + model_instances: Query< + 'w, + 's, + &'static Affiliation, + (With, Without, With), + >, + model_descriptions: + Query<'w, 's, &'static ModelProperty, (With, With)>, + change_asset_source: EventWriter<'w, Change>>, + current_workspace: Res<'w, CurrentWorkspace>, + default_file: Query<'w, 's, &'static DefaultFile>, +} + +impl<'w, 's> WidgetSystem for InspectModelAssetSource<'w, 's> { + fn show( + Inspect { selection, .. }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + let mut params = state.get_mut(world); + let Some(description_entity) = get_selected_description_entity( + selection, + ¶ms.model_instances, + ¶ms.model_descriptions, + ) else { + return; + }; + + let Ok(ModelProperty(source)) = params.model_descriptions.get(description_entity) else { + return; + }; + + let default_file = params + .current_workspace + .root + .map(|e| params.default_file.get(e).ok()) + .flatten(); + + if let Some(new_source) = + InspectAssetSourceComponent::new(source, &RecallAssetSource::default(), default_file) + .show(ui) + { + params + .change_asset_source + .send(Change::new(ModelProperty(new_source), description_entity)); + } + } +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs new file mode 100644 index 00000000..cede6292 --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs @@ -0,0 +1,428 @@ +/* + * Copyright (C) 2024 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::{ + component::ComponentInfo, + query::{ReadOnlyWorldQuery, WorldQuery}, + system::{EntityCommands, SystemParam}, + }, + prelude::*, +}; +use bevy_egui::egui::{CollapsingHeader, ComboBox, RichText, Ui}; +use smallvec::SmallVec; +use std::{any::TypeId, collections::HashMap, fmt::Debug}; + +use crate::{ + site::{ + update_model_instances, Affiliation, AssetSource, Change, ChangePlugin, Group, IsStatic, + ModelMarker, ModelProperty, NameInSite, Scale, + }, + widgets::{prelude::*, Inspect}, + MainInspector, +}; + +pub mod inspect_differential_drive; +pub use inspect_differential_drive::*; + +pub mod inspect_required_properties; +pub use inspect_required_properties::*; + +#[derive(Default)] +pub struct InspectModelDescriptionPlugin {} + +impl Plugin for InspectModelDescriptionPlugin { + fn build(&self, app: &mut App) { + let main_inspector = app.world.resource::().id; + let widget = Widget::new::(&mut app.world); + let id = app.world.spawn(widget).set_parent(main_inspector).id(); + app.world.insert_resource(ModelDescriptionInspector { id }); + app.world.init_resource::(); + } +} + +/// Contains a reference to the model description inspector widget. +#[derive(Resource)] +pub struct ModelDescriptionInspector { + id: Entity, +} + +impl ModelDescriptionInspector { + pub fn get(&self) -> Entity { + self.id + } +} + +/// Function that inserts a default property into an entity +type InsertModelPropertyFn = fn(EntityCommands); + +fn get_insert_model_property_fn() -> InsertModelPropertyFn { + |mut e_commands| { + e_commands.insert(T::default()); + } +} + +/// Function that removes a property, if it exists, from an entity +type RemoveModelPropertyFn = fn(EntityCommands); + +fn get_remove_model_property_fn() -> RemoveModelPropertyFn { + |mut e_commands| { + e_commands.remove::(); + } +} + +/// This resource keeps track of all the properties that can be configured for a model description. +#[derive(Resource)] +pub struct ModelPropertyData { + pub required: HashMap, + pub optional: HashMap, +} + +impl FromWorld for ModelPropertyData { + fn from_world(_world: &mut World) -> Self { + let mut required = HashMap::new(); + required.insert( + TypeId::of::>(), + ( + "Asset Source".to_string(), + get_insert_model_property_fn::>(), + get_remove_model_property_fn::>(), + ), + ); + required.insert( + TypeId::of::>(), + ( + "Scale".to_string(), + get_insert_model_property_fn::>(), + get_remove_model_property_fn::>(), + ), + ); + required.insert( + TypeId::of::>(), + ( + "Is Static".to_string(), + get_insert_model_property_fn::(), + get_remove_model_property_fn::(), + ), + ); + let optional = HashMap::new(); + Self { required, optional } + } +} + +/// Implement this plugin to add a new configurable property of type T to the model description inspector. +pub struct InspectModelPropertyPlugin +where + W: WidgetSystem + 'static + Send + Sync, + T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath + Component, +{ + property_name: String, + _ignore: std::marker::PhantomData<(W, ModelProperty)>, +} + +impl InspectModelPropertyPlugin +where + W: WidgetSystem + 'static + Send + Sync, + T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath + Component, +{ + pub fn new(property_name: String) -> Self { + Self { + property_name: property_name, + _ignore: Default::default(), + } + } +} + +impl Plugin for InspectModelPropertyPlugin +where + W: WidgetSystem + 'static + Send + Sync, + T: 'static + Send + Sync + Debug + Default + Clone + FromReflect + TypePath + Component, +{ + fn build(&self, app: &mut App) { + let type_id = TypeId::of::>(); + if !app + .world + .resource::() + .required + .contains_key(&type_id) + { + app.register_type::>(); + app.add_plugins(ChangePlugin::>::default()); + app.add_systems(PreUpdate, update_model_instances::); + + app.world + .resource_mut::() + .optional + .insert( + type_id, + ( + self.property_name.clone(), + get_insert_model_property_fn::>(), + get_remove_model_property_fn::>(), + ), + ); + } + + let inspector = app.world.resource::().id; + let widget = Widget::::new::(&mut app.world); + app.world.spawn(widget).set_parent(inspector); + } +} + +/// This is the base model description inspector widget, which allows the user to dynamically +/// configure the properties associated with a model description. +#[derive(SystemParam)] +struct InspectModelDescription<'w, 's> { + commands: Commands<'w, 's>, + model_instances: Query< + 'w, + 's, + &'static Affiliation, + (With, Without, With), + >, + model_descriptions: Query<'w, 's, &'static NameInSite, (With, With)>, + model_properties: Res<'w, ModelPropertyData>, + inspect_model_description: Res<'w, ModelDescriptionInspector>, + children: Query<'w, 's, &'static Children>, +} + +impl<'w, 's> WidgetSystem for InspectModelDescription<'w, 's> { + fn show( + Inspect { + selection, + inspection: _, + panel, + }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + // Get description entity from within closure, since inspect_entity requires immutable reference to world + let description_entity = { + let params = state.get_mut(world); + if let Some(description_entity) = get_selected_description_entity( + selection, + ¶ms.model_instances, + ¶ms.model_descriptions, + ) { + description_entity + } else { + return; + } + }; + + let components_info: Vec = world + .inspect_entity(description_entity) + .into_iter() + .cloned() + .collect(); + + let mut inserts_to_execute = Vec::::new(); + let mut removals_to_execute = Vec::::new(); + + { + let params = state.get_mut(world); + + let mut enabled_property_types = Vec::::new(); + for component_info in components_info { + if let Some(type_id) = component_info.type_id() { + if params.model_properties.optional.contains_key(&type_id) { + enabled_property_types.push(type_id); + } + } + } + let mut disabled_property_types = Vec::::new(); + for (type_id, _) in params.model_properties.optional.iter() { + if !enabled_property_types.contains(type_id) { + disabled_property_types.push(*type_id); + } + } + + ui.separator(); + let description_name = params + .model_descriptions + .get(description_entity) + .map(|n| n.0.clone()) + .unwrap_or("Unnamed".to_string()); + ui.label( + RichText::new(format!("Description Properties of [{}]", description_name)) + .size(18.0), + ); + + CollapsingHeader::new("Configure Properties") + .default_open(false) + .show(ui, |ui| { + ui.horizontal_wrapped(|ui| { + // Required + for type_id in params.model_properties.required.keys() { + let (property_name, _, _) = + params.model_properties.required.get(&type_id).unwrap(); + ui.add_enabled_ui(false, |ui| { + ui.selectable_label(true, property_name) + .on_disabled_hover_text( + "This property is required and cannot be toggled", + ); + }); + } + // Enabled + for type_id in enabled_property_types { + let (property_name, _, remove_fn) = + params.model_properties.optional.get(&type_id).unwrap(); + if ui + .selectable_label(true, property_name) + .on_hover_text("Click to toggle") + .clicked() + { + removals_to_execute.push(*remove_fn); + } + } + // Disabled + for type_id in disabled_property_types { + let (property_name, insert_fn, _) = + params.model_properties.optional.get(&type_id).unwrap(); + if ui + .selectable_label(false, property_name) + .on_hover_text("Click to toggle") + .clicked() + { + inserts_to_execute.push(insert_fn.clone()); + } + } + }); + }); + + let children: Result, _> = params + .children + .get(params.inspect_model_description.id) + .map(|children| children.iter().copied().collect()); + let Ok(children) = children else { + return; + }; + + for child in children { + let inspect = Inspect { + selection, + inspection: child, + panel, + }; + ui.add_space(10.0); + let _ = world.try_show_in(child, inspect, ui); + } + } + + for insert_fn in inserts_to_execute { + insert_fn(state.get_mut(world).commands.entity(description_entity)); + } + for remove_fn in removals_to_execute { + remove_fn(state.get_mut(world).commands.entity(description_entity)); + } + } +} + +/// When inspecting a selected instance of a model description, this widget allows the user to view +/// and change its description +#[derive(SystemParam)] +pub struct InspectSelectedModelDescription<'w, 's> { + model_instances: Query< + 'w, + 's, + &'static Affiliation, + (With, Without, With), + >, + model_descriptions: + Query<'w, 's, (Entity, &'static NameInSite), (With, With)>, + change_affiliation: EventWriter<'w, Change>>, +} + +impl<'w, 's> WidgetSystem for InspectSelectedModelDescription<'w, 's> { + fn show( + Inspect { selection, .. }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + let mut params = state.get_mut(world); + params.show_widget(selection, ui); + } +} + +impl<'w, 's> InspectSelectedModelDescription<'w, 's> { + pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { + let Some(current_description_entity) = + get_selected_description_entity(id, &self.model_instances, &self.model_descriptions) + else { + return; + }; + let (current_description_entity, current_description_name) = self + .model_descriptions + .get(current_description_entity) + .unwrap(); + + if self.model_descriptions.get(id).is_ok() { + return; + } + + let mut new_description_entity = current_description_entity.clone(); + ui.horizontal(|ui| { + ui.label("Description"); + ComboBox::from_id_source("model_description_affiliation") + .selected_text(current_description_name.0.as_str()) + .show_ui(ui, |ui| { + for (entity, name, ..) in self.model_descriptions.iter() { + ui.selectable_value(&mut new_description_entity, entity, name.0.as_str()); + } + }); + }); + if new_description_entity != current_description_entity { + self.change_affiliation + .send(Change::new(Affiliation(Some(new_description_entity)), id)); + } + } +} + +/// Helper function to get the corresponding description entity for a given model instance entity +/// if it exists. +fn get_selected_description_entity<'w, 's, S: ReadOnlyWorldQuery, T: WorldQuery>( + selection: Entity, + model_instances: &Query< + 'w, + 's, + &'static Affiliation, + (With, Without, S), + >, + model_descriptions: &Query<'w, 's, T, (With, With)>, +) -> Option { + if model_descriptions.get(selection).ok().is_some() { + return Some(selection); + } + + if let Some(affiliation) = model_instances.get(selection).ok() { + let Some(affiliation) = affiliation.0 else { + warn!("Model instance is not affiliated with a description"); + return None; + }; + + if model_descriptions.get(affiliation).is_ok() { + return Some(affiliation); + } else { + warn!("Model instance is affiliated with a non-existent description"); + return None; + } + } + + return None; +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs index 173dd319..73441286 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs @@ -21,11 +21,11 @@ use crate::{ }; use bevy::prelude::*; use bevy_egui::egui::{DragValue, Grid, Ui}; -use rmf_site_format::Scale; +use rmf_site_format::{Affiliation, Scale}; #[derive(SystemParam)] pub struct InspectScale<'w, 's> { - scales: Query<'w, 's, &'static Scale>, + scales: Query<'w, 's, &'static Scale, Without>>, change_scale: EventWriter<'w, Change>, } @@ -59,13 +59,14 @@ impl<'a> InspectScaleComponent<'a> { pub fn show(self, ui: &mut Ui) -> Option { let mut new_scale = self.scale.clone(); - ui.label("Scale"); Grid::new("inspect_scale").show(ui, |ui| { + ui.label("Scale"); ui.label("x"); ui.label("y"); ui.label("z"); ui.end_row(); + ui.label(""); ui.add( DragValue::new(&mut new_scale.0[0]) .clamp_range(0_f32..=std::f32::INFINITY) diff --git a/rmf_site_editor/src/widgets/inspector/inspect_task.rs b/rmf_site_editor/src/widgets/inspector/inspect_task.rs new file mode 100644 index 00000000..11bc777a --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_task.rs @@ -0,0 +1,269 @@ +/* + * Copyright (C) 2024 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::{ + update_model_instances, ChangePlugin, Group, LocationTags, MobileRobotMarker, + ModelProperty, NameInSite, Point, Task, Tasks, + }, + widgets::{prelude::*, Inspect, InspectionPlugin}, + Icons, ModelPropertyData, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{Align, Button, Color32, ComboBox, DragValue, Frame, Layout, Stroke, Ui}; +use std::any::TypeId; + +#[derive(Default)] +pub struct InspectTaskPlugin {} + +impl Plugin for InspectTaskPlugin { + fn build(&self, app: &mut App) { + // Allows us to toggle MobileRobotMarker as a configurable property + // from the model description inspector + app.register_type::>() + .add_plugins(ChangePlugin::>::default()) + .add_systems( + PreUpdate, + ( + add_remove_mobile_robot_tasks, + update_model_instances::, + ), + ) + .init_resource::() + .world + .resource_mut::() + .optional + .insert( + TypeId::of::>(), + ( + "Mobile Robot".to_string(), + |mut e_cmd| { + e_cmd.insert(ModelProperty::::default()); + }, + |mut e_cmd| { + e_cmd.remove::>(); + }, + ), + ); + + // Ui + app.init_resource::().add_plugins(( + ChangePlugin::>::default(), + InspectionPlugin::::new(), + )); + } +} + +#[derive(SystemParam)] +pub struct InspectTasks<'w, 's> { + mobile_robots: + Query<'w, 's, &'static mut Tasks, (With, Without)>, + locations: Query<'w, 's, (Entity, &'static NameInSite, &'static LocationTags)>, + pending_task: ResMut<'w, PendingTask>, + icons: Res<'w, Icons>, +} + +impl<'w, 's> WidgetSystem for InspectTasks<'w, 's> { + fn show( + Inspect { selection, .. }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + let mut params = state.get_mut(world); + let Ok(mut tasks) = params.mobile_robots.get_mut(selection) else { + return; + }; + + ui.label("Tasks"); + Frame::default() + .inner_margin(4.0) + .rounding(2.0) + .stroke(Stroke::new(1.0, Color32::GRAY)) + .show(ui, |ui| { + ui.set_min_width(ui.available_width()); + + if tasks.0.is_empty() { + ui.label("No Tasks"); + } else { + let mut deleted_ids = Vec::new(); + for (id, task) in tasks.0.iter_mut().enumerate() { + let is_deleted = + edit_task_component(ui, &id, task, ¶ms.locations, true); + if is_deleted { + deleted_ids.push(id); + } + } + for id in deleted_ids { + tasks.0.remove(id); + } + } + }); + + ui.add_space(10.0); + ui.horizontal(|ui| { + // Only allow adding as task if valid + ui.add_enabled_ui( + is_task_valid(¶ms.pending_task.0, ¶ms.locations), + |ui| { + if ui + .add(Button::image_and_text(params.icons.add.egui(), "New")) + .clicked() + { + tasks.0.push(params.pending_task.0.clone()); + } + }, + ); + // Select new task type + ComboBox::from_id_source("pending_edit_task") + .selected_text(params.pending_task.0.label()) + .show_ui(ui, |ui| { + for label in Task::::labels() { + if ui + .selectable_label( + label == params.pending_task.0.label(), + label.to_string(), + ) + .clicked() + { + *params.pending_task = PendingTask(task_from_label(label)); + } + } + }); + }); + + ui.add_space(10.0); + edit_task_component( + ui, + &tasks.0.len(), + &mut params.pending_task.0, + ¶ms.locations, + false, + ); + } +} + +/// Returns true if the task should be deleted +fn edit_task_component( + ui: &mut Ui, + id: &usize, + task: &mut Task, + locations: &Query<(Entity, &NameInSite, &LocationTags)>, + with_delete: bool, +) -> bool { + let mut is_deleted = false; + Frame::default() + .inner_margin(4.0) + .fill(Color32::DARK_GRAY) + .rounding(2.0) + .show(ui, |ui| { + ui.set_min_width(ui.available_width()); + ui.horizontal(|ui| { + ui.label(task.label()); + + match task { + Task::GoToPlace { location } => { + let selected_location_name = locations + .get(location.0) + .map(|(_, name, _)| name.0.clone()) + .unwrap_or("Select Location".to_string()); + + ComboBox::from_id_source(id.to_string() + "select_go_to_location") + .selected_text(selected_location_name) + .show_ui(ui, |ui| { + for (entity, name, _) in locations.iter() { + if ui + .selectable_label(location.0 == entity, name.0.clone()) + .clicked() + { + *location = Point(entity); + } + } + }); + } + Task::WaitFor { duration } => { + ui.add( + DragValue::new(duration) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + } + } + + // Delete + ui.with_layout(Layout::right_to_left(Align::Center), |ui| { + if with_delete { + if ui.button("❌").on_hover_text("Delete task").clicked() { + is_deleted = true; + } + } + }); + }); + }); + return is_deleted; +} + +#[derive(Resource)] +pub struct PendingTask(Task); + +impl FromWorld for PendingTask { + fn from_world(_world: &mut World) -> Self { + PendingTask(Task::GoToPlace { + location: Point(Entity::PLACEHOLDER), + }) + } +} + +/// When the MobileRobotMarker is added or removed, add or remove the Tasks component +fn add_remove_mobile_robot_tasks( + mut commands: Commands, + instances: Query<(Entity, Ref), Without>, + mut removals: RemovedComponents>, +) { + for removal in removals.read() { + if instances.get(removal).is_ok() { + commands.entity(removal).remove::>(); + } + } + + for (e, marker) in instances.iter() { + if marker.is_added() { + commands.entity(e).insert(Tasks::::default()); + } + } +} + +fn is_task_valid( + task: &Task, + locations: &Query<(Entity, &NameInSite, &LocationTags)>, +) -> bool { + match task { + Task::GoToPlace { location } => locations.get(location.0).is_ok(), + _ => true, + } +} + +pub fn task_from_label(label: &str) -> Task { + let labels = Task::::labels(); + match labels.iter().position(|&l| l == label) { + Some(0) => Task::GoToPlace { + location: Point(Entity::PLACEHOLDER), + }, + Some(1) => Task::WaitFor { duration: 0.0 }, + _ => Task::WaitFor { duration: 0.0 }, + } +} diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 544549de..f50b2e69 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -75,6 +75,9 @@ pub use inspect_primitive_shape::*; pub mod inspect_measurement; pub use inspect_measurement::*; +pub mod inspect_model_description; +pub use inspect_model_description::*; + pub mod inspect_motion; pub use inspect_motion::*; @@ -99,6 +102,9 @@ pub use inspect_scale::*; pub mod inspect_side; pub use inspect_side::*; +pub mod inspect_task; +pub use inspect_task::*; + pub mod inspect_texture; pub use inspect_texture::*; @@ -178,6 +184,7 @@ impl Plugin for StandardInspectorPlugin { app.add_plugins(MinimalInspectorPlugin::default()) .add_plugins(( InspectionPlugin::::new(), + InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), @@ -191,10 +198,11 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectionPlugin::::new(), // Reached the tuple limit )) .add_plugins(( + InspectTaskPlugin::default(), + InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), @@ -203,9 +211,18 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectLiftPlugin::default(), - InspectionPlugin::::new(), InspectionPlugin::::new(), + InspectModelDescriptionPlugin::default(), + InspectLiftPlugin::default(), + )) + .add_plugins(( + InspectModelPropertyPlugin::::new("Scale".to_string()), + InspectModelPropertyPlugin::::new( + "Source".to_string(), + ), + InspectModelPropertyPlugin::::new( + "Differential Drve".to_string(), + ), )); } } diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 76f7a4ed..858e6df4 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -113,6 +113,9 @@ use view_layers::*; pub mod view_levels; use view_levels::*; +pub mod view_scenarios; +use view_scenarios::*; + pub mod view_lights; use view_lights::*; @@ -122,6 +125,9 @@ use view_nav_graphs::*; pub mod view_occupancy; use view_occupancy::*; +pub mod view_tasks; +use view_tasks::*; + pub mod prelude { //! This module gives easy access to the traits, structs, and plugins that //! we expect downstream users are likely to want easy access to if they are diff --git a/rmf_site_editor/src/widgets/properties_panel.rs b/rmf_site_editor/src/widgets/properties_panel.rs index f99e218b..b84600b3 100644 --- a/rmf_site_editor/src/widgets/properties_panel.rs +++ b/rmf_site_editor/src/widgets/properties_panel.rs @@ -18,7 +18,8 @@ use crate::widgets::{ show_panel_of_tiles, BuildingPreviewPlugin, CreationPlugin, PanelSide, PanelWidget, StandardInspectorPlugin, Tile, ViewGroupsPlugin, ViewLayersPlugin, ViewLevelsPlugin, - ViewLightsPlugin, ViewNavGraphsPlugin, ViewOccupancyPlugin, Widget, WidgetSystem, + ViewLightsPlugin, ViewNavGraphsPlugin, ViewOccupancyPlugin, ViewScenariosPlugin, ViewTasks, + Widget, WidgetSystem, }; use bevy::prelude::*; @@ -33,11 +34,13 @@ impl Plugin for StandardPropertiesPanelPlugin { app.add_plugins(( PropertiesPanelPlugin::new(PanelSide::Right), ViewLevelsPlugin::default(), + ViewScenariosPlugin::default(), ViewNavGraphsPlugin::default(), ViewLayersPlugin::default(), StandardInspectorPlugin::default(), CreationPlugin::default(), ViewGroupsPlugin::default(), + PropertiesTilePlugin::::new(), ViewLightsPlugin::default(), ViewOccupancyPlugin::default(), BuildingPreviewPlugin::default(), diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index b495462a..97dbc390 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -16,12 +16,17 @@ */ use crate::{ - site::{Change, FiducialMarker, MergeGroups, NameInSite, SiteID, Texture}, + interaction::{ChangeMode, SelectAnchor3D}, + site::{ + Affiliation, Change, Delete, FiducialMarker, Group, MergeGroups, ModelInstance, + ModelMarker, NameInSite, SiteID, Texture, + }, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{Button, CollapsingHeader, Ui}; +use std::any::TypeId; /// Add a widget for viewing different kinds of groups. #[derive(Default)] @@ -37,8 +42,20 @@ impl Plugin for ViewGroupsPlugin { #[derive(SystemParam)] pub struct ViewGroups<'w, 's> { children: Query<'w, 's, &'static Children>, - textures: Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>), With>, - fiducials: Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>), With>, + textures: + Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>), (With, With)>, + fiducials: Query< + 'w, + 's, + (&'static NameInSite, Option<&'static SiteID>), + (With, With), + >, + model_descriptions: Query< + 'w, + 's, + (&'static NameInSite, Option<&'static SiteID>), + (With, With), + >, icons: Res<'w, Icons>, group_view_modes: ResMut<'w, GroupViewModes>, app_state: Res<'w, State>, @@ -49,7 +66,9 @@ pub struct ViewGroups<'w, 's> { pub struct ViewGroupsEvents<'w, 's> { current_workspace: ResMut<'w, CurrentWorkspace>, selector: SelectorWidget<'w, 's>, + change_mode: EventWriter<'w, ChangeMode>, merge_groups: EventWriter<'w, MergeGroups>, + delete: EventWriter<'w, Delete>, name: EventWriter<'w, Change>, commands: Commands<'w, 's>, } @@ -80,6 +99,16 @@ impl<'w, 's> ViewGroups<'w, 's> { let Ok(children) = self.children.get(site) else { return; }; + CollapsingHeader::new("Model Descriptions").show(ui, |ui| { + Self::show_groups( + children, + &self.model_descriptions, + &mut modes.model_descriptions, + &self.icons, + &mut self.events, + ui, + ); + }); CollapsingHeader::new("Textures").show(ui, |ui| { Self::show_groups( children, @@ -104,7 +133,7 @@ impl<'w, 's> ViewGroups<'w, 's> { fn show_groups<'b, T: Component>( children: impl IntoIterator, - q_groups: &Query<(&NameInSite, Option<&SiteID>), With>, + q_groups: &Query<(&NameInSite, Option<&SiteID>), (With, With)>, mode: &mut GroupViewMode, icons: &Res, events: &mut ViewGroupsEvents, @@ -159,6 +188,23 @@ impl<'w, 's> ViewGroups<'w, 's> { ui.horizontal(|ui| { match mode.clone() { GroupViewMode::View => { + if TypeId::of::() == TypeId::of::() { + if ui + .add(Button::image(icons.add.egui())) + .on_hover_text("Add a new model instance of this group") + .clicked() + { + let model_instance: ModelInstance = ModelInstance { + description: Affiliation(Some(child.clone())), + ..Default::default() + }; + events.change_mode.send(ChangeMode::To( + SelectAnchor3D::create_new_point() + .for_model_instance(model_instance) + .into(), + )); + }; + }; events.selector.show_widget(*child, ui); } GroupViewMode::SelectMergeFrom => { @@ -199,8 +245,12 @@ impl<'w, 's> ViewGroups<'w, 's> { .on_hover_text("Delete this group") .clicked() { - events.commands.entity(*child).despawn_recursive(); - *mode = GroupViewMode::View; + if TypeId::of::() == TypeId::of::() { + events.delete.send(Delete::new(*child).and_dependents()); + } else { + events.commands.entity(*child).despawn_recursive(); + *mode = GroupViewMode::View; + } } } } @@ -228,6 +278,7 @@ pub struct GroupViewModes { site: Option, textures: GroupViewMode, fiducials: GroupViewMode, + model_descriptions: GroupViewMode, } impl GroupViewModes { diff --git a/rmf_site_editor/src/widgets/view_occupancy.rs b/rmf_site_editor/src/widgets/view_occupancy.rs index 2580c3a8..27a17de4 100644 --- a/rmf_site_editor/src/widgets/view_occupancy.rs +++ b/rmf_site_editor/src/widgets/view_occupancy.rs @@ -60,6 +60,7 @@ impl<'w> ViewOccupancy<'w> { cell_size: self.display_occupancy.cell_size, floor: 0.01, ceiling: 1.5, + ignore: None, }); } if ui @@ -75,6 +76,7 @@ impl<'w> ViewOccupancy<'w> { cell_size: self.display_occupancy.cell_size, floor: 0.01, ceiling: 1.5, + ignore: None, }); } } diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs new file mode 100644 index 00000000..fa1ce670 --- /dev/null +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -0,0 +1,448 @@ +/* + * Copyright (C) 2024 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::{Select, Selection}, + site::{ + Category, Change, ChangeCurrentScenario, CurrentScenario, Delete, NameInSite, + RemoveScenario, Scenario, ScenarioMarker, + }, + widgets::prelude::*, + CurrentWorkspace, Icons, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{Button, CollapsingHeader, Color32, ScrollArea, Ui}; +use rmf_site_format::{Angle, InstanceMarker, Pose, ScenarioBundle, SiteID}; + +const INSTANCES_VIEWER_HEIGHT: f32 = 200.0; + +/// Add a plugin for viewing and editing a list of all levels +#[derive(Default)] +pub struct ViewScenariosPlugin {} + +impl Plugin for ViewScenariosPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_plugins(PropertiesTilePlugin::::new()); + } +} + +#[derive(SystemParam)] +pub struct ViewScenarios<'w, 's> { + commands: Commands<'w, 's>, + children: Query<'w, 's, &'static Children>, + scenarios: Query< + 'w, + 's, + (Entity, &'static NameInSite, &'static mut Scenario), + With, + >, + change_name: EventWriter<'w, Change>, + change_current_scenario: EventWriter<'w, ChangeCurrentScenario>, + remove_scenario: EventWriter<'w, RemoveScenario>, + display_scenarios: ResMut<'w, ScenarioDisplay>, + current_scenario: ResMut<'w, CurrentScenario>, + current_workspace: Res<'w, CurrentWorkspace>, + instances: Query< + 'w, + 's, + ( + Entity, + &'static NameInSite, + &'static Category, + Option<&'static SiteID>, + ), + With, + >, + selection: Res<'w, Selection>, + select: EventWriter<'w, Select>, + delete: EventWriter<'w, Delete>, + icons: Res<'w, Icons>, +} + +impl<'w, 's> WidgetSystem for ViewScenarios<'w, 's> { + fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) -> () { + let mut params = state.get_mut(world); + CollapsingHeader::new("Scenarios") + .default_open(true) + .show(ui, |ui| { + params.show_widget(ui); + }); + } +} + +impl<'w, 's> ViewScenarios<'w, 's> { + pub fn show_widget(&mut self, ui: &mut Ui) { + // Current Selection Info + if let Some(current_scenario_entity) = self.current_scenario.0 { + if let Ok((_, name, mut scenario)) = self.scenarios.get_mut(current_scenario_entity) { + ui.horizontal(|ui| { + ui.label("Selected: "); + let mut new_name = name.0.clone(); + if ui.text_edit_singleline(&mut new_name).changed() { + self.change_name + .send(Change::new(NameInSite(new_name), current_scenario_entity)); + } + if ui + .button("❌") + .on_hover_text("Delete this scenario and all its child scenarios") + .clicked() + { + self.remove_scenario + .send(RemoveScenario(current_scenario_entity)); + } + }); + ui.label("From Previous:"); + // Added + collapsing_instance_viewer( + &format!("Added: {}", scenario.added_instances.len()), + "scenario_added_instances", + ui, + |ui| { + for (entity, pose) in scenario.added_instances.iter() { + if let Ok((_, name, category, site_id)) = self.instances.get(*entity) { + ui.horizontal(|ui| { + instance_selector( + ui, + name, + site_id, + category, + entity, + &self.selection, + &mut self.select, + ); + if ui.button("❌").on_hover_text("Remove instance").clicked() { + self.delete.send(Delete::new(*entity)); + } + }); + formatted_pose(ui, pose); + } else { + warn!("Instance entity {:?} does not exist, or has invalid components", entity); + } + } + }, + ); + // Moved + let mut undo_moved_ids = Vec::new(); + collapsing_instance_viewer( + &format!("Moved: {}", scenario.moved_instances.len()), + "scenario_moved_instances", + ui, + |ui| { + for (id, (entity, pose)) in scenario.moved_instances.iter().enumerate() { + if let Ok((_, name, category, site_id)) = self.instances.get(*entity) { + ui.horizontal(|ui| { + instance_selector( + ui, + name, + site_id, + category, + entity, + &self.selection, + &mut self.select, + ); + if ui.button("↩").on_hover_text("Undo move").clicked() { + undo_moved_ids.push(id); + } + }); + formatted_pose(ui, pose); + } else { + warn!("Instance entity {:?} does not exist, or has invalid components", entity); + } + } + }, + ); + // Removed + let mut undo_removed_ids = Vec::new(); + collapsing_instance_viewer( + &format!("Removed: {}", scenario.removed_instances.len()), + "scenario_removed_instances", + ui, + |ui| { + for (id, entity) in scenario.removed_instances.iter().enumerate() { + if let Ok((_, name, category, site_id)) = self.instances.get(*entity) { + ui.horizontal(|ui| { + instance_selector( + ui, + name, + site_id, + category, + entity, + &self.selection, + &mut self.select, + ); + if ui.button("↺").on_hover_text("Restore instance").clicked() + { + undo_removed_ids.push(id); + } + }); + } else { + warn!("Instance entity {:?} does not exist, or has invalid components", entity); + } + } + }, + ); + + // Trigger an update if the scenario has been modified + let modified = !undo_removed_ids.is_empty() || !undo_moved_ids.is_empty(); + for id in undo_removed_ids { + scenario.removed_instances.remove(id); + } + for id in undo_moved_ids { + scenario.moved_instances.remove(id); + } + if modified { + self.change_current_scenario + .send(ChangeCurrentScenario(current_scenario_entity)); + } + } + } else { + ui.label("No scenario selected"); + } + + // Create Scenario + ui.separator(); + if self.current_scenario.is_none() { + self.display_scenarios.is_new_scenario_root = true; + } + ui.horizontal(|ui| { + ui.label("Add Scenario: "); + if ui + .selectable_label(self.display_scenarios.is_new_scenario_root, "Root") + .on_hover_text("Add a new root scenario") + .clicked() + { + self.display_scenarios.is_new_scenario_root = true; + }; + ui.add_enabled_ui(self.current_scenario.is_some(), |ui| { + if ui + .selectable_label(!self.display_scenarios.is_new_scenario_root, "Child") + .on_hover_text("Add a new child scenario to the selected scenario") + .clicked() + { + self.display_scenarios.is_new_scenario_root = false; + } + }); + }); + ui.horizontal(|ui| { + if ui.add(Button::image(self.icons.add.egui())).clicked() { + let mut cmd = self + .commands + .spawn(ScenarioBundle::::from_name_parent( + self.display_scenarios.new_scenario_name.clone(), + match self.display_scenarios.is_new_scenario_root { + true => None, + false => self.current_scenario.0, + }, + )); + match self.display_scenarios.is_new_scenario_root { + true => { + if let Some(site_entity) = self.current_workspace.root { + cmd.set_parent(site_entity); + } + } + false => { + if let Some(current_scenario_entity) = self.current_scenario.0 { + cmd.set_parent(current_scenario_entity); + } + } + } + let scenario_entity = cmd.id(); + self.change_current_scenario + .send(ChangeCurrentScenario(scenario_entity)); + } + let mut new_name = self.display_scenarios.new_scenario_name.clone(); + if ui + .text_edit_singleline(&mut new_name) + .on_hover_text("Name for the new scenario") + .changed() + { + self.display_scenarios.new_scenario_name = new_name; + } + }); + + // Scenario Tree starting from root scenarios + ui.separator(); + // A version string is used to differentiate scenarios, and to allow + // egui to distinguish between collapsing headers with the same name + let mut version = 1; + self.scenarios + .iter() + .filter(|(_, _, scenario)| scenario.parent_scenario.0.is_none()) + .for_each(|(scenario_entity, _, _)| { + show_scenario_widget( + ui, + &mut self.change_name, + &mut self.change_current_scenario, + &mut self.current_scenario, + scenario_entity, + vec![version], + &self.children, + &self.scenarios, + &self.icons, + ); + version += 1; + }); + } +} + +fn show_scenario_widget( + ui: &mut Ui, + change_name: &mut EventWriter>, + change_current_scenario: &mut EventWriter, + current_scenario: &mut CurrentScenario, + scenario_entity: Entity, + scenario_version: Vec, + q_children: &Query<&'static Children>, + q_scenario: &Query< + (Entity, &'static NameInSite, &'static mut Scenario), + With, + >, + icons: &Res, +) { + let (entity, name, _) = q_scenario.get(scenario_entity).unwrap(); + let scenario_version_str = scenario_version + .iter() + .map(|v| v.to_string()) + .collect::>() + .join("."); + + // Scenario version and name, e.g. 1.2.3 My Scenario + ui.horizontal(|ui| { + if ui.radio(Some(entity) == **current_scenario, "").clicked() { + change_current_scenario.send(ChangeCurrentScenario(entity)); + } + ui.colored_label(Color32::DARK_GRAY, scenario_version_str.clone()); + let mut new_name = name.0.clone(); + if ui.text_edit_singleline(&mut new_name).changed() { + change_name.send(Change::new(NameInSite(new_name), entity)); + } + }); + + // Display children recursively + // The subversion is used as an id_source so that egui does not + // generate errors when collapsing headers of the same name are created + let mut subversion = 1; + let children = q_children.get(scenario_entity); + CollapsingHeader::new(format!( + "Child Scenarios: {}", + children.map(|c| c.len()).unwrap_or(0) + )) + .default_open(true) + .id_source(scenario_version_str.clone()) + .show(ui, |ui| { + if let Ok(children) = children { + for child in children.iter() { + if let Ok(_) = q_scenario.get(*child) { + let mut version = scenario_version.clone(); + version.push(subversion); + show_scenario_widget( + ui, + change_name, + change_current_scenario, + current_scenario, + *child, + version, + q_children, + q_scenario, + icons, + ); + subversion += 1; + } + } + } else { + ui.label("No Child Scenarios"); + } + }); +} + +/// Creates a collasible header exposing a scroll area for viewing instances +fn collapsing_instance_viewer( + header_name: &str, + id: &str, + ui: &mut Ui, + add_contents: impl FnOnce(&mut Ui) -> R, +) { + CollapsingHeader::new(header_name) + .id_source(id) + .default_open(false) + .show(ui, |ui| { + ScrollArea::vertical() + .max_height(INSTANCES_VIEWER_HEIGHT) + .show(ui, add_contents); + }); +} + +/// Creates a selectable label for an instance +fn instance_selector( + ui: &mut Ui, + name: &NameInSite, + site_id: Option<&SiteID>, + category: &Category, + entity: &Entity, + selection: &Selection, + select: &mut EventWriter, + task_count: &mut u32, +) { + Frame::default() + .inner_margin(4.0) + .fill(Color32::DARK_GRAY) + .rounding(2.0) + .show(ui, |ui| { + ui.set_min_width(ui.available_width()); + + // Mobile Robot + ui.horizontal(|ui| { + ui.label("Robot"); + if ui + .selectable_label( + selected.0.is_some_and(|s| s == *robot_entity), + format!( + "Model #{} [{}]", + robot_site_id + .map(|id| id.to_string()) + .unwrap_or("unsaved".to_string()), + robot_name + ) + .to_string(), + ) + .clicked() + { + select.send(Select(Some(*robot_entity))); + } + }); + + // Task + match task { + Task::GoToPlace { location } => { + ui.horizontal(|ui| { + ui.label("Go To Place: "); + if let Ok((entity, name, _, site_id)) = site_entities.get(location.0) { + if ui + .selectable_label( + selected.0.is_some_and(|s| s == entity), + format!( + "Location #{} [{}]", + site_id + .map(|id| id.to_string()) + .unwrap_or("unsaved".to_string()), + name.0 + ) + .to_string(), + ) + .clicked() + { + select.send(Select(Some(entity))); + } + } + }); + } + Task::WaitFor { duration } => { + ui.label(format!("Wait For: {}", duration)); + } + } + }); + *task_count += 1; +} diff --git a/rmf_site_format/src/asset_source.rs b/rmf_site_format/src/asset_source.rs index 569db7db..1c5ca438 100644 --- a/rmf_site_format/src/asset_source.rs +++ b/rmf_site_format/src/asset_source.rs @@ -25,7 +25,7 @@ use pathdiff::diff_paths; use serde::{Deserialize, Serialize}; use std::path::{Path, PathBuf}; -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] #[cfg_attr(feature = "bevy", reflect(Component))] pub enum AssetSource { diff --git a/rmf_site_format/src/category.rs b/rmf_site_format/src/category.rs index 76a2e0e7..b569ae15 100644 --- a/rmf_site_format/src/category.rs +++ b/rmf_site_format/src/category.rs @@ -53,6 +53,7 @@ pub enum Category { FiducialGroup, TextureGroup, Model, + ModelDescription, Camera, Drawing, Constraint, @@ -84,6 +85,7 @@ impl Category { Self::FiducialGroup => "Fiducial Group", Self::TextureGroup => "Texture Group", Self::Model => "Model", + Self::ModelDescription => "Model Description", Self::Camera => "Camera", Self::Drawing => "Drawing", Self::Constraint => "Constraint", diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 375477e7..ae79a29b 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -1,14 +1,16 @@ use super::{ floor::FloorParameters, level::Level, lift::Lift, wall::WallProperties, PortingError, Result, }; +use crate::ModelInstance; use crate::{ alignment::align_legacy_building, Affiliation, Anchor, Angle, AssetSource, AssociatedGraphs, Category, DisplayColor, Dock as SiteDock, Drawing as SiteDrawing, DrawingProperties, Fiducial as SiteFiducial, FiducialGroup, FiducialMarker, Guided, Lane as SiteLane, LaneMarker, - Level as SiteLevel, LevelElevation, LevelProperties as SiteLevelProperties, Motion, NameInSite, - NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose, - PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, Site, SiteProperties, - Texture as SiteTexture, TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS, + Level as SiteLevel, LevelElevation, LevelProperties as SiteLevelProperties, + ModelDescriptionBundle, Motion, NameInSite, NameOfSite, NavGraph, Navigation, + OrientationConstraint, PixelsPerMeter, Pose, PreferredSemiTransparency, RankingsInLevel, + ReverseLane, Rotation, ScenarioBundle, Site, SiteProperties, Texture as SiteTexture, + TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; @@ -200,6 +202,14 @@ impl BuildingMap { let mut fiducial_groups: BTreeMap = BTreeMap::new(); let mut cartesian_fiducials: HashMap> = HashMap::new(); + + let mut model_descriptions: BTreeMap = BTreeMap::new(); + let mut model_instances: BTreeMap> = BTreeMap::new(); + let mut model_description_name_map = HashMap::::new(); + let mut scenarios: BTreeMap> = BTreeMap::new(); + let default_scenario_id = site_id.next().unwrap(); + scenarios.insert(default_scenario_id, ScenarioBundle::default()); + for (level_name, level) in &self.levels { let level_id = site_id.next().unwrap(); let mut vertex_to_anchor_id: HashMap = Default::default(); @@ -501,9 +511,20 @@ impl BuildingMap { rankings.floors.push(id); } - let mut models = BTreeMap::new(); for model in &level.models { - models.insert(site_id.next().unwrap(), model.to_site()); + let (model_instance_id, model_pose) = model.to_site( + &mut model_description_name_map, + &mut model_descriptions, + &mut model_instances, + &mut site_id, + level_id, + ); + scenarios + .get_mut(&default_scenario_id) + .unwrap() + .scenario + .added_instances + .push((model_instance_id, model_pose)); } let mut physical_cameras = BTreeMap::new(); @@ -548,7 +569,6 @@ impl BuildingMap { drawings, floors, lights, - models, physical_cameras, walls, rankings, @@ -712,6 +732,9 @@ impl BuildingMap { }, }, agents: Default::default(), + scenarios, + model_instances, + model_descriptions, }) } } diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index b356174c..eb217d42 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,9 +1,13 @@ use crate::{ - Angle, AssetSource, IsStatic, Model as SiteModel, ModelMarker, NameInSite, Pose, Rotation, - Scale, + Affiliation, Angle, AssetSource, Group, InstanceMarker, IsStatic, ModelDescriptionBundle, + ModelInstance, ModelMarker, ModelProperty, NameInSite, Pose, Rotation, Scale, SiteParent, }; use glam::DVec2; use serde::{Deserialize, Serialize}; +use std::{ + collections::{BTreeMap, HashMap}, + ops::RangeFrom, +}; #[derive(Deserialize, Serialize, Clone, Default, Debug)] pub struct Model { @@ -24,17 +28,48 @@ impl Model { DVec2::new(self.x, self.y) } - pub fn to_site(&self) -> SiteModel { - SiteModel { + pub fn to_site( + &self, + model_description_name_map: &mut HashMap, + model_descriptions: &mut BTreeMap, + model_instances: &mut BTreeMap>, + site_id: &mut RangeFrom, + level_id: u32, + ) -> (u32, Pose) { + let model_description_id = match model_description_name_map.get(&self.model_name) { + Some(id) => *id, + None => { + let id = site_id.next().unwrap(); + model_description_name_map.insert(self.model_name.clone(), id); + model_descriptions.insert( + id, + ModelDescriptionBundle { + name: NameInSite(self.model_name.split("/").last().unwrap().to_string()), + source: ModelProperty(AssetSource::Search(self.model_name.clone())), + is_static: ModelProperty(IsStatic(self.static_)), + scale: ModelProperty(Scale::default()), + group: Group::default(), + marker: ModelMarker, + }, + ); + id + } + }; + + let model_instance_id = site_id.next().unwrap(); + let pose = Pose { + trans: [self.x as f32, self.y as f32, self.z_offset as f32], + rot: Rotation::Yaw(Angle::Deg(self.yaw.to_degrees() as f32)), + }; + let model_instance = ModelInstance { name: NameInSite(self.instance_name.clone()), - source: AssetSource::Search(self.model_name.clone()), - pose: Pose { - trans: [self.x as f32, self.y as f32, self.z_offset as f32], - rot: Rotation::Yaw(Angle::Deg(self.yaw.to_degrees() as f32)), - }, - is_static: IsStatic(self.static_), - scale: Scale::default(), + pose: pose.clone(), + parent: SiteParent(Some(level_id)), + description: Affiliation(Some(model_description_id)), marker: ModelMarker, - } + instance_marker: InstanceMarker, + }; + model_instances.insert(model_instance_id, model_instance); + (model_instance_id, pose) } } diff --git a/rmf_site_format/src/level.rs b/rmf_site_format/src/level.rs index 6981fc78..baba1059 100644 --- a/rmf_site_format/src/level.rs +++ b/rmf_site_format/src/level.rs @@ -61,8 +61,6 @@ pub struct Level { #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub lights: BTreeMap, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - pub models: BTreeMap, - #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub physical_cameras: BTreeMap, #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub walls: BTreeMap>, @@ -82,7 +80,6 @@ impl Level { drawings: Default::default(), floors: Default::default(), lights: Default::default(), - models: Default::default(), physical_cameras: Default::default(), walls: Default::default(), user_camera_poses: Default::default(), diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index cf8f0e49..4f860192 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -80,6 +80,9 @@ pub use measurement::*; pub mod misc; pub use misc::*; +pub mod mobile_robot; +pub use mobile_robot::*; + pub mod model; pub use model::*; @@ -101,6 +104,9 @@ pub use point::*; pub mod recall; pub use recall::*; +pub mod scenario; +pub use scenario::*; + pub mod sdf; pub use sdf::*; @@ -110,6 +116,9 @@ pub use semver::*; pub mod site; pub use site::*; +pub mod task; +pub use task::*; + pub mod texture; pub use texture::*; diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 4754f510..6d6ec0b8 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -402,10 +402,44 @@ pub struct PreviewableMarker; /// This component is applied to each site element that gets loaded in order to /// remember what its original ID within the Site file was. -#[derive(Clone, Copy, Debug)] +#[derive(Serialize, Deserialize, Clone, Copy, Debug)] #[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] pub struct SiteID(pub u32); +/// This component is applied to an entity as a reference to its parent entity. +#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +#[serde(transparent)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct SiteParent(pub Option); + +impl From for SiteParent { + fn from(value: T) -> Self { + SiteParent(Some(value)) + } +} + +impl From> for SiteParent { + fn from(value: Option) -> Self { + SiteParent(value) + } +} + +impl Default for SiteParent { + fn default() -> Self { + SiteParent(None) + } +} + +impl SiteParent { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + if let Some(x) = self.0 { + Ok(SiteParent(Some(id_map.get(&x).ok_or(x)?.clone()))) + } else { + Ok(SiteParent(None)) + } + } +} + /// The Pending component indicates that an element is not yet ready to be /// saved to file. We will filter out these elements while assigning SiteIDs, /// and that will prevent them from being included while collecting elements @@ -422,14 +456,14 @@ pub struct Pending; pub struct Original(pub T); /// Marks that an entity represents a group -#[derive(Clone, Copy, Debug, Default)] +#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component))] pub struct Group; /// Affiliates an entity with a group. #[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] #[serde(transparent)] -#[cfg_attr(feature = "bevy", derive(Component))] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] pub struct Affiliation(pub Option); impl From for Affiliation { diff --git a/rmf_site_format/src/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs new file mode 100644 index 00000000..000a6bea --- /dev/null +++ b/rmf_site_format/src/mobile_robot.rs @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2024 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. + * +*/ + +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Reflect, ReflectComponent}; +use serde::{Deserialize, Serialize}; + +#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub struct MobileRobotMarker; + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct DifferentialDrive { + pub translational_speed: f32, + pub rotational_speed: f32, + pub bidirectional: bool, + pub collision_radius: f32, + pub rotation_center_offset: [f32; 2], +} + +impl Default for DifferentialDrive { + fn default() -> Self { + Self { + translational_speed: 0.5, + rotational_speed: 1.0, + bidirectional: false, + collision_radius: 0.5, + rotation_center_offset: [0.0, 0.0], + } + } +} diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index 8cda5bd8..89655bcf 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -19,6 +19,7 @@ use crate::*; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; +use std::collections::HashMap; #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] @@ -57,3 +58,67 @@ impl Default for Model { } } } + +/// +/// +/// + +/// Defines a property in a model description, that will be added to all instances +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct ModelProperty(pub T); + +/// Bundle with all required components for a valid model description +#[derive(Serialize, Deserialize, Default, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct ModelDescriptionBundle { + pub name: NameInSite, + pub source: ModelProperty, + #[serde(default, skip_serializing_if = "is_default")] + pub is_static: ModelProperty, + #[serde(default, skip_serializing_if = "is_default")] + pub scale: ModelProperty, + #[serde(skip)] + pub group: Group, + #[serde(skip)] + pub marker: ModelMarker, +} + +/// Bundle with all required components for a valid model instance +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct ModelInstance { + pub name: NameInSite, + pub pose: Pose, + pub parent: SiteParent, + pub description: Affiliation, + #[serde(skip)] + pub marker: ModelMarker, + #[serde(skip)] + pub instance_marker: InstanceMarker, +} + +impl Default for ModelInstance { + fn default() -> Self { + Self { + name: NameInSite("".to_string()), + pose: Pose::default(), + parent: SiteParent::default(), + description: Affiliation::default(), + marker: ModelMarker, + instance_marker: InstanceMarker, + } + } +} + +impl ModelInstance { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(ModelInstance { + name: self.name.clone(), + pose: self.pose.clone(), + parent: self.parent.convert(id_map)?, + description: self.description.convert(id_map)?, + ..Default::default() + }) + } +} diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs new file mode 100644 index 00000000..077b69d5 --- /dev/null +++ b/rmf_site_format/src/scenario.rs @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2024 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::*; +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; +use serde::{Deserialize, Serialize}; +use std::collections::HashMap; + +#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub struct InstanceMarker; + +#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub struct ScenarioMarker; + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct Scenario { + pub parent_scenario: Affiliation, + pub added_instances: Vec<(T, Pose)>, + pub removed_instances: Vec, + pub moved_instances: Vec<(T, Pose)>, +} + +impl Scenario { + pub fn from_parent(parent: T) -> Scenario { + Scenario { + parent_scenario: Affiliation(Some(parent)), + added_instances: Vec::new(), + removed_instances: Vec::new(), + moved_instances: Vec::new(), + } + } +} + +// Create a root scenario without parent +impl Default for Scenario { + fn default() -> Self { + Self { + parent_scenario: Affiliation::default(), + added_instances: Vec::new(), + removed_instances: Vec::new(), + moved_instances: Vec::new(), + } + } +} + +impl Scenario { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(Scenario { + parent_scenario: self.parent_scenario.convert(id_map)?, + added_instances: self + .added_instances + .clone() + .into_iter() + .map(|(id, pose)| { + ( + id_map + .get(&id) + .expect("Scenario contains non existent moved instance") + .clone(), + pose, + ) + }) + .collect(), + removed_instances: self + .removed_instances + .clone() + .into_iter() + .map(|id| { + id_map + .get(&id) + .expect("Scenario contains non existent removed instance") + .clone() + }) + .collect(), + moved_instances: self + .moved_instances + .clone() + .into_iter() + .map(|(id, pose)| { + ( + id_map + .get(&id) + .expect("Scenario contains non existent moved instance") + .clone(), + pose, + ) + }) + .collect(), + }) + } +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct ScenarioBundle { + pub name: NameInSite, + pub scenario: Scenario, + pub marker: ScenarioMarker, +} + +impl ScenarioBundle { + pub fn from_name_parent(name: String, parent: Option) -> ScenarioBundle { + ScenarioBundle { + name: NameInSite(name), + scenario: Scenario { + parent_scenario: Affiliation(parent), + added_instances: Vec::new(), + removed_instances: Vec::new(), + moved_instances: Vec::new(), + }, + marker: ScenarioMarker, + } + } +} + +impl Default for ScenarioBundle { + fn default() -> Self { + Self { + name: NameInSite("Default Scenario".to_string()), + scenario: Scenario::default(), + marker: ScenarioMarker, + } + } +} + +impl ScenarioBundle { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(ScenarioBundle { + name: self.name.clone(), + scenario: self.scenario.convert(id_map)?, + marker: ScenarioMarker, + }) + } +} diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index 3414f0e6..61821d07 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -434,6 +434,11 @@ impl Site { filename: "toggle_floors".into(), ..Default::default() }; + // Only export default scenario into SDF for now + let (_, default_scenario) = self + .scenarios + .first_key_value() + .expect("No scenarios found"); for (level_id, level) in &self.levels { let mut level_model_names = vec![]; let mut model_element_map = ElementMap::default(); @@ -485,24 +490,46 @@ impl Site { }); // TODO(luca) We need this because there is no concept of ingestor or dispenser in // rmf_site yet. Remove when there is - for (model_id, model) in &level.models { + for (model_instance_id, _) in &default_scenario.scenario.added_instances { + let model_instance = self + .model_instances + .get(model_instance_id) + .expect("Model instance not found"); + let (model_description_id, model_description_bundle) = + if let Some(model_description_id) = model_instance.description.0 { + ( + model_description_id, + self.model_descriptions + .get(&model_description_id) + .expect("Model description not found"), + ) + } else { + continue; + }; + + if model_instance.parent.0.is_none() + || model_instance.parent.0.is_some_and(|x| x != *level_id) + { + continue; + } let mut added = false; - if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) + if model_description_bundle.source.0 + == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) { world.include.push(SdfWorldInclude { uri: "model://TeleportIngestor".to_string(), - name: Some(model.name.0.clone()), - pose: Some(model.pose.to_sdf()), + name: Some(model_instance.name.0.clone()), + pose: Some(model_instance.pose.to_sdf()), ..Default::default() }); added = true; - } else if model.source + } else if model_description_bundle.source.0 == AssetSource::Search("OpenRobotics/TeleportDispenser".to_string()) { world.include.push(SdfWorldInclude { uri: "model://TeleportDispenser".to_string(), - name: Some(model.name.0.clone()), - pose: Some(model.pose.to_sdf()), + name: Some(model_instance.name.0.clone()), + pose: Some(model_instance.pose.to_sdf()), ..Default::default() }); added = true; @@ -510,17 +537,20 @@ impl Site { // Non static models are included separately and are not part of the static world // TODO(luca) this will duplicate multiple instances of the model since it uses // NameInSite instead of AssetSource for the URI, fix - else if !model.is_static.0 { + else if !model_description_bundle.is_static.0 .0 { world.model.push(SdfModel { - name: model.name.0.clone(), - r#static: Some(model.is_static.0), - pose: Some(model.pose.to_sdf()), + name: model_instance.name.0.clone(), + r#static: Some(model_description_bundle.is_static.0 .0), + pose: Some(model_instance.pose.to_sdf()), link: vec![SdfLink { name: "link".into(), collision: vec![SdfCollision { name: "collision".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/model_{}_collision.glb", model_id), + uri: format!( + "meshes/model_{}_collision.glb", + model_description_id + ), ..Default::default() }), ..Default::default() @@ -528,7 +558,10 @@ impl Site { visual: vec![SdfVisual { name: "visual".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/model_{}_visual.glb", model_id), + uri: format!( + "meshes/model_{}_visual.glb", + model_description_id + ), ..Default::default() }), ..Default::default() @@ -540,7 +573,7 @@ impl Site { added = true; } if added { - level_model_names.push(model.name.0.clone()); + level_model_names.push(model_description_bundle.name.0.clone()); } } // Now add all the doors @@ -858,7 +891,7 @@ impl Site { #[cfg(test)] mod tests { - use super::*; + use crate::legacy::building_map::BuildingMap; #[test] diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 5fdb36eb..0b4aa14b 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -138,6 +138,16 @@ pub struct Site { /// Properties that describe simulated agents in the site #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub agents: BTreeMap, + + /// Scenarios that exist in the site + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub scenarios: BTreeMap>, + /// Model descriptions available in this site + #[serde(default, skip_serializing_if = "is_default")] + pub model_descriptions: BTreeMap, + /// Model instances that exist in the site + #[serde(default, skip_serializing_if = "is_default")] + pub model_instances: BTreeMap>, } #[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] diff --git a/rmf_site_format/src/task.rs b/rmf_site_format/src/task.rs new file mode 100644 index 00000000..a0755899 --- /dev/null +++ b/rmf_site_format/src/task.rs @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2024 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::*; +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Reflect, ReflectComponent}; +use serde::{Deserialize, Serialize}; +use std::collections::HashMap; + +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct SimpleTask(pub Option); + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub enum Task { + GoToPlace { location: Point }, + WaitFor { duration: f32 }, + // PickUp { payload: Affiliation, location: Point}, + // DropOff { payload: Affiliation, location: Point}, +} + +impl Default for Task { + fn default() -> Self { + Self::WaitFor { duration: 0.0 } + } +} + +impl Task { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(match self { + Task::GoToPlace { location } => Task::GoToPlace { + location: location.convert(id_map)?, + }, + Task::WaitFor { duration } => Task::WaitFor { + duration: *duration, + }, + }) + } +} + +impl Task { + pub fn labels() -> Vec<&'static str> { + vec!["Go To Place", "Wait For"] + } + + pub fn label(&self) -> &'static str { + match self { + Task::GoToPlace { location: _ } => Self::labels()[0], + Task::WaitFor { duration: _ } => Self::labels()[1], + } + } + + pub fn location(&self) -> Option<&Point> { + match self { + Task::GoToPlace { location } => Some(location), + _ => None, + } + } +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct Tasks(pub Vec>); + +impl Default for Tasks { + fn default() -> Self { + Self(Vec::new()) + } +} + +impl Tasks { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + self.0.iter().try_fold(Tasks::default(), |mut tasks, task| { + tasks.0.push(task.convert(id_map)?); + Ok(tasks) + }) + } +}