diff --git a/Cargo.lock b/Cargo.lock index 07a79834..9dcfe244 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2765,6 +2765,12 @@ version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + [[package]] name = "hexasphere" version = "9.1.0" @@ -4033,6 +4039,7 @@ dependencies = [ "serde_json", "serde_yaml", "smallvec", + "strum", "thiserror", "thread_local", "tracing", @@ -4065,6 +4072,8 @@ dependencies = [ "serde", "serde_json", "serde_yaml", + "strum", + "strum_macros", "thiserror", "urdf-rs", "uuid", @@ -4175,6 +4184,12 @@ dependencies = [ "untrusted", ] +[[package]] +name = "rustversion" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e819f2bc632f285be6d7cd36e25940d45b2391dd6d9b939e79de557f7014248" + [[package]] name = "ruzstd" version = "0.4.0" @@ -4415,6 +4430,25 @@ version = "0.11.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5ee073c9e4cd00e28217186dbe12796d692868f432bf2e97ee73bed0c56dfa01" +[[package]] +name = "strum" +version = "0.26.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fec0f0aef304996cf250b31b5a10dee7980c85da9d759361292b8bca5a18f06" + +[[package]] +name = "strum_macros" +version = "0.26.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4c6bee85a5a24955dc440386795aa378cd9cf82acd5f764469152d2270e581be" +dependencies = [ + "heck 0.5.0", + "proc-macro2", + "quote", + "rustversion", + "syn 2.0.52", +] + [[package]] name = "subtle" version = "2.5.0" diff --git a/assets/demo_maps/office.building.yaml b/assets/demo_maps/office.building.yaml index 1a911a24..652928f3 100644 --- a/assets/demo_maps/office.building.yaml +++ b/assets/demo_maps/office.building.yaml @@ -181,7 +181,7 @@ levels: - [814.828, 239.25, 0, "", {is_parking_spot: [4, false]}] - [1210.544, 365.254, 0, ""] - [1191.442, 824.457, 0, ""] - - [1232.421, 658.567, 0, tinyRobot1_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot1], spawn_robot_type: [1, TinyRobot]}] + - [1232.421, 658.567, 0, tinyRobot1_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot1], spawn_robot_type: [1, Open-RMF/TinyRobot]}] - [1991.121, 812.872, 0, ""] - [1990, 638.364, 0, pantry, {is_holding_point: [4, true], is_parking_spot: [4, false], pickup_dispenser: [1, coke_dispenser]}] - [2213.636, 812, 0, "", {is_parking_spot: [4, false]}] @@ -206,7 +206,7 @@ levels: - [769.867, 618.148, 0, ""] - [2016.125, 1310.955, 0, ""] - [2468.096, 1217.693, 0, ""] - - [2412.581, 627.5, 0, tinyRobot2_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot2], spawn_robot_type: [1, TinyRobot]}] + - [2412.581, 627.5, 0, tinyRobot2_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot2], spawn_robot_type: [1, Open-RMF/TinyRobot]}] walls: - [6, 7, {alpha: [3, 1], texture_name: [1, default]}] - [7, 12, {alpha: [3, 1], texture_name: [1, default]}] diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index 4f66bc59..eea77c81 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -49,6 +49,7 @@ pathdiff = "*" ehttp = { version = "0.4", features = ["native-async"] } nalgebra = "0.32.5" anyhow = "*" +strum = "*" [target.'cfg(not(target_arch = "wasm32"))'.dependencies] clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] } diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 3f63db3f..48606ff2 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -23,7 +23,7 @@ use crate::{ use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow}; use bevy_mod_raycast::primitives::{rays::Ray3d, Primitive3d}; -use rmf_site_format::{FloorMarker, Model, WallMarker}; +use rmf_site_format::{FloorMarker, ModelInstance, WallMarker}; use std::collections::HashSet; /// A resource that keeps track of the unique entities that play a role in @@ -120,23 +120,21 @@ impl Cursor { } } - pub fn set_model_preview( + pub fn set_model_instance_preview( &mut self, commands: &mut Commands, model_loader: &mut ModelLoader, - model: Option, + model_instance: Option>, ) { self.remove_preview(commands); - self.preview_model = if let Some(model) = model { + self.preview_model = model_instance.and_then(|model_instance| { Some( model_loader - .spawn_model(self.frame, model.clone()) + .spawn_model_instance(self.frame, model_instance) .insert(Pending) .id(), ) - } else { - None - }; + }); } pub fn should_be_visible(&self) -> bool { diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 234a70c2..9fdb1bff 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -54,6 +54,9 @@ pub use lift::*; pub mod light; pub use light::*; +pub mod model; +pub use model::*; + pub mod model_preview; pub use model_preview::*; @@ -197,6 +200,7 @@ impl Plugin for InteractionPlugin { .add_systems( Update, ( + update_model_instance_visual_cues.after(SelectionServiceStages::Select), update_lane_visual_cues.after(SelectionServiceStages::Select), update_edge_visual_cues.after(SelectionServiceStages::Select), update_point_visual_cues.after(SelectionServiceStages::Select), diff --git a/rmf_site_editor/src/interaction/model.rs b/rmf_site_editor/src/interaction/model.rs new file mode 100644 index 00000000..70a9d81a --- /dev/null +++ b/rmf_site_editor/src/interaction/model.rs @@ -0,0 +1,107 @@ +/* + * 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, &mut Hovered), (With, Without)>, + mut removed_components: RemovedComponents>, +) { + for (instance_entity, mut instance_selected, mut instance_hovered, affiliation, tasks) in + &mut model_instances + { + 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 + .remove(&description_entity); + } + if description_hovered.cue() { + instance_hovered.support_hovering.insert(description_entity); + } else { + instance_hovered + .support_hovering + .remove(&description_entity); + } + } + } + + // 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, mut location_hovered) in locations.iter_mut() { + location_selected.support_selected.remove(&instance_entity); + location_hovered.support_hovering.remove(&instance_entity); + } + } + + if let Some(task_location) = tasks.0.first().and_then(|t| t.location()) { + if let Ok((mut location_selected, mut location_hovered)) = + 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); + } + if instance_hovered.cue() { + location_hovered.support_hovering.insert(instance_entity); + } else { + location_hovered.support_hovering.remove(&instance_entity); + } + } + } + } + } + + // When instances are removed, prevent any location from supporting them + for removed in removed_components.read() { + for (mut location_selected, mut location_hovered) in locations.iter_mut() { + location_selected.support_selected.remove(&removed); + location_hovered.support_hovering.remove(&removed); + } + } +} diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs index ce90f861..abe00641 100644 --- a/rmf_site_editor/src/interaction/select/place_object.rs +++ b/rmf_site_editor/src/interaction/select/place_object.rs @@ -17,7 +17,7 @@ use crate::{ interaction::select::*, - site::{CurrentLevel, Model}, + site::{CurrentLevel, ModelInstance}, }; use bevy::ecs::system::{Command, SystemParam, SystemState}; @@ -51,7 +51,7 @@ pub struct ObjectPlacement<'w, 's> { } impl<'w, 's> ObjectPlacement<'w, 's> { - pub fn place_object_2d(&mut self, object: Model) { + pub fn place_object_2d(&mut self, object: ModelInstance) { let Some(level) = self.current_level.0 else { warn!("Unble to create [object:?] outside a level"); return; @@ -75,17 +75,17 @@ impl<'w, 's> ObjectPlacement<'w, 's> { /// Trait to be implemented to allow placing models with commands pub trait ObjectPlacementExt<'w, 's> { - fn place_object_2d(&mut self, object: Model); + fn place_object_2d(&mut self, object: ModelInstance); } impl<'w, 's> ObjectPlacementExt<'w, 's> for Commands<'w, 's> { - fn place_object_2d(&mut self, object: Model) { + fn place_object_2d(&mut self, object: ModelInstance) { self.add(ObjectPlaceCommand(object)); } } #[derive(Deref, DerefMut)] -pub struct ObjectPlaceCommand(Model); +pub struct ObjectPlaceCommand(ModelInstance); impl Command for ObjectPlaceCommand { fn apply(self, world: &mut World) { diff --git a/rmf_site_editor/src/interaction/select/place_object_2d.rs b/rmf_site_editor/src/interaction/select/place_object_2d.rs index 869eba8c..894e4f20 100644 --- a/rmf_site_editor/src/interaction/select/place_object_2d.rs +++ b/rmf_site_editor/src/interaction/select/place_object_2d.rs @@ -17,7 +17,7 @@ use crate::{ interaction::select::*, - site::{Model, ModelLoader}, + site::{ModelInstance, ModelLoader}, }; use bevy::prelude::Input as UserInput; @@ -104,7 +104,7 @@ pub fn build_place_object_2d_workflow( } pub struct PlaceObject2d { - pub object: Model, + pub object: ModelInstance, pub level: Entity, } @@ -121,7 +121,7 @@ pub fn place_object_2d_setup( let mut access = access.get_mut(&key).or_broken_buffer()?; let state = access.newest_mut().or_broken_buffer()?; - cursor.set_model_preview(&mut commands, &mut model_loader, Some(state.object.clone())); + cursor.set_model_instance_preview(&mut commands, &mut model_loader, Some(state.object.clone())); set_visibility(cursor.dagger, &mut visibility, false); set_visibility(cursor.halo, &mut visibility, false); @@ -206,7 +206,7 @@ pub fn on_placement_chosen_2d( state.object.pose = placement.into(); model_loader - .spawn_model(state.level, state.object) + .spawn_model_instance(state.level, state.object) .insert(Category::Model); Ok(()) diff --git a/rmf_site_editor/src/occupancy.rs b/rmf_site_editor/src/occupancy.rs index 7dea7f08..f3f9c477 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: HashSet, } enum Group { @@ -180,6 +182,12 @@ 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.is_empty() { + if AncestorIter::new(&parents, *e).any(|p| request.ignore.contains(&p)) { + continue; + } + } + let (_, mesh, aabb, tf) = match bodies.get(*e) { Ok(body) => body, Err(_) => continue, 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/load.rs b/rmf_site_editor/src/site/load.rs index bd25bd9c..aa32f34e 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 @@ -226,13 +229,6 @@ fn generate_site_entities( } }); - for (model_id, model) in &level_data.models { - model_loader - .spawn_model(level_entity, model.clone()) - .insert((Category::Model, SiteID(*model_id))); - consider_id(*model_id); - } - // TODO(MXG): Log when a RecencyRanking fails to load correctly. commands .entity(level_entity) @@ -338,6 +334,99 @@ fn generate_site_entities( .for_site(site_id)?, ); + let mut model_description_dependents = HashMap::>::new(); + let mut model_description_to_source = HashMap::::new(); + for (model_description_id, model_description) in &site_data.model_descriptions { + let model_description_entity = 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_entity); + consider_id(*model_description_id); + model_description_dependents.insert(model_description_entity, HashSet::new()); + model_description_to_source + .insert(model_description_entity, model_description.source.0.clone()); + // Insert optional model properties + for optional_property in &model_description.optional_properties.0 { + match optional_property { + OptionalModelProperty::DifferentialDrive(diff_drive) => commands + .entity(model_description_entity) + .insert(ModelProperty(diff_drive.clone())), + OptionalModelProperty::MobileRobotMarker(robot_marker) => commands + .entity(model_description_entity) + .insert(ModelProperty(robot_marker.clone())), + _ => continue, + }; + } + } + + for (model_instance_id, model_instance_data) in &site_data.model_instances { + let model_instance = model_instance_data + .convert(&id_to_entity) + .for_site(site_id)?; + + let model_instance_entity = model_loader + .spawn_model_instance( + model_instance.parent.0.unwrap_or(site_id), + model_instance.clone(), + ) + .insert((Category::Model, SiteID(*model_instance_id))) + .id(); + id_to_entity.insert(*model_instance_id, model_instance_entity); + consider_id(*model_instance_id); + + if let Some(instances) = model_instance + .description + .0 + .map(|e| model_description_dependents.get_mut(&e)) + .flatten() + { + instances.insert(model_instance_entity); + } else { + error!( + "Model description missing for instance {}. This should \ + not happen, please report this bug to the maintainers of \ + rmf_site_editor.", + model_instance.name.0, + ); + } + + // Insert optional model properties + for optional_property in &model_instance.optional_properties.0 { + match optional_property { + OptionalModelProperty::Tasks(tasks) => { + commands.entity(model_instance_entity).insert(tasks.clone()) + } + _ => continue, + }; + } + } + + for (model_description_entity, dependents) in model_description_dependents { + commands + .entity(model_description_entity) + .insert(Dependents(dependents)); + } + + for (scenario_id, scenario_bundle_data) in &site_data.scenarios { + let parent = match scenario_bundle_data.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_data + .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); + } + let nav_graph_rankings = match RecencyRanking::::from_u32( &site_data.navigation.guided.ranking, &id_to_entity, @@ -400,7 +489,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/location.rs b/rmf_site_editor/src/site/location.rs index 932a78ee..dd5fc5f3 100644 --- a/rmf_site_editor/src/site/location.rs +++ b/rmf_site_editor/src/site/location.rs @@ -34,8 +34,7 @@ fn location_halo_tf(tag: &LocationTag) -> Transform { LocationTag::Charger => 0, LocationTag::ParkingSpot => 1, LocationTag::HoldingPoint => 2, - LocationTag::SpawnRobot(_) => 3, - LocationTag::Workcell(_) => 4, + LocationTag::Workcell(_) => 3, }; Transform { translation: Vec3::new(0., 0., 0.01), @@ -121,8 +120,8 @@ pub fn add_location_visuals( tag_meshes.holding_point = Some(id); assets.holding_point_material.clone() } - // Workcells and robots are not visualized - LocationTag::SpawnRobot(_) | LocationTag::Workcell(_) => continue, + // Workcells are not visualized + LocationTag::Workcell(_) => continue, }; commands.entity(id).insert(PbrBundle { mesh: assets.location_tag_mesh.clone(), @@ -271,8 +270,8 @@ pub fn update_location_for_changed_location_tags( continue; } } - // Workcells and robots are not visualized - LocationTag::SpawnRobot(_) | LocationTag::Workcell(_) => continue, + // Workcells are not visualized + LocationTag::Workcell(_) => continue, }; commands.entity(id).insert(PbrBundle { mesh: assets.location_tag_mesh.clone(), @@ -389,9 +388,7 @@ pub fn handle_consider_location_tag( if let Ok(mut recall) = recalls.get_mut(consider.for_element) { recall.consider_tag = consider.tag.clone(); let r = recall.as_mut(); - if let Some(LocationTag::SpawnRobot(model)) | Some(LocationTag::Workcell(model)) = - &r.consider_tag - { + if let Some(LocationTag::Workcell(model)) = &r.consider_tag { r.consider_tag_asset_source_recall.remember(&model.source); } } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 220fc6eb..2bbc59d2 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -99,6 +99,9 @@ pub use recall_plugin::RecallPlugin; pub mod save; pub use save::*; +pub mod scenario; +pub use scenario::*; + pub mod sdf_exporter; pub use sdf_exporter::*; @@ -163,6 +166,7 @@ impl Plugin for SitePlugin { ) .chain(), ) + .add_systems(Startup, setup_instance_deletion_filter) .add_systems( PreUpdate, apply_deferred.in_set(SiteUpdateSet::ProcessChangesFlush), @@ -189,10 +193,14 @@ impl Plugin for SitePlugin { ) .insert_resource(ClearColor(Color::rgb(0., 0., 0.))) .init_resource::() + .init_resource::() + .init_resource::() .init_resource::() .add_event::() .add_event::() .add_event::() + .add_event::() + .add_event::() .add_event::() .add_event::() .add_event::() @@ -247,6 +255,11 @@ impl Plugin for SitePlugin { ModelLoadingPlugin::default(), FuelPlugin::default(), )) + .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( @@ -292,6 +305,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::, @@ -338,6 +352,9 @@ 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), update_changed_lane, update_lane_for_moved_anchor, ) @@ -373,6 +390,9 @@ impl Plugin for SitePlugin { add_measurement_visuals, update_changed_measurement, update_measurement_for_moved_anchors, + 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 a18d2193..af8999d4 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -17,7 +17,7 @@ use crate::{ interaction::{DragPlaneBundle, Preview, MODEL_PREVIEW_LAYER}, - site::SiteAssets, + site::{CurrentLevel, SiteAssets, SiteParent}, site_asset_io::MODEL_ENVIRONMENT_VARIABLE, }; use bevy::{ @@ -29,9 +29,11 @@ use bevy::{ }; use bevy_impulse::*; use bevy_mod_outline::OutlineMeshExt; -use rmf_site_format::{AssetSource, Model, ModelMarker, Pending, Scale}; +use rmf_site_format::{ + Affiliation, AssetSource, Group, ModelInstance, ModelMarker, ModelProperty, Pending, Scale, +}; use smallvec::SmallVec; -use std::{any::TypeId, fmt, future::Future}; +use std::{any::TypeId, collections::HashSet, fmt, future::Future}; use thiserror::Error; /// Denotes the properties of the current spawned scene for the model, to despawn when updating AssetSource @@ -66,6 +68,8 @@ pub fn get_all_for_source(source: &AssetSource) -> SmallVec<[AssetSource; 6]> { pub type ModelLoadingResult = Result; +pub type InstanceSpawningResult = Result; + #[derive(Resource)] /// Services that deal with model loading // TODO(luca) revisit pub / private-ness of struct and fields @@ -75,6 +79,7 @@ struct ModelLoadingServices { check_scene_is_spawned: Service, /// System that tries to load a model and returns a result. pub load_model: Service, + pub spawn_instance: Service, } #[derive(Default)] @@ -316,34 +321,69 @@ fn handle_model_loading_errors( result } +fn instance_spawn_request_into_model_load_request( + In(request): In, + descriptions: Query<&ModelProperty>, +) -> Result { + let Some(affiliation) = request.affiliation.0 else { + return Err(InstanceSpawningError::NoAffiliation); + }; + + let Ok(source) = descriptions.get(affiliation) else { + return Err(InstanceSpawningError::AffiliationMissing); + }; + + Ok(ModelLoadingRequest { + parent: request.parent, + source: source.0.clone(), + }) +} + /// `SystemParam` used to request for model loading operations #[derive(SystemParam)] pub struct ModelLoader<'w, 's> { services: Res<'w, ModelLoadingServices>, commands: Commands<'w, 's>, + model_instances: Query< + 'w, + 's, + (Entity, &'static Affiliation), + (With, Without, With), + >, } impl<'w, 's> ModelLoader<'w, 's> { - /// Spawn a new model and begin a workflow to load its asset source. + /// Spawn a new model instance and begin a workflow to load its asset source + /// from the affiliated model description. /// This is only for brand new models does not support reacting to the load finishing. - pub fn spawn_model(&mut self, parent: Entity, model: Model) -> EntityCommands<'w, 's, '_> { - self.spawn_model_impulse(parent, model, move |impulse| { + pub fn spawn_model_instance( + &mut self, + parent: Entity, + instance: ModelInstance, + ) -> EntityCommands<'w, 's, '_> { + self.spawn_model_instance_impulse(parent, instance, move |impulse| { impulse.detach(); }) } - /// Spawn a new model and begin a workflow to load its asset source. + /// Spawn a new model instance and begin a workflow to load its asset source. /// Additionally build on the impulse chain of the asset source loading workflow. - pub fn spawn_model_impulse( + pub fn spawn_model_instance_impulse( &mut self, parent: Entity, - model: Model, - impulse: impl FnOnce(Impulse), + instance: ModelInstance, + impulse: impl FnOnce(Impulse), ) -> EntityCommands<'w, 's, '_> { - let source = model.source.clone(); - let id = self.commands.spawn(model).set_parent(parent).id(); - let loading_impulse = self.update_asset_source_impulse(id, source); - (impulse)(loading_impulse); + let affiliation = instance.description.clone(); + let id = self.commands.spawn(instance).set_parent(parent).id(); + let spawning_impulse = self.commands.request( + InstanceSpawningRequest::new(id, affiliation), + self.services + .spawn_instance + .clone() + .instruct(SpawnModelLabel(id).preempt()), + ); + (impulse)(spawning_impulse); self.commands.entity(id) } @@ -368,6 +408,23 @@ impl<'w, 's> ModelLoader<'w, 's> { .instruct(SpawnModelLabel(entity).preempt()), ) } + + /// Update the asset source of all model instances affiliated with the provided + /// model description + pub fn update_description_asset_source(&mut self, entity: Entity, source: AssetSource) { + let mut instance_entities = HashSet::new(); + for (e, affiliation) in self.model_instances.iter() { + if let Some(description_entity) = affiliation.0 { + if entity == description_entity { + instance_entities.insert(e); + } + } + } + for e in instance_entities.iter() { + self.update_asset_source_impulse(*e, source.clone()) + .detach(); + } + } } fn load_model_dependencies( @@ -472,9 +529,22 @@ impl ModelLoadingServices { .connect(scope.terminate) }); + // Model instance spawning workflow + let spawn_instance = app.world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .then(instance_spawn_request_into_model_load_request.into_blocking_callback()) + .connect_on_err(scope.terminate) + .then(load_model) + .map_block(|res| res.map_err(InstanceSpawningError::ModelError)) + .connect(scope.terminate) + }); + Self { load_model, check_scene_is_spawned, + spawn_instance, } } } @@ -546,6 +616,28 @@ pub enum ModelLoadingErrorKind { FailedLoadingDependency(String), } +#[derive(Clone, Debug)] +pub struct InstanceSpawningRequest { + pub parent: Entity, + pub affiliation: Affiliation, +} + +impl InstanceSpawningRequest { + pub fn new(parent: Entity, affiliation: Affiliation) -> Self { + Self { + parent, + affiliation, + } + } +} + +#[derive(Clone, Debug)] +pub enum InstanceSpawningError { + NoAffiliation, + AffiliationMissing, + ModelError(ModelLoadingError), +} + pub fn update_model_scales( changed_scales: Query<(&Scale, &ModelScene), Or<(Changed, Changed)>>, mut transforms: Query<&mut Transform>, @@ -659,3 +751,57 @@ pub fn propagate_model_property( } } } + +/// 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>, (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.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 3a6c42fb..a05e863d 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -115,6 +115,9 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration Without, ), >, + Query, With)>, + Query, Without, Without)>, + Query>, Query< Entity, ( @@ -146,6 +149,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, @@ -185,9 +191,9 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration if drawings.contains(*child) { if let Ok(drawing_children) = children.get(*child) { - for child in drawing_children { - if !site_ids.contains(*child) { - new_entities.push(*child); + for drawing_child in drawing_children { + if !site_ids.contains(*drawing_child) { + new_entities.push(*drawing_child); } } } @@ -197,6 +203,34 @@ 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); + } + } + + if let Ok(model_instance) = model_instances.get(*site_child) { + if !site_ids.contains(model_instance) { + new_entities.push(model_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 +370,6 @@ fn generate_levels( ), (With, Without), >, - Query< - (&NameInSite, &AssetSource, &Pose, &IsStatic, &Scale, &SiteID), - (With, Without, Without), - >, Query<(&NameInSite, &Pose, &PhysicalCameraProperties, &SiteID), Without>, Query< ( @@ -377,7 +407,6 @@ fn generate_levels( q_floors, q_lights, q_measurements, - q_models, q_physical_cameras, q_walls, q_levels, @@ -553,19 +582,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 +1197,218 @@ 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>, + // Optional model properties + Query<&ModelProperty>, + Query<&ModelProperty>, + )> = SystemState::new(world); + let (model_descriptions, children, differential_drive, robot_marker) = 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) { + let mut desc_bundle = ModelDescriptionBundle { + name: name.clone(), + source: source.clone(), + is_static: is_static.clone(), + scale: scale.clone(), + ..Default::default() + }; + if let Ok(diff_drive) = differential_drive.get(*child) { + desc_bundle.optional_properties.0.push( + OptionalModelProperty::DifferentialDrive(diff_drive.0.clone()), + ); + }; + if let Ok(mobile_robot) = robot_marker.get(*child) { + desc_bundle.optional_properties.0.push( + OptionalModelProperty::MobileRobotMarker(mobile_robot.0.clone()), + ); + }; + res.insert(site_id.0, desc_bundle); + } + } + } + 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<(&Point, &SiteID), (With, Without)>, + Query<&Children>, + Query<&Parent>, + Query<&Tasks, (With, Without)>, + )> = SystemState::new(world); + let (model_descriptions, model_instances, levels, locations, _, parents, tasks) = + 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() + { + let Ok(parent) = instance_parent + .0 + .map(|p| site_levels_ids.get(&p).copied().ok_or(())) + .transpose() + else { + error!("Unable to find parent for instance [{}]", instance_name.0); + continue; + }; + let mut model_instance = ModelInstance:: { + name: instance_name.clone(), + pose: instance_pose.clone(), + parent: SiteParent(parent), + description: Affiliation( + instance_affiliation + .0 + .map(|e| model_descriptions.get(e).ok().map(|d| d.0)) + .flatten(), + ), + ..Default::default() + }; + if let Ok(robot_tasks) = tasks.get(_instance_entity) { + let tasks: Vec> = robot_tasks + .0 + .clone() + .iter() + .map(|task| match task { + Task::GoToPlace(go_to_place) => locations + .get(go_to_place.location.unwrap().0) + .map(|(_, location_id)| { + Task::GoToPlace(GoToPlace { + location: Some(Point(location_id.0)), + }) + }) + .unwrap(), + Task::WaitFor(wait_for) => Task::WaitFor(WaitFor { + duration: wait_for.duration.clone(), + }), + }) + .collect::>>(); + model_instance + .optional_properties + .0 + .push(OptionalModelProperty::Tasks(Tasks(tasks.clone()))); + } + res.insert(instance_id.0, model_instance); + } + 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>, + )> = SystemState::new(world); + let (scenarios, instances, children) = 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, + }, + ); + } + } + } + } + } + info!("Added scenarios: {:?}", res.len()); + Ok(res) +} + pub fn generate_site( world: &mut World, site: Entity, @@ -1199,6 +1427,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 +1451,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 +1662,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..f68240ca --- /dev/null +++ b/rmf_site_editor/src/site/scenario.rs @@ -0,0 +1,303 @@ +/* + * 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::{ + Affiliation, CurrentScenario, Delete, DeletionBox, DeletionFilters, Dependents, + InstanceMarker, Pending, Pose, Scenario, ScenarioBundle, ScenarioMarker, SiteParent, + }, + CurrentWorkspace, +}; +use bevy::prelude::*; +use std::collections::{HashMap, HashSet}; + +#[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, + >, +) { + if let Some(ChangeCurrentScenario(scenario_entity)) = change_current_scenario.read().last() { + // 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 dependents))) = scenarios.get_mut(request.0) { + dependents.extend(subtree_dependents.iter()); + } else { + commands + .entity(request.0) + .insert(Dependents(subtree_dependents)); + } + delete.send(Delete::new(request.0).and_dependents()); + } +} + +pub fn setup_instance_deletion_filter(mut deletion_filter: ResMut) { + deletion_filter.insert(DeletionBox(Box::new(IntoSystem::into_system( + filter_instance_deletion, + )))); +} + +/// 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. +fn filter_instance_deletion( + In(mut input): In>, + mut scenarios: Query<&mut Scenario>, + current_scenario: ResMut, + mut change_current_scenario: EventWriter, + model_instance: Query<&Affiliation, With>, + selection: Res, + mut select: EventWriter, +) { + if ui + .selectable_label( + selection.0.is_some_and(|s| s == *entity), + format!( + "{} #{}", + category.label(), + site_id + .map(|s| s.0.to_string()) + .unwrap_or("unsaved".to_string()), + ), + ) + .clicked() + { + select.send(Select::new(Some(*entity))); + }; + ui.label(format!("[{}]", name.0)); +} + +/// Creates a formatted label for a pose +fn formatted_pose(ui: &mut Ui, pose: &Pose) { + ui.colored_label( + Color32::GRAY, + format!( + "[x: {:.3}, y: {:.3}, z: {:.3}, yaw: {:.3}]", + pose.trans[0], + pose.trans[1], + pose.trans[2], + match pose.rot.yaw() { + Angle::Rad(r) => r, + Angle::Deg(d) => d.to_radians(), + } + ), + ); +} + +#[derive(Resource)] +pub struct ScenarioDisplay { + pub new_scenario_name: String, + pub is_new_scenario_root: bool, +} + +impl Default for ScenarioDisplay { + fn default() -> Self { + Self { + new_scenario_name: "".to_string(), + is_new_scenario_root: true, + } + } +} diff --git a/rmf_site_editor/src/widgets/view_tasks.rs b/rmf_site_editor/src/widgets/view_tasks.rs new file mode 100644 index 00000000..bbce121e --- /dev/null +++ b/rmf_site_editor/src/widgets/view_tasks.rs @@ -0,0 +1,176 @@ +/* + * 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::*, + Tile, WidgetSystem, +}; +use bevy::{ + ecs::system::{SystemParam, SystemState}, + prelude::*, +}; +use bevy_egui::egui::{CollapsingHeader, Color32, Frame, Stroke, Ui}; + +#[derive(SystemParam)] +pub struct ViewTasks<'w, 's> { + mobile_robots: Query< + 'w, + 's, + ( + Entity, + &'static NameInSite, + Option<&'static SiteID>, + &'static mut Tasks, + ), + (With, Without), + >, + site_entities: Query< + 'w, + 's, + ( + Entity, + &'static NameInSite, + &'static Category, + Option<&'static SiteID>, + ), + Without, + >, + selection: Res<'w, Selection>, + select: EventWriter<'w, Select>, +} + +impl<'w, 's> WidgetSystem for ViewTasks<'w, 's> { + fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) { + let mut params = state.get_mut(world); + CollapsingHeader::new("Tasks") + .default_open(false) + .show(ui, |ui| { + params.show_widget(ui); + }); + } +} + +impl<'w, 's> ViewTasks<'w, 's> { + pub fn show_widget(&mut self, ui: &mut Ui) { + 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()); + + let mut total_task_count: u32 = 0; + for (robot_entity, robot_name, robot_site_id, robot_tasks) in + self.mobile_robots.iter() + { + for task in robot_tasks.0.iter() { + show_task( + ui, + task, + &robot_name.0, + &robot_entity, + robot_site_id, + &self.site_entities, + &self.selection, + &mut self.select, + &mut total_task_count, + ); + } + } + if total_task_count == 0 { + ui.label("No tasks"); + } + }); + } +} + +fn show_task( + ui: &mut Ui, + task: &Task, + robot_name: &str, + robot_entity: &Entity, + robot_site_id: Option<&SiteID>, + site_entities: &Query<(Entity, &NameInSite, &Category, Option<&SiteID>), Without>, + selected: &Selection, + select: &mut EventWriter