From 205d3ec38fbc22a2b3d002ba539846f1c3446ad3 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <grey@openrobotics.org>
Date: Tue, 27 Aug 2024 15:00:52 +0800
Subject: [PATCH] Fix filtering for fiducial anchor selection

Signed-off-by: Michael X. Grey <grey@openrobotics.org>
---
 .../src/interaction/select/select_anchor.rs   | 87 +++++++++++++++----
 rmf_site_editor/src/site/fiducial.rs          | 20 +++--
 .../src/widgets/inspector/inspect_fiducial.rs |  9 +-
 3 files changed, 91 insertions(+), 25 deletions(-)

diff --git a/rmf_site_editor/src/interaction/select/select_anchor.rs b/rmf_site_editor/src/interaction/select/select_anchor.rs
index 01c41d14..5e30f42b 100644
--- a/rmf_site_editor/src/interaction/select/select_anchor.rs
+++ b/rmf_site_editor/src/interaction/select/select_anchor.rs
@@ -18,8 +18,8 @@
 use bevy::prelude::*;
 use bevy_impulse::*;
 
-use crate::interaction::select::*;
-use rmf_site_format::{Fiducial, Floor, Location, Path, Point};
+use crate::{site::CurrentLevel, interaction::select::*};
+use rmf_site_format::{Fiducial, Floor, Location, Path, Point, LevelElevation};
 
 #[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Resource)]
 pub enum AnchorScope {
@@ -407,6 +407,7 @@ pub fn anchor_selection_setup<State: Borrow<AnchorScope>>(
     mut current_anchor_scope: ResMut<AnchorScope>,
     mut cursor: ResMut<Cursor>,
     mut highlight: ResMut<HighlightAnchors>,
+    mut gizmo_blockers: ResMut<GizmoBlockers>,
 ) -> SelectionNodeResult
 where
     State: 'static + Send + Sync,
@@ -436,6 +437,7 @@ where
     }
 
     highlight.0 = true;
+    gizmo_blockers.selecting = true;
 
     *current_anchor_scope = *scope;
 
@@ -453,6 +455,7 @@ pub fn cleanup_anchor_selection(
     mut hidden_anchors: ResMut<HiddenSelectAnchorEntities>,
     mut anchor_scope: ResMut<AnchorScope>,
     mut highlight: ResMut<HighlightAnchors>,
+    mut gizmo_blockers: ResMut<GizmoBlockers>,
 ) {
     cursor.remove_mode(SELECT_ANCHOR_MODE_LABEL, &mut visibility);
     set_visibility(cursor.site_anchor_placement, &mut visibility, false);
@@ -462,6 +465,7 @@ pub fn cleanup_anchor_selection(
     }
 
     highlight.0 = false;
+    gizmo_blockers.selecting = false;
 
     *anchor_scope = AnchorScope::General;
 }
@@ -509,29 +513,24 @@ pub struct AnchorFilter<'w, 's> {
     commands: Commands<'w, 's>,
     current_drawing: Res<'w, CurrentEditDrawing>,
     drawings: Query<'w, 's, &'static PixelsPerMeter, With<DrawingMarker>>,
+    parents: Query<'w, 's, &'static Parent>,
+    levels: Query<'w, 's, (), With<LevelElevation>>,
+    current_level: Res<'w, CurrentLevel>,
 }
 
 impl<'w, 's> SelectionFilter for AnchorFilter<'w, 's> {
     fn filter_pick(&mut self, select: Entity) -> Option<Entity> {
         self.inspect.filter_pick(select).and_then(|e| {
-            if self.anchors.contains(e) {
-                Some(e)
-            } else {
-                None
-            }
+            self.filter_target(e)
         })
     }
 
     fn filter_select(&mut self, target: Entity) -> Option<Entity> {
-        if self.anchors.contains(target) {
-            Some(target)
-        } else {
-            None
-        }
+        self.filter_target(target)
     }
 
     fn on_click(&mut self, hovered: Hover) -> Option<Select> {
-        if let Some(candidate) = hovered.0 {
+        if let Some(candidate) = hovered.0.and_then(|e| self.filter_target(e)) {
             return Some(Select::new(Some(candidate)));
         }
 
@@ -577,9 +576,14 @@ impl<'w, 's> SelectionFilter for AnchorFilter<'w, 's> {
                     .id()
             }
             AnchorScope::General => {
-                // TODO(@mxgrey): Consider putting the anchor directly into the
-                // current level instead of relying on orphan behavior
-                self.commands.spawn(AnchorBundle::at_transform(tf)).id()
+                let Some(level) = self.current_level.0 else {
+                    error!("No current level selected to place the anchor");
+                    return None;
+                };
+                self.commands
+                    .spawn(AnchorBundle::at_transform(tf))
+                    .set_parent(level)
+                    .id()
             }
         };
 
@@ -587,6 +591,57 @@ impl<'w, 's> SelectionFilter for AnchorFilter<'w, 's> {
     }
 }
 
+impl<'w, 's> AnchorFilter<'w ,'s> {
+    fn filter_anchor(&mut self, target: Entity) -> Option<Entity> {
+        if self.anchors.contains(target) {
+            Some(target)
+        } else {
+            None
+        }
+    }
+
+    fn filter_scope(&mut self, target: Entity) -> Option<Entity> {
+        let parent = match self.parents.get(target) {
+            Ok(parent) => parent.get(),
+            Err(err) => {
+                error!("Unable to detect parent for target anchor {target:?}: {err}");
+                return None;
+            }
+        };
+
+        match &*self.anchor_scope {
+            AnchorScope::General => {
+                let is_site = self.open_sites.contains(parent);
+                let is_level = self.levels.contains(parent);
+                if is_site || is_level {
+                    Some(target)
+                } else {
+                    None
+                }
+            }
+            AnchorScope::Site => {
+                if self.open_sites.contains(parent) {
+                    Some(target)
+                } else {
+                    None
+                }
+            }
+            AnchorScope::Drawing => {
+                if self.drawings.contains(parent) {
+                    Some(target)
+                } else {
+                    None
+                }
+            }
+        }
+    }
+
+    fn filter_target(&mut self, target: Entity) -> Option<Entity> {
+        self.filter_anchor(target)
+            .and_then(|target| self.filter_scope(target))
+    }
+}
+
 fn compute_parent_inverse_pose(
     tf: &GlobalTransform,
     transforms: &Query<&GlobalTransform>,
diff --git a/rmf_site_editor/src/site/fiducial.rs b/rmf_site_editor/src/site/fiducial.rs
index da6f4ed4..58133c3c 100644
--- a/rmf_site_editor/src/site/fiducial.rs
+++ b/rmf_site_editor/src/site/fiducial.rs
@@ -266,9 +266,13 @@ pub fn update_changed_fiducial(
     anchors: AnchorParams,
 ) {
     for (e, point, mut tf) in fiducials.iter_mut() {
-        let position = anchors
-            .point_in_parent_frame_of(point.0, Category::Fiducial, e)
-            .unwrap();
+        let position = match anchors.point_in_parent_frame_of(point.0, Category::Fiducial, e) {
+            Ok(position) => position,
+            Err(err) => {
+                error!("failed to update fiducial: {err}");
+                return;
+            }
+        };
         tf.translation = position;
     }
 }
@@ -287,9 +291,13 @@ pub fn update_fiducial_for_moved_anchors(
     for dependents in &changed_anchors {
         for dependent in dependents.iter() {
             if let Ok((e, point, mut tf)) = fiducials.get_mut(*dependent) {
-                let position = anchors
-                    .point_in_parent_frame_of(point.0, Category::Fiducial, e)
-                    .unwrap();
+                let position = match anchors.point_in_parent_frame_of(point.0, Category::Fiducial, e) {
+                    Ok(position) => position,
+                    Err(err) => {
+                        error!("failed to update fiducial: {err}");
+                        continue;
+                    }
+                };
                 tf.translation = position;
             }
         }
diff --git a/rmf_site_editor/src/widgets/inspector/inspect_fiducial.rs b/rmf_site_editor/src/widgets/inspector/inspect_fiducial.rs
index bce6ae9d..225fda9b 100644
--- a/rmf_site_editor/src/widgets/inspector/inspect_fiducial.rs
+++ b/rmf_site_editor/src/widgets/inspector/inspect_fiducial.rs
@@ -17,8 +17,8 @@
 
 use crate::{
     site::{
-        Affiliation, Change, FiducialGroup, FiducialMarker, FiducialUsage, Group, NameInSite,
-        SiteID,
+        Affiliation, Change, FiducialGroup, FiducialMarker, FiducialUsage,
+        Group, NameInSite, NameOfSite,
     },
     widgets::{prelude::*, Icons, Inspect, InspectionPlugin},
 };
@@ -79,6 +79,8 @@ pub struct InspectFiducial<'w, 's> {
     search_for_fiducial: ResMut<'w, SearchForFiducial>,
     commands: Commands<'w, 's>,
     change_affiliation: EventWriter<'w, Change<Affiliation<Entity>>>,
+    names: Query<'w, 's, &'static NameInSite>,
+    name_of_site: Query<'w, 's, &'static NameOfSite>,
 }
 
 impl<'w, 's> WidgetSystem<Inspect> for InspectFiducial<'w, 's> {
@@ -96,8 +98,9 @@ impl<'w, 's> WidgetSystem<Inspect> for InspectFiducial<'w, 's> {
         };
         let Ok(tracker) = params.usage.get(parent.get()) else {
             error!(
-                "Unable to find usage tracker for parent {:?} of fiducial {:?}",
+                "Unable to find usage tracker for parent {:?} [{}] of fiducial {:?}",
                 parent.get(),
+                params.names.get(parent.get()).ok().map(|n| n.0.as_str()).unwrap_or("<name missing>"),
                 selection,
             );
             return;