From ce017cb6d1ae815e2966505b7b8d37a74e2a2fbe Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Thu, 27 Jun 2024 09:06:55 +0800 Subject: [PATCH 01/32] Scenarios V2 Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/save.rs | 1 + rmf_site_format/src/asset_source.rs | 2 +- rmf_site_format/src/legacy/building_map.rs | 33 +++++++++++-- rmf_site_format/src/legacy/model.rs | 46 +++++++++++++++++- rmf_site_format/src/lib.rs | 3 ++ rmf_site_format/src/misc.rs | 2 +- rmf_site_format/src/model/decor.rs | 46 ++++++++++++++++++ rmf_site_format/src/model/mobile_robot.rs | 48 +++++++++++++++++++ .../src/{model.rs => model/mod.rs} | 31 ++++++++++++ rmf_site_format/src/scenario.rs | 42 ++++++++++++++++ rmf_site_format/src/site.rs | 6 +++ 11 files changed, 252 insertions(+), 8 deletions(-) create mode 100644 rmf_site_format/src/model/decor.rs create mode 100644 rmf_site_format/src/model/mobile_robot.rs rename rmf_site_format/src/{model.rs => model/mod.rs} (67%) create mode 100644 rmf_site_format/src/scenario.rs diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 458d8319..8a5351ee 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -1220,6 +1220,7 @@ pub fn generate_site( }, // TODO(MXG): Parse agent information once the spec is figured out agents: Default::default(), + ..Default::default() }); } 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/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 375477e7..eaeaeb91 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -5,10 +5,11 @@ 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, ModelDescriptions, + Motion, NameInSite, NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, + Pose, PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, Scenario, + ScenarioProperties, 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 +201,13 @@ impl BuildingMap { let mut fiducial_groups: BTreeMap = BTreeMap::new(); let mut cartesian_fiducials: HashMap> = HashMap::new(); + + let mut model_descriptions = ModelDescriptions::default(); + let mut model_description_name_map = HashMap::::new(); + let mut scenarios = BTreeMap::new(); + let mut default_scenario_id = site_id.next().unwrap(); + scenarios.insert(default_scenario_id, Scenario::default()); + for (level_name, level) in &self.levels { let level_id = site_id.next().unwrap(); let mut vertex_to_anchor_id: HashMap = Default::default(); @@ -506,6 +514,21 @@ impl BuildingMap { models.insert(site_id.next().unwrap(), model.to_site()); } + for model in &level.models { + let model_instance = model.to_decor_instance( + &level_id, + &mut site_id, + &mut model_description_name_map, + &mut model_descriptions, + ); + + scenarios + .get_mut(&default_scenario_id) + .unwrap() + .model_instances + .insert(site_id.next().unwrap(), model_instance); + } + let mut physical_cameras = BTreeMap::new(); for cam in &level.physical_cameras { physical_cameras.insert(site_id.next().unwrap(), cam.to_site()); @@ -712,6 +735,8 @@ impl BuildingMap { }, }, agents: Default::default(), + scenarios, + model_descriptions, }) } } diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index b356174c..d9b4fa1b 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,9 +1,10 @@ use crate::{ - Angle, AssetSource, IsStatic, Model as SiteModel, ModelMarker, NameInSite, Pose, Rotation, - Scale, + Angle, AssetSource, DecorDescription, DecorMarker, Group, IsStatic, MobileRobotDescription, Model as SiteModel, ModelDescriptions, ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale }; +use bevy::utils::default; use glam::DVec2; use serde::{Deserialize, Serialize}; +use std::{collections::HashMap, ops::RangeFrom}; #[derive(Deserialize, Serialize, Clone, Default, Debug)] pub struct Model { @@ -37,4 +38,45 @@ impl Model { marker: ModelMarker, } } + + pub fn to_decor_instance( + &self, + level_id: &u32, + site_id: &mut RangeFrom, + model_description_name_map: &mut HashMap, + model_descriptions: &mut ModelDescriptions, + ) -> ModelInstance { + 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_description_id = model_description_name_map + .entry(self.model_name.clone()) + .or_insert_with(|| { + let id = site_id.next().unwrap(); + model_descriptions.decors.insert( + id, + DecorDescription { + name: NameInSite(self.model_name.clone()), + source: AssetSource::Search(self.model_name.clone()), + marker: DecorMarker, + group: Group, + }, + ); + id + }); + + ModelInstance { + parent: level_id.clone(), + model_description: model_description_id.clone(), + bundle: crate::ModelInstanceBundle { + name: NameInSite(self.instance_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)), + }, + }, + } + } } diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index cf8f0e49..6eee49d7 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -101,6 +101,9 @@ pub use point::*; pub mod recall; pub use recall::*; +pub mod scenario; +pub use scenario::*; + pub mod sdf; pub use sdf::*; diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 4754f510..fe82f2af 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -422,7 +422,7 @@ 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; diff --git a/rmf_site_format/src/model/decor.rs b/rmf_site_format/src/model/decor.rs new file mode 100644 index 00000000..2e0a2fd0 --- /dev/null +++ b/rmf_site_format/src/model/decor.rs @@ -0,0 +1,46 @@ +/* + * 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}; + +#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub struct DecorMarker; + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct DecorInstance { + pub pose: Pose, + pub affiliation: Affiliation, + #[serde(skip)] + pub marker: DecorMarker, +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct DecorDescription { + pub name: NameInSite, + pub source: AssetSource, + #[serde(skip)] + pub group: Group, + #[serde(skip)] + pub marker: DecorMarker, +} diff --git a/rmf_site_format/src/model/mobile_robot.rs b/rmf_site_format/src/model/mobile_robot.rs new file mode 100644 index 00000000..9cc250f1 --- /dev/null +++ b/rmf_site_format/src/model/mobile_robot.rs @@ -0,0 +1,48 @@ +/* + * 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}; + +#[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(Bundle))] +pub struct MobileRobotInstance { + pub pose: Pose, + pub affiliation: Affiliation, + #[serde(skip)] + pub marker: MobileRobotMarker, +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct MobileRobotDescription { + pub name: NameInSite, + pub source: AssetSource, + #[serde(default, skip_serializing_if = "is_default")] + #[serde(skip)] + pub group: Group, + #[serde(skip)] + pub marker: MobileRobotMarker, +} + diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model/mod.rs similarity index 67% rename from rmf_site_format/src/model.rs rename to rmf_site_format/src/model/mod.rs index 8cda5bd8..05f811ac 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model/mod.rs @@ -16,6 +16,12 @@ */ use crate::*; +pub mod mobile_robot; +pub use mobile_robot::*; +pub mod decor; +pub use decor::*; + +use std::collections::BTreeMap; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; @@ -57,3 +63,28 @@ impl Default for Model { } } } + +#[derive(Serialize, Deserialize, Debug, Clone, Default, PartialEq)] +pub struct ModelDescriptions { + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub decors: BTreeMap, + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub mobile_robots: BTreeMap, + // #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + // pub workcells: BTreeMap, +} + +#[derive(Serialize, Deserialize, Debug, Clone)] +pub struct ModelInstance { + pub parent: u32, + pub model_description: u32, + #[serde(flatten)] + pub bundle: ModelInstanceBundle, +} + +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct ModelInstanceBundle { + pub name: NameInSite, + pub pose: Pose, +} \ No newline at end of file diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs new file mode 100644 index 00000000..18118b8c --- /dev/null +++ b/rmf_site_format/src/scenario.rs @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::*; +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Entity}; +use serde::{Deserialize, Serialize}; +use std::collections::BTreeMap; + +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct ScenarioProperties { + pub name: String, +} + +impl Default for ScenarioProperties { + fn default() -> Self { + Self { + name: "new_scenario".to_owned(), + } + } +} + +#[derive(Serialize, Deserialize, Debug, Clone, Default)] +pub struct Scenario { + pub properties: ScenarioProperties, + pub model_instances: BTreeMap, +} \ No newline at end of file diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 5fdb36eb..4ead1ca8 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -138,6 +138,12 @@ 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, + #[serde(default, skip_serializing_if = "is_default")] + pub model_descriptions: ModelDescriptions, } #[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] From f00141d63bab806607c48a611b3492a2ff88e189 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Mon, 1 Jul 2024 14:51:40 +0800 Subject: [PATCH 02/32] Use groups Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/load.rs | 27 ++++++++- rmf_site_format/src/legacy/building_map.rs | 17 +++--- rmf_site_format/src/legacy/model.rs | 58 +++++++++---------- rmf_site_format/src/misc.rs | 7 ++- .../src/{model/mod.rs => model.rs} | 43 +++++++------- rmf_site_format/src/model/decor.rs | 46 --------------- rmf_site_format/src/model/mobile_robot.rs | 48 --------------- rmf_site_format/src/scenario.rs | 2 +- rmf_site_format/src/site.rs | 2 +- 9 files changed, 91 insertions(+), 159 deletions(-) rename rmf_site_format/src/{model/mod.rs => model.rs} (72%) delete mode 100644 rmf_site_format/src/model/decor.rs delete mode 100644 rmf_site_format/src/model/mobile_robot.rs diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 7bb2f73d..0968ab47 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -119,6 +119,18 @@ fn generate_site_entities( consider_id(*group_id); } + for (model_description_id, model_description) in &site_data.model_descriptions { + let model_description = commands + .spawn(model_description.clone()) + .insert(SiteID(*model_description_id)) + .set_parent(site_id) + .id(); + id_to_entity.insert(*model_description_id, model_description); + consider_id(*model_description_id); + } + + let (_, default_scenario) = site_data.scenarios.first_key_value().unwrap(); + for (level_id, level_data) in &site_data.levels { let level_entity = commands.spawn(SiteID(*level_id)).set_parent(site_id).id(); @@ -210,9 +222,18 @@ 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 (model_id, model) in &level_data.models { + // level.spawn(model.clone()).insert(SiteID(*model_id)); + // consider_id(*model_id); + // } + + for (model_instance_id, model_instance) in &default_scenario.model_instances { + if model_instance.parent.0 == *level_id { + level + .spawn(model_instance.clone()) + .insert(SiteID(*model_instance_id)); + consider_id(*model_instance_id); + } } for (physical_camera_id, physical_camera) in &level_data.physical_cameras { diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index eaeaeb91..e841d8a4 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -1,3 +1,4 @@ +use super::model::Model; use super::{ floor::FloorParameters, level::Level, lift::Lift, wall::WallProperties, PortingError, Result, }; @@ -5,7 +6,7 @@ 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, ModelDescriptions, + Level as SiteLevel, LevelElevation, LevelProperties as SiteLevelProperties, ModelDescription, Motion, NameInSite, NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose, PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, Scenario, ScenarioProperties, Site, SiteProperties, Texture as SiteTexture, TextureGroup, UserCameraPose, @@ -202,7 +203,7 @@ impl BuildingMap { let mut fiducial_groups: BTreeMap = BTreeMap::new(); let mut cartesian_fiducials: HashMap> = HashMap::new(); - let mut model_descriptions = ModelDescriptions::default(); + let mut model_descriptions: BTreeMap = BTreeMap::new(); let mut model_description_name_map = HashMap::::new(); let mut scenarios = BTreeMap::new(); let mut default_scenario_id = site_id.next().unwrap(); @@ -510,16 +511,16 @@ impl BuildingMap { } let mut models = BTreeMap::new(); - for model in &level.models { - models.insert(site_id.next().unwrap(), model.to_site()); - } + // for model in &level.models { + // models.insert(site_id.next().unwrap(), model.to_site()); + // } for model in &level.models { - let model_instance = model.to_decor_instance( - &level_id, - &mut site_id, + let model_instance = model.to_model_instance( &mut model_description_name_map, &mut model_descriptions, + &mut site_id, + level_id, ); scenarios diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index d9b4fa1b..367b615f 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,10 +1,13 @@ use crate::{ - Angle, AssetSource, DecorDescription, DecorMarker, Group, IsStatic, MobileRobotDescription, Model as SiteModel, ModelDescriptions, ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale + Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, + ModelInstance, ModelMarker, NameInSite, SiteParentID, Pose, Rotation, Scale, }; -use bevy::utils::default; use glam::DVec2; use serde::{Deserialize, Serialize}; -use std::{collections::HashMap, ops::RangeFrom}; +use std::{ + collections::{BTreeMap, HashMap}, + ops::RangeFrom, +}; #[derive(Deserialize, Serialize, Clone, Default, Debug)] pub struct Model { @@ -39,44 +42,41 @@ impl Model { } } - pub fn to_decor_instance( + pub fn to_model_instance( &self, - level_id: &u32, - site_id: &mut RangeFrom, model_description_name_map: &mut HashMap, - model_descriptions: &mut ModelDescriptions, - ) -> ModelInstance { - 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_description_id = model_description_name_map - .entry(self.model_name.clone()) - .or_insert_with(|| { + model_descriptions: &mut BTreeMap, + site_id: &mut RangeFrom, + level_id: u32, + ) -> ModelInstance { + let model_description_id = match model_description_name_map.get(&self.model_name) { + Some(id) => *id, + None => { let id = site_id.next().unwrap(); - model_descriptions.decors.insert( + model_description_name_map.insert(self.model_name.clone(), id); + model_descriptions.insert( id, - DecorDescription { + ModelDescription { name: NameInSite(self.model_name.clone()), source: AssetSource::Search(self.model_name.clone()), - marker: DecorMarker, - group: Group, + group: Group::default(), + marker: ModelMarker, }, ); id - }); + } + }; ModelInstance { - parent: level_id.clone(), - model_description: model_description_id.clone(), - bundle: crate::ModelInstanceBundle { - name: NameInSite(self.instance_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)), - }, + 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)), }, + parent: SiteParentID(level_id), + description: Affiliation(Some(model_description_id)), + marker: ModelMarker, } } } diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index fe82f2af..1aabc248 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -402,10 +402,15 @@ 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 to indicate that it is defined in relation +#[derive(Serialize, Deserialize, Debug, Clone, Copy)] +#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] +pub struct SiteParentID(pub u32); + /// 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 diff --git a/rmf_site_format/src/model/mod.rs b/rmf_site_format/src/model.rs similarity index 72% rename from rmf_site_format/src/model/mod.rs rename to rmf_site_format/src/model.rs index 05f811ac..dc627268 100644 --- a/rmf_site_format/src/model/mod.rs +++ b/rmf_site_format/src/model.rs @@ -16,12 +16,7 @@ */ use crate::*; -pub mod mobile_robot; -pub use mobile_robot::*; -pub mod decor; -pub use decor::*; -use std::collections::BTreeMap; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; @@ -64,27 +59,31 @@ impl Default for Model { } } -#[derive(Serialize, Deserialize, Debug, Clone, Default, PartialEq)] -pub struct ModelDescriptions { - #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - pub decors: BTreeMap, - #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - pub mobile_robots: BTreeMap, - // #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - // pub workcells: BTreeMap, -} +/// +/// +/// -#[derive(Serialize, Deserialize, Debug, Clone)] -pub struct ModelInstance { - pub parent: u32, - pub model_description: u32, - #[serde(flatten)] - pub bundle: ModelInstanceBundle, +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct ModelDescription { + pub name: NameInSite, + pub source: AssetSource, + #[serde(default, skip_serializing_if = "is_default")] + #[serde(skip)] + pub group: Group, + #[serde(skip)] + pub marker: ModelMarker, } #[derive(Serialize, Deserialize, Debug, Clone)] #[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct ModelInstanceBundle { +pub struct ModelInstance { pub name: NameInSite, + #[serde(skip)] + pub source: AssetSource, pub pose: Pose, -} \ No newline at end of file + pub parent: SiteParentID, + pub description: Affiliation, + #[serde(skip)] + pub marker: ModelMarker, +} diff --git a/rmf_site_format/src/model/decor.rs b/rmf_site_format/src/model/decor.rs deleted file mode 100644 index 2e0a2fd0..00000000 --- a/rmf_site_format/src/model/decor.rs +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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}; - -#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] -#[cfg_attr(feature = "bevy", derive(Component, Reflect))] -#[cfg_attr(feature = "bevy", reflect(Component))] -pub struct DecorMarker; - -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct DecorInstance { - pub pose: Pose, - pub affiliation: Affiliation, - #[serde(skip)] - pub marker: DecorMarker, -} - -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct DecorDescription { - pub name: NameInSite, - pub source: AssetSource, - #[serde(skip)] - pub group: Group, - #[serde(skip)] - pub marker: DecorMarker, -} diff --git a/rmf_site_format/src/model/mobile_robot.rs b/rmf_site_format/src/model/mobile_robot.rs deleted file mode 100644 index 9cc250f1..00000000 --- a/rmf_site_format/src/model/mobile_robot.rs +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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}; - -#[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(Bundle))] -pub struct MobileRobotInstance { - pub pose: Pose, - pub affiliation: Affiliation, - #[serde(skip)] - pub marker: MobileRobotMarker, -} - -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct MobileRobotDescription { - pub name: NameInSite, - pub source: AssetSource, - #[serde(default, skip_serializing_if = "is_default")] - #[serde(skip)] - pub group: Group, - #[serde(skip)] - pub marker: MobileRobotMarker, -} - diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index 18118b8c..783c024d 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -38,5 +38,5 @@ impl Default for ScenarioProperties { #[derive(Serialize, Deserialize, Debug, Clone, Default)] pub struct Scenario { pub properties: ScenarioProperties, - pub model_instances: BTreeMap, + pub model_instances: BTreeMap>, } \ No newline at end of file diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 4ead1ca8..b40ae025 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -143,7 +143,7 @@ pub struct Site { #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub scenarios: BTreeMap, #[serde(default, skip_serializing_if = "is_default")] - pub model_descriptions: ModelDescriptions, + pub model_descriptions: BTreeMap, } #[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] From 6167ec14b2b74f6b44a6b551247fba4e55e18aa8 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Mon, 1 Jul 2024 16:00:59 +0800 Subject: [PATCH 03/32] Add scale and static to instances Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/load.rs | 5 ----- rmf_site_format/src/legacy/building_map.rs | 4 ---- rmf_site_format/src/legacy/model.rs | 4 +++- rmf_site_format/src/model.rs | 8 ++++++-- rmf_site_format/src/scenario.rs | 2 +- 5 files changed, 10 insertions(+), 13 deletions(-) diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 0968ab47..71bd36e7 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -222,11 +222,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 (model_instance_id, model_instance) in &default_scenario.model_instances { if model_instance.parent.0 == *level_id { level diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index e841d8a4..ea55ac44 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -511,10 +511,6 @@ impl BuildingMap { } let mut models = BTreeMap::new(); - // for model in &level.models { - // models.insert(site_id.next().unwrap(), model.to_site()); - // } - for model in &level.models { let model_instance = model.to_model_instance( &mut model_description_name_map, diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index 367b615f..d32f9080 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,6 +1,6 @@ use crate::{ Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, - ModelInstance, ModelMarker, NameInSite, SiteParentID, Pose, Rotation, Scale, + ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale, SiteParentID, }; use glam::DVec2; use serde::{Deserialize, Serialize}; @@ -74,6 +74,8 @@ impl Model { 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(), parent: SiteParentID(level_id), description: Affiliation(Some(model_description_id)), marker: ModelMarker, diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index dc627268..45966055 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -60,8 +60,8 @@ impl Default for Model { } /// -/// -/// +/// +/// #[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Bundle))] @@ -84,6 +84,10 @@ pub struct ModelInstance { pub pose: Pose, pub parent: SiteParentID, pub description: Affiliation, + #[serde(default, skip_serializing_if = "is_default")] + pub is_static: IsStatic, + #[serde(default, skip_serializing_if = "is_default")] + pub scale: Scale, #[serde(skip)] pub marker: ModelMarker, } diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index 783c024d..028ce12e 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -39,4 +39,4 @@ impl Default for ScenarioProperties { pub struct Scenario { pub properties: ScenarioProperties, pub model_instances: BTreeMap>, -} \ No newline at end of file +} From 03cace21b377dcd2a9609c58bb8866e9649c24b0 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Tue, 2 Jul 2024 13:24:42 +0800 Subject: [PATCH 04/32] fix description names Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/model.rs | 4 ++++ rmf_site_editor/src/site/save.rs | 14 -------------- .../src/widgets/inspector/inspect_group.rs | 9 +++++++-- rmf_site_editor/src/widgets/view_groups.rs | 15 ++++++++++++++- rmf_site_format/src/legacy/building_map.rs | 2 -- rmf_site_format/src/legacy/model.rs | 2 +- rmf_site_format/src/level.rs | 3 --- rmf_site_format/src/model.rs | 15 +++++++++++++++ rmf_site_format/src/sdf.rs | 10 +++++++++- 9 files changed, 50 insertions(+), 24 deletions(-) diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 01bed237..f115de95 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -31,6 +31,10 @@ use rmf_site_format::{AssetSource, ModelMarker, Pending, Pose, Scale}; use smallvec::SmallVec; use std::any::TypeId; +#[derive(Component, Debug)] +pub struct ModelDescriptionUsage; + + #[derive(Component, Debug, Clone)] pub struct ModelScene { source: AssetSource, diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 8a5351ee..67295987 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -553,19 +553,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, @@ -1429,7 +1416,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/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index f7b0df47..648494f7 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, AssetSource, Change, DefaultFile, Group, Members, NameInSite, Texture}, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, CurrentWorkspace, }; @@ -29,6 +29,7 @@ pub struct InspectGroup<'w, 's> { affiliation: Query<'w, 's, &'static Affiliation>, names: Query<'w, 's, &'static NameInSite>, textures: Query<'w, 's, &'static Texture>, + assets: Query<'w, 's, &'static AssetSource>, members: Query<'w, 's, &'static Members>, default_file: Query<'w, 's, &'static DefaultFile>, current_workspace: Res<'w, CurrentWorkspace>, @@ -69,7 +70,11 @@ impl<'w, 's> InspectGroup<'w, 's> { .root .map(|e| self.default_file.get(e).ok()) .flatten(); - + if let Ok(asset) = self.assets.get(id) { + ui.label(RichText::new("Asset Source").size(18.0)); + ui.label(format!("{:?}", asset)); + ui.add_space(10.0); + } if let Ok(texture) = self.textures.get(id) { ui.label(RichText::new("Texture Properties").size(18.0)); if let Some(new_texture) = InspectTexture::new(texture, default_file).show(ui) { diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index b495462a..dfeef80d 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -16,7 +16,7 @@ */ use crate::{ - site::{Change, FiducialMarker, MergeGroups, NameInSite, SiteID, Texture}, + site::{Change, FiducialMarker, MergeGroups, ModelDescription, ModelMarker, NameInSite, SiteID, Texture}, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, }; @@ -39,6 +39,8 @@ 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>, + model_descriptions: + Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>), With>, icons: Res<'w, Icons>, group_view_modes: ResMut<'w, GroupViewModes>, app_state: Res<'w, State>, @@ -80,6 +82,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, @@ -228,6 +240,7 @@ pub struct GroupViewModes { site: Option, textures: GroupViewMode, fiducials: GroupViewMode, + model_descriptions: GroupViewMode, } impl GroupViewModes { diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index ea55ac44..ea8a1aa3 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -510,7 +510,6 @@ impl BuildingMap { rankings.floors.push(id); } - let mut models = BTreeMap::new(); for model in &level.models { let model_instance = model.to_model_instance( &mut model_description_name_map, @@ -568,7 +567,6 @@ impl BuildingMap { drawings, floors, lights, - models, physical_cameras, walls, rankings, diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index d32f9080..1335ead5 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -57,7 +57,7 @@ impl Model { model_descriptions.insert( id, ModelDescription { - name: NameInSite(self.model_name.clone()), + name: NameInSite(self.model_name.split("/").last().unwrap().to_string()), source: AssetSource::Search(self.model_name.clone()), group: Group::default(), marker: ModelMarker, 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/model.rs b/rmf_site_format/src/model.rs index 45966055..74510f7e 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -91,3 +91,18 @@ pub struct ModelInstance { #[serde(skip)] pub marker: ModelMarker, } + +impl Default for ModelInstance { + fn default() -> Self { + Self { + name: NameInSite("".to_string()), + source: AssetSource::default(), + pose: Pose::default(), + parent: SiteParentID(0), + description: Affiliation::default(), + is_static: IsStatic(false), + scale: Scale::default(), + marker: ModelMarker, + } + } +} \ No newline at end of file diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index 3414f0e6..dc50c87b 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_id, 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,7 +490,10 @@ 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_id, model) in &default_scenario.model_instances { + if (model.parent.0 != *level_id) { + continue; + } let mut added = false; if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) { From 0010b9f80f4c18be063f9de6c7eb34c437462b53 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Tue, 2 Jul 2024 15:37:33 +0800 Subject: [PATCH 05/32] Added convert function for model instance Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/load.rs | 2 +- rmf_site_editor/src/site/model.rs | 23 ++++++++++++------- .../src/widgets/inspector/inspect_group.rs | 7 +++--- rmf_site_editor/src/widgets/view_groups.rs | 5 +++- rmf_site_format/src/category.rs | 2 ++ rmf_site_format/src/model.rs | 18 ++++++++++++++- 6 files changed, 42 insertions(+), 15 deletions(-) diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 71bd36e7..83f7d1ef 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -225,7 +225,7 @@ fn generate_site_entities( for (model_instance_id, model_instance) in &default_scenario.model_instances { if model_instance.parent.0 == *level_id { level - .spawn(model_instance.clone()) + .spawn(model_instance.convert(&id_to_entity).for_site(site_id).expect("Model instance does not have a corresponding description spawned")) .insert(SiteID(*model_instance_id)); consider_id(*model_instance_id); } diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index f115de95..7ddfcb62 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -29,11 +29,7 @@ use bevy::{ use bevy_mod_outline::OutlineMeshExt; use rmf_site_format::{AssetSource, ModelMarker, Pending, Pose, Scale}; use smallvec::SmallVec; -use std::any::TypeId; - -#[derive(Component, Debug)] -pub struct ModelDescriptionUsage; - +use std::{any::TypeId, collections::HashMap}; #[derive(Component, Debug, Clone)] pub struct ModelScene { @@ -181,7 +177,7 @@ pub fn update_model_scenes( ( Entity, &AssetSource, - &Pose, + Option<&Pose>, &TentativeModelFormat, Option<&Visibility>, ), @@ -194,12 +190,16 @@ pub fn update_model_scenes( fn spawn_model( e: Entity, source: &AssetSource, - pose: &Pose, + pose: Option<&Pose>, asset_server: &AssetServer, tentative_format: &TentativeModelFormat, has_visibility: bool, commands: &mut Commands, ) { + let (category, pose) = match pose { + Some(pose) => (Category::Model, pose), + None => (Category::ModelDescription, &Pose::default()), + }; let mut commands = commands.entity(e); commands .insert(ModelScene { @@ -208,7 +208,7 @@ pub fn update_model_scenes( entity: None, }) .insert(TransformBundle::from_transform(pose.transform())) - .insert(Category::Model); + .insert(category); if !has_visibility { // NOTE: We separate this out because for CollisionMeshMarker @@ -484,3 +484,10 @@ pub fn propagate_model_render_layers( } } } + +#[derive(Component, Debug)] +pub struct ModelDescriptionUsage { + site: Entity, + used: HashMap, + unused: HashMap, +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index 648494f7..9601202b 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -18,10 +18,11 @@ use crate::{ site::{Affiliation, AssetSource, Change, DefaultFile, Group, Members, NameInSite, Texture}, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, - CurrentWorkspace, + CurrentWorkspace, InspectAssetSource, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{CollapsingHeader, RichText, Ui}; +use rmf_site_format::RecallAssetSource; #[derive(SystemParam)] pub struct InspectGroup<'w, 's> { @@ -54,7 +55,6 @@ impl<'w, 's> InspectGroup<'w, 's> { if self.is_group.contains(id) { self.show_group_properties(id, ui); } - if let Ok(Affiliation(Some(group))) = self.affiliation.get(id) { ui.separator(); let name = self.names.get(*group).map(|n| n.0.as_str()).unwrap_or(""); @@ -63,7 +63,6 @@ impl<'w, 's> InspectGroup<'w, 's> { self.show_group_properties(*group, ui); } } - pub fn show_group_properties(&mut self, id: Entity, ui: &mut Ui) { let default_file = self .current_workspace @@ -72,7 +71,7 @@ impl<'w, 's> InspectGroup<'w, 's> { .flatten(); if let Ok(asset) = self.assets.get(id) { ui.label(RichText::new("Asset Source").size(18.0)); - ui.label(format!("{:?}", asset)); + let a = 5; ui.add_space(10.0); } if let Ok(texture) = self.textures.get(id) { diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index dfeef80d..66f55e78 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -16,7 +16,10 @@ */ use crate::{ - site::{Change, FiducialMarker, MergeGroups, ModelDescription, ModelMarker, NameInSite, SiteID, Texture}, + site::{ + Change, FiducialMarker, MergeGroups, ModelDescription, ModelMarker, NameInSite, SiteID, + Texture, + }, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, }; 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/model.rs b/rmf_site_format/src/model.rs index 74510f7e..c83e8801 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -20,6 +20,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))] @@ -105,4 +106,19 @@ impl Default for ModelInstance { marker: ModelMarker, } } -} \ No newline at end of file +} + +impl ModelInstance { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(ModelInstance { + name: self.name.clone(), + source: self.source.clone(), + pose: self.pose.clone(), + parent: self.parent.clone(), + description: self.description.convert(id_map)?, + is_static: self.is_static.clone(), + scale: self.scale.clone(), + marker: Default::default(), + }) + } +} From 8386209bbb959ed35895de501bb664197bb1921d Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Mon, 8 Jul 2024 12:34:20 +0800 Subject: [PATCH 06/32] add from group list Signed-off-by: Reuben Thomas --- rmf_site_editor/src/interaction/cursor.rs | 19 +++++- .../src/interaction/select_anchor.rs | 39 +++++++++++- rmf_site_editor/src/widgets/view_groups.rs | 62 ++++++++++++++++--- rmf_site_format/src/misc.rs | 2 +- rmf_site_format/src/mobile_robot.rs | 44 +++++++++++++ rmf_site_format/src/model.rs | 3 +- 6 files changed, 156 insertions(+), 13 deletions(-) create mode 100644 rmf_site_format/src/mobile_robot.rs diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 375845fe..565970a5 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, Model, ModelInstance, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel, +}; use std::collections::HashSet; /// A resource that keeps track of the unique entities that play a role in @@ -124,6 +126,21 @@ impl Cursor { } } + 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(); + commands.entity(self.frame).push_children(&[e]); + Some(e) + } else { + None + } + } + pub fn set_workcell_model_preview( &mut self, commands: &mut Commands, diff --git a/rmf_site_editor/src/interaction/select_anchor.rs b/rmf_site_editor/src/interaction/select_anchor.rs index 87a24776..d08cac3e 100644 --- a/rmf_site_editor/src/interaction/select_anchor.rs +++ b/rmf_site_editor/src/interaction/select_anchor.rs @@ -27,7 +27,8 @@ 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, + ModelInstance, NameInWorkcell, NameOfSite, Path, PixelsPerMeter, Point, Pose, Side, Wall, + WorkcellModel, }; use std::collections::HashSet; use std::sync::Arc; @@ -1306,6 +1307,15 @@ impl SelectAnchorPointBuilder { } } + pub fn for_model_instance(self, model: ModelInstance) -> SelectAnchor3D { + SelectAnchor3D { + bundle: PlaceableObject::ModelInstance(model), + parent: None, + target: self.for_element, + continuity: self.continuity, + } + } + pub fn for_visual(self, model: WorkcellModel) -> SelectAnchor3D { SelectAnchor3D { bundle: PlaceableObject::VisualMesh(model), @@ -1687,6 +1697,7 @@ impl SelectAnchor { #[derive(Clone)] enum PlaceableObject { Model(Model), + ModelInstance(ModelInstance), Anchor, VisualMesh(WorkcellModel), CollisionMesh(WorkcellModel), @@ -2157,6 +2168,12 @@ pub fn handle_select_anchor_3d_mode( .cursor .set_model_preview(&mut params.commands, Some(m.clone())); } + PlaceableObject::ModelInstance(ref m) => { + // Spawn the model as a child of the cursor + params + .cursor + .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 params @@ -2220,6 +2237,26 @@ pub fn handle_select_anchor_3d_mode( params.commands.spawn(model).id() } } + 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) { + let child_id = params.commands.spawn(model).id(); + params + .commands + .spawn(( + AnchorBundle::new(Anchor::Pose3D(pose)) + .dependents(Dependents::single(child_id)), + FrameMarker, + NameInWorkcell("model_root".to_string()), + )) + .add_child(child_id) + .id() + } else { + model.pose = pose; + params.commands.spawn(model).id() + } + } PlaceableObject::VisualMesh(ref a) => { let mut model = a.clone(); model.pose = pose; diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index 66f55e78..9eef0d7b 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -16,9 +16,10 @@ */ use crate::{ + interaction::{ChangeMode, SelectAnchor3D}, site::{ - Change, FiducialMarker, MergeGroups, ModelDescription, ModelMarker, NameInSite, SiteID, - Texture, + Affiliation, AssetSource, Change, FiducialMarker, Group, MergeGroups, ModelInstance, + ModelMarker, NameInSite, SiteID, Texture, }, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, @@ -40,10 +41,36 @@ 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>, - model_descriptions: - Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>), With>, + textures: Query< + 'w, + 's, + ( + &'static NameInSite, + Option<&'static AssetSource>, + Option<&'static SiteID>, + ), + With, + >, + fiducials: Query< + 'w, + 's, + ( + &'static NameInSite, + Option<&'static AssetSource>, + Option<&'static SiteID>, + ), + With, + >, + model_descriptions: Query< + 'w, + 's, + ( + &'static NameInSite, + Option<&'static AssetSource>, + Option<&'static SiteID>, + ), + With, + >, icons: Res<'w, Icons>, group_view_modes: ResMut<'w, GroupViewModes>, app_state: Res<'w, State>, @@ -54,6 +81,7 @@ 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>, name: EventWriter<'w, Change>, commands: Commands<'w, 's>, @@ -119,7 +147,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<&AssetSource>, Option<&SiteID>), With>, mode: &mut GroupViewMode, icons: &Res, events: &mut ViewGroupsEvents, @@ -165,7 +193,7 @@ impl<'w, 's> ViewGroups<'w, 's> { }); for child in children { - let Ok((name, site_id)) = q_groups.get(*child) else { + let Ok((name, asset_source, site_id)) = q_groups.get(*child) else { continue; }; let text = site_id @@ -174,6 +202,24 @@ impl<'w, 's> ViewGroups<'w, 's> { ui.horizontal(|ui| { match mode.clone() { GroupViewMode::View => { + if let Some(asset_source) = asset_source { + if ui + .add(Button::image(icons.add.egui())) + .on_hover_text("Add a new model instance of this group") + .clicked() + { + let instance: ModelInstance = ModelInstance { + source: asset_source.clone(), + description: Affiliation(Some(child.clone())), + ..Default::default() + }; + events.change_mode.send(ChangeMode::To( + SelectAnchor3D::create_new_point() + .for_model_instance(instance) + .into(), + )); + }; + }; events.selector.show_widget(*child, ui); } GroupViewMode::SelectMergeFrom => { diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 1aabc248..64802180 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -407,7 +407,7 @@ pub struct PreviewableMarker; pub struct SiteID(pub u32); /// This component is applied to an entity to indicate that it is defined in relation -#[derive(Serialize, Deserialize, Debug, Clone, Copy)] +#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] pub struct SiteParentID(pub u32); diff --git a/rmf_site_format/src/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs new file mode 100644 index 00000000..a816746c --- /dev/null +++ b/rmf_site_format/src/mobile_robot.rs @@ -0,0 +1,44 @@ +/* + * 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}; +use serde::{Deserialize, Serialize}; + +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct MobileRobot { + pub model_name: NameInSite, + pub source: AssetSource, + #[serde(default, skip_serializing_if = "is_default")] + pub scale: Scale, + pub kinematics: MobileRobotKinematics, +} + +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub enum MobileRobotKinematics { + DifferentialDrive(DifferentialDrive), +} + +#[derive(Serialize, Deserialize, Debug, Clone)] +pub struct DifferentialDrive { + pub translational_speed: f32, + pub rotational_speed: f32, + pub bidirectional: bool, +} \ No newline at end of file diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index c83e8801..d109b062 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -69,14 +69,13 @@ impl Default for Model { pub struct ModelDescription { pub name: NameInSite, pub source: AssetSource, - #[serde(default, skip_serializing_if = "is_default")] #[serde(skip)] pub group: Group, #[serde(skip)] pub marker: ModelMarker, } -#[derive(Serialize, Deserialize, Debug, Clone)] +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct ModelInstance { pub name: NameInSite, From 9f30004a3dbdfa54fd4ed6a29ac06985216723ce Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 10 Jul 2024 15:43:32 +0800 Subject: [PATCH 07/32] redefine description, instances, scenarios Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/load.rs | 39 ++++++- rmf_site_editor/src/site/mod.rs | 4 + rmf_site_editor/src/site/model.rs | 14 +-- rmf_site_editor/src/site/scenario.rs | 68 ++++++++++++ rmf_site_editor/src/site/site.rs | 4 + rmf_site_editor/src/widgets/mod.rs | 3 + .../src/widgets/properties_panel.rs | 4 +- rmf_site_editor/src/widgets/view_groups.rs | 20 ++-- rmf_site_editor/src/widgets/view_scenarios.rs | 86 +++++++++++++++ rmf_site_format/src/legacy/building_map.rs | 26 +++-- rmf_site_format/src/legacy/model.rs | 30 +++--- rmf_site_format/src/mobile_robot.rs | 44 -------- rmf_site_format/src/model.rs | 28 ++--- rmf_site_format/src/scenario.rs | 100 ++++++++++++++++-- rmf_site_format/src/sdf.rs | 55 +++++++--- rmf_site_format/src/site.rs | 6 +- 16 files changed, 399 insertions(+), 132 deletions(-) create mode 100644 rmf_site_editor/src/site/scenario.rs create mode 100644 rmf_site_editor/src/widgets/view_scenarios.rs delete mode 100644 rmf_site_format/src/mobile_robot.rs diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 83f7d1ef..e72bf0b5 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -129,7 +129,34 @@ fn generate_site_entities( consider_id(*model_description_id); } - let (_, default_scenario) = site_data.scenarios.first_key_value().unwrap(); + for (model_instance_id, model_instance) in &site_data.model_instances { + let model_instance = commands + .spawn(model_instance.clone()) + .insert(SiteID(*model_instance_id)) + .set_parent(site_id) + .id(); + id_to_entity.insert(*model_instance_id, model_instance); + consider_id(*model_instance_id); + } + + let (_, default_scenario) = site_data + .scenarios + .first_key_value() + .expect("No scenarios found"); + 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).unwrap(); + 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 (level_id, level_data) in &site_data.levels { let level_entity = commands.spawn(SiteID(*level_id)).set_parent(site_id).id(); @@ -222,7 +249,15 @@ fn generate_site_entities( consider_id(*light_id); } - for (model_instance_id, model_instance) in &default_scenario.model_instances { + for model_instance_id in &default_scenario.scenario.added_model_instances { + let model_instance = site_data.model_instances.get(model_instance_id).expect( + format!( + "Scenario {} contains model instance {} which does not exist", + "Scenario Name", + model_instance_id + ) + .as_str(), + ); if model_instance.parent.0 == *level_id { level .spawn(model_instance.convert(&id_to_entity).for_site(site_id).expect("Model instance does not have a corresponding description spawned")) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 3dd7de9c..5614a9ec 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::*; @@ -188,6 +191,7 @@ impl Plugin for SitePlugin { .init_resource::() .init_resource::() .init_resource::() + .init_resource::() .init_resource::() .init_resource::() .init_resource::() diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 7ddfcb62..7908cdbe 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -27,9 +27,12 @@ use bevy::{ render::view::RenderLayers, }; use bevy_mod_outline::OutlineMeshExt; -use rmf_site_format::{AssetSource, ModelMarker, Pending, Pose, Scale}; +use rmf_site_format::{ + Affiliation, AssetSource, Group, ModelDescription, ModelMarker, NameInSite, Pending, Pose, + Scale, +}; use smallvec::SmallVec; -use std::{any::TypeId, collections::HashMap}; +use std::any::TypeId; #[derive(Component, Debug, Clone)] pub struct ModelScene { @@ -484,10 +487,3 @@ pub fn propagate_model_render_layers( } } } - -#[derive(Component, Debug)] -pub struct ModelDescriptionUsage { - site: Entity, - used: HashMap, - unused: HashMap, -} diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs new file mode 100644 index 00000000..70f5a8af --- /dev/null +++ b/rmf_site_editor/src/site/scenario.rs @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::site::*; +use crate::{interaction::CameraControls, CurrentWorkspace}; +use bevy::prelude::*; + +// pub fn update_level_visibility( +// mut levels: Query<(Entity, &mut Visibility), With>, +// current_level: Res, +// ) { +// if current_level.is_changed() { +// for (e, mut visibility) in &mut levels { +// *visibility = if Some(e) == **current_level { +// Visibility::Inherited +// } else { +// Visibility::Hidden +// }; +// } +// } +// } + +// pub fn assign_orphan_levels_to_site( +// mut commands: Commands, +// new_levels: Query, Added)>, +// open_sites: Query>, +// current_workspace: Res, +// ) { +// if let Some(site) = current_workspace.to_site(&open_sites) { +// for level in &new_levels { +// commands.entity(site).add_child(level); +// } +// } else { +// warn!( +// "Unable to assign level to any site because there is no \ +// current site" +// ); +// } +// } + +// pub fn assign_orphan_elements_to_level( +// mut commands: Commands, +// orphan_elements: Query, Without)>, +// current_level: Res, +// ) { +// let current_level = match current_level.0 { +// Some(c) => c, +// None => return, +// }; + +// for orphan in &orphan_elements { +// commands.entity(current_level).add_child(orphan); +// } +// } diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs index b3cd666a..fbdb3376 100644 --- a/rmf_site_editor/src/site/site.rs +++ b/rmf_site_editor/src/site/site.rs @@ -34,6 +34,10 @@ pub struct ChangeCurrentSite { #[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)] diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 76f7a4ed..cd196c4a 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::*; diff --git a/rmf_site_editor/src/widgets/properties_panel.rs b/rmf_site_editor/src/widgets/properties_panel.rs index f99e218b..ba92e03f 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, Widget, + WidgetSystem, }; use bevy::prelude::*; @@ -33,6 +34,7 @@ impl Plugin for StandardPropertiesPanelPlugin { app.add_plugins(( PropertiesPanelPlugin::new(PanelSide::Right), ViewLevelsPlugin::default(), + ViewScenariosPlugin::default(), ViewNavGraphsPlugin::default(), ViewLayersPlugin::default(), StandardInspectorPlugin::default(), diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index 9eef0d7b..419e87cd 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -208,16 +208,16 @@ impl<'w, 's> ViewGroups<'w, 's> { .on_hover_text("Add a new model instance of this group") .clicked() { - let instance: ModelInstance = ModelInstance { - source: asset_source.clone(), - description: Affiliation(Some(child.clone())), - ..Default::default() - }; - events.change_mode.send(ChangeMode::To( - SelectAnchor3D::create_new_point() - .for_model_instance(instance) - .into(), - )); + // let instance: ModelInstance = ModelInstance { + // // source: asset_source.clone(), + // description: Affiliation(Some(child.clone())), + // ..Default::default() + // }; + // events.change_mode.send(ChangeMode::To( + // SelectAnchor3D::create_new_point() + // .for_model_instance(instance) + // .into(), + // )); }; }; events.selector.show_widget(*child, ui); 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..1fa808eb --- /dev/null +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2022 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::{ + Category, Change, CurrentScenario, Delete, DrawingMarker, FloorMarker, LevelElevation, + LevelProperties, NameInSite, Scenario, ScenarioMarker, + }, + widgets::{prelude::*, Icons}, + AppState, CurrentWorkspace, RecencyRanking, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{CollapsingHeader, DragValue, ImageButton, Ui}; +use std::cmp::{Ordering, Reverse}; + +/// 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> { + scenarios: Query< + 'w, + 's, + (Entity, &'static NameInSite, &'static Scenario), + With, + >, + icons: Res<'w, Icons>, + display_scenarios: ResMut<'w, ScenarioDisplay>, + current_scenario: ResMut<'w, CurrentScenario>, + commands: Commands<'w, 's>, + app_state: Res<'w, State>, +} + +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) { + return; + } +} + +#[derive(Resource)] +pub struct ScenarioDisplay { + pub new_name: String, + pub order: Vec>, +} + +impl Default for ScenarioDisplay { + fn default() -> Self { + Self { + new_name: "New Scenario".to_string(), + order: Vec::new(), + } + } +} diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index ea8a1aa3..1f90bbba 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -2,15 +2,16 @@ use super::model::Model; 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, ModelDescription, - Motion, NameInSite, NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, - Pose, PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, Scenario, - ScenarioProperties, Site, SiteProperties, Texture as SiteTexture, TextureGroup, UserCameraPose, - DEFAULT_NAV_GRAPH_COLORS, + ModelDescriptionBundle, Motion, NameInSite, NameOfSite, NavGraph, Navigation, + OrientationConstraint, PixelsPerMeter, Pose, PreferredSemiTransparency, RankingsInLevel, + ReverseLane, Rotation, Scenario, ScenarioBundle, Site, SiteProperties, Texture as SiteTexture, + TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; @@ -203,11 +204,12 @@ 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_descriptions: BTreeMap = BTreeMap::new(); + let mut model_instances: BTreeMap> = BTreeMap::new(); let mut model_description_name_map = HashMap::::new(); - let mut scenarios = BTreeMap::new(); + let mut scenarios: BTreeMap> = BTreeMap::new(); let mut default_scenario_id = site_id.next().unwrap(); - scenarios.insert(default_scenario_id, Scenario::default()); + scenarios.insert(default_scenario_id, ScenarioBundle::default()); for (level_name, level) in &self.levels { let level_id = site_id.next().unwrap(); @@ -511,18 +513,19 @@ impl BuildingMap { } for model in &level.models { - let model_instance = model.to_model_instance( + let model_instance_id = model.update_instances_descriptions( &mut model_description_name_map, &mut model_descriptions, + &mut model_instances, &mut site_id, level_id, ); - scenarios .get_mut(&default_scenario_id) .unwrap() - .model_instances - .insert(site_id.next().unwrap(), model_instance); + .scenario + .added_model_instances + .push(model_instance_id); } let mut physical_cameras = BTreeMap::new(); @@ -731,6 +734,7 @@ 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 1335ead5..da065314 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,6 +1,7 @@ use crate::{ - Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, - ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale, SiteParentID, + model, Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, + ModelDescriptionBundle, ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale, + SiteParentID, }; use glam::DVec2; use serde::{Deserialize, Serialize}; @@ -42,13 +43,14 @@ impl Model { } } - pub fn to_model_instance( + pub fn update_instances_descriptions( &self, model_description_name_map: &mut HashMap, - model_descriptions: &mut BTreeMap, + model_descriptions: &mut BTreeMap, + model_instances: &mut BTreeMap>, site_id: &mut RangeFrom, level_id: u32, - ) -> ModelInstance { + ) -> u32 { let model_description_id = match model_description_name_map.get(&self.model_name) { Some(id) => *id, None => { @@ -56,9 +58,13 @@ impl Model { model_description_name_map.insert(self.model_name.clone(), id); model_descriptions.insert( id, - ModelDescription { + ModelDescriptionBundle { name: NameInSite(self.model_name.split("/").last().unwrap().to_string()), - source: AssetSource::Search(self.model_name.clone()), + description: ModelDescription { + source: AssetSource::Search(self.model_name.clone()), + is_static: IsStatic(self.static_), + scale: Scale::default(), + }, group: Group::default(), marker: ModelMarker, }, @@ -67,18 +73,18 @@ impl Model { } }; - ModelInstance { + let model_instance_id = site_id.next().unwrap(); + 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(), parent: SiteParentID(level_id), description: Affiliation(Some(model_description_id)), marker: ModelMarker, - } + }; + model_instances.insert(model_instance_id, model_instance); + model_instance_id } } diff --git a/rmf_site_format/src/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs deleted file mode 100644 index a816746c..00000000 --- a/rmf_site_format/src/mobile_robot.rs +++ /dev/null @@ -1,44 +0,0 @@ -/* - * 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}; -use serde::{Deserialize, Serialize}; - -#[derive(Serialize, Deserialize, Debug, Clone)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub struct MobileRobot { - pub model_name: NameInSite, - pub source: AssetSource, - #[serde(default, skip_serializing_if = "is_default")] - pub scale: Scale, - pub kinematics: MobileRobotKinematics, -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub enum MobileRobotKinematics { - DifferentialDrive(DifferentialDrive), -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -pub struct DifferentialDrive { - pub translational_speed: f32, - pub rotational_speed: f32, - pub bidirectional: bool, -} \ No newline at end of file diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index d109b062..76904fd6 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -64,30 +64,36 @@ impl Default for Model { /// /// -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] -#[cfg_attr(feature = "bevy", derive(Bundle))] +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] pub struct ModelDescription { - pub name: NameInSite, pub source: AssetSource, + #[serde(default, skip_serializing_if = "is_default")] + pub is_static: IsStatic, + #[serde(default, skip_serializing_if = "is_default")] + pub scale: Scale, +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct ModelDescriptionBundle { + pub name: NameInSite, + pub description: ModelDescription, #[serde(skip)] pub group: Group, #[serde(skip)] pub marker: ModelMarker, } + #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct ModelInstance { pub name: NameInSite, #[serde(skip)] - pub source: AssetSource, pub pose: Pose, pub parent: SiteParentID, pub description: Affiliation, - #[serde(default, skip_serializing_if = "is_default")] - pub is_static: IsStatic, - #[serde(default, skip_serializing_if = "is_default")] - pub scale: Scale, #[serde(skip)] pub marker: ModelMarker, } @@ -96,12 +102,9 @@ impl Default for ModelInstance { fn default() -> Self { Self { name: NameInSite("".to_string()), - source: AssetSource::default(), pose: Pose::default(), parent: SiteParentID(0), description: Affiliation::default(), - is_static: IsStatic(false), - scale: Scale::default(), marker: ModelMarker, } } @@ -111,12 +114,9 @@ impl ModelInstance { pub fn convert(&self, id_map: &HashMap) -> Result, T> { Ok(ModelInstance { name: self.name.clone(), - source: self.source.clone(), pose: self.pose.clone(), parent: self.parent.clone(), description: self.description.convert(id_map)?, - is_static: self.is_static.clone(), - scale: self.scale.clone(), marker: Default::default(), }) } diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index 028ce12e..fbb9022a 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -17,26 +17,104 @@ use crate::*; #[cfg(feature = "bevy")] -use bevy::prelude::{Component, Entity}; +use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; -use std::collections::BTreeMap; +use std::collections::HashMap; -#[derive(Serialize, Deserialize, Debug, Clone)] +#[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 ScenarioProperties { - pub name: String, +pub struct Scenario { + pub parent_scenario: Affiliation, + pub added_model_instances: Vec, + pub removed_model_instances: Vec, + pub moved_model_instances: Vec<(T, Pose)>, } -impl Default for ScenarioProperties { +// Create a root scenario without parent +impl Default for Scenario { fn default() -> Self { Self { - name: "new_scenario".to_owned(), + parent_scenario: Affiliation::default(), + added_model_instances: Vec::new(), + removed_model_instances: Vec::new(), + moved_model_instances: Vec::new(), } } } -#[derive(Serialize, Deserialize, Debug, Clone, Default)] -pub struct Scenario { - pub properties: ScenarioProperties, - pub model_instances: BTreeMap>, +impl Scenario { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(Scenario { + parent_scenario: self.parent_scenario.convert(id_map)?, + added_model_instances: self + .added_model_instances + .clone() + .into_iter() + .map(|id| { + id_map + .get(&id) + .expect("Scenario contains non existent added instance") + .clone() + }) + .collect(), + removed_model_instances: self + .removed_model_instances + .clone() + .into_iter() + .map(|id| { + id_map + .get(&id) + .expect("Scenario contains non existent removed instance") + .clone() + }) + .collect(), + moved_model_instances: self + .moved_model_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 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, + }) + } +} \ No newline at end of file diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index dc50c87b..bf856dd3 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -16,7 +16,7 @@ */ use crate::{ - Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing, + legacy::model, Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing }; use glam::Vec3; use once_cell::sync::Lazy; @@ -490,27 +490,44 @@ 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 &default_scenario.model_instances { - if (model.parent.0 != *level_id) { + for model_instance_id in &default_scenario.scenario.added_model_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 != *level_id { continue; } let mut added = false; - if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) + if model_description_bundle.description.source + == 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.description.source == 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; @@ -518,17 +535,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.description.is_static.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.description.is_static.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() @@ -536,7 +556,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() @@ -548,7 +571,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 diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index b40ae025..3f65a014 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -141,9 +141,11 @@ pub struct Site { /// Scenarios that exist in the site #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] - pub scenarios: BTreeMap, + pub scenarios: BTreeMap>, #[serde(default, skip_serializing_if = "is_default")] - pub model_descriptions: BTreeMap, + pub model_descriptions: BTreeMap, + #[serde(default, skip_serializing_if = "is_default")] + pub model_instances: BTreeMap>, } #[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)] From 1215fa9d6d69ac8835ae2fcfd493e5e84001e41d Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Thu, 11 Jul 2024 11:48:06 +0800 Subject: [PATCH 08/32] scenario panel prototype Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/model.rs | 33 +++---- rmf_site_editor/src/widgets/view_groups.rs | 21 ++--- rmf_site_editor/src/widgets/view_scenarios.rs | 85 ++++++++++++++++++- rmf_site_format/src/model.rs | 3 +- rmf_site_format/src/scenario.rs | 2 +- rmf_site_format/src/sdf.rs | 3 +- 6 files changed, 114 insertions(+), 33 deletions(-) diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 7908cdbe..50279e91 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -91,10 +91,7 @@ pub struct ModelSceneRoot; pub fn handle_model_loaded_events( mut commands: Commands, - loading_models: Query< - (Entity, &PendingSpawning, &Scale, Option<&RenderLayers>), - With, - >, + loading_models: Query<(Entity, &ModelDescription, &PendingSpawning), With>, mut current_scenes: Query<&mut ModelScene>, asset_server: Res, site_assets: Res, @@ -104,12 +101,16 @@ pub fn handle_model_loaded_events( // For each model that is loading, check if its scene has finished loading // yet. If the scene has finished loading, then insert it as a child of the // model entity and make it selectable. - for (e, h, scale, render_layer) in loading_models.iter() { + for (e, model_description, h) in loading_models.iter() { + println!("Model loaded: {:?}", model_description.source); + let render_layer: Option = None; + let scale = model_description.scale.0; if asset_server.is_loaded_with_dependencies(h.id()) { let Some(h) = untyped_assets.get(&**h) else { warn!("Broken reference to untyped asset, this should not happen!"); continue; }; + let h = &h.handle; let type_id = h.type_id(); let model_id = if type_id == TypeId::of::() { @@ -127,7 +128,7 @@ pub fn handle_model_loaded_events( commands .spawn(SceneBundle { scene, - transform: Transform::from_scale(**scale), + transform: Transform::from_scale(scale), ..default() }) .id(), @@ -138,7 +139,7 @@ pub fn handle_model_loaded_events( commands .spawn(SceneBundle { scene, - transform: Transform::from_scale(**scale), + transform: Transform::from_scale(scale), ..default() }) .id(), @@ -150,7 +151,7 @@ pub fn handle_model_loaded_events( .spawn(PbrBundle { mesh, material: site_assets.default_mesh_grey_material.clone(), - transform: Transform::from_scale(**scale), + transform: Transform::from_scale(scale), ..default() }) .id(), @@ -179,7 +180,7 @@ pub fn update_model_scenes( changed_models: Query< ( Entity, - &AssetSource, + &ModelDescription, Option<&Pose>, &TentativeModelFormat, Option<&Visibility>, @@ -258,7 +259,8 @@ pub fn update_model_scenes( } // update changed models - for (e, source, pose, tentative_format, vis_option) in changed_models.iter() { + for (e, model_description, pose, tentative_format, vis_option) in changed_models.iter() { + let source = &model_description.source; if let Ok(current_scene) = current_scenes.get_mut(e) { // Avoid respawning if spurious change detection was triggered if current_scene.source != *source || current_scene.format != *tentative_format { @@ -294,29 +296,30 @@ pub fn update_model_scenes( pub fn update_model_tentative_formats( mut commands: Commands, - changed_models: Query, With)>, - mut loading_models: Query< + changed_model_descriptions: Query, With)>, + mut loading_model_descriptions: Query< ( Entity, &mut TentativeModelFormat, &PendingSpawning, - &AssetSource, + &ModelDescription, ), With, >, asset_server: Res, ) { static SUPPORTED_EXTENSIONS: &[&str] = &["obj", "stl", "sdf", "glb", "gltf"]; - for e in changed_models.iter() { + for e in changed_model_descriptions.iter() { // Reset to the first format commands.entity(e).insert(TentativeModelFormat::default()); } // Check from the asset server if any format failed, if it did try the next - for (e, mut tentative_format, h, source) in loading_models.iter_mut() { + for (e, mut tentative_format, h, model_description) in loading_model_descriptions.iter_mut() { if matches!(asset_server.get_load_state(h.id()), Some(LoadState::Failed)) { let mut cmd = commands.entity(e); cmd.remove::(); // We want to iterate only for search asset types, for others just print an error + let source = &model_description.source; if matches!(source, AssetSource::Search(_)) { if let Some(fmt) = tentative_format.next() { *tentative_format = fmt; diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index 419e87cd..fb52d83b 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -26,6 +26,8 @@ use crate::{ }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{Button, CollapsingHeader, Ui}; +use rmf_site_format::ModelDescription; +use std::any::TypeId; /// Add a widget for viewing different kinds of groups. #[derive(Default)] @@ -46,10 +48,10 @@ pub struct ViewGroups<'w, 's> { 's, ( &'static NameInSite, - Option<&'static AssetSource>, + Option<&'static Texture>, Option<&'static SiteID>, ), - With, + (With, With), >, fiducials: Query< 'w, @@ -59,17 +61,17 @@ pub struct ViewGroups<'w, 's> { Option<&'static AssetSource>, Option<&'static SiteID>, ), - With, + (With, With), >, model_descriptions: Query< 'w, 's, ( &'static NameInSite, - Option<&'static AssetSource>, + Option<&'static ModelDescription>, Option<&'static SiteID>, ), - With, + (With, With), >, icons: Res<'w, Icons>, group_view_modes: ResMut<'w, GroupViewModes>, @@ -145,9 +147,9 @@ impl<'w, 's> ViewGroups<'w, 's> { }); } - fn show_groups<'b, T: Component>( + fn show_groups<'b, T: Component, S: Component>( children: impl IntoIterator, - q_groups: &Query<(&NameInSite, Option<&AssetSource>, Option<&SiteID>), With>, + q_groups: &Query<(&NameInSite, Option<&S>, Option<&SiteID>), (With, With)>, mode: &mut GroupViewMode, icons: &Res, events: &mut ViewGroupsEvents, @@ -202,14 +204,13 @@ impl<'w, 's> ViewGroups<'w, 's> { ui.horizontal(|ui| { match mode.clone() { GroupViewMode::View => { - if let Some(asset_source) = asset_source { + if TypeId::of::() == TypeId::of::() { if ui - .add(Button::image(icons.add.egui())) + .add(Button::new("")) .on_hover_text("Add a new model instance of this group") .clicked() { // let instance: ModelInstance = ModelInstance { - // // source: asset_source.clone(), // description: Affiliation(Some(child.clone())), // ..Default::default() // }; diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index 1fa808eb..9c7b2bd5 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -17,14 +17,14 @@ use crate::{ site::{ - Category, Change, CurrentScenario, Delete, DrawingMarker, FloorMarker, LevelElevation, + Affiliation, CurrentScenario, Delete, DrawingMarker, FloorMarker, LevelElevation, LevelProperties, NameInSite, Scenario, ScenarioMarker, }, widgets::{prelude::*, Icons}, AppState, CurrentWorkspace, RecencyRanking, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{CollapsingHeader, DragValue, ImageButton, Ui}; +use bevy_egui::egui::{Button, CollapsingHeader, DragValue, ImageButton, Ui}; use std::cmp::{Ordering, Reverse}; /// Add a plugin for viewing and editing a list of all levels @@ -40,13 +40,13 @@ impl Plugin for ViewScenariosPlugin { #[derive(SystemParam)] pub struct ViewScenarios<'w, 's> { + children: Query<'w, 's, &'static Children>, scenarios: Query< 'w, 's, (Entity, &'static NameInSite, &'static Scenario), With, >, - icons: Res<'w, Icons>, display_scenarios: ResMut<'w, ScenarioDisplay>, current_scenario: ResMut<'w, CurrentScenario>, commands: Commands<'w, 's>, @@ -66,10 +66,87 @@ impl<'w, 's> WidgetSystem for ViewScenarios<'w, 's> { impl<'w, 's> ViewScenarios<'w, 's> { pub fn show_widget(&mut self, ui: &mut Ui) { - return; + // Show scenarios, starting from the root + let mut scenario_version: Vec = vec![1]; + self.scenarios + .iter() + .filter(|(_, _, scenario)| scenario.parent_scenario.0.is_none()) + .for_each(|(scenario_entity, _, _)| { + show_scenario_widget( + ui, + scenario_entity, + scenario_version.clone(), + &self.children, + &self.scenarios, + ); + scenario_version[0] += 1; + }); } } +fn show_scenario_widget( + ui: &mut Ui, + scenario_entity: Entity, + scenario_version: Vec, + q_children: &Query<&'static Children>, + q_scenario: &Query< + (Entity, &'static NameInSite, &'static Scenario), + With, + >, +) { + let (entity, name, scenario) = q_scenario.get(scenario_entity).unwrap(); + ui.horizontal(|ui| { + // Select + if ui + .add(bevy_egui::egui::RadioButton::new(false, "")) + .clicked() + { + println!("Select scenario {}", name.0); + } + + // Add sub scenario + if ui + .add(Button::new("+")) + .on_hover_text(&format!("Add child scenario for {}", name.0)) + .clicked() + { + println!("Add child scenario for {}", name.0); + } + + // Show version + ui.label(if scenario_version.len() > 1 { + scenario_version + .iter() + .map(|v| v.to_string()) + .collect::>() + .join(".") + } else { + format!("{}.0", scenario_version[0]) + }); + + // Renameable label + let mut new_name = name.0.clone(); + if ui.text_edit_singleline(&mut new_name).changed() { + println!("Rename scenario {}", name.0); + } + }); + + CollapsingHeader::new("Properties") + .default_open(false) + .show(ui, |ui| { + ui.label(format!("Added: {}", scenario.added_model_instances.len())); + ui.label(format!("Moved: {}", scenario.moved_model_instances.len())); + ui.label(format!( + "Removed: {}", + scenario.removed_model_instances.len() + )); + }); + + CollapsingHeader::new("Sub Scenarios: 0") + .default_open(false) + .show(ui, |ui| {}); +} + #[derive(Resource)] pub struct ScenarioDisplay { pub new_name: String, diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index 76904fd6..ee8aa3f3 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -75,7 +75,7 @@ pub struct ModelDescription { } #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] -#[cfg_attr(feature = "bevy", derive(Component))] +#[cfg_attr(feature = "bevy", derive(Bundle))] pub struct ModelDescriptionBundle { pub name: NameInSite, pub description: ModelDescription, @@ -85,7 +85,6 @@ pub struct ModelDescriptionBundle { pub marker: ModelMarker, } - #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct ModelInstance { diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index fbb9022a..e1e9ab5e 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -117,4 +117,4 @@ impl ScenarioBundle { marker: ScenarioMarker, }) } -} \ No newline at end of file +} diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index bf856dd3..31d7dc1d 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -16,7 +16,8 @@ */ use crate::{ - legacy::model, Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing + legacy::model, Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, + Rotation, Site, Swing, }; use glam::Vec3; use once_cell::sync::Lazy; From 1c25265253656c7e9b777cb18a373f56500b97d7 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Fri, 12 Jul 2024 12:19:30 +0800 Subject: [PATCH 09/32] senario menu Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/level.rs | 2 +- rmf_site_editor/src/site/load.rs | 2 +- rmf_site_editor/src/site/mod.rs | 3 +- rmf_site_editor/src/site/model.rs | 5 +- rmf_site_editor/src/site/scenario.rs | 3 - rmf_site_editor/src/site/site.rs | 4 +- .../src/widgets/inspector/inspect_group.rs | 3 +- rmf_site_editor/src/widgets/view_groups.rs | 8 +- rmf_site_editor/src/widgets/view_scenarios.rs | 138 ++++++++++-------- rmf_site_format/src/legacy/building_map.rs | 7 +- rmf_site_format/src/legacy/model.rs | 2 +- rmf_site_format/src/location.rs | 1 + rmf_site_format/src/scenario.rs | 11 ++ rmf_site_format/src/sdf.rs | 4 +- 14 files changed, 103 insertions(+), 90 deletions(-) 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 e72bf0b5..8cc6d822 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -447,7 +447,7 @@ 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 5614a9ec..efb321f2 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -100,7 +100,6 @@ pub mod save; pub use save::*; pub mod scenario; -pub use scenario::*; pub mod sdf_exporter; pub use sdf_exporter::*; @@ -121,7 +120,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}; diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 50279e91..2faf5512 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -27,10 +27,7 @@ use bevy::{ render::view::RenderLayers, }; use bevy_mod_outline::OutlineMeshExt; -use rmf_site_format::{ - Affiliation, AssetSource, Group, ModelDescription, ModelMarker, NameInSite, Pending, Pose, - Scale, -}; +use rmf_site_format::{AssetSource, ModelDescription, ModelMarker, Pending, Pose, Scale}; use smallvec::SmallVec; use std::any::TypeId; diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index 70f5a8af..1fce9044 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -15,9 +15,6 @@ * */ -use crate::site::*; -use crate::{interaction::CameraControls, CurrentWorkspace}; -use bevy::prelude::*; // pub fn update_level_visibility( // mut levels: Query<(Entity, &mut Visibility), With>, diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs index fbdb3376..ae461b5a 100644 --- a/rmf_site_editor/src/site/site.rs +++ b/rmf_site_editor/src/site/site.rs @@ -24,10 +24,9 @@ use rmf_site_format::{ /// Used as an event to command that a new site should be made the current one #[derive(Clone, Copy, Debug, Event)] pub struct ChangeCurrentSite { - /// What should the current site be pub site: Entity, - /// What should its current level be pub level: Option, + pub scenario: Option, } /// Used as a resource that keeps track of the current level entity @@ -53,6 +52,7 @@ pub fn change_site( mut change_current_site: EventReader, mut current_workspace: ResMut, mut current_level: ResMut, + mut current_scenario: ResMut, cached_levels: Query<&CachedLevel>, mut visibility: Query<&mut Visibility>, open_sites: Query>, diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index 9601202b..6c4e1d3b 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -18,11 +18,10 @@ use crate::{ site::{Affiliation, AssetSource, Change, DefaultFile, Group, Members, NameInSite, Texture}, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, - CurrentWorkspace, InspectAssetSource, + CurrentWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{CollapsingHeader, RichText, Ui}; -use rmf_site_format::RecallAssetSource; #[derive(SystemParam)] pub struct InspectGroup<'w, 's> { diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index fb52d83b..02069828 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -16,10 +16,10 @@ */ use crate::{ - interaction::{ChangeMode, SelectAnchor3D}, + interaction::ChangeMode, site::{ - Affiliation, AssetSource, Change, FiducialMarker, Group, MergeGroups, ModelInstance, - ModelMarker, NameInSite, SiteID, Texture, + AssetSource, Change, FiducialMarker, Group, MergeGroups, ModelMarker, NameInSite, SiteID, + Texture, }, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, @@ -206,7 +206,7 @@ impl<'w, 's> ViewGroups<'w, 's> { GroupViewMode::View => { if TypeId::of::() == TypeId::of::() { if ui - .add(Button::new("")) + .add(Button::image_and_text(icons.merge.egui(), &name.0)) .on_hover_text("Add a new model instance of this group") .clicked() { diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index 9c7b2bd5..ef17bce9 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -16,16 +16,12 @@ */ use crate::{ - site::{ - Affiliation, CurrentScenario, Delete, DrawingMarker, FloorMarker, LevelElevation, - LevelProperties, NameInSite, Scenario, ScenarioMarker, - }, - widgets::{prelude::*, Icons}, - AppState, CurrentWorkspace, RecencyRanking, + site::{Change, CurrentScenario, NameInSite, Scenario, ScenarioBundle, ScenarioMarker}, + widgets::prelude::*, + AppState, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{Button, CollapsingHeader, DragValue, ImageButton, Ui}; -use std::cmp::{Ordering, Reverse}; +use bevy_egui::egui::{Button, CollapsingHeader, Ui}; /// Add a plugin for viewing and editing a list of all levels #[derive(Default)] @@ -47,6 +43,7 @@ pub struct ViewScenarios<'w, 's> { (Entity, &'static NameInSite, &'static Scenario), With, >, + change_name: EventWriter<'w, Change>, display_scenarios: ResMut<'w, ScenarioDisplay>, current_scenario: ResMut<'w, CurrentScenario>, commands: Commands<'w, 's>, @@ -66,26 +63,32 @@ impl<'w, 's> WidgetSystem for ViewScenarios<'w, 's> { impl<'w, 's> ViewScenarios<'w, 's> { pub fn show_widget(&mut self, ui: &mut Ui) { - // Show scenarios, starting from the root - let mut scenario_version: Vec = vec![1]; + // Show scenarios window, star + 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.commands, + &mut self.change_name, + &mut self.current_scenario, scenario_entity, - scenario_version.clone(), + vec![version], &self.children, &self.scenarios, ); - scenario_version[0] += 1; + version += 1; }); } } fn show_scenario_widget( ui: &mut Ui, + commands: &mut Commands, + change_name: &mut EventWriter>, + current_scenario: &mut CurrentScenario, scenario_entity: Entity, scenario_version: Vec, q_children: &Query<&'static Children>, @@ -95,69 +98,76 @@ fn show_scenario_widget( >, ) { let (entity, name, scenario) = q_scenario.get(scenario_entity).unwrap(); + let scenario_version_str = scenario_version + .iter() + .map(|v| v.to_string()) + .collect::>() + .join("."); ui.horizontal(|ui| { - // Select + // Selection if ui - .add(bevy_egui::egui::RadioButton::new(false, "")) + .add(bevy_egui::egui::RadioButton::new( + current_scenario.is_some_and(|e| e == entity), + "", + )) .clicked() { - println!("Select scenario {}", name.0); + //TODO: Replace this with the appropriiate change + *current_scenario = CurrentScenario(Some(entity)); } - - // Add sub scenario + // Version and name label + ui.label(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)); + } + }); + ui.horizontal(|ui| { + let children = q_children.get(scenario_entity); + let mut subversion = 1; + CollapsingHeader::new(format!("Sub-Scenarios {}", scenario_version_str)) + .default_open(false) + .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, + commands, + change_name, + current_scenario, + *child, + version, + q_children, + q_scenario, + ); + subversion += 1; + } + } + } else { + ui.label("No sub-scenarios"); + } + }); + // Add child scenario if ui - .add(Button::new("+")) - .on_hover_text(&format!("Add child scenario for {}", name.0)) + .button(" + ") + .on_hover_text(format!("Add a child scenario to {}", name.0)) .clicked() { - println!("Add child scenario for {}", name.0); - } - - // Show version - ui.label(if scenario_version.len() > 1 { - scenario_version - .iter() - .map(|v| v.to_string()) - .collect::>() - .join(".") - } else { - format!("{}.0", scenario_version[0]) - }); - - // Renameable label - let mut new_name = name.0.clone(); - if ui.text_edit_singleline(&mut new_name).changed() { - println!("Rename scenario {}", name.0); + commands + .spawn(ScenarioBundle { + name: name.clone(), + scenario: Scenario::from_parent(entity), + marker: ScenarioMarker, + }) + .set_parent(entity); } }); - - CollapsingHeader::new("Properties") - .default_open(false) - .show(ui, |ui| { - ui.label(format!("Added: {}", scenario.added_model_instances.len())); - ui.label(format!("Moved: {}", scenario.moved_model_instances.len())); - ui.label(format!( - "Removed: {}", - scenario.removed_model_instances.len() - )); - }); - - CollapsingHeader::new("Sub Scenarios: 0") - .default_open(false) - .show(ui, |ui| {}); } -#[derive(Resource)] +#[derive(Resource, Default)] pub struct ScenarioDisplay { - pub new_name: String, - pub order: Vec>, -} - -impl Default for ScenarioDisplay { - fn default() -> Self { - Self { - new_name: "New Scenario".to_string(), - order: Vec::new(), - } - } + pub new_root_scenario_name: String, } diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 1f90bbba..c3576e9b 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -1,4 +1,3 @@ -use super::model::Model; use super::{ floor::FloorParameters, level::Level, lift::Lift, wall::WallProperties, PortingError, Result, }; @@ -7,10 +6,10 @@ 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, ModelDescription, + Level as SiteLevel, LevelElevation, LevelProperties as SiteLevelProperties, ModelDescriptionBundle, Motion, NameInSite, NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose, PreferredSemiTransparency, RankingsInLevel, - ReverseLane, Rotation, Scenario, ScenarioBundle, Site, SiteProperties, Texture as SiteTexture, + ReverseLane, Rotation, ScenarioBundle, Site, SiteProperties, Texture as SiteTexture, TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; @@ -208,7 +207,7 @@ impl BuildingMap { let mut model_instances: BTreeMap> = BTreeMap::new(); let mut model_description_name_map = HashMap::::new(); let mut scenarios: BTreeMap> = BTreeMap::new(); - let mut default_scenario_id = site_id.next().unwrap(); + let default_scenario_id = site_id.next().unwrap(); scenarios.insert(default_scenario_id, ScenarioBundle::default()); for (level_name, level) in &self.levels { diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index da065314..9089699a 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,5 +1,5 @@ use crate::{ - model, Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, + Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, ModelDescriptionBundle, ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale, SiteParentID, }; diff --git a/rmf_site_format/src/location.rs b/rmf_site_format/src/location.rs index 9d3ea516..5d094c99 100644 --- a/rmf_site_format/src/location.rs +++ b/rmf_site_format/src/location.rs @@ -134,6 +134,7 @@ impl RecallLocationTags { _ => return tag.clone(), } } + if current.0.iter().find(|t| t.is_charger()).is_none() { return LocationTag::Charger; } diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index e1e9ab5e..15c80a38 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -35,6 +35,17 @@ pub struct Scenario { pub moved_model_instances: Vec<(T, Pose)>, } +impl Scenario { + pub fn from_parent(parent: T) -> Scenario { + Scenario { + parent_scenario: Affiliation(Some(parent)), + added_model_instances: Vec::new(), + removed_model_instances: Vec::new(), + moved_model_instances: Vec::new(), + } + } +} + // Create a root scenario without parent impl Default for Scenario { fn default() -> Self { diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index 31d7dc1d..553cdcb8 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -16,7 +16,7 @@ */ use crate::{ - legacy::model, Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, + Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing, }; use glam::Vec3; @@ -890,7 +890,7 @@ impl Site { #[cfg(test)] mod tests { - use super::*; + use crate::legacy::building_map::BuildingMap; #[test] From 7adb5c69f761b4235fb3ae9a346ecfda6acd560a Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Tue, 16 Jul 2024 23:08:49 +0800 Subject: [PATCH 10/32] improve scenario selector ui Signed-off-by: Reuben Thomas --- rmf_site_editor/src/interaction/cursor.rs | 1 + rmf_site_editor/src/site/group.rs | 4 + rmf_site_editor/src/site/load.rs | 118 ++++---- rmf_site_editor/src/site/mod.rs | 7 + rmf_site_editor/src/site/model.rs | 66 +++-- rmf_site_editor/src/site/scenario.rs | 72 ++--- .../src/widgets/inspector/inspect_group.rs | 14 +- rmf_site_editor/src/widgets/view_groups.rs | 32 +-- rmf_site_editor/src/widgets/view_scenarios.rs | 270 ++++++++++++++---- rmf_site_format/src/legacy/building_map.rs | 4 +- rmf_site_format/src/legacy/model.rs | 56 ++-- rmf_site_format/src/misc.rs | 38 ++- rmf_site_format/src/model.rs | 22 +- rmf_site_format/src/scenario.rs | 35 ++- rmf_site_format/src/sdf.rs | 19 +- rmf_site_format/src/site.rs | 2 + 16 files changed, 490 insertions(+), 270 deletions(-) diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 565970a5..14553e31 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -133,6 +133,7 @@ impl Cursor { ) { self.remove_preview(commands); self.preview_model = if let Some(model) = model { + println!("Spawn model instance preview"); let e = commands.spawn(model).insert(Pending).id(); commands.entity(self.frame).push_children(&[e]); Some(e) diff --git a/rmf_site_editor/src/site/group.rs b/rmf_site_editor/src/site/group.rs index 6d28d026..d6fe2f1f 100644 --- a/rmf_site_editor/src/site/group.rs +++ b/rmf_site_editor/src/site/group.rs @@ -34,6 +34,10 @@ impl Members { pub fn iter(&self) -> impl Iterator { self.0.iter() } + + pub fn len(&self) -> usize { + self.0.len() + } } #[derive(Component, Clone, Copy)] diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 8cc6d822..f839eb60 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::{BTreeMap, HashMap}, + path::PathBuf, +}; use thiserror::Error as ThisError; /// This component is given to the site to keep track of what file it should be @@ -119,45 +122,6 @@ fn generate_site_entities( consider_id(*group_id); } - for (model_description_id, model_description) in &site_data.model_descriptions { - let model_description = commands - .spawn(model_description.clone()) - .insert(SiteID(*model_description_id)) - .set_parent(site_id) - .id(); - id_to_entity.insert(*model_description_id, model_description); - consider_id(*model_description_id); - } - - for (model_instance_id, model_instance) in &site_data.model_instances { - let model_instance = commands - .spawn(model_instance.clone()) - .insert(SiteID(*model_instance_id)) - .set_parent(site_id) - .id(); - id_to_entity.insert(*model_instance_id, model_instance); - consider_id(*model_instance_id); - } - - let (_, default_scenario) = site_data - .scenarios - .first_key_value() - .expect("No scenarios found"); - 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).unwrap(); - 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 (level_id, level_data) in &site_data.levels { let level_entity = commands.spawn(SiteID(*level_id)).set_parent(site_id).id(); @@ -249,23 +213,6 @@ fn generate_site_entities( consider_id(*light_id); } - for model_instance_id in &default_scenario.scenario.added_model_instances { - let model_instance = site_data.model_instances.get(model_instance_id).expect( - format!( - "Scenario {} contains model instance {} which does not exist", - "Scenario Name", - model_instance_id - ) - .as_str(), - ); - if model_instance.parent.0 == *level_id { - level - .spawn(model_instance.convert(&id_to_entity).for_site(site_id).expect("Model instance does not have a corresponding description spawned")) - .insert(SiteID(*model_instance_id)); - consider_id(*model_instance_id); - } - } - for (physical_camera_id, physical_camera) in &level_data.physical_cameras { level .spawn(physical_camera.clone()) @@ -299,6 +246,57 @@ fn generate_site_entities( consider_id(*level_id); } + for (model_description_id, model_description) in &site_data.model_descriptions { + let model_description = commands + .spawn(model_description.clone()) + .insert(SiteID(*model_description_id)) + .set_parent(site_id) + .id(); + id_to_entity.insert(*model_description_id, model_description); + consider_id(*model_description_id); + } + + let (default_scenario_id, default_scenario) = site_data + .scenarios + .first_key_value() + .expect("No scenarios found"); + let active_model_instance_ids: std::collections::HashSet = default_scenario + .scenario + .added_model_instances + .iter() + .map(|(id, _)| *id) + .collect(); + 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_parent = if active_model_instance_ids.contains(model_instance_id) { + model_instance.parent.0.unwrap_or(site_id) + } else { + site_id + }; + let model_instance_entity = commands + .spawn(model_instance.clone()) + .insert(SiteID(*model_instance_id)) + .set_parent(model_instance_parent) + .id(); + id_to_entity.insert(*model_instance_id, model_instance_entity); + consider_id(*model_instance_id); + } + + 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(); @@ -447,7 +445,11 @@ pub fn load_site( } if cmd.focus { - change_current_site.send(ChangeCurrentSite { site, level: None, scenario: 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 efb321f2..e3ea8017 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -100,6 +100,7 @@ pub mod save; pub use save::*; pub mod scenario; +pub use scenario::*; pub mod sdf_exporter; pub use sdf_exporter::*; @@ -208,6 +209,7 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() + .add_event::() .add_event::() .add_event::() .add_event::() @@ -363,6 +365,8 @@ impl Plugin for SitePlugin { add_location_visuals, add_fiducial_visuals, update_level_visibility, + update_scenario_properties, + update_current_scenario, update_changed_lane, update_lane_for_moved_anchor, ) @@ -400,6 +404,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 2faf5512..7cf5a59b 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, Selectable, MODEL_PREVIEW_LAYER}, - site::{Category, PreventDeletion, SiteAssets}, + site::{Category, Group, Members, PreventDeletion, SiteAssets}, site_asset_io::MODEL_ENVIRONMENT_VARIABLE, }; use bevy::{ @@ -27,9 +27,12 @@ use bevy::{ render::view::RenderLayers, }; use bevy_mod_outline::OutlineMeshExt; -use rmf_site_format::{AssetSource, ModelDescription, ModelMarker, Pending, Pose, Scale}; +use rmf_site_format::{ + Affiliation, AssetSource, ModelMarker, ModelProperty, NameInSite, Pending, Pose, Scale, +}; use smallvec::SmallVec; -use std::any::TypeId; +use std::{any::TypeId, collections::HashMap}; +use yaserde::xml::name; #[derive(Component, Debug, Clone)] pub struct ModelScene { @@ -88,7 +91,10 @@ pub struct ModelSceneRoot; pub fn handle_model_loaded_events( mut commands: Commands, - loading_models: Query<(Entity, &ModelDescription, &PendingSpawning), With>, + loading_models: Query< + (Entity, &PendingSpawning, &Scale, Option<&RenderLayers>), + With, + >, mut current_scenes: Query<&mut ModelScene>, asset_server: Res, site_assets: Res, @@ -98,16 +104,12 @@ pub fn handle_model_loaded_events( // For each model that is loading, check if its scene has finished loading // yet. If the scene has finished loading, then insert it as a child of the // model entity and make it selectable. - for (e, model_description, h) in loading_models.iter() { - println!("Model loaded: {:?}", model_description.source); - let render_layer: Option = None; - let scale = model_description.scale.0; + for (e, h, scale, render_layer) in loading_models.iter() { if asset_server.is_loaded_with_dependencies(h.id()) { let Some(h) = untyped_assets.get(&**h) else { warn!("Broken reference to untyped asset, this should not happen!"); continue; }; - let h = &h.handle; let type_id = h.type_id(); let model_id = if type_id == TypeId::of::() { @@ -125,7 +127,7 @@ pub fn handle_model_loaded_events( commands .spawn(SceneBundle { scene, - transform: Transform::from_scale(scale), + transform: Transform::from_scale(**scale), ..default() }) .id(), @@ -136,7 +138,7 @@ pub fn handle_model_loaded_events( commands .spawn(SceneBundle { scene, - transform: Transform::from_scale(scale), + transform: Transform::from_scale(**scale), ..default() }) .id(), @@ -148,7 +150,7 @@ pub fn handle_model_loaded_events( .spawn(PbrBundle { mesh, material: site_assets.default_mesh_grey_material.clone(), - transform: Transform::from_scale(scale), + transform: Transform::from_scale(**scale), ..default() }) .id(), @@ -177,7 +179,7 @@ pub fn update_model_scenes( changed_models: Query< ( Entity, - &ModelDescription, + &AssetSource, Option<&Pose>, &TentativeModelFormat, Option<&Visibility>, @@ -256,8 +258,7 @@ pub fn update_model_scenes( } // update changed models - for (e, model_description, pose, tentative_format, vis_option) in changed_models.iter() { - let source = &model_description.source; + for (e, source, pose, tentative_format, vis_option) in changed_models.iter() { if let Ok(current_scene) = current_scenes.get_mut(e) { // Avoid respawning if spurious change detection was triggered if current_scene.source != *source || current_scene.format != *tentative_format { @@ -293,30 +294,29 @@ pub fn update_model_scenes( pub fn update_model_tentative_formats( mut commands: Commands, - changed_model_descriptions: Query, With)>, - mut loading_model_descriptions: Query< + changed_models: Query, With)>, + mut loading_models: Query< ( Entity, &mut TentativeModelFormat, &PendingSpawning, - &ModelDescription, + &AssetSource, ), With, >, asset_server: Res, ) { static SUPPORTED_EXTENSIONS: &[&str] = &["obj", "stl", "sdf", "glb", "gltf"]; - for e in changed_model_descriptions.iter() { + for e in changed_models.iter() { // Reset to the first format commands.entity(e).insert(TentativeModelFormat::default()); } // Check from the asset server if any format failed, if it did try the next - for (e, mut tentative_format, h, model_description) in loading_model_descriptions.iter_mut() { + for (e, mut tentative_format, h, source) in loading_models.iter_mut() { if matches!(asset_server.get_load_state(h.id()), Some(LoadState::Failed)) { let mut cmd = commands.entity(e); cmd.remove::(); // We want to iterate only for search asset types, for others just print an error - let source = &model_description.source; if matches!(source, AssetSource::Search(_)) { if let Some(fmt) = tentative_format.next() { *tentative_format = fmt; @@ -487,3 +487,27 @@ pub fn propagate_model_render_layers( } } } + +pub fn update_model_instances( + mut commands: Commands, + mut model_properties: Query< + (Entity, &NameInSite, Ref>), + (With, With), + >, + model_instances: Query<(Entity, Ref>), (With, Without)>, +) { + for (instance_entity, affiliation) in model_instances.iter() { + if let Some(description_entity) = affiliation.0 { + if let Ok((_, name, 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()); + } + } + } + } +} + +#[derive(Component)] +pub struct Inactive; diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index 1fce9044..f320dec4 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -15,51 +15,35 @@ * */ +use crate::site::CurrentScenario; +use bevy::prelude::*; +use rmf_site_format::{Group, ModelMarker, NameInSite, Pose}; -// pub fn update_level_visibility( -// mut levels: Query<(Entity, &mut Visibility), With>, -// current_level: Res, -// ) { -// if current_level.is_changed() { -// for (e, mut visibility) in &mut levels { -// *visibility = if Some(e) == **current_level { -// Visibility::Inherited -// } else { -// Visibility::Hidden -// }; -// } -// } -// } +#[derive(Clone, Copy, Debug, Event)] +pub struct ChangeCurrentScenario(pub Entity); -// pub fn assign_orphan_levels_to_site( -// mut commands: Commands, -// new_levels: Query, Added)>, -// open_sites: Query>, -// current_workspace: Res, -// ) { -// if let Some(site) = current_workspace.to_site(&open_sites) { -// for level in &new_levels { -// commands.entity(site).add_child(level); -// } -// } else { -// warn!( -// "Unable to assign level to any site because there is no \ -// current site" -// ); -// } -// } +pub fn update_current_scenario( + mut change_current_scenario: EventReader, + mut current_scenario: ResMut, + scenarios: Query, +) { + for ChangeCurrentScenario(new_scenario_entity) in change_current_scenario.read() { + *current_scenario = CurrentScenario(Some(*new_scenario_entity)); + println!("Changed scenario"); + } +} -// pub fn assign_orphan_elements_to_level( -// mut commands: Commands, -// orphan_elements: Query, Without)>, -// current_level: Res, -// ) { -// let current_level = match current_level.0 { -// Some(c) => c, -// None => return, -// }; +pub fn update_scenario_properties( + current_scenario: Res, + changed_models: Query<(Entity, &NameInSite, Ref), (With, Without)>, +) { + for (e, name, pose) in changed_models.iter() { + if pose.is_added() { + println!("Added: {}", name.0); + } -// for orphan in &orphan_elements { -// commands.entity(current_level).add_child(orphan); -// } -// } + if !pose.is_added() && pose.is_changed() { + println!("Moved: {}", name.0) + } + } +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index 6c4e1d3b..b7d2368f 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -16,7 +16,10 @@ */ use crate::{ - site::{Affiliation, AssetSource, Change, DefaultFile, Group, Members, NameInSite, Texture}, + site::{ + Affiliation, AssetSource, Change, DefaultFile, Group, IsStatic, Members, ModelProperty, + NameInSite, Scale, Texture, + }, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, CurrentWorkspace, }; @@ -29,7 +32,9 @@ pub struct InspectGroup<'w, 's> { affiliation: Query<'w, 's, &'static Affiliation>, names: Query<'w, 's, &'static NameInSite>, textures: Query<'w, 's, &'static Texture>, - assets: Query<'w, 's, &'static AssetSource>, + model_assets: Query<'w, 's, &'static ModelProperty>, + is_static: Query<'w, 's, &'static ModelProperty>, + scale: Query<'w, 's, &'static ModelProperty>, members: Query<'w, 's, &'static Members>, default_file: Query<'w, 's, &'static DefaultFile>, current_workspace: Res<'w, CurrentWorkspace>, @@ -68,9 +73,8 @@ impl<'w, 's> InspectGroup<'w, 's> { .root .map(|e| self.default_file.get(e).ok()) .flatten(); - if let Ok(asset) = self.assets.get(id) { - ui.label(RichText::new("Asset Source").size(18.0)); - let a = 5; + if let Ok(ModelProperty(asset)) = self.model_assets.get(id) { + ui.label(RichText::new("Model Description Properties").size(18.0)); ui.add_space(10.0); } if let Ok(texture) = self.textures.get(id) { diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index 02069828..f404ec08 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -16,17 +16,16 @@ */ use crate::{ - interaction::ChangeMode, + interaction::{ChangeMode, SelectAnchor3D}, site::{ - AssetSource, Change, FiducialMarker, Group, MergeGroups, ModelMarker, NameInSite, SiteID, - Texture, + Affiliation, AssetSource, Change, 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 rmf_site_format::ModelDescription; use std::any::TypeId; /// Add a widget for viewing different kinds of groups. @@ -68,7 +67,7 @@ pub struct ViewGroups<'w, 's> { 's, ( &'static NameInSite, - Option<&'static ModelDescription>, + Option<&'static AssetSource>, Option<&'static SiteID>, ), (With, With), @@ -204,21 +203,22 @@ impl<'w, 's> ViewGroups<'w, 's> { ui.horizontal(|ui| { match mode.clone() { GroupViewMode::View => { - if TypeId::of::() == TypeId::of::() { + if TypeId::of::() == TypeId::of::() { if ui - .add(Button::image_and_text(icons.merge.egui(), &name.0)) + .add(Button::image(icons.add.egui())) .on_hover_text("Add a new model instance of this group") .clicked() { - // let instance: ModelInstance = ModelInstance { - // description: Affiliation(Some(child.clone())), - // ..Default::default() - // }; - // events.change_mode.send(ChangeMode::To( - // SelectAnchor3D::create_new_point() - // .for_model_instance(instance) - // .into(), - // )); + 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(), + )); + println!("Add a new model instance of this group"); }; }; events.selector.show_widget(*child, ui); diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index ef17bce9..53ef513e 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -16,12 +16,16 @@ */ use crate::{ - site::{Change, CurrentScenario, NameInSite, Scenario, ScenarioBundle, ScenarioMarker}, + site::{ + Change, ChangeCurrentScenario, CurrentScenario, ModelMarker, NameInSite, Scenario, + ScenarioMarker, + }, widgets::prelude::*, - AppState, + AppState, Icons, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{Button, CollapsingHeader, Ui}; +use bevy_egui::egui::{Button, CollapsingHeader, Color32, Ui}; +use rmf_site_format::{Angle, ScenarioBundle, SiteID}; /// Add a plugin for viewing and editing a list of all levels #[derive(Default)] @@ -36,6 +40,7 @@ impl Plugin for ViewScenariosPlugin { #[derive(SystemParam)] pub struct ViewScenarios<'w, 's> { + commands: Commands<'w, 's>, children: Query<'w, 's, &'static Children>, scenarios: Query< 'w, @@ -44,10 +49,12 @@ pub struct ViewScenarios<'w, 's> { With, >, change_name: EventWriter<'w, Change>, + change_current_scenario: EventWriter<'w, ChangeCurrentScenario>, display_scenarios: ResMut<'w, ScenarioDisplay>, current_scenario: ResMut<'w, CurrentScenario>, - commands: Commands<'w, 's>, - app_state: Res<'w, State>, + model_instances: + Query<'w, 's, (Entity, &'static NameInSite, &'static SiteID), With>, + icons: Res<'w, Icons>, } impl<'w, 's> WidgetSystem for ViewScenarios<'w, 's> { @@ -63,21 +70,160 @@ impl<'w, 's> WidgetSystem for ViewScenarios<'w, 's> { impl<'w, 's> ViewScenarios<'w, 's> { pub fn show_widget(&mut self, ui: &mut Ui) { - // Show scenarios window, star + // Current Selection Info + if let Some(current_scenario_entity) = self.current_scenario.0 { + if let Ok((_, name, scenario)) = self.scenarios.get(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)); + } + }); + + ui.label("From Previous:"); + CollapsingHeader::new(format!( + "Added Models: {}", + scenario.added_model_instances.len() + )) + .default_open(false) + .show(ui, |ui| { + for (entity, pose) in scenario.added_model_instances.iter() { + if let Ok((_, name, site_id)) = self.model_instances.get(*entity) { + ui.label(format!("#{} {}", site_id.0, name.0,)); + 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(), + } + ), + ); + } + } + }); + CollapsingHeader::new(format!( + "Moved Models: {}", + scenario.moved_model_instances.len() + )) + .default_open(false) + .show(ui, |ui| { + for (entity, pose) in scenario.moved_model_instances.iter() { + if let Ok((_, name, site_id)) = self.model_instances.get(*entity) { + ui.label(format!("#{} {}", site_id.0, name.0,)); + 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(), + } + ), + ); + } + } + }); + CollapsingHeader::new(format!( + "Removed Models: {}", + scenario.removed_model_instances.len() + )) + .default_open(false) + .show(ui, |ui| { + for entity in scenario.removed_model_instances.iter() { + if let Ok((_, name, site_id)) = self.model_instances.get(*entity) { + ui.label(format!("#{} {}", site_id.0, name.0)); + } else { + ui.label("Unavailable"); + } + } + }); + } + } 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 parent_scenario_entity = if self.display_scenarios.is_new_scenario_root { + None + } else { + self.current_scenario.0 + }; + let mut cmd = self + .commands + .spawn(ScenarioBundle::::from_name_parent( + self.display_scenarios.new_scenario_name.clone(), + parent_scenario_entity, + )); + if !self.display_scenarios.is_new_scenario_root { + if let Some(current_scenario_entity) = self.current_scenario.0 { + cmd.set_parent(current_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()) + .filter(|(e, name, scenario)| scenario.parent_scenario.0.is_none()) .for_each(|(scenario_entity, _, _)| { show_scenario_widget( ui, - &mut self.commands, &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; }); @@ -86,8 +232,8 @@ impl<'w, 's> ViewScenarios<'w, 's> { fn show_scenario_widget( ui: &mut Ui, - commands: &mut Commands, change_name: &mut EventWriter>, + change_current_scenario: &mut EventWriter, current_scenario: &mut CurrentScenario, scenario_entity: Entity, scenario_version: Vec, @@ -96,6 +242,7 @@ fn show_scenario_widget( (Entity, &'static NameInSite, &'static Scenario), With, >, + icons: &Res, ) { let (entity, name, scenario) = q_scenario.get(scenario_entity).unwrap(); let scenario_version_str = scenario_version @@ -103,71 +250,68 @@ fn show_scenario_widget( .map(|v| v.to_string()) .collect::>() .join("."); + + // Scenario version and name, e.g. 1.2.3 My Scenario ui.horizontal(|ui| { - // Selection - if ui - .add(bevy_egui::egui::RadioButton::new( - current_scenario.is_some_and(|e| e == entity), - "", - )) - .clicked() - { - //TODO: Replace this with the appropriiate change - *current_scenario = CurrentScenario(Some(entity)); + if ui.radio(Some(entity) == **current_scenario, "").clicked() { + change_current_scenario.send(ChangeCurrentScenario(entity)); } - // Version and name label - ui.label(scenario_version_str.clone()); + ui.colored_label(Color32::DARK_GRAY, scenario_version_str.clone()); + ui.label(name.0.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)); } }); - ui.horizontal(|ui| { - let children = q_children.get(scenario_entity); - let mut subversion = 1; - CollapsingHeader::new(format!("Sub-Scenarios {}", scenario_version_str)) - .default_open(false) - .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, - commands, - change_name, - current_scenario, - *child, - version, - q_children, - q_scenario, - ); - subversion += 1; - } - } - } else { - ui.label("No sub-scenarios"); + + // 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(false) + .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; } - }); - // Add child scenario - if ui - .button(" + ") - .on_hover_text(format!("Add a child scenario to {}", name.0)) - .clicked() - { - commands - .spawn(ScenarioBundle { - name: name.clone(), - scenario: Scenario::from_parent(entity), - marker: ScenarioMarker, - }) - .set_parent(entity); + } + } else { + ui.label("No Child Scenarios"); } }); } -#[derive(Resource, Default)] +#[derive(Resource)] pub struct ScenarioDisplay { - pub new_root_scenario_name: String, + 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_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index c3576e9b..66985928 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -512,7 +512,7 @@ impl BuildingMap { } for model in &level.models { - let model_instance_id = model.update_instances_descriptions( + let (model_instance_id, model_pose) = model.to_site( &mut model_description_name_map, &mut model_descriptions, &mut model_instances, @@ -524,7 +524,7 @@ impl BuildingMap { .unwrap() .scenario .added_model_instances - .push(model_instance_id); + .push((model_instance_id, model_pose)); } let mut physical_cameras = BTreeMap::new(); diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index 9089699a..b9d428ad 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,7 +1,6 @@ use crate::{ - Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescription, - ModelDescriptionBundle, ModelInstance, ModelMarker, NameInSite, Pose, Rotation, Scale, - SiteParentID, + Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescriptionBundle, + ModelInstance, ModelMarker, ModelProperty, NameInSite, Pose, Rotation, Scale, SiteParent, }; use glam::DVec2; use serde::{Deserialize, Serialize}; @@ -29,28 +28,28 @@ impl Model { DVec2::new(self.x, self.y) } - pub fn to_site(&self) -> SiteModel { - SiteModel { - 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(), - marker: ModelMarker, - } - } + // pub fn to_site(&self) -> SiteModel { + // SiteModel { + // 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(), + // marker: ModelMarker, + // } + // } - pub fn update_instances_descriptions( + 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 { + ) -> (u32, Pose) { let model_description_id = match model_description_name_map.get(&self.model_name) { Some(id) => *id, None => { @@ -60,11 +59,9 @@ impl Model { id, ModelDescriptionBundle { name: NameInSite(self.model_name.split("/").last().unwrap().to_string()), - description: ModelDescription { - source: AssetSource::Search(self.model_name.clone()), - is_static: IsStatic(self.static_), - scale: Scale::default(), - }, + source: ModelProperty(AssetSource::Search(self.model_name.clone())), + is_static: ModelProperty(IsStatic(self.static_)), + scale: ModelProperty(Scale::default()), group: Group::default(), marker: ModelMarker, }, @@ -74,17 +71,18 @@ impl Model { }; 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()), - 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)), - }, - parent: SiteParentID(level_id), + pose: pose.clone(), + parent: SiteParent(Some(level_id)), description: Affiliation(Some(model_description_id)), marker: ModelMarker, }; model_instances.insert(model_instance_id, model_instance); - model_instance_id + (model_instance_id, pose) } } diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 64802180..0cdbb36a 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -15,7 +15,7 @@ * */ -use crate::RefTrait; +use crate::{RefTrait, Site}; #[cfg(feature = "bevy")] use bevy::prelude::*; use glam::{Quat, Vec2, Vec3}; @@ -407,9 +407,39 @@ pub struct PreviewableMarker; pub struct SiteID(pub u32); /// This component is applied to an entity to indicate that it is defined in relation -#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq)] -#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] -pub struct SiteParentID(pub u32); +/// Affiliates an entity with a group. +#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] +#[serde(transparent)] +#[cfg_attr(feature = "bevy", derive(Component))] +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, diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index ee8aa3f3..fa3b4746 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -64,21 +64,19 @@ impl Default for Model { /// /// -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Component))] -pub struct ModelDescription { - pub source: AssetSource, - #[serde(default, skip_serializing_if = "is_default")] - pub is_static: IsStatic, - #[serde(default, skip_serializing_if = "is_default")] - pub scale: Scale, -} +pub struct ModelProperty(pub T); #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct ModelDescriptionBundle { pub name: NameInSite, - pub description: ModelDescription, + 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)] @@ -91,7 +89,7 @@ pub struct ModelInstance { pub name: NameInSite, #[serde(skip)] pub pose: Pose, - pub parent: SiteParentID, + pub parent: SiteParent, pub description: Affiliation, #[serde(skip)] pub marker: ModelMarker, @@ -102,7 +100,7 @@ impl Default for ModelInstance { Self { name: NameInSite("".to_string()), pose: Pose::default(), - parent: SiteParentID(0), + parent: SiteParent::default(), description: Affiliation::default(), marker: ModelMarker, } @@ -114,7 +112,7 @@ impl ModelInstance { Ok(ModelInstance { name: self.name.clone(), pose: self.pose.clone(), - parent: self.parent.clone(), + parent: self.parent.convert(id_map)?, description: self.description.convert(id_map)?, marker: Default::default(), }) diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index 15c80a38..6b83e45c 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -17,7 +17,10 @@ use crate::*; #[cfg(feature = "bevy")] -use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; +use bevy::{ + ecs::entity::Entity, + prelude::{Bundle, Component, Reflect, ReflectComponent}, +}; use serde::{Deserialize, Serialize}; use std::collections::HashMap; @@ -30,7 +33,7 @@ pub struct ScenarioMarker; #[cfg_attr(feature = "bevy", derive(Component))] pub struct Scenario { pub parent_scenario: Affiliation, - pub added_model_instances: Vec, + pub added_model_instances: Vec<(T, Pose)>, pub removed_model_instances: Vec, pub moved_model_instances: Vec<(T, Pose)>, } @@ -66,11 +69,14 @@ impl Scenario { .added_model_instances .clone() .into_iter() - .map(|id| { - id_map - .get(&id) - .expect("Scenario contains non existent added instance") - .clone() + .map(|(id, pose)| { + ( + id_map + .get(&id) + .expect("Scenario contains non existent moved instance") + .clone(), + pose, + ) }) .collect(), removed_model_instances: self @@ -110,6 +116,21 @@ pub struct ScenarioBundle { 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_model_instances: Vec::new(), + removed_model_instances: Vec::new(), + moved_model_instances: Vec::new(), + }, + marker: ScenarioMarker, + } + } +} + impl Default for ScenarioBundle { fn default() -> Self { Self { diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index 553cdcb8..182b1d4b 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -16,8 +16,7 @@ */ use crate::{ - Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, - Rotation, Site, Swing, + Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing, }; use glam::Vec3; use once_cell::sync::Lazy; @@ -491,7 +490,7 @@ 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_instance_id in &default_scenario.scenario.added_model_instances { + for (model_instance_id, _) in &default_scenario.scenario.added_model_instances { let model_instance = self .model_instances .get(model_instance_id) @@ -508,11 +507,13 @@ impl Site { continue; }; - if model_instance.parent.0 != *level_id { + 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_description_bundle.description.source + if model_description_bundle.source.0 == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) { world.include.push(SdfWorldInclude { @@ -522,7 +523,7 @@ impl Site { ..Default::default() }); added = true; - } else if model_description_bundle.description.source + } else if model_description_bundle.source.0 == AssetSource::Search("OpenRobotics/TeleportDispenser".to_string()) { world.include.push(SdfWorldInclude { @@ -536,10 +537,10 @@ 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_description_bundle.description.is_static.0 { + else if !model_description_bundle.is_static.0 .0 { world.model.push(SdfModel { name: model_instance.name.0.clone(), - r#static: Some(model_description_bundle.description.is_static.0), + r#static: Some(model_description_bundle.is_static.0 .0), pose: Some(model_instance.pose.to_sdf()), link: vec![SdfLink { name: "link".into(), @@ -890,7 +891,7 @@ impl Site { #[cfg(test)] mod tests { - + use crate::legacy::building_map::BuildingMap; #[test] diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 3f65a014..0b4aa14b 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -142,8 +142,10 @@ pub struct Site { /// 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>, } From ce9f3458dc28b0a96435511d9d7381a403044df2 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 17 Jul 2024 17:10:55 +0800 Subject: [PATCH 11/32] functional scenario switching Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/load.rs | 22 +--- rmf_site_editor/src/site/model.rs | 12 +- rmf_site_editor/src/site/scenario.rs | 105 ++++++++++++++++-- rmf_site_editor/src/site/site.rs | 34 +++++- .../src/widgets/inspector/inspect_group.rs | 8 +- rmf_site_editor/src/widgets/view_groups.rs | 2 +- rmf_site_editor/src/widgets/view_scenarios.rs | 7 +- 7 files changed, 138 insertions(+), 52 deletions(-) diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index f839eb60..cfc7c24e 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -17,10 +17,7 @@ use crate::{recency::RecencyRanking, site::*, WorkspaceMarker}; use bevy::{ecs::system::SystemParam, prelude::*}; -use std::{ - collections::{BTreeMap, HashMap}, - path::PathBuf, -}; +use std::{collections::HashMap, path::PathBuf}; use thiserror::Error as ThisError; /// This component is given to the site to keep track of what file it should be @@ -256,27 +253,12 @@ fn generate_site_entities( consider_id(*model_description_id); } - let (default_scenario_id, default_scenario) = site_data - .scenarios - .first_key_value() - .expect("No scenarios found"); - let active_model_instance_ids: std::collections::HashSet = default_scenario - .scenario - .added_model_instances - .iter() - .map(|(id, _)| *id) - .collect(); 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_parent = if active_model_instance_ids.contains(model_instance_id) { - model_instance.parent.0.unwrap_or(site_id) - } else { - site_id - }; let model_instance_entity = commands .spawn(model_instance.clone()) .insert(SiteID(*model_instance_id)) - .set_parent(model_instance_parent) + .set_parent(site_id) .id(); id_to_entity.insert(*model_instance_id, model_instance_entity); consider_id(*model_instance_id); diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 7cf5a59b..d3d64455 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, Selectable, MODEL_PREVIEW_LAYER}, - site::{Category, Group, Members, PreventDeletion, SiteAssets}, + site::{Category, Group, PreventDeletion, SiteAssets}, site_asset_io::MODEL_ENVIRONMENT_VARIABLE, }; use bevy::{ @@ -31,8 +31,7 @@ use rmf_site_format::{ Affiliation, AssetSource, ModelMarker, ModelProperty, NameInSite, Pending, Pose, Scale, }; use smallvec::SmallVec; -use std::{any::TypeId, collections::HashMap}; -use yaserde::xml::name; +use std::any::TypeId; #[derive(Component, Debug, Clone)] pub struct ModelScene { @@ -490,7 +489,7 @@ pub fn propagate_model_render_layers( pub fn update_model_instances( mut commands: Commands, - mut model_properties: Query< + model_properties: Query< (Entity, &NameInSite, Ref>), (With, With), >, @@ -498,7 +497,7 @@ pub fn update_model_instances( ) { for (instance_entity, affiliation) in model_instances.iter() { if let Some(description_entity) = affiliation.0 { - if let Ok((_, name, property)) = model_properties.get(description_entity) { + 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::>(); @@ -508,6 +507,3 @@ pub fn update_model_instances( } } } - -#[derive(Component)] -pub struct Inactive; diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index f320dec4..015b3a23 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -15,35 +15,116 @@ * */ -use crate::site::CurrentScenario; +use crate::{site::CurrentScenario, CurrentWorkspace}; use bevy::prelude::*; -use rmf_site_format::{Group, ModelMarker, NameInSite, Pose}; +use rmf_site_format::{Group, ModelMarker, NameInSite, Pose, Scenario, SiteParent}; +use std::collections::HashMap; #[derive(Clone, Copy, Debug, Event)] pub struct ChangeCurrentScenario(pub Entity); pub fn update_current_scenario( + mut commands: Commands, mut change_current_scenario: EventReader, mut current_scenario: ResMut, - scenarios: Query, + current_workspace: Res, + scenarios: Query<&Scenario>, + mut model_instances: Query< + (Entity, &mut Pose, &SiteParent, &mut Visibility), + (With, Without), + >, ) { - for ChangeCurrentScenario(new_scenario_entity) in change_current_scenario.read() { - *current_scenario = CurrentScenario(Some(*new_scenario_entity)); - println!("Changed scenario"); + 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 and poses in this model + let mut active_model_instances = HashMap::::new(); + for scenario in scenario_stack.iter().rev() { + for (e, pose) in scenario.added_model_instances.iter() { + active_model_instances.insert(*e, pose.clone()); + } + for (e, pose) in scenario.moved_model_instances.iter() { + active_model_instances.insert(*e, pose.clone()); + } + for e in scenario.removed_model_instances.iter() { + active_model_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 model_instances.iter_mut() { + if let Some(new_pose) = active_model_instances.get(&entity) { + commands.entity(entity).set_parent(parent.0.unwrap()); + *pose = new_pose.clone(); + *visibility = Visibility::Inherited; + } else { + commands.entity(entity).set_parent(current_site_entity); + *visibility = Visibility::Hidden; + } + } + + *current_scenario = CurrentScenario(Some(*scenario_entity)); } } pub fn update_scenario_properties( current_scenario: Res, + mut scenarios: Query<&mut Scenario>, changed_models: Query<(Entity, &NameInSite, Ref), (With, Without)>, ) { - for (e, name, pose) in changed_models.iter() { - if pose.is_added() { - println!("Added: {}", name.0); - } + if let Some(mut current_scenario) = current_scenario + .0 + .and_then(|entity| scenarios.get_mut(entity).ok()) + { + for (entity, _, pose) in changed_models.iter() { + if pose.is_changed() { + let existing_added_model = current_scenario + .added_model_instances + .iter_mut() + .find(|(e, _)| *e == entity); + if let Some(existing_added_model) = existing_added_model { + existing_added_model.1 = pose.clone(); + return; + } else if pose.is_added() { + current_scenario + .added_model_instances + .push((entity, pose.clone())); + return; + } - if !pose.is_added() && pose.is_changed() { - println!("Moved: {}", name.0) + let existing_moved_model = current_scenario + .moved_model_instances + .iter_mut() + .find(|(e, _)| *e == entity); + if let Some(existing_moved_model) = existing_moved_model { + existing_moved_model.1 = pose.clone(); + return; + } else { + current_scenario + .moved_model_instances + .push((entity, pose.clone())); + return; + } + } } } } diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs index ae461b5a..56a6129d 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 { @@ -50,15 +54,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, - mut current_scenario: 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) { @@ -139,6 +145,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/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index b7d2368f..64979d93 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -17,8 +17,8 @@ use crate::{ site::{ - Affiliation, AssetSource, Change, DefaultFile, Group, IsStatic, Members, ModelProperty, - NameInSite, Scale, Texture, + Affiliation, AssetSource, Change, DefaultFile, Group, Members, ModelProperty, NameInSite, + Texture, }, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, CurrentWorkspace, @@ -33,8 +33,6 @@ pub struct InspectGroup<'w, 's> { names: Query<'w, 's, &'static NameInSite>, textures: Query<'w, 's, &'static Texture>, model_assets: Query<'w, 's, &'static ModelProperty>, - is_static: Query<'w, 's, &'static ModelProperty>, - scale: Query<'w, 's, &'static ModelProperty>, members: Query<'w, 's, &'static Members>, default_file: Query<'w, 's, &'static DefaultFile>, current_workspace: Res<'w, CurrentWorkspace>, @@ -73,7 +71,7 @@ impl<'w, 's> InspectGroup<'w, 's> { .root .map(|e| self.default_file.get(e).ok()) .flatten(); - if let Ok(ModelProperty(asset)) = self.model_assets.get(id) { + if let Ok(ModelProperty(_)) = self.model_assets.get(id) { ui.label(RichText::new("Model Description Properties").size(18.0)); ui.add_space(10.0); } diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index f404ec08..f536d2d0 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -194,7 +194,7 @@ impl<'w, 's> ViewGroups<'w, 's> { }); for child in children { - let Ok((name, asset_source, site_id)) = q_groups.get(*child) else { + let Ok((name, _, site_id)) = q_groups.get(*child) else { continue; }; let text = site_id diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index 53ef513e..a05d6cb8 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -21,7 +21,7 @@ use crate::{ ScenarioMarker, }, widgets::prelude::*, - AppState, Icons, + Icons, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{Button, CollapsingHeader, Color32, Ui}; @@ -212,7 +212,7 @@ impl<'w, 's> ViewScenarios<'w, 's> { let mut version = 1; self.scenarios .iter() - .filter(|(e, name, scenario)| scenario.parent_scenario.0.is_none()) + .filter(|(_, _, scenario)| scenario.parent_scenario.0.is_none()) .for_each(|(scenario_entity, _, _)| { show_scenario_widget( ui, @@ -244,7 +244,7 @@ fn show_scenario_widget( >, icons: &Res, ) { - let (entity, name, scenario) = q_scenario.get(scenario_entity).unwrap(); + let (entity, name, _) = q_scenario.get(scenario_entity).unwrap(); let scenario_version_str = scenario_version .iter() .map(|v| v.to_string()) @@ -257,7 +257,6 @@ fn show_scenario_widget( change_current_scenario.send(ChangeCurrentScenario(entity)); } ui.colored_label(Color32::DARK_GRAY, scenario_version_str.clone()); - ui.label(name.0.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)); From 97f8dbff2a231f7f76769468bdf31aee0753d556 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 17 Jul 2024 17:19:10 +0800 Subject: [PATCH 12/32] additions Signed-off-by: Reuben Thomas --- rmf_site_format/src/legacy/model.rs | 4 ++-- rmf_site_format/src/misc.rs | 2 +- rmf_site_format/src/model.rs | 9 +++++++-- rmf_site_format/src/scenario.rs | 5 +---- rmf_site_format/src/sdf.rs | 2 +- 5 files changed, 12 insertions(+), 10 deletions(-) diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index b9d428ad..5a0ee072 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,6 +1,6 @@ use crate::{ - Affiliation, Angle, AssetSource, Group, IsStatic, Model as SiteModel, ModelDescriptionBundle, - ModelInstance, ModelMarker, ModelProperty, NameInSite, Pose, Rotation, Scale, SiteParent, + Affiliation, Angle, AssetSource, Group, IsStatic, ModelDescriptionBundle, ModelInstance, + ModelMarker, ModelProperty, NameInSite, Pose, Rotation, Scale, SiteParent, }; use glam::DVec2; use serde::{Deserialize, Serialize}; diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 0cdbb36a..9cec3837 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -15,7 +15,7 @@ * */ -use crate::{RefTrait, Site}; +use crate::RefTrait; #[cfg(feature = "bevy")] use bevy::prelude::*; use glam::{Quat, Vec2, Vec3}; diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index fa3b4746..77b0a927 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -64,9 +64,14 @@ impl Default for Model { /// /// +#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub struct InactiveModelInstanceContainer; + #[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub struct ModelProperty(pub T); +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct ModelProperty(pub T); #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index 6b83e45c..c5de08ef 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -17,10 +17,7 @@ use crate::*; #[cfg(feature = "bevy")] -use bevy::{ - ecs::entity::Entity, - prelude::{Bundle, Component, Reflect, ReflectComponent}, -}; +use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; use std::collections::HashMap; diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index 182b1d4b..5e8f5951 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -435,7 +435,7 @@ impl Site { ..Default::default() }; // Only export default scenario into SDF for now - let (default_scenario_id, default_scenario) = self + let (_, default_scenario) = self .scenarios .first_key_value() .expect("No scenarios found"); From 537ff761be8dc6c741dcdde27c5e941023ee7283 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Thu, 18 Jul 2024 10:48:19 +0800 Subject: [PATCH 13/32] Description inspector prototype Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/mod.rs | 5 + .../widgets/inspector/inspect_asset_source.rs | 11 +- .../inspector/inspect_model_description.rs | 134 ++++++++++++++++++ .../src/widgets/inspector/inspect_scale.rs | 4 +- rmf_site_editor/src/widgets/inspector/mod.rs | 4 + 5 files changed, 153 insertions(+), 5 deletions(-) create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_model_description.rs diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index e3ea8017..ced2bf19 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -269,6 +269,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( 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..d496e4df 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,20 @@ 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>), + // (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_model_description.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs new file mode 100644 index 00000000..78987602 --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + inspector::{InspectAssetSourceComponent, InspectScaleComponent}, + site::{Category, Change, DefaultFile}, + widgets::{prelude::*, Inspect}, + CurrentWorkspace, Icons, WorkspaceMarker, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{ComboBox, Ui}; +use rmf_site_format::{ + Affiliation, AssetSource, Group, IsStatic, ModelMarker, ModelProperty, NameInSite, + RecallAssetSource, Scale, +}; + +#[derive(SystemParam)] +pub struct InspectModelDescription<'w, 's> { + model_instances: Query< + 'w, + 's, + (&'static Category, &'static Affiliation), + (With, Without), + >, + model_descriptions: Query< + 'w, + 's, + ( + Entity, + &'static NameInSite, + &'static ModelProperty, + &'static ModelProperty, + ), + (With, With), + >, + change_affiliation: EventWriter<'w, Change>>, + change_asset_source: EventWriter<'w, Change>>, + change_scale: EventWriter<'w, Change>>, + current_workspace: Res<'w, CurrentWorkspace>, + default_file: Query<'w, 's, &'static DefaultFile>, +} + +impl<'w, 's> WidgetSystem for InspectModelDescription<'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> InspectModelDescription<'w, 's> { + pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { + let Ok((_, current_description_entity)) = self.model_instances.get(id) else { + return; + }; + let Ok(( + current_description_entity, + current_description_name, + current_description_source, + current_description_scale, + )) = self.model_descriptions.get( + current_description_entity + .0 + .expect("Model instances should have valid affiliation"), + ) + else { + return; + }; + + ui.separator(); + ui.label("Model Description"); + + 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)); + } + + // Asset Source + let default_file = self + .current_workspace + .root + .map(|e| self.default_file.get(e).ok()) + .flatten(); + if let Some(new_source) = InspectAssetSourceComponent::new( + ¤t_description_source.0, + &RecallAssetSource::default(), + default_file, + ) + .show(ui) + { + self.change_asset_source.send(Change::new( + ModelProperty(new_source), + current_description_entity, + )); + } + + // Scale + if let Some(new_scale) = InspectScaleComponent::new(¤t_description_scale.0).show(ui) { + self.change_scale.send(Change::new( + ModelProperty(new_scale), + current_description_entity, + )); + } + } +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs index 173dd319..cc733065 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>, } diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 544549de..c5b1f697 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::*; @@ -199,6 +202,7 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), + InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), From b3f232dbd4372e9c26814679ef9ecce187c028fb Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Fri, 19 Jul 2024 10:56:17 +0800 Subject: [PATCH 14/32] organize instance and group properties Signed-off-by: Reuben Thomas --- .../src/widgets/inspector/inspect_group.rs | 45 ++++++++- .../inspector/inspect_model_description.rs | 72 ++------------ rmf_site_editor/src/widgets/inspector/mod.rs | 4 +- rmf_site_editor/src/widgets/mod.rs | 3 + .../src/widgets/properties_panel.rs | 5 +- rmf_site_editor/src/widgets/view_tasks.rs | 70 ++++++++++++++ rmf_site_format/src/lib.rs | 3 + rmf_site_format/src/mobile_robot.rs | 96 +++++++++++++++++++ rmf_site_format/src/model.rs | 5 - 9 files changed, 227 insertions(+), 76 deletions(-) create mode 100644 rmf_site_editor/src/widgets/view_tasks.rs create mode 100644 rmf_site_format/src/mobile_robot.rs diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index 64979d93..335f4c88 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -18,13 +18,14 @@ use crate::{ site::{ Affiliation, AssetSource, Change, DefaultFile, Group, Members, ModelProperty, NameInSite, - Texture, + Scale, Texture, }, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, - CurrentWorkspace, + CurrentWorkspace, InspectAssetSourceComponent, InspectScaleComponent, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{CollapsingHeader, RichText, Ui}; +use rmf_site_format::RecallAssetSource; #[derive(SystemParam)] pub struct InspectGroup<'w, 's> { @@ -32,11 +33,20 @@ pub struct InspectGroup<'w, 's> { affiliation: Query<'w, 's, &'static Affiliation>, names: Query<'w, 's, &'static NameInSite>, textures: Query<'w, 's, &'static Texture>, - model_assets: Query<'w, 's, &'static ModelProperty>, + model_descriptions: Query< + 'w, + 's, + ( + &'static ModelProperty, + &'static ModelProperty, + ), + >, members: Query<'w, 's, &'static Members>, default_file: Query<'w, 's, &'static DefaultFile>, - current_workspace: Res<'w, CurrentWorkspace>, + change_asset_source: EventWriter<'w, Change>>, + change_scale: EventWriter<'w, Change>>, change_texture: EventWriter<'w, Change>, + current_workspace: Res<'w, CurrentWorkspace>, selector: SelectorWidget<'w, 's>, } @@ -71,9 +81,34 @@ impl<'w, 's> InspectGroup<'w, 's> { .root .map(|e| self.default_file.get(e).ok()) .flatten(); - if let Ok(ModelProperty(_)) = self.model_assets.get(id) { + if let Ok((ModelProperty(current_source), ModelProperty(current_scale))) = + self.model_descriptions.get(id) + { ui.label(RichText::new("Model Description Properties").size(18.0)); ui.add_space(10.0); + + // Asset Source + let default_file = self + .current_workspace + .root + .map(|e| self.default_file.get(e).ok()) + .flatten(); + if let Some(new_source) = InspectAssetSourceComponent::new( + ¤t_source, + &RecallAssetSource::default(), + default_file, + ) + .show(ui) + { + self.change_asset_source + .send(Change::new(ModelProperty(new_source), id)); + } + + // Scale + if let Some(new_scale) = InspectScaleComponent::new(¤t_scale).show(ui) { + self.change_scale + .send(Change::new(ModelProperty(new_scale), id)); + } } if let Ok(texture) = self.textures.get(id) { ui.label(RichText::new("Texture Properties").size(18.0)); diff --git a/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs index 78987602..7dbfc8f6 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs @@ -16,17 +16,12 @@ */ use crate::{ - inspector::{InspectAssetSourceComponent, InspectScaleComponent}, - site::{Category, Change, DefaultFile}, + site::{Category, Change}, widgets::{prelude::*, Inspect}, - CurrentWorkspace, Icons, WorkspaceMarker, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{ComboBox, Ui}; -use rmf_site_format::{ - Affiliation, AssetSource, Group, IsStatic, ModelMarker, ModelProperty, NameInSite, - RecallAssetSource, Scale, -}; +use rmf_site_format::{Affiliation, Group, ModelMarker, NameInSite}; #[derive(SystemParam)] pub struct InspectModelDescription<'w, 's> { @@ -36,22 +31,9 @@ pub struct InspectModelDescription<'w, 's> { (&'static Category, &'static Affiliation), (With, Without), >, - model_descriptions: Query< - 'w, - 's, - ( - Entity, - &'static NameInSite, - &'static ModelProperty, - &'static ModelProperty, - ), - (With, With), - >, + model_descriptions: + Query<'w, 's, (Entity, &'static NameInSite), (With, With)>, change_affiliation: EventWriter<'w, Change>>, - change_asset_source: EventWriter<'w, Change>>, - change_scale: EventWriter<'w, Change>>, - current_workspace: Res<'w, CurrentWorkspace>, - default_file: Query<'w, 's, &'static DefaultFile>, } impl<'w, 's> WidgetSystem for InspectModelDescription<'w, 's> { @@ -71,23 +53,16 @@ impl<'w, 's> InspectModelDescription<'w, 's> { let Ok((_, current_description_entity)) = self.model_instances.get(id) else { return; }; - let Ok(( - current_description_entity, - current_description_name, - current_description_source, - current_description_scale, - )) = self.model_descriptions.get( - current_description_entity - .0 - .expect("Model instances should have valid affiliation"), - ) + let Ok((current_description_entity, current_description_name)) = + self.model_descriptions.get( + current_description_entity + .0 + .expect("Model instances should have valid affiliation"), + ) else { return; }; - ui.separator(); - ui.label("Model Description"); - let mut new_description_entity = current_description_entity.clone(); ui.horizontal(|ui| { ui.label("Description"); @@ -103,32 +78,5 @@ impl<'w, 's> InspectModelDescription<'w, 's> { self.change_affiliation .send(Change::new(Affiliation(Some(new_description_entity)), id)); } - - // Asset Source - let default_file = self - .current_workspace - .root - .map(|e| self.default_file.get(e).ok()) - .flatten(); - if let Some(new_source) = InspectAssetSourceComponent::new( - ¤t_description_source.0, - &RecallAssetSource::default(), - default_file, - ) - .show(ui) - { - self.change_asset_source.send(Change::new( - ModelProperty(new_source), - current_description_entity, - )); - } - - // Scale - if let Some(new_scale) = InspectScaleComponent::new(¤t_description_scale.0).show(ui) { - self.change_scale.send(Change::new( - ModelProperty(new_scale), - current_description_entity, - )); - } } } diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index c5b1f697..46d3b17d 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -181,6 +181,7 @@ impl Plugin for StandardInspectorPlugin { app.add_plugins(MinimalInspectorPlugin::default()) .add_plugins(( InspectionPlugin::::new(), + InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), @@ -194,15 +195,14 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectionPlugin::::new(), // Reached the tuple limit )) .add_plugins(( + InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index cd196c4a..303fcddc 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -125,6 +125,9 @@ use view_nav_graphs::*; pub mod view_occupancy; use view_occupancy::*; +pub mod view_tasks; +pub 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 ba92e03f..d6d4fbb9 100644 --- a/rmf_site_editor/src/widgets/properties_panel.rs +++ b/rmf_site_editor/src/widgets/properties_panel.rs @@ -18,8 +18,8 @@ use crate::widgets::{ show_panel_of_tiles, BuildingPreviewPlugin, CreationPlugin, PanelSide, PanelWidget, StandardInspectorPlugin, Tile, ViewGroupsPlugin, ViewLayersPlugin, ViewLevelsPlugin, - ViewLightsPlugin, ViewNavGraphsPlugin, ViewOccupancyPlugin, ViewScenariosPlugin, Widget, - WidgetSystem, + ViewLightsPlugin, ViewNavGraphsPlugin, ViewOccupancyPlugin, ViewScenariosPlugin, + ViewTasksPlugin, Widget, WidgetSystem, }; use bevy::prelude::*; @@ -42,6 +42,7 @@ impl Plugin for StandardPropertiesPanelPlugin { ViewGroupsPlugin::default(), ViewLightsPlugin::default(), ViewOccupancyPlugin::default(), + ViewTasksPlugin::default(), BuildingPreviewPlugin::default(), )); } 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..d275f770 --- /dev/null +++ b/rmf_site_editor/src/widgets/view_tasks.rs @@ -0,0 +1,70 @@ +/* + * 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::{ + Change, ChangeCurrentScenario, CurrentScenario, ModelMarker, NameInSite, Scenario, + ScenarioMarker, + }, + widgets::prelude::*, + Icons, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::{Button, CollapsingHeader, Color32, Ui}; +use rmf_site_format::{Angle, ScenarioBundle, SiteID}; + +#[derive(Default)] +pub struct ViewTasksPlugin {} + +impl Plugin for ViewTasksPlugin { + fn build(&self, app: &mut App) { + app.add_plugins(PropertiesTilePlugin::::new()); + } +} + +#[derive(SystemParam)] +pub struct ViewTasks<'w, 's> { + commands: Commands<'w, 's>, + children: Query<'w, 's, &'static Children>, + scenarios: Query< + 'w, + 's, + (Entity, &'static NameInSite, &'static Scenario), + With, + >, + change_name: EventWriter<'w, Change>, + change_current_scenario: EventWriter<'w, ChangeCurrentScenario>, + current_scenario: ResMut<'w, CurrentScenario>, + model_instances: + Query<'w, 's, (Entity, &'static NameInSite, &'static SiteID), With>, + icons: Res<'w, Icons>, +} + +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(true) + .show(ui, |ui| { + params.show_widget(ui); + }); + } +} + +impl<'w, 's> ViewTasks<'w, 's> { + pub fn show_widget(&mut self, ui: &mut Ui) {} +} diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index 6eee49d7..51db0be6 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::*; diff --git a/rmf_site_format/src/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs new file mode 100644 index 00000000..35074d9f --- /dev/null +++ b/rmf_site_format/src/mobile_robot.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::*; + +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; +use serde::{Deserialize, Serialize}; +use std::collections::HashMap; + +#[derive(Serialize, Deserialize, Clone, PartialEq)] +pub enum Task { + GoToPlace(Pose, SiteParent), + WaitFor(u32), + PickUp(Affiliation), + DropOff(Affiliation), +} + +impl Default for Task { + fn default() -> Self { + Self::WaitFor(0) + } +} + +impl Task { + pub fn convert(&self, id_map: &HashMap) -> Result, T> { + Ok(match self { + Task::GoToPlace(pose, parent) => Task::GoToPlace(pose.clone(), parent.convert(id_map)?), + Task::WaitFor(time) => Task::WaitFor(*time), + Task::PickUp(affiliation) => Task::PickUp(affiliation.convert(id_map)?), + Task::DropOff(affiliation) => Task::DropOff(affiliation.convert(id_map)?), + }) + } +} + +#[derive(Serialize, Deserialize, 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) + }) + } +} + +#[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, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct DifferentialDrive { + pub translaitonal_speed: f32, + pub rotational_speed: f32, + pub bidirectional: bool, + pub rotation_center_offset: [f32; 3], +} + +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct HolonomicDrive { + pub translational_speed: f32, + pub rotational_speed: f32, + pub rotation_center_offset: [f32; 3], +} + +// #[derive(Serialize, Deserialize, Clone, PartialEq)] +// #[cfg_attr(feature = "bevy", derive(Bundle))] +// pub struct MobileRobotBundle { +// pub tasks: Tasks, +// pub drive: T, +// } diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index 77b0a927..68f33000 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -64,11 +64,6 @@ impl Default for Model { /// /// -#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] -#[cfg_attr(feature = "bevy", derive(Component, Reflect))] -#[cfg_attr(feature = "bevy", reflect(Component))] -pub struct InactiveModelInstanceContainer; - #[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] pub struct ModelProperty(pub T); From c284a64b8d67b64c1c206d08e7c4a05e7e5864a2 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Tue, 23 Jul 2024 11:44:34 +0800 Subject: [PATCH 15/32] model description inspector plugin Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/mod.rs | 1 + rmf_site_editor/src/site/model.rs | 6 +- .../src/widgets/inspector/inspect_group.rs | 49 +- .../inspector/inspect_model_description.rs | 429 +++++++++++++++++- .../src/widgets/inspector/inspect_scale.rs | 2 +- rmf_site_editor/src/widgets/inspector/mod.rs | 9 +- rmf_site_format/src/legacy/model.rs | 14 - rmf_site_format/src/model.rs | 5 +- 8 files changed, 427 insertions(+), 88 deletions(-) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index ced2bf19..cb85a583 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -119,6 +119,7 @@ pub use util::*; pub mod wall; pub use wall::*; +use yaserde::ser::Config; use crate::recency::{RecencyRank, RecencyRankingPlugin}; use crate::{AppState, RegisterIssueType}; diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index d3d64455..34c9aaa4 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -28,10 +28,11 @@ use bevy::{ }; use bevy_mod_outline::OutlineMeshExt; use rmf_site_format::{ - Affiliation, AssetSource, ModelMarker, ModelProperty, NameInSite, Pending, Pose, Scale, + Affiliation, AssetSource, DifferentialDrive, ModelMarker, ModelProperty, NameInSite, Pending, + Pose, Scale, }; use smallvec::SmallVec; -use std::any::TypeId; +use std::{any::TypeId, collections::HashMap}; #[derive(Component, Debug, Clone)] pub struct ModelScene { @@ -487,6 +488,7 @@ 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< diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index 335f4c88..7ea4441b 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -16,35 +16,21 @@ */ use crate::{ - site::{ - Affiliation, AssetSource, Change, DefaultFile, Group, Members, ModelProperty, NameInSite, - Scale, Texture, - }, + site::{Affiliation, Change, DefaultFile, Group, Members, ModelMarker, NameInSite, Texture}, widgets::{inspector::InspectTexture, prelude::*, Inspect, SelectorWidget}, - CurrentWorkspace, InspectAssetSourceComponent, InspectScaleComponent, + CurrentWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{CollapsingHeader, RichText, Ui}; -use rmf_site_format::RecallAssetSource; #[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>, - model_descriptions: Query< - 'w, - 's, - ( - &'static ModelProperty, - &'static ModelProperty, - ), - >, members: Query<'w, 's, &'static Members>, default_file: Query<'w, 's, &'static DefaultFile>, - change_asset_source: EventWriter<'w, Change>>, - change_scale: EventWriter<'w, Change>>, change_texture: EventWriter<'w, Change>, current_workspace: Res<'w, CurrentWorkspace>, selector: SelectorWidget<'w, 's>, @@ -81,35 +67,6 @@ impl<'w, 's> InspectGroup<'w, 's> { .root .map(|e| self.default_file.get(e).ok()) .flatten(); - if let Ok((ModelProperty(current_source), ModelProperty(current_scale))) = - self.model_descriptions.get(id) - { - ui.label(RichText::new("Model Description Properties").size(18.0)); - ui.add_space(10.0); - - // Asset Source - let default_file = self - .current_workspace - .root - .map(|e| self.default_file.get(e).ok()) - .flatten(); - if let Some(new_source) = InspectAssetSourceComponent::new( - ¤t_source, - &RecallAssetSource::default(), - default_file, - ) - .show(ui) - { - self.change_asset_source - .send(Change::new(ModelProperty(new_source), id)); - } - - // Scale - if let Some(new_scale) = InspectScaleComponent::new(¤t_scale).show(ui) { - self.change_scale - .send(Change::new(ModelProperty(new_scale), id)); - } - } if let Ok(texture) = self.textures.get(id) { ui.label(RichText::new("Texture Properties").size(18.0)); if let Some(new_texture) = InspectTexture::new(texture, default_file).show(ui) { diff --git a/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs index 7dbfc8f6..abff9b88 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs @@ -16,27 +16,255 @@ */ use crate::{ - site::{Category, Change}, + interaction::Selection, + site::{AssetSource, Change, DefaultFile, ModelProperty, Pending}, widgets::{prelude::*, Inspect}, + CurrentWorkspace, InspectAssetSourceComponent, InspectScaleComponent, MainInspector, }; -use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{ComboBox, Ui}; -use rmf_site_format::{Affiliation, Group, ModelMarker, NameInSite}; +use rmf_site_format::{ + Affiliation, DifferentialDrive, Group, IsStatic, ModelMarker, NameInSite, RecallAssetSource, + Scale, +}; + +use bevy::{ + ecs::{component::ComponentInfo, query::WorldQuery, system::SystemParam}, + prelude::*, +}; +use bevy_egui::egui::{CollapsingHeader, ComboBox, RichText, Ui}; +use smallvec::SmallVec; +use std::{any::TypeId, collections::HashMap}; + +#[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 + } +} + +#[derive(Resource)] +pub struct ModelPropertyDefault(pub ModelProperty); + +/// This resource keeps track of all the properties that can be configured for a model description. +#[derive(Resource)] +pub struct ModelPropertyNames { + pub required: HashMap, + pub optional: HashMap, +} + +impl FromWorld for ModelPropertyNames { + fn from_world(_world: &mut World) -> Self { + let mut required = HashMap::new(); + required.insert( + TypeId::of::>(), + "Asset Source".to_string(), + ); + required.insert(TypeId::of::>(), "Scale".to_string()); + required.insert( + TypeId::of::>(), + "Is Static".to_string(), + ); + + let mut optional = HashMap::new(); + optional.insert( + TypeId::of::>(), + "Differential Drive".to_string(), + ); + Self { required, optional } + } +} + +pub struct InspectModelPropertyPlugin +where + W: WidgetSystem + 'static + Send + Sync, + T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath, +{ + property_name: String, + _property: ModelProperty, + _ignore: std::marker::PhantomData, +} + +impl InspectModelPropertyPlugin +where + W: WidgetSystem + 'static + Send + Sync, + T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath, +{ + pub fn new(property_name: String) -> Self { + Self { + property_name: property_name, + _property: ModelProperty::::default(), + _ignore: Default::default(), + } + } +} + +impl Plugin for InspectModelPropertyPlugin +where + W: WidgetSystem + 'static + Send + Sync, + T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath, +{ + fn build(&self, app: &mut App) { + app.register_type::>(); + // If type has already been loaded as required type, do not allow it to be loaded as optional + let type_id = TypeId::of::>(); + if !app + .world + .resource::() + .required + .contains_key(&type_id) + { + app.world + .resource_mut::() + .optional + .insert(type_id, self.property_name.clone()); + } + + 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)] -pub struct InspectModelDescription<'w, 's> { - model_instances: Query< - 'w, - 's, - (&'static Category, &'static Affiliation), - (With, Without), - >, - model_descriptions: +struct InspectModelDescription<'w, 's> { + model_instances: + Query<'w, 's, &'static Affiliation, (With, Without)>, + model_descriptions: Query<'w, 's, &'static NameInSite, (With, With)>, + model_property_names: Query<'w, 's, (Entity, &'static NameInSite), (With, With)>, - change_affiliation: EventWriter<'w, Change>>, + model_properties: Res<'w, ModelPropertyNames>, + 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 params = state.get_mut(world); + + let mut description_property_types = Vec::::new(); + for component_info in components_info { + if let Some(type_id) = component_info.type_id() { + if params.model_properties.required.contains_key(&type_id) { + description_property_types.push(type_id); + } else if params.model_properties.optional.contains_key(&type_id) { + description_property_types.push(type_id); + } + } + } + let mut available_property_types = Vec::::new(); + for (type_id, _) in params.model_properties.optional.iter() { + if !description_property_types.contains(type_id) { + available_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!("Model Properties of [{}]", description_name)).size(18.0)); + + CollapsingHeader::new("Configure Properties") + .default_open(false) + .show(ui, |ui| { + ui.horizontal_wrapped(|ui| { + for type_id in description_property_types { + let property_name = params.model_properties.required.get(&type_id).unwrap(); + ui.add_enabled_ui(false, |ui| { + ui.toggle_value(&mut true, property_name); + }); + } + + for type_id in available_property_types { + let property_name = params.model_properties.optional.get(&type_id).unwrap(); + if ui.toggle_value(&mut false, property_name).clicked() { + println!("Add property: {:?}", property_name); + } + } + }); + }); + + 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, + }; + let _ = world.try_show_in(child, inspect, ui); + } + } +} + +/// 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)>, + 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, @@ -48,20 +276,17 @@ impl<'w, 's> WidgetSystem for InspectModelDescription<'w, 's> { } } -impl<'w, 's> InspectModelDescription<'w, 's> { +impl<'w, 's> InspectSelectedModelDescription<'w, 's> { pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { - let Ok((_, current_description_entity)) = self.model_instances.get(id) else { - return; - }; - let Ok((current_description_entity, current_description_name)) = - self.model_descriptions.get( - current_description_entity - .0 - .expect("Model instances should have valid affiliation"), - ) + 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(); let mut new_description_entity = current_description_entity.clone(); ui.horizontal(|ui| { @@ -80,3 +305,161 @@ impl<'w, 's> InspectModelDescription<'w, 's> { } } } + +/// Helper function to get the corresponding description entity for a given model instance entity +/// if it exists. +fn get_selected_description_entity<'w, 's, T: WorldQuery>( + selection: Entity, + model_instances: &Query< + 'w, + 's, + &'static Affiliation, + (With, Without), + >, + model_descriptions: &Query<'w, 's, T, (With, With)>, +) -> Option { + match model_descriptions.get(selection) { + Ok(_) => Some(selection), + Err(_) => match model_instances + .get(selection) + .ok() + .and_then(|affiliation| affiliation.0) + { + Some(affiliation) => { + if model_descriptions.get(affiliation).is_ok() { + Some(affiliation) + } else { + warn!("Model instance is affiliated with a non-existent description"); + None + } + } + None => None, + }, + } +} + +/// +/// Basic model properties inspector here for the time being +/// + +#[derive(SystemParam)] +pub struct InspectModelScale<'w, 's> { + model_instances: + Query<'w, 's, &'static Affiliation, (With, Without)>, + 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)); + } + ui.add_space(10.0); + } +} + +#[derive(SystemParam)] +pub struct InspectModelAssetSource<'w, 's> { + model_instances: + Query<'w, 's, &'static Affiliation, (With, Without)>, + 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)); + } + } +} + +#[derive(SystemParam)] +pub struct InspectModelDifferentialDrive<'w, 's> { + model_instances: + Query<'w, 's, &'static Affiliation, (With, Without)>, + model_descriptions: Query< + 'w, + 's, + ( + &'static ModelProperty, + &'static RecallAssetSource, + ), + (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 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; + ui.label("Differential Drive"); + }; + } +} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs index cc733065..e052abfe 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs @@ -25,7 +25,7 @@ use rmf_site_format::{Affiliation, Scale}; #[derive(SystemParam)] pub struct InspectScale<'w, 's> { - scales: Query<'w, 's, (&'static Scale), (Without>)>, + scales: Query<'w, 's, &'static Scale, (Without>)>, change_scale: EventWriter<'w, Change>, } diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 46d3b17d..5a13ad4c 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -181,7 +181,7 @@ impl Plugin for StandardInspectorPlugin { app.add_plugins(MinimalInspectorPlugin::default()) .add_plugins(( InspectionPlugin::::new(), - InspectionPlugin::::new(), + InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), @@ -210,6 +210,13 @@ impl Plugin for StandardInspectorPlugin { InspectLiftPlugin::default(), InspectionPlugin::::new(), InspectionPlugin::::new(), + InspectModelDescriptionPlugin::default(), + )) + .add_plugins(( + InspectModelPropertyPlugin::::new("Scale".to_string()), + InspectModelPropertyPlugin::::new( + "Source".to_string(), + ), )); } } diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index 5a0ee072..09794067 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -28,20 +28,6 @@ impl Model { DVec2::new(self.x, self.y) } - // pub fn to_site(&self) -> SiteModel { - // SiteModel { - // 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(), - // marker: ModelMarker, - // } - // } - pub fn to_site( &self, model_description_name_map: &mut HashMap, diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index 68f33000..ba6b0c97 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -20,7 +20,7 @@ use crate::*; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; -use std::collections::HashMap; +use std::{any::TypeId, collections::HashMap}; #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] @@ -64,10 +64,12 @@ 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, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct ModelDescriptionBundle { @@ -83,6 +85,7 @@ pub struct ModelDescriptionBundle { 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 { From 8cf25e79e3ca04bb911233467303f733cd786fc8 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Tue, 23 Jul 2024 20:21:18 +0800 Subject: [PATCH 16/32] Add differential drive Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/model.rs | 14 + .../inspector/inspect_model_description.rs | 465 ------------------ .../inspect_differential_drive.rs | 129 +++++ .../inspect_required_properties.rs | 120 +++++ .../inspect_model_description/mod.rs | 422 ++++++++++++++++ rmf_site_editor/src/widgets/inspector/mod.rs | 4 + rmf_site_format/src/mobile_robot.rs | 4 +- 7 files changed, 691 insertions(+), 467 deletions(-) delete mode 100644 rmf_site_editor/src/widgets/inspector/inspect_model_description.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_differential_drive.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_required_properties.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 34c9aaa4..992933d7 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -22,6 +22,7 @@ use crate::{ }; use bevy::{ asset::{LoadState, LoadedUntypedAsset}, + ecs::removal_detection::{RemovedComponentEntity, RemovedComponentEvents}, gltf::Gltf, prelude::*, render::view::RenderLayers, @@ -496,7 +497,20 @@ pub fn update_model_instances( (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) { diff --git a/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs b/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs deleted file mode 100644 index abff9b88..00000000 --- a/rmf_site_editor/src/widgets/inspector/inspect_model_description.rs +++ /dev/null @@ -1,465 +0,0 @@ -/* - * Copyright (C) 2023 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -use crate::{ - interaction::Selection, - site::{AssetSource, Change, DefaultFile, ModelProperty, Pending}, - widgets::{prelude::*, Inspect}, - CurrentWorkspace, InspectAssetSourceComponent, InspectScaleComponent, MainInspector, -}; -use rmf_site_format::{ - Affiliation, DifferentialDrive, Group, IsStatic, ModelMarker, NameInSite, RecallAssetSource, - Scale, -}; - -use bevy::{ - ecs::{component::ComponentInfo, query::WorldQuery, system::SystemParam}, - prelude::*, -}; -use bevy_egui::egui::{CollapsingHeader, ComboBox, RichText, Ui}; -use smallvec::SmallVec; -use std::{any::TypeId, collections::HashMap}; - -#[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 - } -} - -#[derive(Resource)] -pub struct ModelPropertyDefault(pub ModelProperty); - -/// This resource keeps track of all the properties that can be configured for a model description. -#[derive(Resource)] -pub struct ModelPropertyNames { - pub required: HashMap, - pub optional: HashMap, -} - -impl FromWorld for ModelPropertyNames { - fn from_world(_world: &mut World) -> Self { - let mut required = HashMap::new(); - required.insert( - TypeId::of::>(), - "Asset Source".to_string(), - ); - required.insert(TypeId::of::>(), "Scale".to_string()); - required.insert( - TypeId::of::>(), - "Is Static".to_string(), - ); - - let mut optional = HashMap::new(); - optional.insert( - TypeId::of::>(), - "Differential Drive".to_string(), - ); - Self { required, optional } - } -} - -pub struct InspectModelPropertyPlugin -where - W: WidgetSystem + 'static + Send + Sync, - T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath, -{ - property_name: String, - _property: ModelProperty, - _ignore: std::marker::PhantomData, -} - -impl InspectModelPropertyPlugin -where - W: WidgetSystem + 'static + Send + Sync, - T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath, -{ - pub fn new(property_name: String) -> Self { - Self { - property_name: property_name, - _property: ModelProperty::::default(), - _ignore: Default::default(), - } - } -} - -impl Plugin for InspectModelPropertyPlugin -where - W: WidgetSystem + 'static + Send + Sync, - T: 'static + Send + Sync + Default + Clone + FromReflect + TypePath, -{ - fn build(&self, app: &mut App) { - app.register_type::>(); - - // If type has already been loaded as required type, do not allow it to be loaded as optional - let type_id = TypeId::of::>(); - if !app - .world - .resource::() - .required - .contains_key(&type_id) - { - app.world - .resource_mut::() - .optional - .insert(type_id, self.property_name.clone()); - } - - 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> { - model_instances: - Query<'w, 's, &'static Affiliation, (With, Without)>, - model_descriptions: Query<'w, 's, &'static NameInSite, (With, With)>, - model_property_names: - Query<'w, 's, (Entity, &'static NameInSite), (With, With)>, - model_properties: Res<'w, ModelPropertyNames>, - 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 params = state.get_mut(world); - - let mut description_property_types = Vec::::new(); - for component_info in components_info { - if let Some(type_id) = component_info.type_id() { - if params.model_properties.required.contains_key(&type_id) { - description_property_types.push(type_id); - } else if params.model_properties.optional.contains_key(&type_id) { - description_property_types.push(type_id); - } - } - } - let mut available_property_types = Vec::::new(); - for (type_id, _) in params.model_properties.optional.iter() { - if !description_property_types.contains(type_id) { - available_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!("Model Properties of [{}]", description_name)).size(18.0)); - - CollapsingHeader::new("Configure Properties") - .default_open(false) - .show(ui, |ui| { - ui.horizontal_wrapped(|ui| { - for type_id in description_property_types { - let property_name = params.model_properties.required.get(&type_id).unwrap(); - ui.add_enabled_ui(false, |ui| { - ui.toggle_value(&mut true, property_name); - }); - } - - for type_id in available_property_types { - let property_name = params.model_properties.optional.get(&type_id).unwrap(); - if ui.toggle_value(&mut false, property_name).clicked() { - println!("Add property: {:?}", property_name); - } - } - }); - }); - - 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, - }; - let _ = world.try_show_in(child, inspect, ui); - } - } -} - -/// 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)>, - 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(); - - 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, T: WorldQuery>( - selection: Entity, - model_instances: &Query< - 'w, - 's, - &'static Affiliation, - (With, Without), - >, - model_descriptions: &Query<'w, 's, T, (With, With)>, -) -> Option { - match model_descriptions.get(selection) { - Ok(_) => Some(selection), - Err(_) => match model_instances - .get(selection) - .ok() - .and_then(|affiliation| affiliation.0) - { - Some(affiliation) => { - if model_descriptions.get(affiliation).is_ok() { - Some(affiliation) - } else { - warn!("Model instance is affiliated with a non-existent description"); - None - } - } - None => None, - }, - } -} - -/// -/// Basic model properties inspector here for the time being -/// - -#[derive(SystemParam)] -pub struct InspectModelScale<'w, 's> { - model_instances: - Query<'w, 's, &'static Affiliation, (With, Without)>, - 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)); - } - ui.add_space(10.0); - } -} - -#[derive(SystemParam)] -pub struct InspectModelAssetSource<'w, 's> { - model_instances: - Query<'w, 's, &'static Affiliation, (With, Without)>, - 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)); - } - } -} - -#[derive(SystemParam)] -pub struct InspectModelDifferentialDrive<'w, 's> { - model_instances: - Query<'w, 's, &'static Affiliation, (With, Without)>, - model_descriptions: Query< - 'w, - 's, - ( - &'static ModelProperty, - &'static RecallAssetSource, - ), - (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 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; - ui.label("Differential Drive"); - }; - } -} 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..fe7e5e20 --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_differential_drive.rs @@ -0,0 +1,129 @@ +/* + * 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}, + 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>>, +} + +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.horizontal(|ui| { + ui.label("Translation Velocity [m/s]:"); + ui.add( + DragValue::new(&mut new_differential_drive.translational_speed) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + }); + ui.label("Max Velocity"); + ui.horizontal(|ui| { + ui.label("Lateral:"); + 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.label("Rotational:"); + 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.horizontal(|ui| { + ui.label("Center Offset"); + ui.label("x"); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[0]) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.label("y"); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[1]) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + }); + + ui.horizontal(|ui| { + ui.label("Bidirectional:"); + if ui + .selectable_label(new_differential_drive.bidirectional, "Enabled") + .clicked() + { + new_differential_drive.bidirectional = true; + } + if ui + .selectable_label(!new_differential_drive.bidirectional, "Disabled") + .clicked() + { + new_differential_drive.bidirectional = false; + } + }); + + 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..40027762 --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/inspect_required_properties.rs @@ -0,0 +1,120 @@ +/* + * 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)); + } + ui.add_space(10.0); + } +} + +#[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..d5161ac7 --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs @@ -0,0 +1,422 @@ +/* + * 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!("Model 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, + }; + 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(); + + 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/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 5a13ad4c..dbae9476 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -110,6 +110,7 @@ pub use inspect_value::*; pub mod inspect_workcell_parent; pub use inspect_workcell_parent::*; +use itertools::Diff; use crate::{ interaction::Selection, @@ -217,6 +218,9 @@ impl Plugin for StandardInspectorPlugin { InspectModelPropertyPlugin::::new( "Source".to_string(), ), + InspectModelPropertyPlugin::::new( + "Differential Drve".to_string(), + ), )); } } diff --git a/rmf_site_format/src/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs index 35074d9f..ab23e823 100644 --- a/rmf_site_format/src/mobile_robot.rs +++ b/rmf_site_format/src/mobile_robot.rs @@ -74,10 +74,10 @@ pub struct MobileRobotMarker; #[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] pub struct DifferentialDrive { - pub translaitonal_speed: f32, + pub translational_speed: f32, pub rotational_speed: f32, pub bidirectional: bool, - pub rotation_center_offset: [f32; 3], + pub rotation_center_offset: [f32; 2], } #[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] From 9c96e3271a5b6c680954f2aeaa1b383f3119d42f Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 24 Jul 2024 11:51:11 +0800 Subject: [PATCH 17/32] deletions Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/deletion.rs | 19 ++- rmf_site_editor/src/site/mod.rs | 2 + rmf_site_editor/src/site/scenario.rs | 72 +++++++++- .../inspect_differential_drive.rs | 124 ++++++++++-------- .../inspect_required_properties.rs | 1 - .../inspect_model_description/mod.rs | 1 + 6 files changed, 157 insertions(+), 62 deletions(-) diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index d57f9af2..3718fc05 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, + RemoveModelInstance, 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, MeshConstraint, ModelMarker, Path, Point, +}; use std::collections::HashSet; // TODO(MXG): Use this module to implement the deletion buffer. The role of the @@ -82,6 +84,8 @@ struct DeletionParams<'w, 's> { edges: Query<'w, 's, &'static Edge>, points: Query<'w, 's, &'static Point>, paths: Query<'w, 's, &'static Path>, + model_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 +94,7 @@ struct DeletionParams<'w, 's> { selection: Res<'w, Selection>, current_level: ResMut<'w, CurrentLevel>, levels: Query<'w, 's, Entity, With>, + remove_model_instance: EventWriter<'w, RemoveModelInstance>, select: EventWriter<'w, Select>, log: EventWriter<'w, Log>, issues: Query<'w, 's, (Entity, &'static mut Issue)>, @@ -137,6 +142,16 @@ fn cautious_delete(element: Entity, params: &mut DeletionParams) { } } + if let Ok(_) = params.model_instances.get(element) { + params + .remove_model_instance + .send(RemoveModelInstance(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/mod.rs b/rmf_site_editor/src/site/mod.rs index cb85a583..dcb92af4 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -211,6 +211,7 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() + .add_event::() .add_event::() .add_event::() .add_event::() @@ -372,6 +373,7 @@ impl Plugin for SitePlugin { add_fiducial_visuals, update_level_visibility, update_scenario_properties, + remove_instances, update_current_scenario, update_changed_lane, update_lane_for_moved_anchor, diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index 015b3a23..b617b6ae 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -15,10 +15,13 @@ * */ -use crate::{site::CurrentScenario, CurrentWorkspace}; +use crate::{ + site::{CurrentScenario, Delete}, + CurrentWorkspace, +}; use bevy::prelude::*; use rmf_site_format::{Group, ModelMarker, NameInSite, Pose, Scenario, SiteParent}; -use std::collections::HashMap; +use std::{collections::HashMap, thread::current}; #[derive(Clone, Copy, Debug, Event)] pub struct ChangeCurrentScenario(pub Entity); @@ -97,7 +100,22 @@ pub fn update_scenario_properties( { for (entity, _, pose) in changed_models.iter() { if pose.is_changed() { - let existing_added_model = current_scenario + let existing_removed_model = current_scenario + .removed_model_instances + .iter_mut() + .find(|e| **e == entity) + .map(|e| e.clone()); + if let Some(existing_removed_model) = existing_removed_model { + current_scenario + .moved_model_instances + .retain(|(e, _)| *e != existing_removed_model); + current_scenario + .added_model_instances + .retain(|(e, _)| *e != existing_removed_model); + return; + } + + let existing_added_model: Option<&mut (Entity, Pose)> = current_scenario .added_model_instances .iter_mut() .find(|(e, _)| *e == entity); @@ -128,3 +146,51 @@ pub fn update_scenario_properties( } } } + +#[derive(Debug, Clone, Copy, Event)] +pub struct RemoveModelInstance(pub Entity); + +pub fn remove_instances( + mut commands: Commands, + mut scenarios: Query<&mut Scenario>, + mut current_scenario: ResMut, + mut change_current_scenario: EventWriter, + mut removals: EventReader, + mut delete: EventWriter, +) { + for removal in removals.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_model_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_model_instances + .iter() + .position(|(e, _)| *e == removal.0) + { + current_scenario.added_model_instances.remove(added_id); + commands.entity(removal.0).remove::(); + delete.send(Delete::new(removal.0)); + return; + } + // Otherwise, remove + current_scenario + .moved_model_instances + .retain(|(e, _)| *e != removal.0); + current_scenario.removed_model_instances.push(removal.0); + change_current_scenario.send(ChangeCurrentScenario(current_scenario_entity)); + } + } +} 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 index fe7e5e20..e61e2a3e 100644 --- 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 @@ -21,7 +21,8 @@ use crate::{ widgets::{prelude::*, Inspect}, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{DragValue, Grid}; +use bevy_egui::egui::{Align, Direction, DragValue, Grid, InnerResponse, Layout}; +use serde::de::value; #[derive(SystemParam)] pub struct InspectModelDifferentialDrive<'w, 's> { @@ -61,63 +62,66 @@ impl<'w, 's> WidgetSystem for InspectModelDifferentialDrive<'w, 's> { ui.label("Differential Drive"); - ui.horizontal(|ui| { - ui.label("Translation Velocity [m/s]:"); - ui.add( - DragValue::new(&mut new_differential_drive.translational_speed) - .clamp_range(0_f32..=std::f32::INFINITY) - .speed(0.01), - ); - }); - ui.label("Max Velocity"); - ui.horizontal(|ui| { - ui.label("Lateral:"); - 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.label("Rotational:"); - 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.set_min_width(ui.available_width()); - ui.horizontal(|ui| { - ui.label("Center Offset"); - ui.label("x"); - ui.add( - DragValue::new(&mut new_differential_drive.rotation_center_offset[0]) - .clamp_range(0_f32..=std::f32::INFINITY) - .speed(0.01), - ); - ui.label("y"); - ui.add( - DragValue::new(&mut new_differential_drive.rotation_center_offset[1]) - .clamp_range(0_f32..=std::f32::INFINITY) - .speed(0.01), - ); - }); + Grid::new("inspect_differential_drive") + .num_columns(2) + .show(ui, |ui| { + // Max Velocity + ui.label("max velocity"); + ui.end_row(); - ui.horizontal(|ui| { - ui.label("Bidirectional:"); - if ui - .selectable_label(new_differential_drive.bidirectional, "Enabled") - .clicked() - { - new_differential_drive.bidirectional = true; - } - if ui - .selectable_label(!new_differential_drive.bidirectional, "Disabled") - .clicked() - { - new_differential_drive.bidirectional = false; - } - }); + value_name(ui, |ui| { + ui.label("translation"); + }); + 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(); + value_name(ui, |ui| { + ui.label("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(); + + // Center Offset + ui.label("center offset"); + ui.end_row(); + value_name(ui, |ui| { + ui.label("x"); + }); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[0]) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.label("m"); + ui.end_row(); + value_name(ui, |ui| { + ui.label("y"); + }); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[1]) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.label("m"); + ui.end_row(); + + // Bidirectional + ui.label("bidirectional"); + ui.end_row(); + value_name(ui, |ui| ui.label("enabled")); + ui.checkbox(&mut new_differential_drive.bidirectional, ""); + }); if new_differential_drive != *differential_drive { params.change_differential_drive.send(Change::new( @@ -127,3 +131,11 @@ impl<'w, 's> WidgetSystem for InspectModelDifferentialDrive<'w, 's> { } } } + +/// Displays a right indented value name +fn value_name(ui: &mut Ui, add_contents: impl FnOnce(&mut Ui) -> R) -> InnerResponse { + ui.with_layout( + Layout::from_main_dir_and_cross_align(Direction::RightToLeft, Align::Max), + add_contents, + ) +} 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 index 40027762..d631588f 100644 --- 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 @@ -63,7 +63,6 @@ impl<'w, 's> WidgetSystem for InspectModelScale<'w, 's> { .change_scale .send(Change::new(ModelProperty(new_scale), description_entity)); } - ui.add_space(10.0); } } 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 index d5161ac7..20589b58 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs @@ -318,6 +318,7 @@ impl<'w, 's> WidgetSystem for InspectModelDescription<'w, 's> { inspection: child, panel, }; + ui.add_space(10.0); let _ = world.try_show_in(child, inspect, ui); } } From f6124a251d786e94c9667dc2eae65c66db24c9d6 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 24 Jul 2024 12:44:22 +0800 Subject: [PATCH 18/32] remove task view Signed-off-by: Reuben Thomas --- rmf_site_editor/src/widgets/creation.rs | 2 +- rmf_site_editor/src/widgets/mod.rs | 3 - .../src/widgets/properties_panel.rs | 5 +- rmf_site_editor/src/widgets/view_tasks.rs | 70 ------------------- 4 files changed, 3 insertions(+), 77 deletions(-) delete mode 100644 rmf_site_editor/src/widgets/view_tasks.rs diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 9166a137..8b99bd42 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -171,7 +171,7 @@ impl<'w, 's> Creation<'w, 's> { AppState::MainMenu | AppState::SiteDrawingEditor | AppState::SiteVisualizer => {} AppState::SiteEditor | AppState::WorkcellEditor => { ui.add_space(10.0); - CollapsingHeader::new("New model") + CollapsingHeader::new("New model description") .default_open(false) .show(ui, |ui| { let default_file = self diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 303fcddc..cd196c4a 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -125,9 +125,6 @@ use view_nav_graphs::*; pub mod view_occupancy; use view_occupancy::*; -pub mod view_tasks; -pub 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 d6d4fbb9..ba92e03f 100644 --- a/rmf_site_editor/src/widgets/properties_panel.rs +++ b/rmf_site_editor/src/widgets/properties_panel.rs @@ -18,8 +18,8 @@ use crate::widgets::{ show_panel_of_tiles, BuildingPreviewPlugin, CreationPlugin, PanelSide, PanelWidget, StandardInspectorPlugin, Tile, ViewGroupsPlugin, ViewLayersPlugin, ViewLevelsPlugin, - ViewLightsPlugin, ViewNavGraphsPlugin, ViewOccupancyPlugin, ViewScenariosPlugin, - ViewTasksPlugin, Widget, WidgetSystem, + ViewLightsPlugin, ViewNavGraphsPlugin, ViewOccupancyPlugin, ViewScenariosPlugin, Widget, + WidgetSystem, }; use bevy::prelude::*; @@ -42,7 +42,6 @@ impl Plugin for StandardPropertiesPanelPlugin { ViewGroupsPlugin::default(), ViewLightsPlugin::default(), ViewOccupancyPlugin::default(), - ViewTasksPlugin::default(), BuildingPreviewPlugin::default(), )); } diff --git a/rmf_site_editor/src/widgets/view_tasks.rs b/rmf_site_editor/src/widgets/view_tasks.rs deleted file mode 100644 index d275f770..00000000 --- a/rmf_site_editor/src/widgets/view_tasks.rs +++ /dev/null @@ -1,70 +0,0 @@ -/* - * 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::{ - Change, ChangeCurrentScenario, CurrentScenario, ModelMarker, NameInSite, Scenario, - ScenarioMarker, - }, - widgets::prelude::*, - Icons, -}; -use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{Button, CollapsingHeader, Color32, Ui}; -use rmf_site_format::{Angle, ScenarioBundle, SiteID}; - -#[derive(Default)] -pub struct ViewTasksPlugin {} - -impl Plugin for ViewTasksPlugin { - fn build(&self, app: &mut App) { - app.add_plugins(PropertiesTilePlugin::::new()); - } -} - -#[derive(SystemParam)] -pub struct ViewTasks<'w, 's> { - commands: Commands<'w, 's>, - children: Query<'w, 's, &'static Children>, - scenarios: Query< - 'w, - 's, - (Entity, &'static NameInSite, &'static Scenario), - With, - >, - change_name: EventWriter<'w, Change>, - change_current_scenario: EventWriter<'w, ChangeCurrentScenario>, - current_scenario: ResMut<'w, CurrentScenario>, - model_instances: - Query<'w, 's, (Entity, &'static NameInSite, &'static SiteID), With>, - icons: Res<'w, Icons>, -} - -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(true) - .show(ui, |ui| { - params.show_widget(ui); - }); - } -} - -impl<'w, 's> ViewTasks<'w, 's> { - pub fn show_widget(&mut self, ui: &mut Ui) {} -} From e84dc58f0280ef7559e2cb7461159e069e18a2dc Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Thu, 25 Jul 2024 12:44:24 +0800 Subject: [PATCH 19/32] edit scenarios Signed-off-by: Reuben Thomas --- rmf_site_editor/src/interaction/cursor.rs | 1 - rmf_site_editor/src/site/load.rs | 1 + rmf_site_editor/src/site/mod.rs | 1 - rmf_site_editor/src/site/model.rs | 6 +- rmf_site_editor/src/site/scenario.rs | 4 +- rmf_site_editor/src/widgets/creation.rs | 515 ++++++++++++------ .../widgets/inspector/inspect_asset_source.rs | 1 + .../inspect_differential_drive.rs | 101 ++-- .../inspect_model_description/mod.rs | 4 + .../src/widgets/inspector/inspect_scale.rs | 5 +- .../src/widgets/inspector/inspect_task.rs | 176 ++++++ rmf_site_editor/src/widgets/inspector/mod.rs | 6 +- rmf_site_editor/src/widgets/view_groups.rs | 1 - rmf_site_editor/src/widgets/view_scenarios.rs | 139 +++-- rmf_site_format/src/misc.rs | 4 +- rmf_site_format/src/mobile_robot.rs | 57 +- rmf_site_format/src/model.rs | 2 +- rmf_site_format/src/scenario.rs | 5 + 18 files changed, 668 insertions(+), 361 deletions(-) create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_task.rs diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 14553e31..565970a5 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -133,7 +133,6 @@ impl Cursor { ) { self.remove_preview(commands); self.preview_model = if let Some(model) = model { - println!("Spawn model instance preview"); let e = commands.spawn(model).insert(Pending).id(); commands.entity(self.frame).push_children(&[e]); Some(e) diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index cfc7c24e..594be650 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -247,6 +247,7 @@ fn generate_site_entities( 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); diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index dcb92af4..cd0b6f78 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -119,7 +119,6 @@ pub use util::*; pub mod wall; pub use wall::*; -use yaserde::ser::Config; use crate::recency::{RecencyRank, RecencyRankingPlugin}; use crate::{AppState, RegisterIssueType}; diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 992933d7..5451fd4b 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -22,18 +22,16 @@ use crate::{ }; use bevy::{ asset::{LoadState, LoadedUntypedAsset}, - ecs::removal_detection::{RemovedComponentEntity, RemovedComponentEvents}, gltf::Gltf, prelude::*, render::view::RenderLayers, }; use bevy_mod_outline::OutlineMeshExt; use rmf_site_format::{ - Affiliation, AssetSource, DifferentialDrive, ModelMarker, ModelProperty, NameInSite, Pending, - Pose, Scale, + Affiliation, AssetSource, ModelMarker, ModelProperty, NameInSite, Pending, Pose, Scale, }; use smallvec::SmallVec; -use std::{any::TypeId, collections::HashMap}; +use std::any::TypeId; #[derive(Component, Debug, Clone)] pub struct ModelScene { diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index b617b6ae..90263af1 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -21,7 +21,7 @@ use crate::{ }; use bevy::prelude::*; use rmf_site_format::{Group, ModelMarker, NameInSite, Pose, Scenario, SiteParent}; -use std::{collections::HashMap, thread::current}; +use std::collections::HashMap; #[derive(Clone, Copy, Debug, Event)] pub struct ChangeCurrentScenario(pub Entity); @@ -153,7 +153,7 @@ pub struct RemoveModelInstance(pub Entity); pub fn remove_instances( mut commands: Commands, mut scenarios: Query<&mut Scenario>, - mut current_scenario: ResMut, + current_scenario: ResMut, mut change_current_scenario: EventWriter, mut removals: EventReader, mut delete: EventWriter, diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 8b99bd42..d75f4523 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, + AppState, CurrentWorkspace, Icons, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{CollapsingHeader, Ui}; +use bevy_egui::egui::{Button, 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, 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,9 +47,9 @@ 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>, + creation_data: ResMut<'w, CreationData>, asset_gallery: Option>, + icons: Res<'w, Icons>, commands: Commands<'w, 's>, } @@ -67,209 +69,368 @@ impl<'w, 's> WidgetSystem for Creation<'w, 's> { } impl<'w, 's> Creation<'w, 's> { + 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 = match *self.creation_data { + CreationData::ModelDescription(ref mut pending_model) => pending_model, + _ => return, + }; + } + } + } + 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 description") - .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.label("Properties"); + 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.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 => { 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() - }; - self.change_mode.send(ChangeMode::To( - SelectAnchor3D::create_new_point() - .for_model(model) - .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(), - )); - } - 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() - }; + + ui.horizontal(|ui| { + if ui + .add_enabled( + self.current_workspace.root.is_some(), + Button::image_and_text(self.icons.add.egui(), "Load"), + ) + .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_collision(workcell_model) + .for_model_instance(model_instance) .into(), )); } - ui.add_space(10.0); } } + if ui + .selectable_label(pending_model.spawn_instance, "With Instance") + .clicked() + { + pending_model.spawn_instance = !pending_model.spawn_instance; + } + ui.text_edit_singleline(&mut pending_model.instance_name); + }); + + ui.add_space(3.0); + if ui.button("Browse fuel").clicked() { + asset_gallery.show = true; + } + } + 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: 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); + } + } } } - }); + } } } #[derive(Resource, Clone, Default)] +enum CreationData { + #[default] + SiteObject, + Drawing(PendingDrawing), + ModelDescription(PendingModel), + ModelInstance(PendingModel), +} + +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(PendingModel::default()), + "Model Instance" => Self::ModelInstance(PendingModel::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)] +#[derive(Clone, Default)] struct PendingModel { + pub name: String, pub source: AssetSource, pub recall_source: RecallAssetSource, pub scale: Scale, + pub spawn_instance: bool, + pub instance_name: String, } 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 d496e4df..a4dfba3d 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs @@ -145,6 +145,7 @@ impl<'a> InspectAssetSourceComponent<'a> { false }; + ui.add_space(3.0); ui.horizontal(|ui| { // Button to load from file, disabled for wasm since there are no local files #[cfg(not(target_arch = "wasm32"))] 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 index e61e2a3e..94cc5e05 100644 --- 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 @@ -21,8 +21,7 @@ use crate::{ widgets::{prelude::*, Inspect}, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{Align, Direction, DragValue, Grid, InnerResponse, Layout}; -use serde::de::value; +use bevy_egui::egui::{DragValue, Grid}; #[derive(SystemParam)] pub struct InspectModelDifferentialDrive<'w, 's> { @@ -61,67 +60,55 @@ impl<'w, 's> WidgetSystem for InspectModelDifferentialDrive<'w, 's> { let mut new_differential_drive = differential_drive.clone(); ui.label("Differential Drive"); + ui.indent("inspect_differential_drive_properties", |ui| { + Grid::new("inspect_differential_drive_speed") + .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.set_min_width(ui.available_width()); - - Grid::new("inspect_differential_drive") - .num_columns(2) - .show(ui, |ui| { - // Max Velocity - ui.label("max velocity"); - ui.end_row(); - - value_name(ui, |ui| { - ui.label("translation"); + 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.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(); - value_name(ui, |ui| { - ui.label("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(); - // Center Offset - ui.label("center offset"); - ui.end_row(); - value_name(ui, |ui| { + Grid::new("inspect_differential_drive_offset") + .num_columns(3) + .show(ui, |ui| { + ui.label("center offset"); ui.label("x"); - }); - ui.add( - DragValue::new(&mut new_differential_drive.rotation_center_offset[0]) - .clamp_range(0_f32..=std::f32::INFINITY) - .speed(0.01), - ); - ui.label("m"); - ui.end_row(); - value_name(ui, |ui| { ui.label("y"); + ui.end_row(); + + ui.label(""); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[0]) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.add( + DragValue::new(&mut new_differential_drive.rotation_center_offset[1]) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); }); - ui.add( - DragValue::new(&mut new_differential_drive.rotation_center_offset[1]) - .clamp_range(0_f32..=std::f32::INFINITY) - .speed(0.01), - ); - ui.label("m"); - ui.end_row(); - // Bidirectional + ui.horizontal(|ui| { ui.label("bidirectional"); - ui.end_row(); - value_name(ui, |ui| ui.label("enabled")); ui.checkbox(&mut new_differential_drive.bidirectional, ""); }); + }); if new_differential_drive != *differential_drive { params.change_differential_drive.send(Change::new( @@ -131,11 +118,3 @@ impl<'w, 's> WidgetSystem for InspectModelDifferentialDrive<'w, 's> { } } } - -/// Displays a right indented value name -fn value_name(ui: &mut Ui, add_contents: impl FnOnce(&mut Ui) -> R) -> InnerResponse { - ui.with_layout( - Layout::from_main_dir_and_cross_align(Direction::RightToLeft, Align::Max), - add_contents, - ) -} 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 index 20589b58..c0062658 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs @@ -371,6 +371,10 @@ impl<'w, 's> InspectSelectedModelDescription<'w, 's> { .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"); diff --git a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs index e052abfe..73441286 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_scale.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_scale.rs @@ -25,7 +25,7 @@ use rmf_site_format::{Affiliation, Scale}; #[derive(SystemParam)] pub struct InspectScale<'w, 's> { - scales: Query<'w, 's, &'static Scale, (Without>)>, + 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..0347c55b --- /dev/null +++ b/rmf_site_editor/src/widgets/inspector/inspect_task.rs @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{ + inspector::InspectPoseComponent, + interaction::Selection, + site::{Change, ChangePlugin, Delete}, + widgets::{prelude::*, Inspect, InspectionPlugin}, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use bevy_egui::egui::Ui; +use rmf_site_format::{ + AssetSource, DifferentialDrive, Group, ModelMarker, Pose, Scale, SimpleTask, +}; + +#[derive(Default)] +pub struct InspectTaskPlugin {} + +impl Plugin for InspectTaskPlugin { + fn build(&self, app: &mut App) { + app.init_resource::().add_plugins(( + ChangePlugin::::default(), + InspectionPlugin::::new(), + )); + } +} + +#[derive(SystemParam)] +pub struct InspectTask<'w, 's> { + commands: Commands<'w, 's>, + selection: Res<'w, Selection>, + change_task: EventWriter<'w, Change>, + change_pose: EventWriter<'w, Change>, + delete: EventWriter<'w, Delete>, + model_instances: Query< + 'w, + 's, + ( + Option<&'static mut SimpleTask>, + &'static mut AssetSource, + Option<&'static mut Scale>, + ), + (With, With, Without), + >, + task_preview: ResMut<'w, TaskPreview>, +} + +impl<'w, 's> WidgetSystem for InspectTask<'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> InspectTask<'w, 's> { + pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { + // Reset viewing previews when selection changes + if self.selection.is_changed() { + if let Some(preview_entity) = self.task_preview.0 { + self.delete.send(Delete::new(preview_entity)); + self.task_preview.0 = None; + } + } + + if let Ok((task, source, scale)) = self.model_instances.get(id) { + // Create new task if it doesn't already exist + let mut new_task = match task { + Some(task) => task.clone(), + None => { + self.commands.entity(id).insert(SimpleTask(None)); + SimpleTask(None) + } + }; + + ui.horizontal(|ui| { + ui.label("Task"); + match new_task.0 { + Some(_) => { + if ui.button("Remove").clicked() { + new_task.0 = None; + } + } + None => { + if ui.button("Add").clicked() { + new_task.0 = Some(Pose::default()); + } + } + } + }); + + ui.add_enabled_ui(new_task.0.is_some(), |ui| { + match &new_task.0 { + Some(pose) => { + if let Some(new_pose) = InspectPoseComponent::new(pose).show(ui) { + new_task.0 = Some(new_pose); + } + } + None => { + InspectPoseComponent::new(&Pose::default()).show(ui); + } + } + + if new_task.0.is_none() { + if let Some(preview_entity) = self.task_preview.0 { + self.delete.send(Delete::new(preview_entity)); + self.task_preview.0 = None; + } + } + if ui + .selectable_label(self.task_preview.0.is_some(), "Preview") + .clicked() + { + match self.task_preview.0 { + Some(preview_entity) => { + self.delete.send(Delete::new(preview_entity)); + self.task_preview.0 = None; + } + None => { + let task_preview_bundle = TaskPreviewBundle { + pose: new_task.0.unwrap_or_default(), + scale: scale.cloned().unwrap_or_default(), + source: source.clone(), + marker: ModelMarker, + }; + let task_preview_entity = self.commands.spawn(task_preview_bundle).id(); + self.task_preview.0 = Some(task_preview_entity); + } + } + } + }); + + if task.is_some_and(|task| task != &new_task) { + println!("Task: {:?}", new_task); + self.change_task.send(Change::new(new_task.clone(), id)); + } + if task.is_some_and(|task| task.0 != new_task.0) { + if let Some(task_preview_entity) = self.task_preview.0 { + if let Some(new_pose) = new_task.0 { + self.change_pose + .send(Change::new(new_pose, task_preview_entity)); + } + } + } + } + } +} + +#[derive(Bundle, Default)] +pub struct TaskPreviewBundle { + pub pose: Pose, + pub scale: Scale, + pub source: AssetSource, + pub marker: ModelMarker, +} + +#[derive(Resource, Clone, Default)] +pub struct TaskPreview(Option); diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index dbae9476..0216c88b 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -102,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::*; @@ -110,7 +113,6 @@ pub use inspect_value::*; pub mod inspect_workcell_parent; pub use inspect_workcell_parent::*; -use itertools::Diff; use crate::{ interaction::Selection, @@ -209,7 +211,7 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectLiftPlugin::default(), - InspectionPlugin::::new(), + InspectTaskPlugin::default(), InspectionPlugin::::new(), InspectModelDescriptionPlugin::default(), )) diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index f536d2d0..d920b67f 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -218,7 +218,6 @@ impl<'w, 's> ViewGroups<'w, 's> { .for_model_instance(model_instance) .into(), )); - println!("Add a new model instance of this group"); }; }; events.selector.show_widget(*child, ui); diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index a05d6cb8..967f3bda 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -17,15 +17,15 @@ use crate::{ site::{ - Change, ChangeCurrentScenario, CurrentScenario, ModelMarker, NameInSite, Scenario, - ScenarioMarker, + Category, Change, ChangeCurrentScenario, CurrentScenario, Delete, ModelMarker, NameInSite, + Scenario, ScenarioMarker, }, widgets::prelude::*, Icons, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{Button, CollapsingHeader, Color32, Ui}; -use rmf_site_format::{Angle, ScenarioBundle, SiteID}; +use rmf_site_format::{Angle, Pose, ScenarioBundle, SiteID}; /// Add a plugin for viewing and editing a list of all levels #[derive(Default)] @@ -52,8 +52,18 @@ pub struct ViewScenarios<'w, 's> { change_current_scenario: EventWriter<'w, ChangeCurrentScenario>, display_scenarios: ResMut<'w, ScenarioDisplay>, current_scenario: ResMut<'w, CurrentScenario>, - model_instances: - Query<'w, 's, (Entity, &'static NameInSite, &'static SiteID), With>, + model_instances: Query< + 'w, + 's, + ( + Entity, + &'static NameInSite, + &'static Category, + &'static SiteID, + ), + With, + >, + delete: EventWriter<'w, Delete>, icons: Res<'w, Icons>, } @@ -72,7 +82,7 @@ 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, scenario)) = self.scenarios.get(current_scenario_entity) { + if let Ok((_, name, scenario)) = self.scenarios.get_mut(current_scenario_entity) { ui.horizontal(|ui| { ui.label("Selected: "); let mut new_name = name.0.clone(); @@ -82,66 +92,78 @@ impl<'w, 's> ViewScenarios<'w, 's> { } }); + fn format_name( + ui: &mut Ui, + name: &NameInSite, + site_id: &SiteID, + category: &Category, + ) { + ui.label(format!("{} #{} [{}]", category.label(), site_id.0, name.0)); + } + fn format_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(), + } + ), + ); + } + ui.label("From Previous:"); - CollapsingHeader::new(format!( - "Added Models: {}", - scenario.added_model_instances.len() - )) - .default_open(false) - .show(ui, |ui| { - for (entity, pose) in scenario.added_model_instances.iter() { - if let Ok((_, name, site_id)) = self.model_instances.get(*entity) { - ui.label(format!("#{} {}", site_id.0, name.0,)); - 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(), + CollapsingHeader::new(format!("Added: {}", scenario.added_model_instances.len())) + .default_open(false) + .show(ui, |ui| { + for (entity, pose) in scenario.added_model_instances.iter() { + if let Ok((_, name, category, site_id)) = + self.model_instances.get(*entity) + { + ui.horizontal(|ui| { + format_name(ui, name, site_id, category); + if ui.button("❌").on_hover_text("Remove instance").clicked() { + self.delete.send(Delete::new(*entity)); } - ), - ); + }); + format_pose(ui, pose); + } } - } - }); - CollapsingHeader::new(format!( - "Moved Models: {}", - scenario.moved_model_instances.len() - )) - .default_open(false) - .show(ui, |ui| { - for (entity, pose) in scenario.moved_model_instances.iter() { - if let Ok((_, name, site_id)) = self.model_instances.get(*entity) { - ui.label(format!("#{} {}", site_id.0, name.0,)); - 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(), - } - ), - ); + }); + CollapsingHeader::new(format!("Moved: {}", scenario.moved_model_instances.len())) + .default_open(false) + .show(ui, |ui| { + for (_id, (entity, pose)) in + scenario.moved_model_instances.iter().enumerate() + { + if let Ok((_, name, category, site_id)) = + self.model_instances.get(*entity) + { + ui.horizontal(|ui| { + format_name(ui, name, site_id, category); + if ui.button("↩").on_hover_text("Undo move").clicked() {} + }); + format_pose(ui, pose); + } } - } - }); + }); CollapsingHeader::new(format!( - "Removed Models: {}", + "Removed: {}", scenario.removed_model_instances.len() )) .default_open(false) .show(ui, |ui| { for entity in scenario.removed_model_instances.iter() { - if let Ok((_, name, site_id)) = self.model_instances.get(*entity) { - ui.label(format!("#{} {}", site_id.0, name.0)); + if let Ok((_, name, category, site_id)) = self.model_instances.get(*entity) + { + ui.horizontal(|ui| { + format_name(ui, name, site_id, category); + if ui.button("↺").on_hover_text("Restore instance").clicked() {} + }); } else { ui.label("Unavailable"); } @@ -194,6 +216,9 @@ impl<'w, 's> ViewScenarios<'w, 's> { 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 @@ -272,7 +297,7 @@ fn show_scenario_widget( "Child Scenarios: {}", children.map(|c| c.len()).unwrap_or(0) )) - .default_open(false) + .default_open(true) .id_source(scenario_version_str.clone()) .show(ui, |ui| { if let Ok(children) = children { diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 9cec3837..c82457eb 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -410,7 +410,7 @@ pub struct SiteID(pub u32); /// 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 SiteParent(pub Option); impl From for SiteParent { @@ -464,7 +464,7 @@ 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 index ab23e823..395414a8 100644 --- a/rmf_site_format/src/mobile_robot.rs +++ b/rmf_site_format/src/mobile_robot.rs @@ -18,53 +18,17 @@ use crate::*; #[cfg(feature = "bevy")] -use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; +use bevy::prelude::{Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; -use std::collections::HashMap; -#[derive(Serialize, Deserialize, Clone, PartialEq)] -pub enum Task { - GoToPlace(Pose, SiteParent), - WaitFor(u32), - PickUp(Affiliation), - DropOff(Affiliation), -} - -impl Default for Task { - fn default() -> Self { - Self::WaitFor(0) - } -} - -impl Task { - pub fn convert(&self, id_map: &HashMap) -> Result, T> { - Ok(match self { - Task::GoToPlace(pose, parent) => Task::GoToPlace(pose.clone(), parent.convert(id_map)?), - Task::WaitFor(time) => Task::WaitFor(*time), - Task::PickUp(affiliation) => Task::PickUp(affiliation.convert(id_map)?), - Task::DropOff(affiliation) => Task::DropOff(affiliation.convert(id_map)?), - }) - } -} - -#[derive(Serialize, Deserialize, Clone, PartialEq)] +#[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] -pub struct Tasks(pub Vec>); - -impl Default for Tasks { - fn default() -> Self { - Self(Vec::new()) - } -} +#[cfg_attr(feature = "bevy", reflect(Component))] +pub struct TaskMarker; -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) - }) - } -} +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +pub struct SimpleTask(pub Option); #[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] @@ -87,10 +51,3 @@ pub struct HolonomicDrive { pub rotational_speed: f32, pub rotation_center_offset: [f32; 3], } - -// #[derive(Serialize, Deserialize, Clone, PartialEq)] -// #[cfg_attr(feature = "bevy", derive(Bundle))] -// pub struct MobileRobotBundle { -// pub tasks: Tasks, -// pub drive: T, -// } diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index ba6b0c97..a158ec35 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -20,7 +20,7 @@ use crate::*; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component, Reflect, ReflectComponent}; use serde::{Deserialize, Serialize}; -use std::{any::TypeId, collections::HashMap}; +use std::collections::HashMap; #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index c5de08ef..15e12786 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -21,6 +21,11 @@ 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 InstaceMarker; + #[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] #[cfg_attr(feature = "bevy", reflect(Component))] From 6d0a7e86f5021f3762950e3ac1f834096a411f67 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Thu, 25 Jul 2024 15:40:47 +0800 Subject: [PATCH 20/32] fix description deletions, instances without parent Signed-off-by: Reuben Thomas --- rmf_site_editor/src/site/load.rs | 22 ++++++++++- rmf_site_editor/src/site/mod.rs | 1 + rmf_site_editor/src/site/model.rs | 31 +++++++++++++-- rmf_site_editor/src/site/scenario.rs | 14 +++++-- rmf_site_editor/src/widgets/creation.rs | 12 ++---- rmf_site_editor/src/widgets/view_groups.rs | 45 ++++++++-------------- 6 files changed, 80 insertions(+), 45 deletions(-) diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index 594be650..14996ad9 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 @@ -243,6 +246,7 @@ 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()) @@ -252,6 +256,7 @@ fn generate_site_entities( .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 { @@ -263,6 +268,20 @@ fn generate_site_entities( .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)); } for (scenario_id, scenario_bundle) in &site_data.scenarios { @@ -297,7 +316,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)?) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index cd0b6f78..e16def69 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -321,6 +321,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::, diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 5451fd4b..9bc96c1d 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, Group, 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,9 +30,6 @@ use bevy::{ render::view::RenderLayers, }; use bevy_mod_outline::OutlineMeshExt; -use rmf_site_format::{ - Affiliation, AssetSource, ModelMarker, ModelProperty, NameInSite, Pending, Pose, Scale, -}; use smallvec::SmallVec; use std::any::TypeId; @@ -521,3 +521,26 @@ pub fn update_model_instances( } } } + +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/scenario.rs b/rmf_site_editor/src/site/scenario.rs index 90263af1..14e44679 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -16,11 +16,13 @@ */ use crate::{ - site::{CurrentScenario, Delete}, + site::{ + CurrentLevel, CurrentScenario, Delete, Group, ModelMarker, NameInSite, Pose, Scenario, + SiteParent, + }, CurrentWorkspace, }; use bevy::prelude::*; -use rmf_site_format::{Group, ModelMarker, NameInSite, Pose, Scenario, SiteParent}; use std::collections::HashMap; #[derive(Clone, Copy, Debug, Event)] @@ -31,6 +33,7 @@ pub fn update_current_scenario( mut change_current_scenario: EventReader, mut current_scenario: ResMut, current_workspace: Res, + current_level: Res, scenarios: Query<&Scenario>, mut model_instances: Query< (Entity, &mut Pose, &SiteParent, &mut Visibility), @@ -76,7 +79,12 @@ pub fn update_current_scenario( // If active, assign parent to level, otherwise assign parent to site for (entity, mut pose, parent, mut visibility) in model_instances.iter_mut() { if let Some(new_pose) = active_model_instances.get(&entity) { - commands.entity(entity).set_parent(parent.0.unwrap()); + 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 { diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index d75f4523..4233a8e2 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -283,13 +283,7 @@ impl<'w, 's> Creation<'w, 's> { ui.add_space(5.0); ui.horizontal(|ui| { - if ui - .add_enabled( - self.current_workspace.root.is_some(), - Button::image_and_text(self.icons.add.egui(), "Load"), - ) - .clicked() - { + if ui.button("➕ Load").clicked() { if let Some(site_entity) = self.current_workspace.root { let model_description_bundle = ModelDescriptionBundle { name: NameInSite(pending_model.name.clone()), @@ -331,7 +325,9 @@ impl<'w, 's> Creation<'w, 's> { { pending_model.spawn_instance = !pending_model.spawn_instance; } - ui.text_edit_singleline(&mut pending_model.instance_name); + ui.add_enabled_ui(pending_model.spawn_instance, |ui| { + ui.text_edit_singleline(&mut pending_model.instance_name); + }); }); ui.add_space(3.0); diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index d920b67f..cdd0e1a7 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -18,8 +18,8 @@ use crate::{ interaction::{ChangeMode, SelectAnchor3D}, site::{ - Affiliation, AssetSource, Change, FiducialMarker, Group, MergeGroups, ModelInstance, - ModelMarker, NameInSite, SiteID, Texture, + Affiliation, AssetSource, Change, Delete, FiducialMarker, Group, MergeGroups, + ModelInstance, ModelMarker, NameInSite, SiteID, Texture, }, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, @@ -42,34 +42,18 @@ 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 Texture>, - Option<&'static SiteID>, - ), - (With, With), - >, + textures: + Query<'w, 's, (&'static NameInSite, Option<&'static SiteID>), (With, With)>, fiducials: Query< 'w, 's, - ( - &'static NameInSite, - Option<&'static AssetSource>, - Option<&'static SiteID>, - ), + (&'static NameInSite, Option<&'static SiteID>), (With, With), >, model_descriptions: Query< 'w, 's, - ( - &'static NameInSite, - Option<&'static AssetSource>, - Option<&'static SiteID>, - ), + (&'static NameInSite, Option<&'static SiteID>), (With, With), >, icons: Res<'w, Icons>, @@ -84,6 +68,7 @@ pub struct ViewGroupsEvents<'w, 's> { 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>, } @@ -146,9 +131,9 @@ impl<'w, 's> ViewGroups<'w, 's> { }); } - fn show_groups<'b, T: Component, S: Component>( + fn show_groups<'b, T: Component>( children: impl IntoIterator, - q_groups: &Query<(&NameInSite, Option<&S>, Option<&SiteID>), (With, With)>, + q_groups: &Query<(&NameInSite, Option<&SiteID>), (With, With)>, mode: &mut GroupViewMode, icons: &Res, events: &mut ViewGroupsEvents, @@ -194,7 +179,7 @@ impl<'w, 's> ViewGroups<'w, 's> { }); for child in children { - let Ok((name, _, site_id)) = q_groups.get(*child) else { + let Ok((name, site_id)) = q_groups.get(*child) else { continue; }; let text = site_id @@ -203,7 +188,7 @@ impl<'w, 's> ViewGroups<'w, 's> { ui.horizontal(|ui| { match mode.clone() { GroupViewMode::View => { - if TypeId::of::() == TypeId::of::() { + if TypeId::of::() == TypeId::of::() { if ui .add(Button::image(icons.add.egui())) .on_hover_text("Add a new model instance of this group") @@ -260,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; + } } } } From 63fb8b92de19a73fbf76f1360797055acbbacbc1 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Fri, 26 Jul 2024 15:11:33 +0800 Subject: [PATCH 21/32] scenarios: fix undos, allow for different instance types Signed-off-by: Reuben Thomas --- rmf_site_editor/src/interaction/cursor.rs | 11 - .../src/interaction/select_anchor.rs | 39 +-- rmf_site_editor/src/site/mobile_robot.rs | 39 +++ rmf_site_editor/src/site/mod.rs | 5 +- rmf_site_editor/src/site/scenario.rs | 92 ++--- rmf_site_editor/src/widgets/creation.rs | 35 +- .../src/widgets/fuel_asset_browser.rs | 29 +- .../src/widgets/inspector/inspect_task.rs | 317 ++++++++++++------ rmf_site_editor/src/widgets/inspector/mod.rs | 4 +- rmf_site_editor/src/widgets/view_scenarios.rs | 232 +++++++++---- rmf_site_format/src/legacy/building_map.rs | 2 +- rmf_site_format/src/legacy/model.rs | 5 +- rmf_site_format/src/mobile_robot.rs | 58 +++- rmf_site_format/src/model.rs | 9 +- rmf_site_format/src/scenario.rs | 38 +-- rmf_site_format/src/sdf.rs | 2 +- 16 files changed, 596 insertions(+), 321 deletions(-) create mode 100644 rmf_site_editor/src/site/mobile_robot.rs diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 565970a5..dec97bf3 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -115,17 +115,6 @@ impl Cursor { } // TODO(luca) reduce duplication here - pub fn set_model_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(); - commands.entity(self.frame).push_children(&[e]); - Some(e) - } else { - None - } - } - pub fn set_model_instance_preview( &mut self, commands: &mut Commands, diff --git a/rmf_site_editor/src/interaction/select_anchor.rs b/rmf_site_editor/src/interaction/select_anchor.rs index d08cac3e..8eab7bc2 100644 --- a/rmf_site_editor/src/interaction/select_anchor.rs +++ b/rmf_site_editor/src/interaction/select_anchor.rs @@ -1187,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); } @@ -1298,15 +1299,6 @@ impl SelectAnchorPointBuilder { } } - pub fn for_model(self, model: Model) -> SelectAnchor3D { - SelectAnchor3D { - bundle: PlaceableObject::Model(model), - parent: None, - target: self.for_element, - continuity: self.continuity, - } - } - pub fn for_model_instance(self, model: ModelInstance) -> SelectAnchor3D { SelectAnchor3D { bundle: PlaceableObject::ModelInstance(model), @@ -1696,7 +1688,6 @@ impl SelectAnchor { #[derive(Clone)] enum PlaceableObject { - Model(Model), ModelInstance(ModelInstance), Anchor, VisualMesh(WorkcellModel), @@ -2162,12 +2153,6 @@ pub fn handle_select_anchor_3d_mode( PlaceableObject::Anchor => { set_visibility(params.cursor.frame_placement, &mut params.visibility, true); } - PlaceableObject::Model(ref m) => { - // Spawn the model as a child of the cursor - params - .cursor - .set_model_preview(&mut params.commands, Some(m.clone())); - } PlaceableObject::ModelInstance(ref m) => { // Spawn the model as a child of the cursor params @@ -2217,26 +2202,6 @@ pub fn handle_select_anchor_3d_mode( NameInWorkcell("Unnamed".to_string()), )) .id(), - PlaceableObject::Model(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) { - let child_id = params.commands.spawn(model).id(); - params - .commands - .spawn(( - AnchorBundle::new(Anchor::Pose3D(pose)) - .dependents(Dependents::single(child_id)), - FrameMarker, - NameInWorkcell("model_root".to_string()), - )) - .add_child(child_id) - .id() - } else { - model.pose = pose; - params.commands.spawn(model).id() - } - } PlaceableObject::ModelInstance(ref a) => { let mut model = a.clone(); // If we are in workcell mode, add a "base link" frame to the model diff --git a/rmf_site_editor/src/site/mobile_robot.rs b/rmf_site_editor/src/site/mobile_robot.rs new file mode 100644 index 00000000..d8213010 --- /dev/null +++ b/rmf_site_editor/src/site/mobile_robot.rs @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2022 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::{DragPlaneBundle, Selectable, MODEL_PREVIEW_LAYER}, + site::{ + Affiliation, AssetSource, Category, CurrentLevel, Group, ModelMarker, ModelProperty, + NameInSite, Pending, Pose, PreventDeletion, Scale, SiteAssets, SiteParent, + }, + site_asset_io::MODEL_ENVIRONMENT_VARIABLE, +}; +use bevy::{ + asset::{LoadState, LoadedUntypedAsset}, + gltf::Gltf, + prelude::*, + render::view::RenderLayers, +}; +use bevy_mod_outline::OutlineMeshExt; +use smallvec::SmallVec; +use std::any::TypeId; + +#[derive(Resource, Clone)] +pub struct PoseGoal { + pub pose: Pose, +} diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index e16def69..09d1954f 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -78,6 +78,9 @@ pub use measurement::*; pub mod model; pub use model::*; +pub mod mobile_robot; +pub use mobile_robot::*; + pub mod nav_graph; pub use nav_graph::*; @@ -372,9 +375,9 @@ impl Plugin for SitePlugin { add_location_visuals, add_fiducial_visuals, update_level_visibility, + update_current_scenario.before(update_scenario_properties), update_scenario_properties, remove_instances, - update_current_scenario, update_changed_lane, update_lane_for_moved_anchor, ) diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index 14e44679..ddcd65e4 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -16,8 +16,9 @@ */ use crate::{ + interaction::Selection, site::{ - CurrentLevel, CurrentScenario, Delete, Group, ModelMarker, NameInSite, Pose, Scenario, + CurrentScenario, Delete, Group, InstanceMarker, ModelMarker, NameInSite, Pose, Scenario, SiteParent, }, CurrentWorkspace, @@ -30,14 +31,14 @@ pub struct ChangeCurrentScenario(pub Entity); pub fn update_current_scenario( mut commands: Commands, + mut selection: ResMut, mut change_current_scenario: EventReader, mut current_scenario: ResMut, current_workspace: Res, - current_level: Res, scenarios: Query<&Scenario>, - mut model_instances: Query< + mut instances: Query< (Entity, &mut Pose, &SiteParent, &mut Visibility), - (With, Without), + With, >, ) { for ChangeCurrentScenario(scenario_entity) in change_current_scenario.read() { @@ -57,17 +58,17 @@ pub fn update_current_scenario( } } - // Iterate stack to identify instances and poses in this model - let mut active_model_instances = HashMap::::new(); + // 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_model_instances.iter() { - active_model_instances.insert(*e, pose.clone()); + for (e, pose) in scenario.added_instances.iter() { + active_instances.insert(*e, pose.clone()); } - for (e, pose) in scenario.moved_model_instances.iter() { - active_model_instances.insert(*e, pose.clone()); + for (e, pose) in scenario.moved_instances.iter() { + active_instances.insert(*e, pose.clone()); } - for e in scenario.removed_model_instances.iter() { - active_model_instances.remove(e); + for e in scenario.removed_instances.iter() { + active_instances.remove(e); } } @@ -77,8 +78,8 @@ pub fn update_current_scenario( }; // If active, assign parent to level, otherwise assign parent to site - for (entity, mut pose, parent, mut visibility) in model_instances.iter_mut() { - if let Some(new_pose) = active_model_instances.get(&entity) { + 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 { @@ -93,6 +94,15 @@ pub fn update_current_scenario( } } + // 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)); } } @@ -100,53 +110,59 @@ pub fn update_current_scenario( pub fn update_scenario_properties( current_scenario: Res, mut scenarios: Query<&mut Scenario>, - changed_models: Query<(Entity, &NameInSite, Ref), (With, Without)>, + mut change_current_scenario: EventReader, + changed_instances: Query<(Entity, Ref), With>, ) { + // 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_models.iter() { + for (entity, pose) in changed_instances.iter() { if pose.is_changed() { - let existing_removed_model = current_scenario - .removed_model_instances + let existing_removed_instance = current_scenario + .removed_instances .iter_mut() .find(|e| **e == entity) .map(|e| e.clone()); - if let Some(existing_removed_model) = existing_removed_model { + if let Some(existing_removed_instance) = existing_removed_instance { current_scenario - .moved_model_instances - .retain(|(e, _)| *e != existing_removed_model); + .moved_instances + .retain(|(e, _)| *e != existing_removed_instance); current_scenario - .added_model_instances - .retain(|(e, _)| *e != existing_removed_model); + .added_instances + .retain(|(e, _)| *e != existing_removed_instance); return; } - let existing_added_model: Option<&mut (Entity, Pose)> = current_scenario - .added_model_instances + let existing_added_instance: Option<&mut (Entity, Pose)> = current_scenario + .added_instances .iter_mut() .find(|(e, _)| *e == entity); - if let Some(existing_added_model) = existing_added_model { - existing_added_model.1 = pose.clone(); + if let Some(existing_added_instance) = existing_added_instance { + existing_added_instance.1 = pose.clone(); return; } else if pose.is_added() { current_scenario - .added_model_instances + .added_instances .push((entity, pose.clone())); return; } - let existing_moved_model = current_scenario - .moved_model_instances + let existing_moved_instance = current_scenario + .moved_instances .iter_mut() .find(|(e, _)| *e == entity); - if let Some(existing_moved_model) = existing_moved_model { - existing_moved_model.1 = pose.clone(); + if let Some(existing_moved_instance) = existing_moved_instance { + existing_moved_instance.1 = pose.clone(); return; } else { current_scenario - .moved_model_instances + .moved_instances .push((entity, pose.clone())); return; } @@ -176,7 +192,7 @@ pub fn remove_instances( // Delete if root scenario if current_scenario.parent_scenario.0.is_none() { current_scenario - .added_model_instances + .added_instances .retain(|(e, _)| *e != removal.0); commands.entity(removal.0).remove::(); delete.send(Delete::new(removal.0)); @@ -184,20 +200,20 @@ pub fn remove_instances( } // Delete if added in this scenario if let Some(added_id) = current_scenario - .added_model_instances + .added_instances .iter() .position(|(e, _)| *e == removal.0) { - current_scenario.added_model_instances.remove(added_id); + current_scenario.added_instances.remove(added_id); commands.entity(removal.0).remove::(); delete.send(Delete::new(removal.0)); return; } // Otherwise, remove current_scenario - .moved_model_instances + .moved_instances .retain(|(e, _)| *e != removal.0); - current_scenario.removed_model_instances.push(removal.0); + current_scenario.removed_instances.push(removal.0); change_current_scenario.send(ChangeCurrentScenario(current_scenario_entity)); } } diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 4233a8e2..2339656a 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -283,7 +283,7 @@ impl<'w, 's> Creation<'w, 's> { ui.add_space(5.0); ui.horizontal(|ui| { - if ui.button("➕ Load").clicked() { + if ui.button("➕").clicked() { if let Some(site_entity) = self.current_workspace.root { let model_description_bundle = ModelDescriptionBundle { name: NameInSite(pending_model.name.clone()), @@ -319,15 +319,32 @@ impl<'w, 's> Creation<'w, 's> { } } } - if ui - .selectable_label(pending_model.spawn_instance, "With Instance") - .clicked() - { - pending_model.spawn_instance = !pending_model.spawn_instance; - } - ui.add_enabled_ui(pending_model.spawn_instance, |ui| { + ComboBox::from_id_source("load_or_load_and_spawn") + .selected_text(if pending_model.spawn_instance { + "Load and Spawn" + } else { + "Load" + }) + .show_ui(ui, |ui| { + if ui + .selectable_label( + pending_model.spawn_instance, + "Load and Spawn", + ) + .clicked() + { + pending_model.spawn_instance = true; + } + if ui + .selectable_label(!pending_model.spawn_instance, "Load") + .clicked() + { + pending_model.spawn_instance = false; + } + }); + if pending_model.spawn_instance { ui.text_edit_singleline(&mut pending_model.instance_name); - }); + } }); ui.add_space(3.0); diff --git a/rmf_site_editor/src/widgets/fuel_asset_browser.rs b/rmf_site_editor/src/widgets/fuel_asset_browser.rs index 0c7a2885..b4be69d0 100644 --- a/rmf_site_editor/src/widgets/fuel_asset_browser.rs +++ b/rmf_site_editor/src/widgets/fuel_asset_browser.rs @@ -17,8 +17,12 @@ use crate::{ interaction::{ChangeMode, ModelPreviewCamera, SelectAnchor3D}, - site::{AssetSource, FuelClient, Model, SetFuelApiKey, UpdateFuelCache}, + site::{ + AssetSource, FuelClient, Group, IsStatic, Model, ModelDescriptionBundle, ModelMarker, + 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,6 +76,7 @@ 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>, @@ -268,16 +273,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_task.rs b/rmf_site_editor/src/widgets/inspector/inspect_task.rs index 0347c55b..9e0ff98a 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_task.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_task.rs @@ -18,48 +18,73 @@ use crate::{ inspector::InspectPoseComponent, interaction::Selection, - site::{Change, ChangePlugin, Delete}, + site::{ + update_model_instances, Affiliation, AssetSource, Change, ChangePlugin, Delete, + DifferentialDrive, Group, MobileRobotMarker, ModelMarker, ModelProperty, Pose, Scale, + SiteParent, Task, Tasks, + }, widgets::{prelude::*, Inspect, InspectionPlugin}, + Icons, ModelPropertyData, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::Ui; -use rmf_site_format::{ - AssetSource, DifferentialDrive, Group, ModelMarker, Pose, Scale, SimpleTask, -}; +use bevy_egui::egui::{Align, Button, Color32, ComboBox, DragValue, Frame, Layout, Stroke, Ui}; +use std::any::TypeId; + +use super::InspectPose; #[derive(Default)] pub struct InspectTaskPlugin {} impl Plugin for InspectTaskPlugin { fn build(&self, app: &mut App) { - app.init_resource::().add_plugins(( - ChangePlugin::::default(), - InspectionPlugin::::new(), + // 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 InspectTask<'w, 's> { +pub struct InspectTasks<'w, 's> { commands: Commands<'w, 's>, selection: Res<'w, Selection>, - change_task: EventWriter<'w, Change>, - change_pose: EventWriter<'w, Change>, - delete: EventWriter<'w, Delete>, - model_instances: Query< - 'w, - 's, - ( - Option<&'static mut SimpleTask>, - &'static mut AssetSource, - Option<&'static mut Scale>, - ), - (With, With, Without), - >, - task_preview: ResMut<'w, TaskPreview>, + change_tasks: EventWriter<'w, Change>>, + mobile_robots: + Query<'w, 's, &'static mut Tasks, (With, Without)>, + pending_task: ResMut<'w, PendingEditTask>, + icons: Res<'w, Icons>, } -impl<'w, 's> WidgetSystem for InspectTask<'w, 's> { +impl<'w, 's> WidgetSystem for InspectTasks<'w, 's> { fn show( Inspect { selection, .. }: Inspect, ui: &mut Ui, @@ -67,110 +92,186 @@ impl<'w, 's> WidgetSystem for InspectTask<'w, 's> { world: &mut World, ) { let mut params = state.get_mut(world); - params.show_widget(selection, ui); - } -} + let Ok(mut tasks) = params.mobile_robots.get_mut(selection) else { + return; + }; -impl<'w, 's> InspectTask<'w, 's> { - pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { - // Reset viewing previews when selection changes - if self.selection.is_changed() { - if let Some(preview_entity) = self.task_preview.0 { - self.delete.send(Delete::new(preview_entity)); - self.task_preview.0 = None; - } - } + ui.label("Tasks"); - if let Ok((task, source, scale)) = self.model_instances.get(id) { - // Create new task if it doesn't already exist - let mut new_task = match task { - Some(task) => task.clone(), - None => { - self.commands.entity(id).insert(SimpleTask(None)); - SimpleTask(None) - } - }; + 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()); - ui.horizontal(|ui| { - ui.label("Task"); - match new_task.0 { - Some(_) => { - if ui.button("Remove").clicked() { - new_task.0 = None; - } - } - None => { - if ui.button("Add").clicked() { - new_task.0 = Some(Pose::default()); - } + if tasks.0.is_empty() { + ui.label("No Tasks"); + } else { + for task in tasks.0.iter_mut() { + edit_task_component(ui, &mut PendingEditTask::from_task(task), true); } } }); - ui.add_enabled_ui(new_task.0.is_some(), |ui| { - match &new_task.0 { - Some(pose) => { - if let Some(new_pose) = InspectPoseComponent::new(pose).show(ui) { - new_task.0 = Some(new_pose); + ui.add_space(10.0); + ui.horizontal(|ui| { + // Only allow adding as task if valid + ui.add_enabled_ui(params.pending_task.to_task().is_some(), |ui| { + if ui + .add(Button::image_and_text(params.icons.add.egui(), "New")) + .clicked() + { + tasks.0.push(params.pending_task.to_task().unwrap()); + } + }); + // Select new task type + ComboBox::from_id_source("pending_edit_task") + .selected_text(params.pending_task.label()) + .show_ui(ui, |ui| { + for label in PendingEditTask::labels() { + if ui + .selectable_label( + label == params.pending_task.label(), + label.to_string(), + ) + .clicked() + { + *params.pending_task = PendingEditTask::from_label(label); } } - None => { - InspectPoseComponent::new(&Pose::default()).show(ui); - } - } + }); + }); + + ui.add_space(10.0); + edit_task_component(ui, &mut params.pending_task, false); + } +} + +/// Returns true if delete +fn edit_task_component(ui: &mut Ui, task: &mut PendingEditTask, with_delete: bool) { + Frame::default() + .inner_margin(4.0) + .fill(Color32::DARK_GRAY) + .rounding(2.0) + .show(ui, |ui| { + ui.set_min_width(ui.available_width()); - if new_task.0.is_none() { - if let Some(preview_entity) = self.task_preview.0 { - self.delete.send(Delete::new(preview_entity)); - self.task_preview.0 = None; + // Header with selection button / simple data + ui.horizontal(|ui| { + ui.label(task.label()); + + match task { + PendingEditTask::GoToPlace(pose, site_parent) => { + ui.selectable_label(true, "Select Goal"); + } + PendingEditTask::WaitFor(duration) => { + ui.add( + DragValue::new(duration) + .clamp_range(0_f32..=std::f32::INFINITY) + .speed(0.01), + ); + ui.label("s"); + } + PendingEditTask::PickUp(_) => { + ui.selectable_label(true, "Model #129"); + } + PendingEditTask::DropOff((_, _)) => { + ui.selectable_label(true, "Model #29"); } } - if ui - .selectable_label(self.task_preview.0.is_some(), "Preview") - .clicked() - { - match self.task_preview.0 { - Some(preview_entity) => { - self.delete.send(Delete::new(preview_entity)); - self.task_preview.0 = None; - } - None => { - let task_preview_bundle = TaskPreviewBundle { - pose: new_task.0.unwrap_or_default(), - scale: scale.cloned().unwrap_or_default(), - source: source.clone(), - marker: ModelMarker, - }; - let task_preview_entity = self.commands.spawn(task_preview_bundle).id(); - self.task_preview.0 = Some(task_preview_entity); + + ui.with_layout(Layout::right_to_left(Align::Center), |ui| { + if with_delete { + if ui.button("❌").on_hover_text("Delete task").clicked() { + *task = PendingEditTask::default(); } } - } + }); }); + }); +} + +#[derive(Resource)] +pub enum PendingEditTask { + GoToPlace(Option, SiteParent), + WaitFor(f32), + PickUp(Option>), + DropOff((Option>, Option)), +} - if task.is_some_and(|task| task != &new_task) { - println!("Task: {:?}", new_task); - self.change_task.send(Change::new(new_task.clone(), id)); +impl Default for PendingEditTask { + fn default() -> Self { + Self::WaitFor(0.0) + } +} + +impl PendingEditTask { + fn from_task(task: &Task) -> Self { + match task { + Task::GoToPlace(pose, parent) => PendingEditTask::GoToPlace(Some(*pose), *parent), + Task::WaitFor(time) => PendingEditTask::WaitFor(*time), + Task::PickUp(affiliation) => PendingEditTask::PickUp(Some(*affiliation)), + Task::DropOff((affiliation, pose)) => { + PendingEditTask::DropOff((Some(*affiliation), Some(*pose))) } - if task.is_some_and(|task| task.0 != new_task.0) { - if let Some(task_preview_entity) = self.task_preview.0 { - if let Some(new_pose) = new_task.0 { - self.change_pose - .send(Change::new(new_pose, task_preview_entity)); - } - } + } + } + + fn to_task(&self) -> Option> { + match self { + PendingEditTask::GoToPlace(Some(pose), parent) => { + Some(Task::GoToPlace(pose.clone(), parent.clone())) } + PendingEditTask::WaitFor(time) => Some(Task::WaitFor(*time)), + PendingEditTask::PickUp(Some(affiliation)) => Some(Task::PickUp(affiliation.clone())), + PendingEditTask::DropOff((Some(affiliation), Some(pose))) => { + Some(Task::DropOff((affiliation.clone(), pose.clone()))) + } + _ => None, } } -} -#[derive(Bundle, Default)] -pub struct TaskPreviewBundle { - pub pose: Pose, - pub scale: Scale, - pub source: AssetSource, - pub marker: ModelMarker, + fn labels() -> Vec<&'static str> { + vec!["Go To Place", "Wait For", "Pick Up", "Drop Off"] + } + + fn label(&self) -> &str { + match self { + PendingEditTask::GoToPlace(_, _) => Self::labels()[0], + PendingEditTask::WaitFor(_) => Self::labels()[1], + PendingEditTask::PickUp(_) => Self::labels()[2], + PendingEditTask::DropOff(_) => Self::labels()[3], + } + } + + fn from_label(label: &str) -> Self { + let labels = Self::labels(); + match labels.iter().position(|&l| l == label) { + Some(0) => PendingEditTask::GoToPlace(None, SiteParent::default()), + Some(1) => PendingEditTask::WaitFor(0.0), + Some(2) => PendingEditTask::PickUp(None), + Some(3) => PendingEditTask::DropOff((None, None)), + _ => PendingEditTask::WaitFor(0.0), + } + } } -#[derive(Resource, Clone, Default)] -pub struct TaskPreview(Option); +/// 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()); + } + } +} diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 0216c88b..f50b2e69 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -201,6 +201,7 @@ impl Plugin for StandardInspectorPlugin { // Reached the tuple limit )) .add_plugins(( + InspectTaskPlugin::default(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), @@ -210,10 +211,9 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectLiftPlugin::default(), - InspectTaskPlugin::default(), InspectionPlugin::::new(), InspectModelDescriptionPlugin::default(), + InspectLiftPlugin::default(), )) .add_plugins(( InspectModelPropertyPlugin::::new("Scale".to_string()), diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index 967f3bda..a526aeef 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -16,16 +16,19 @@ */ use crate::{ + interaction::{Select, Selection}, site::{ - Category, Change, ChangeCurrentScenario, CurrentScenario, Delete, ModelMarker, NameInSite, - Scenario, ScenarioMarker, + Category, Change, ChangeCurrentScenario, CurrentScenario, Delete, NameInSite, Scenario, + ScenarioMarker, }, widgets::prelude::*, Icons, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{Button, CollapsingHeader, Color32, Ui}; -use rmf_site_format::{Angle, Pose, ScenarioBundle, SiteID}; +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)] @@ -45,24 +48,26 @@ pub struct ViewScenarios<'w, 's> { scenarios: Query< 'w, 's, - (Entity, &'static NameInSite, &'static Scenario), + (Entity, &'static NameInSite, &'static mut Scenario), With, >, change_name: EventWriter<'w, Change>, change_current_scenario: EventWriter<'w, ChangeCurrentScenario>, display_scenarios: ResMut<'w, ScenarioDisplay>, current_scenario: ResMut<'w, CurrentScenario>, - model_instances: Query< + instances: Query< 'w, 's, ( Entity, &'static NameInSite, &'static Category, - &'static SiteID, + Option<&'static SiteID>, ), - With, + With, >, + selection: Res<'w, Selection>, + select: EventWriter<'w, Select>, delete: EventWriter<'w, Delete>, icons: Res<'w, Icons>, } @@ -82,7 +87,7 @@ 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, scenario)) = self.scenarios.get_mut(current_scenario_entity) { + 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(); @@ -92,83 +97,102 @@ impl<'w, 's> ViewScenarios<'w, 's> { } }); - fn format_name( - ui: &mut Ui, - name: &NameInSite, - site_id: &SiteID, - category: &Category, - ) { - ui.label(format!("{} #{} [{}]", category.label(), site_id.0, name.0)); - } - fn format_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(), - } - ), - ); - } - ui.label("From Previous:"); - CollapsingHeader::new(format!("Added: {}", scenario.added_model_instances.len())) - .default_open(false) - .show(ui, |ui| { - for (entity, pose) in scenario.added_model_instances.iter() { - if let Ok((_, name, category, site_id)) = - self.model_instances.get(*entity) - { + // Added + collapsing_instance_viewer( + &format!("Added: {}", scenario.added_instances.len()), + ui, + |ui| { + for (entity, pose) in scenario.added_instances.iter() { + if let Ok((_, name, category, site_id)) = self.instances.get(*entity) { ui.horizontal(|ui| { - format_name(ui, name, site_id, category); + 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)); } }); - format_pose(ui, pose); + formatted_pose(ui, pose); } } - }); - CollapsingHeader::new(format!("Moved: {}", scenario.moved_model_instances.len())) - .default_open(false) - .show(ui, |ui| { - for (_id, (entity, pose)) in - scenario.moved_model_instances.iter().enumerate() - { - if let Ok((_, name, category, site_id)) = - self.model_instances.get(*entity) - { + }, + ); + // Moved + let mut undo_moved_ids = Vec::new(); + collapsing_instance_viewer( + &format!("Moved: {}", scenario.moved_instances.len()), + 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| { - format_name(ui, name, site_id, category); - if ui.button("↩").on_hover_text("Undo move").clicked() {} + 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); + } }); - format_pose(ui, pose); + formatted_pose(ui, pose); } } - }); - CollapsingHeader::new(format!( - "Removed: {}", - scenario.removed_model_instances.len() - )) - .default_open(false) - .show(ui, |ui| { - for entity in scenario.removed_model_instances.iter() { - if let Ok((_, name, category, site_id)) = self.model_instances.get(*entity) - { - ui.horizontal(|ui| { - format_name(ui, name, site_id, category); - if ui.button("↺").on_hover_text("Restore instance").clicked() {} - }); - } else { - ui.label("Unavailable"); + }, + ); + // Removed + let mut undo_removed_ids = Vec::new(); + collapsing_instance_viewer( + &format!("Removed: {}", scenario.removed_instances.len()), + 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 { + ui.label("Unavailable"); + } } - } - }); + }, + ); + + // 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"); @@ -264,7 +288,7 @@ fn show_scenario_widget( scenario_version: Vec, q_children: &Query<&'static Children>, q_scenario: &Query< - (Entity, &'static NameInSite, &'static Scenario), + (Entity, &'static NameInSite, &'static mut Scenario), With, >, icons: &Res, @@ -325,6 +349,66 @@ fn show_scenario_widget( }); } +/// Creates a collasible header exposing a scroll area for viewing instances +fn collapsing_instance_viewer( + header_name: &str, + ui: &mut Ui, + add_contents: impl FnOnce(&mut Ui) -> R, +) { + CollapsingHeader::new(header_name) + .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, + mut task_count: &mut u32, +) { + 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()); + + // 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/lib.rs b/rmf_site_format/src/lib.rs index 51db0be6..4f860192 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -116,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/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs index fa5c679a..0130b775 100644 --- a/rmf_site_format/src/mobile_robot.rs +++ b/rmf_site_format/src/mobile_robot.rs @@ -21,81 +21,6 @@ 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(SiteParent), - WaitFor(f32), -} - -impl Default for Task { - fn default() -> Self { - Self::WaitFor(0.0) - } -} - -impl Task { - pub fn convert(&self, id_map: &HashMap) -> Result, T> { - Ok(match self { - Task::GoToPlace(affiliation) => Task::GoToPlace(affiliation.convert(id_map)?), - Task::WaitFor(time) => Task::WaitFor(*time), - }) - } -} - -impl Task { - pub fn labels() -> Vec<&'static str> { - vec!["Go To Place", "Wait For"] - } - - pub fn is_valid(&self) -> bool { - match self { - Task::GoToPlace(affiliation) => affiliation.0.is_some(), - Task::WaitFor(time) => *time >= 0.0, - } - } - - pub fn label(&self) -> &'static str { - match self { - Task::GoToPlace(_) => Self::labels()[0], - Task::WaitFor(_) => Self::labels()[1], - } - } - - pub fn from_label(label: &str) -> Self { - let labels = Self::labels(); - match labels.iter().position(|&l| l == label) { - Some(0) => Task::GoToPlace(SiteParent::(None)), - Some(1) => Task::WaitFor(0.0), - _ => Task::WaitFor(0.0), - } - } -} - -#[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) - }) - } -} - #[derive(Serialize, Deserialize, Debug, Clone, Copy, Default, PartialEq, Eq)] #[cfg_attr(feature = "bevy", derive(Component, Reflect))] #[cfg_attr(feature = "bevy", reflect(Component))] diff --git a/rmf_site_format/src/task.rs b/rmf_site_format/src/task.rs new file mode 100644 index 00000000..511da305 --- /dev/null +++ b/rmf_site_format/src/task.rs @@ -0,0 +1,87 @@ +/* + * 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], + } + } +} + +#[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) + }) + } +} From f7a24beb0e2cc6526063c0858752f56ee9f56fc9 Mon Sep 17 00:00:00 2001 From: Reuben Thomas Date: Wed, 31 Jul 2024 15:41:06 +0800 Subject: [PATCH 26/32] fixes, address comment Signed-off-by: Reuben Thomas --- rmf_site_editor/src/interaction/cursor.rs | 2 +- .../src/interaction/select_anchor.rs | 2 +- rmf_site_editor/src/site/deletion.rs | 15 +++---- rmf_site_editor/src/site/group.rs | 12 +----- rmf_site_editor/src/site/mobile_robot.rs | 39 ------------------- rmf_site_editor/src/site/mod.rs | 5 +-- rmf_site_editor/src/site/model.rs | 10 ++--- rmf_site_editor/src/site/scenario.rs | 19 ++++----- rmf_site_editor/src/site/site.rs | 3 ++ rmf_site_editor/src/widgets/creation.rs | 19 +++++---- .../src/widgets/fuel_asset_browser.rs | 9 ++--- .../widgets/inspector/inspect_asset_source.rs | 1 - .../src/widgets/inspector/inspect_group.rs | 5 ++- .../inspect_model_description/mod.rs | 1 + .../src/widgets/inspector/inspect_task.rs | 2 +- rmf_site_editor/src/widgets/view_groups.rs | 4 +- rmf_site_editor/src/widgets/view_scenarios.rs | 7 +++- rmf_site_editor/src/widgets/view_tasks.rs | 3 +- rmf_site_format/src/location.rs | 1 - rmf_site_format/src/mobile_robot.rs | 2 - 20 files changed, 53 insertions(+), 108 deletions(-) delete mode 100644 rmf_site_editor/src/site/mobile_robot.rs diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index dec97bf3..78df82c4 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::{deferred::RaycastMesh, deferred::RaycastSource, primitives::rays::Ray3d}; use rmf_site_format::{ - FloorMarker, Model, ModelInstance, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel, + FloorMarker, ModelInstance, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel, }; use std::collections::HashSet; diff --git a/rmf_site_editor/src/interaction/select_anchor.rs b/rmf_site_editor/src/interaction/select_anchor.rs index 8eab7bc2..2f950ab3 100644 --- a/rmf_site_editor/src/interaction/select_anchor.rs +++ b/rmf_site_editor/src/interaction/select_anchor.rs @@ -26,7 +26,7 @@ use crate::{ }; use bevy::{ecs::system::SystemParam, prelude::*}; use rmf_site_format::{ - Door, Edge, Fiducial, Floor, FrameMarker, Lane, LiftProperties, Location, Measurement, Model, + Door, Edge, Fiducial, Floor, FrameMarker, Lane, LiftProperties, Location, Measurement, ModelInstance, NameInWorkcell, NameOfSite, Path, PixelsPerMeter, Point, Pose, Side, Wall, WorkcellModel, }; diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index 3718fc05..3f1546aa 100644 --- a/rmf_site_editor/src/site/deletion.rs +++ b/rmf_site_editor/src/site/deletion.rs @@ -20,13 +20,13 @@ use crate::{ log::Log, site::{ Category, CurrentLevel, Dependents, LevelElevation, LevelProperties, NameInSite, - RemoveModelInstance, SiteUpdateSet, + RemoveInstance, SiteUpdateSet, }, AppState, Issue, }; use bevy::{ecs::system::SystemParam, prelude::*}; use rmf_site_format::{ - Affiliation, ConstraintDependents, Edge, Group, MeshConstraint, ModelMarker, Path, Point, + Affiliation, ConstraintDependents, Edge, Group, InstanceMarker, MeshConstraint, Path, Point, }; use std::collections::HashSet; @@ -84,8 +84,7 @@ struct DeletionParams<'w, 's> { edges: Query<'w, 's, &'static Edge>, points: Query<'w, 's, &'static Point>, paths: Query<'w, 's, &'static Path>, - model_instances: - Query<'w, 's, &'static Affiliation, (With, Without)>, + 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>, @@ -94,7 +93,7 @@ struct DeletionParams<'w, 's> { selection: Res<'w, Selection>, current_level: ResMut<'w, CurrentLevel>, levels: Query<'w, 's, Entity, With>, - remove_model_instance: EventWriter<'w, RemoveModelInstance>, + remove_instance: EventWriter<'w, RemoveInstance>, select: EventWriter<'w, Select>, log: EventWriter<'w, Log>, issues: Query<'w, 's, (Entity, &'static mut Issue)>, @@ -142,10 +141,8 @@ fn cautious_delete(element: Entity, params: &mut DeletionParams) { } } - if let Ok(_) = params.model_instances.get(element) { - params - .remove_model_instance - .send(RemoveModelInstance(element)); + if let Ok(_) = params.instances.get(element) { + params.remove_instance.send(RemoveInstance(element)); if **params.selection == Some(element) { params.select.send(Select(None)); } diff --git a/rmf_site_editor/src/site/group.rs b/rmf_site_editor/src/site/group.rs index d6fe2f1f..bba98218 100644 --- a/rmf_site_editor/src/site/group.rs +++ b/rmf_site_editor/src/site/group.rs @@ -27,19 +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() - } - - pub fn len(&self) -> usize { - self.0.len() - } -} - #[derive(Component, Clone, Copy)] struct LastAffiliation(Option); diff --git a/rmf_site_editor/src/site/mobile_robot.rs b/rmf_site_editor/src/site/mobile_robot.rs deleted file mode 100644 index d8213010..00000000 --- a/rmf_site_editor/src/site/mobile_robot.rs +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (C) 2022 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::{DragPlaneBundle, Selectable, MODEL_PREVIEW_LAYER}, - site::{ - Affiliation, AssetSource, Category, CurrentLevel, Group, ModelMarker, ModelProperty, - NameInSite, Pending, Pose, PreventDeletion, Scale, SiteAssets, SiteParent, - }, - site_asset_io::MODEL_ENVIRONMENT_VARIABLE, -}; -use bevy::{ - asset::{LoadState, LoadedUntypedAsset}, - gltf::Gltf, - prelude::*, - render::view::RenderLayers, -}; -use bevy_mod_outline::OutlineMeshExt; -use smallvec::SmallVec; -use std::any::TypeId; - -#[derive(Resource, Clone)] -pub struct PoseGoal { - pub pose: Pose, -} diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 09d1954f..5fb642d4 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -78,9 +78,6 @@ pub use measurement::*; pub mod model; pub use model::*; -pub mod mobile_robot; -pub use mobile_robot::*; - pub mod nav_graph; pub use nav_graph::*; @@ -213,7 +210,7 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() - .add_event::() + .add_event::() .add_event::() .add_event::() .add_event::() diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index 9bc96c1d..d4f274b3 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/rmf_site_editor/src/site/model.rs @@ -179,7 +179,7 @@ pub fn update_model_scenes( ( Entity, &AssetSource, - Option<&Pose>, + &Pose, &TentativeModelFormat, Option<&Visibility>, ), @@ -192,16 +192,12 @@ pub fn update_model_scenes( fn spawn_model( e: Entity, source: &AssetSource, - pose: Option<&Pose>, + pose: &Pose, asset_server: &AssetServer, tentative_format: &TentativeModelFormat, has_visibility: bool, commands: &mut Commands, ) { - let (category, pose) = match pose { - Some(pose) => (Category::Model, pose), - None => (Category::ModelDescription, &Pose::default()), - }; let mut commands = commands.entity(e); commands .insert(ModelScene { @@ -210,7 +206,7 @@ pub fn update_model_scenes( entity: None, }) .insert(TransformBundle::from_transform(pose.transform())) - .insert(category); + .insert(Category::Model); if !has_visibility { // NOTE: We separate this out because for CollisionMeshMarker diff --git a/rmf_site_editor/src/site/scenario.rs b/rmf_site_editor/src/site/scenario.rs index ddcd65e4..989742ec 100644 --- a/rmf_site_editor/src/site/scenario.rs +++ b/rmf_site_editor/src/site/scenario.rs @@ -17,10 +17,7 @@ use crate::{ interaction::Selection, - site::{ - CurrentScenario, Delete, Group, InstanceMarker, ModelMarker, NameInSite, Pose, Scenario, - SiteParent, - }, + site::{CurrentScenario, Delete, InstanceMarker, Pending, Pose, Scenario, SiteParent}, CurrentWorkspace, }; use bevy::prelude::*; @@ -107,11 +104,12 @@ pub fn update_current_scenario( } } +/// 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>, + 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() { @@ -172,14 +170,17 @@ pub fn update_scenario_properties( } #[derive(Debug, Clone, Copy, Event)] -pub struct RemoveModelInstance(pub Entity); +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 remove_instances( mut commands: Commands, mut scenarios: Query<&mut Scenario>, current_scenario: ResMut, mut change_current_scenario: EventWriter, - mut removals: EventReader, + mut removals: EventReader, mut delete: EventWriter, ) { for removal in removals.read() { @@ -194,7 +195,7 @@ pub fn remove_instances( current_scenario .added_instances .retain(|(e, _)| *e != removal.0); - commands.entity(removal.0).remove::(); + commands.entity(removal.0).remove::(); delete.send(Delete::new(removal.0)); return; } @@ -205,7 +206,7 @@ pub fn remove_instances( .position(|(e, _)| *e == removal.0) { current_scenario.added_instances.remove(added_id); - commands.entity(removal.0).remove::(); + commands.entity(removal.0).remove::(); delete.send(Delete::new(removal.0)); return; } diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs index 56a6129d..1fd35de4 100644 --- a/rmf_site_editor/src/site/site.rs +++ b/rmf_site_editor/src/site/site.rs @@ -28,8 +28,11 @@ 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 { + /// What should the current site be pub site: Entity, + /// What should its current level be pub level: Option, + /// What should its current scenario be pub scenario: Option, } diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 2339656a..466c004d 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -20,10 +20,10 @@ use crate::{ interaction::{ChangeMode, SelectAnchor, SelectAnchor3D}, site::{AssetSource, Category, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale}, widgets::{prelude::*, AssetGalleryStatus}, - AppState, CurrentWorkspace, Icons, + AppState, CurrentWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{Button, CollapsingHeader, ComboBox, Grid, Ui}; +use bevy_egui::egui::{CollapsingHeader, ComboBox, Grid, Ui}; use rmf_site_format::{ Affiliation, DrawingProperties, Geometry, Group, IsStatic, ModelDescriptionBundle, @@ -49,7 +49,6 @@ struct Creation<'w, 's> { current_workspace: Res<'w, CurrentWorkspace>, creation_data: ResMut<'w, CreationData>, asset_gallery: Option>, - icons: Res<'w, Icons>, commands: Commands<'w, 's>, } @@ -123,19 +122,19 @@ impl<'w, 's> Creation<'w, 's> { Grid::new("create_site_objects") .num_columns(3) .show(ui, |ui| { - if ui.button("Lane").clicked() { + if ui.button("↔ Lane").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_new_edge_sequence().for_lane().into(), )); } - if ui.button("Location").clicked() { + if ui.button("📌 Location").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_new_point().for_location().into(), )); } - if ui.button("Wall").clicked() { + if ui.button("■ Wall").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_new_edge_sequence().for_wall().into(), )); @@ -143,19 +142,19 @@ impl<'w, 's> Creation<'w, 's> { ui.end_row(); - if ui.button("Door").clicked() { + if ui.button("🚪 Door").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_one_new_edge().for_door().into(), )); } - if ui.button("Lift").clicked() { + if ui.button("⬍ Lift").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_one_new_edge().for_lift().into(), )); } - if ui.button("Floor").clicked() { + if ui.button("✏ Floor").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_new_path().for_floor().into(), )); @@ -163,7 +162,7 @@ impl<'w, 's> Creation<'w, 's> { ui.end_row(); - if ui.button("Fiducial").clicked() { + if ui.button("☉ Fiducial").clicked() { self.change_mode.send(ChangeMode::To( SelectAnchor::create_new_point().for_site_fiducial().into(), )); diff --git a/rmf_site_editor/src/widgets/fuel_asset_browser.rs b/rmf_site_editor/src/widgets/fuel_asset_browser.rs index b4be69d0..a5c349bb 100644 --- a/rmf_site_editor/src/widgets/fuel_asset_browser.rs +++ b/rmf_site_editor/src/widgets/fuel_asset_browser.rs @@ -16,10 +16,10 @@ */ use crate::{ - interaction::{ChangeMode, ModelPreviewCamera, SelectAnchor3D}, + interaction::ModelPreviewCamera, site::{ - AssetSource, FuelClient, Group, IsStatic, Model, ModelDescriptionBundle, ModelMarker, - ModelProperty, NameInSite, Scale, SetFuelApiKey, UpdateFuelCache, + AssetSource, FuelClient, Model, ModelDescriptionBundle, ModelProperty, NameInSite, Scale, + SetFuelApiKey, UpdateFuelCache, }, widgets::prelude::*, CurrentWorkspace, @@ -82,7 +82,6 @@ pub struct FuelAssetBrowser<'w, 's> { 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) { @@ -275,7 +274,7 @@ impl<'w, 's> FuelAssetBrowser<'w, 's> { if let Some(selected) = &gallery_status.selected { if ui.button("Load as Description").clicked() { let model_description = ModelDescriptionBundle { - name: NameInSite({ selected.owner.clone() + "/" + &selected.name }), + name: NameInSite(selected.owner.clone() + "/" + &selected.name), source: ModelProperty(AssetSource::Remote( selected.owner.clone() + "/" + &selected.name + "/model.sdf", )), 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 a4dfba3d..bb76a48b 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs @@ -35,7 +35,6 @@ pub struct InspectAssetSource<'w, 's> { 's, (&'static AssetSource, &'static RecallAssetSource), (Without, Without>), - // (Without), >, change_asset_source: EventWriter<'w, Change>, current_workspace: Res<'w, CurrentWorkspace>, diff --git a/rmf_site_editor/src/widgets/inspector/inspect_group.rs b/rmf_site_editor/src/widgets/inspector/inspect_group.rs index 7ea4441b..092c066b 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_group.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_group.rs @@ -31,8 +31,8 @@ pub struct InspectGroup<'w, 's> { textures: Query<'w, 's, &'static Texture>, members: Query<'w, 's, &'static Members>, default_file: Query<'w, 's, &'static DefaultFile>, - change_texture: EventWriter<'w, Change>, current_workspace: Res<'w, CurrentWorkspace>, + change_texture: EventWriter<'w, Change>, selector: SelectorWidget<'w, 's>, } @@ -53,6 +53,7 @@ impl<'w, 's> InspectGroup<'w, 's> { if self.is_group.contains(id) { self.show_group_properties(id, ui); } + if let Ok(Affiliation(Some(group))) = self.affiliation.get(id) { ui.separator(); let name = self.names.get(*group).map(|n| n.0.as_str()).unwrap_or(""); @@ -61,12 +62,14 @@ impl<'w, 's> InspectGroup<'w, 's> { self.show_group_properties(*group, ui); } } + pub fn show_group_properties(&mut self, id: Entity, ui: &mut Ui) { let default_file = self .current_workspace .root .map(|e| self.default_file.get(e).ok()) .flatten(); + if let Ok(texture) = self.textures.get(id) { ui.label(RichText::new("Texture Properties").size(18.0)); if let Some(new_texture) = InspectTexture::new(texture, default_file).show(ui) { 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 index c0062658..774de867 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_model_description/mod.rs @@ -385,6 +385,7 @@ impl<'w, 's> InspectSelectedModelDescription<'w, 's> { ui.selectable_value(&mut new_description_entity, entity, name.0.as_str()); } }); + if ui.button("Configure").clicked() {} }); if new_description_entity != current_description_entity { self.change_affiliation diff --git a/rmf_site_editor/src/widgets/inspector/inspect_task.rs b/rmf_site_editor/src/widgets/inspector/inspect_task.rs index 961837d9..9aac58ad 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_task.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_task.rs @@ -221,7 +221,7 @@ fn edit_task_component( pub struct PendingTask(Task); impl FromWorld for PendingTask { - fn from_world(world: &mut World) -> Self { + fn from_world(_world: &mut World) -> Self { PendingTask(Task::GoToPlace { location: Point(Entity::PLACEHOLDER), }) diff --git a/rmf_site_editor/src/widgets/view_groups.rs b/rmf_site_editor/src/widgets/view_groups.rs index cdd0e1a7..97dbc390 100644 --- a/rmf_site_editor/src/widgets/view_groups.rs +++ b/rmf_site_editor/src/widgets/view_groups.rs @@ -18,8 +18,8 @@ use crate::{ interaction::{ChangeMode, SelectAnchor3D}, site::{ - Affiliation, AssetSource, Change, Delete, FiducialMarker, Group, MergeGroups, - ModelInstance, ModelMarker, NameInSite, SiteID, Texture, + Affiliation, Change, Delete, FiducialMarker, Group, MergeGroups, ModelInstance, + ModelMarker, NameInSite, SiteID, Texture, }, widgets::{prelude::*, SelectorWidget}, AppState, CurrentWorkspace, Icons, diff --git a/rmf_site_editor/src/widgets/view_scenarios.rs b/rmf_site_editor/src/widgets/view_scenarios.rs index 67424016..0feac8d9 100644 --- a/rmf_site_editor/src/widgets/view_scenarios.rs +++ b/rmf_site_editor/src/widgets/view_scenarios.rs @@ -96,7 +96,6 @@ impl<'w, 's> ViewScenarios<'w, 's> { .send(Change::new(NameInSite(new_name), current_scenario_entity)); } }); - ui.label("From Previous:"); // Added collapsing_instance_viewer( @@ -121,6 +120,8 @@ impl<'w, 's> ViewScenarios<'w, 's> { } }); formatted_pose(ui, pose); + } else { + warn!("Instance entity {:?} does not exist, or has invalid components", entity); } } }, @@ -149,6 +150,8 @@ impl<'w, 's> ViewScenarios<'w, 's> { } }); formatted_pose(ui, pose); + } else { + warn!("Instance entity {:?} does not exist, or has invalid components", entity); } } }, @@ -178,7 +181,7 @@ impl<'w, 's> ViewScenarios<'w, 's> { } }); } else { - ui.label("Unavailable"); + warn!("Instance entity {:?} does not exist, or has invalid components", entity); } } }, diff --git a/rmf_site_editor/src/widgets/view_tasks.rs b/rmf_site_editor/src/widgets/view_tasks.rs index 90356da9..e45ef964 100644 --- a/rmf_site_editor/src/widgets/view_tasks.rs +++ b/rmf_site_editor/src/widgets/view_tasks.rs @@ -108,9 +108,8 @@ fn show_task( site_entities: &Query<(Entity, &NameInSite, &Category, Option<&SiteID>), Without>, selected: &Selection, select: &mut EventWriter