Do not query SiteParent (#260)

* Do not query SiteParent

Signed-off-by: Xiyu Oh <[email protected]>

* Add diagnostic if unable to add to current level

Signed-off-by: Xiyu Oh <[email protected]>

* Fix merge conflicts

Signed-off-by: Michael X. Grey <[email protected]>

* Remove misalignment and fix deviation in commit history

Signed-off-by: Xiyu Oh <[email protected]>

* Use Parented

Signed-off-by: Xiyu Oh <[email protected]>

* Review comments

Signed-off-by: Xiyu Oh <[email protected]>

---------

Signed-off-by: Xiyu Oh <[email protected]>
Signed-off-by: Michael X. Grey <[email protected]>
Co-authored-by: Michael X. Grey <[email protected]>

Author: xiyuoh

rmf_site_editor/src/interaction/cursor.rs

@@ -127,13 +127,11 @@ impl Cursor {
         model_instance: Option<ModelInstance<Entity>>,
     ) {
         self.remove_preview(commands);
-        self.preview_model = model_instance.and_then(|model_instance| {
-            Some(
-                model_loader
-                    .spawn_model_instance(self.frame, model_instance)
-                    .insert(Pending)
-                    .id(),
-            )
+        self.preview_model = model_instance.map(|instance| {
+            model_loader
+                .spawn_model_instance(self.frame, instance)
+                .insert(Pending)
+                .id()
         });
     }
 

rmf_site_editor/src/site/load.rs

@@ -362,16 +362,20 @@ fn generate_site_entities(
         }
     }
 
-    for (model_instance_id, model_instance_data) in &site_data.model_instances {
-        let model_instance = model_instance_data
+    for (model_instance_id, parented_model_instance) in &site_data.model_instances {
+        let model_instance = parented_model_instance
+            .bundle
             .convert(&id_to_entity)
             .for_site(site_id)?;
 
+        // The parent id is invalid, we do not spawn this model instance and generate
+        // an error instead
+        let parent = id_to_entity
+            .get(&parented_model_instance.parent)
+            .ok_or_else(|| LoadSiteError::new(site_id, parented_model_instance.parent))?;
+
         let model_instance_entity = model_loader
-            .spawn_model_instance(
-                model_instance.parent.0.unwrap_or(site_id),
-                model_instance.clone(),
-            )
+            .spawn_model_instance(*parent, model_instance.clone())
             .insert((Category::Model, SiteID(*model_instance_id)))
             .id();
         id_to_entity.insert(*model_instance_id, model_instance_entity);

rmf_site_editor/src/site/mod.rs

@@ -281,6 +281,7 @@ impl Plugin for SitePlugin {
                 check_for_duplicated_dock_names,
                 check_for_fiducials_without_affiliation,
                 check_for_close_unconnected_anchors,
+                check_for_orphan_model_instances,
                 fetch_image_for_texture,
                 detect_last_selected_texture::<FloorMarker>,
                 apply_last_selected_texture::<FloorMarker>
@@ -305,7 +306,6 @@ 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::<DoorMarker>,
                 assign_orphan_elements_to_level::<DrawingMarker>,
                 assign_orphan_elements_to_level::<FloorMarker>,

rmf_site_editor/src/site/model.rs

@@ -17,20 +17,23 @@
 
 use crate::{
     interaction::{DragPlaneBundle, Preview, MODEL_PREVIEW_LAYER},
-    site::{CurrentLevel, SiteAssets, SiteParent},
+    site::SiteAssets,
     site_asset_io::MODEL_ENVIRONMENT_VARIABLE,
+    Issue, ValidateWorkspace,
 };
 use bevy::{
     ecs::system::{EntityCommands, SystemParam},
     gltf::Gltf,
     prelude::*,
     render::view::RenderLayers,
     scene::SceneInstance,
+    utils::Uuid,
 };
 use bevy_impulse::*;
 use bevy_mod_outline::OutlineMeshExt;
 use rmf_site_format::{
-    Affiliation, AssetSource, Group, ModelInstance, ModelMarker, ModelProperty, Pending, Scale,
+    Affiliation, AssetSource, Group, IssueKey, ModelInstance, ModelMarker, ModelProperty,
+    NameInSite, Pending, Scale,
 };
 use smallvec::SmallVec;
 use std::{any::TypeId, collections::HashSet, fmt, future::Future};
@@ -783,25 +786,48 @@ pub fn update_model_instances<T: Component + Default + Clone>(
     }
 }
 
-pub fn assign_orphan_model_instances_to_level(
+/// Unique UUID to identify issue of orphan model instance
+pub const ORPHAN_MODEL_INSTANCE_ISSUE_UUID: Uuid =
+    Uuid::from_u128(0x4e98ce0bc28e4fe528cb0a028f4d5c08u128);
+
+pub fn check_for_orphan_model_instances(
     mut commands: Commands,
+    mut validate_events: EventReader<ValidateWorkspace>,
     mut orphan_instances: Query<
-        (Entity, Option<&Parent>, &mut SiteParent<Entity>),
-        (With<ModelMarker>, Without<Group>),
+        (Entity, &NameInSite, &Affiliation<Entity>),
+        (With<ModelMarker>, Without<Group>, Without<Parent>),
     >,
-    current_level: Res<CurrentLevel>,
+    model_descriptions: Query<&NameInSite, (With<ModelMarker>, With<Group>)>,
 ) {
-    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);
+    for root in validate_events.read() {
+        for (instance_entity, instance_name, affiliation) in orphan_instances.iter_mut() {
+            let brief = match affiliation
+                .0
+                .map(|e| model_descriptions.get(e).ok())
+                .flatten()
+            {
+                Some(description_name) => format!(
+                    "Parent level entity not found for model instance {:?} with \
+                    affiliated model description {:?}",
+                    instance_name, description_name
+                ),
+                None => format!(
+                    "Parent level entity not found for model instance {:?} when saving",
+                    instance_name,
+                ),
+            };
+            let issue = Issue {
+                key: IssueKey {
+                    entities: [instance_entity].into(),
+                    kind: ORPHAN_MODEL_INSTANCE_ISSUE_UUID,
+                },
+                brief,
+                hint: "Model instances need to be assigned to a parent level entity. \
+                      Respawn the orphan model instance"
+                    .to_string(),
+            };
+            let issue_id = commands.spawn(issue).id();
+            commands.entity(**root).add_child(issue_id);
         }
     }
 }

rmf_site_editor/src/site/save.rs

@@ -1250,18 +1250,11 @@ fn generate_model_descriptions(
 fn generate_model_instances(
     site: Entity,
     world: &mut World,
-) -> Result<BTreeMap<u32, ModelInstance<u32>>, SiteGenerationError> {
+) -> Result<BTreeMap<u32, Parented<u32, ModelInstance<u32>>>, SiteGenerationError> {
     let mut state: SystemState<(
         Query<&SiteID, (With<ModelMarker>, With<Group>, Without<Pending>)>,
         Query<
-            (
-                Entity,
-                &SiteID,
-                &NameInSite,
-                &Pose,
-                &SiteParent<Entity>,
-                &Affiliation<Entity>,
-            ),
+            (Entity, &SiteID, &NameInSite, &Pose, &Affiliation<Entity>),
             (With<ModelMarker>, Without<Group>, Without<Pending>),
         >,
         Query<(Entity, &SiteID), With<LevelElevation>>,
@@ -1279,28 +1272,22 @@ fn generate_model_instances(
             site_levels_ids.insert(level_entity, site_id.0);
         }
     }
-    let mut res = BTreeMap::<u32, ModelInstance<u32>>::new();
-    for (
-        _instance_entity,
-        instance_id,
-        instance_name,
-        instance_pose,
-        instance_parent,
-        instance_affiliation,
-    ) in model_instances.iter()
+    let mut res = BTreeMap::<u32, Parented<u32, ModelInstance<u32>>>::new();
+    for (instance_entity, instance_id, instance_name, instance_pose, instance_affiliation) in
+        model_instances.iter()
     {
-        let Ok(parent) = instance_parent
-            .0
-            .map(|p| site_levels_ids.get(&p).copied().ok_or(()))
-            .transpose()
+        let Some(level_id) = parents
+            .get(instance_entity)
+            .ok()
+            .map(|p| site_levels_ids.get(&p.get()).copied())
+            .flatten()
         else {
             error!("Unable to find parent for instance [{}]", instance_name.0);
             continue;
         };
         let mut model_instance = ModelInstance::<u32> {
             name: instance_name.clone(),
             pose: instance_pose.clone(),
-            parent: SiteParent(parent),
             description: Affiliation(
                 instance_affiliation
                     .0
@@ -1309,7 +1296,7 @@ fn generate_model_instances(
             ),
             ..Default::default()
         };
-        if let Ok(robot_tasks) = tasks.get(_instance_entity) {
+        if let Ok(robot_tasks) = tasks.get(instance_entity) {
             let tasks: Vec<Task<u32>> = robot_tasks
                 .0
                 .clone()
@@ -1333,7 +1320,13 @@ fn generate_model_instances(
                 .0
                 .push(OptionalModelProperty::Tasks(Tasks(tasks.clone())));
         }
-        res.insert(instance_id.0, model_instance);
+        res.insert(
+            instance_id.0,
+            Parented {
+                parent: level_id,
+                bundle: model_instance,
+            },
+        );
     }
     Ok(res)
 }

rmf_site_editor/src/site/scenario.rs

@@ -19,7 +19,7 @@ use crate::{
     interaction::{Select, Selection},
     site::{
         Affiliation, CurrentScenario, Delete, DeletionBox, DeletionFilters, Dependents,
-        InstanceMarker, Pending, Pose, Scenario, ScenarioBundle, ScenarioMarker, SiteParent,
+        InstanceMarker, Pending, Pose, Scenario, ScenarioBundle, ScenarioMarker,
     },
     CurrentWorkspace,
 };
@@ -37,10 +37,7 @@ pub fn update_current_scenario(
     mut current_scenario: ResMut<CurrentScenario>,
     current_workspace: Res<CurrentWorkspace>,
     scenarios: Query<&Scenario<Entity>>,
-    mut instances: Query<
-        (Entity, &mut Pose, &SiteParent<Entity>, &mut Visibility),
-        With<InstanceMarker>,
-    >,
+    mut instances: Query<(Entity, &mut Pose, &mut Visibility), With<InstanceMarker>>,
 ) {
     if let Some(ChangeCurrentScenario(scenario_entity)) = change_current_scenario.read().last() {
         // Used to build a scenario from root
@@ -79,14 +76,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 instances.iter_mut() {
+        for (entity, mut pose, mut visibility) in instances.iter_mut() {
             if let Some(new_pose) = active_instances.get(&entity) {
-                if let Some(parent_entity) = parent.0 {
-                    commands.entity(entity).set_parent(parent_entity);
-                } else {
-                    commands.entity(entity).set_parent(current_site_entity);
-                    warn!("Model instance {:?} has no valid site parent", entity);
-                }
                 *pose = new_pose.clone();
                 *visibility = Visibility::Inherited;
             } else {

rmf_site_format/src/legacy/building_map.rs

@@ -7,8 +7,8 @@ use crate::{
     Drawing as SiteDrawing, DrawingProperties, Fiducial as SiteFiducial, FiducialGroup,
     FiducialMarker, Guided, Lane as SiteLane, LaneMarker, Level as SiteLevel, LevelElevation,
     LevelProperties as SiteLevelProperties, ModelDescriptionBundle, ModelInstance, Motion,
-    NameInSite, NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose,
-    PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, ScenarioBundle, Site,
+    NameInSite, NameOfSite, NavGraph, Navigation, OrientationConstraint, Parented, PixelsPerMeter,
+    Pose, PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, ScenarioBundle, Site,
     SiteProperties, Texture as SiteTexture, TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS,
 };
 use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot};
@@ -203,7 +203,7 @@ impl BuildingMap {
         let mut cartesian_fiducials: HashMap<u32, Vec<DVec2>> = HashMap::new();
 
         let mut model_descriptions: BTreeMap<u32, ModelDescriptionBundle<u32>> = BTreeMap::new();
-        let mut model_instances: BTreeMap<u32, ModelInstance<u32>> = BTreeMap::new();
+        let mut model_instances: BTreeMap<u32, Parented<u32, ModelInstance<u32>>> = BTreeMap::new();
         let mut model_description_name_map = HashMap::<String, u32>::new();
         let mut scenarios: BTreeMap<u32, ScenarioBundle<u32>> = BTreeMap::new();
         let default_scenario_id = site_id.next().unwrap();

rmf_site_format/src/legacy/model.rs

@@ -1,6 +1,6 @@
 use crate::{
     Affiliation, Angle, AssetSource, InstanceMarker, IsStatic, ModelDescriptionBundle,
-    ModelInstance, ModelMarker, ModelProperty, NameInSite, Pose, Rotation, Scale, SiteParent,
+    ModelInstance, ModelMarker, ModelProperty, NameInSite, Parented, Pose, Rotation, Scale,
 };
 use glam::DVec2;
 use serde::{Deserialize, Serialize};
@@ -33,7 +33,7 @@ impl Model {
         &self,
         model_description_name_map: &mut HashMap<String, u32>,
         model_descriptions: &mut BTreeMap<u32, ModelDescriptionBundle<u32>>,
-        model_instances: &mut BTreeMap<u32, ModelInstance<u32>>,
+        model_instances: &mut BTreeMap<u32, Parented<u32, ModelInstance<u32>>>,
         site_id: &mut RangeFrom<u32>,
         level_id: u32,
     ) -> (u32, Pose) {
@@ -64,13 +64,18 @@ impl Model {
         let model_instance = ModelInstance {
             name: NameInSite(self.instance_name.clone()),
             pose: pose.clone(),
-            parent: SiteParent(Some(level_id)),
             description: Affiliation(Some(model_description_id)),
             marker: ModelMarker,
             instance_marker: InstanceMarker,
             ..Default::default()
         };
-        model_instances.insert(model_instance_id, model_instance);
+        model_instances.insert(
+            model_instance_id,
+            Parented {
+                parent: level_id,
+                bundle: model_instance,
+            },
+        );
         (model_instance_id, pose)
     }
 }

rmf_site_format/src/misc.rs

@@ -472,38 +472,11 @@ pub struct PreviewableMarker;
 #[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))]
 pub struct SiteID(pub u32);
 
-/// This component is applied to an entity as a reference to its parent entity.
-#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
-#[serde(transparent)]
-#[cfg_attr(feature = "bevy", derive(Component, Reflect))]
-pub struct SiteParent<T: RefTrait>(pub Option<T>);
-
-impl<T: RefTrait> From<T> for SiteParent<T> {
-    fn from(value: T) -> Self {
-        SiteParent(Some(value))
-    }
-}
-
-impl<T: RefTrait> From<Option<T>> for SiteParent<T> {
-    fn from(value: Option<T>) -> Self {
-        SiteParent(value)
-    }
-}
-
-impl<T: RefTrait> Default for SiteParent<T> {
-    fn default() -> Self {
-        SiteParent(None)
-    }
-}
-
-impl<T: RefTrait> SiteParent<T> {
-    pub fn convert<U: RefTrait>(&self, id_map: &HashMap<T, U>) -> Result<SiteParent<U>, T> {
-        if let Some(x) = self.0 {
-            Ok(SiteParent(Some(id_map.get(&x).ok_or(x)?.clone())))
-        } else {
-            Ok(SiteParent(None))
-        }
-    }
+/// Helper structure to serialize / deserialize entities with parents
+#[derive(Serialize, Deserialize, Clone, Debug)]
+pub struct Parented<P: RefTrait, T> {
+    pub parent: P,
+    pub bundle: T,
 }
 
 /// The Pending component indicates that an element is not yet ready to be

rmf_site_format/src/model.rs

@@ -158,7 +158,6 @@ impl<T: RefTrait> Default for ModelDescriptionBundle<T> {
 pub struct ModelInstance<T: RefTrait> {
     pub name: NameInSite,
     pub pose: Pose,
-    pub parent: SiteParent<T>,
     pub description: Affiliation<T>,
     #[serde(skip)]
     pub marker: ModelMarker,
@@ -172,7 +171,6 @@ impl<T: RefTrait> Default for ModelInstance<T> {
         Self {
             name: NameInSite("<Unnamed>".to_string()),
             pose: Pose::default(),
-            parent: SiteParent::default(),
             description: Affiliation::default(),
             marker: ModelMarker,
             instance_marker: InstanceMarker,
@@ -186,7 +184,6 @@ impl<T: RefTrait> ModelInstance<T> {
         Ok(ModelInstance {
             name: self.name.clone(),
             pose: self.pose.clone(),
-            parent: self.parent.convert(id_map)?,
             description: self.description.convert(id_map)?,
             optional_properties: self.optional_properties.convert(id_map)?,
             ..Default::default()