Implement a diagnostic tool (#154)

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

assets/textures/attribution.md

@@ -7,6 +7,7 @@
 * [`add.png`](https://thenounproject.com/icon/plus-1809810/)
 * [`search.png`](https://thenounproject.com/icon/search-3743008/)
 * [`merge.png`](https://thenounproject.com/icon/merge-3402180/)
+* [`hide.png`](https://thenounproject.com/icon/hide-eye-796162/)
 * `trash.png`: @mxgrey
 * `selected.png`: @mxgrey
 * `alignment.png`: @mxgrey

rmf_site_editor/src/issue.rs

@@ -0,0 +1,121 @@
+/*
+ * 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::site::ChangePlugin;
+use crate::widgets::{
+    diagnostic_window::DiagnosticWindowState,
+    menu_bar::{MenuEvent, MenuItem, ToolMenu},
+};
+use bevy::prelude::*;
+use bevy::utils::{HashMap, Uuid};
+use rmf_site_format::{FilteredIssueKinds, FilteredIssues, IssueKey};
+
+#[derive(Component, Debug, Clone)]
+pub struct Issue {
+    pub key: IssueKey<Entity>,
+    /// Short description of the issue
+    pub brief: String,
+    /// Hint on how to approach solving the issue
+    pub hint: String,
+}
+
+pub trait RegisterIssueType {
+    fn add_issue_type(&mut self, type_uuid: &Uuid, name: &str) -> &mut Self;
+}
+
+impl RegisterIssueType for App {
+    fn add_issue_type(&mut self, type_uuid: &Uuid, name: &str) -> &mut Self {
+        let mut issue_dictionary = self
+            .world
+            .get_resource_or_insert_with::<IssueDictionary>(Default::default);
+        issue_dictionary.insert(type_uuid.clone(), name.into());
+        self
+    }
+}
+
+/// Used as an event to request validation of a workspace
+#[derive(Deref, DerefMut)]
+pub struct ValidateWorkspace(pub Entity);
+
+// Maps a uuid to the issue name
+#[derive(Default, Resource, Deref, DerefMut)]
+pub struct IssueDictionary(HashMap<Uuid, String>);
+
+#[derive(Default)]
+pub struct IssuePlugin;
+
+#[derive(Resource)]
+pub struct IssueMenu {
+    diagnostic_tool: Entity,
+}
+
+impl FromWorld for IssueMenu {
+    fn from_world(world: &mut World) -> Self {
+        // Tools menu
+        let diagnostic_tool = world
+            .spawn(MenuItem::Text("Diagnostic Tool".to_string()))
+            .id();
+
+        let tool_header = world.resource::<ToolMenu>().get();
+        world
+            .entity_mut(tool_header)
+            .push_children(&[diagnostic_tool]);
+
+        IssueMenu { diagnostic_tool }
+    }
+}
+
+fn handle_diagnostic_window_visibility(
+    mut menu_events: EventReader<MenuEvent>,
+    issue_menu: Res<IssueMenu>,
+    mut diagnostic_window: ResMut<DiagnosticWindowState>,
+) {
+    for event in menu_events.iter() {
+        if event.clicked() && event.source() == issue_menu.diagnostic_tool {
+            diagnostic_window.show = true;
+        }
+    }
+}
+
+impl Plugin for IssuePlugin {
+    fn build(&self, app: &mut App) {
+        app.add_event::<ValidateWorkspace>()
+            .add_plugin(ChangePlugin::<FilteredIssues<Entity>>::default())
+            .add_plugin(ChangePlugin::<FilteredIssueKinds>::default())
+            .init_resource::<IssueDictionary>()
+            .init_resource::<IssueMenu>()
+            .add_system(handle_diagnostic_window_visibility);
+    }
+}
+
+pub fn clear_old_issues_on_new_validate_event(
+    mut commands: Commands,
+    mut validate_events: EventReader<ValidateWorkspace>,
+    children: Query<&Children>,
+    issues: Query<Entity, With<Issue>>,
+) {
+    for root in validate_events.iter() {
+        let Ok(children) = children.get(**root) else {
+            return;
+        };
+        for e in children {
+            if issues.get(*e).is_ok() {
+                commands.entity(*e).despawn_recursive();
+            }
+        }
+    }
+}

rmf_site_editor/src/lib.rs

@@ -29,6 +29,8 @@ use widgets::{menu_bar::MenuPluginManager, *};
 
 pub mod occupancy;
 use occupancy::OccupancyPlugin;
+pub mod issue;
+use issue::*;
 
 pub mod demo_world;
 pub mod log;
@@ -222,6 +224,8 @@ impl Plugin for SiteEditor {
             .add_plugin(AnimationPlugin)
             .add_plugin(OccupancyPlugin)
             .add_plugin(WorkspacePlugin)
+            // Note order matters, issue and OSMView plugins must be initialized after the UI
+            .add_plugin(IssuePlugin)
             .add_plugin(OSMViewPlugin);
     }
 }

rmf_site_editor/src/site/anchor.rs

@@ -15,9 +15,11 @@
  *
 */
 
-use crate::site::*;
-use bevy::{prelude::*, render::primitives::Sphere};
-use rmf_site_format::{Anchor, LevelProperties, LiftCabin};
+use crate::{site::*, Issue, ValidateWorkspace};
+use bevy::{prelude::*, render::primitives::Sphere, utils::Uuid};
+use itertools::Itertools;
+use rmf_site_format::{Anchor, LevelElevation, LiftCabin};
+use std::collections::HashMap;
 
 #[derive(Bundle, Debug)]
 pub struct AnchorBundle {
@@ -176,3 +178,69 @@ pub fn assign_orphan_anchors_to_parent(
         commands.entity(parent).add_child(e_anchor);
     }
 }
+
+/// Unique UUID to identify issue of anchors being close but not connected
+pub const UNCONNECTED_ANCHORS_ISSUE_UUID: Uuid =
+    Uuid::from_u128(0xe1ef2a60c3bc45829effdf8ca7dd3403u128);
+
+// When triggered by a validation request event, check if there are anchors that are very close to
+// each other but not connected
+pub fn check_for_close_unconnected_anchors(
+    mut commands: Commands,
+    mut validate_events: EventReader<ValidateWorkspace>,
+    parents: Query<&Parent>,
+    anchors: AnchorParams,
+    anchor_entities: Query<Entity, With<Anchor>>,
+    levels: Query<Entity, With<LevelElevation>>,
+    dependents: Query<&Dependents>,
+) {
+    const ISSUE_HINT: &str = "Pair of anchors that are very close but not connected was found, \
+                        review if this is intended and, if it is, suppress the issue";
+    // TODO(luca) make this configurable
+    const DISTANCE_THRESHOLD: f32 = 0.2;
+    for root in validate_events.iter() {
+        // Key is level id, value is vector of (Entity, Global tf's position)
+        let mut anchor_poses: HashMap<Entity, Vec<(Entity, Vec3)>> = HashMap::new();
+        for e in &anchor_entities {
+            if let Some(level) = AncestorIter::new(&parents, e).find(|p| levels.get(*p).is_ok()) {
+                if AncestorIter::new(&parents, level).any(|p| p == **root) {
+                    // Level that belongs to requested workspace
+                    let poses = anchor_poses.entry(level).or_default();
+                    poses.push((
+                        e,
+                        anchors
+                            .point_in_parent_frame_of(e, Category::General, level)
+                            .expect("Failed fetching anchor pose"),
+                    ));
+                }
+            }
+        }
+        // Now find close unconnected pairs, sadly n^2 problem for anchors, unless we use better
+        // data structures that sort in space
+        for values in anchor_poses.values() {
+            for ((e0, p0), (e1, p1)) in values.iter().tuple_combinations() {
+                if p0.distance(*p1) < DISTANCE_THRESHOLD {
+                    let mut edge_found = false;
+                    if let (Ok(d0), Ok(d1)) = (dependents.get(*e0), dependents.get(*e1)) {
+                        edge_found = d0.iter().any(|d| d1.contains(d));
+                    }
+                    if !edge_found {
+                        let issue = Issue {
+                            key: IssueKey {
+                                entities: [*e0, *e1].into(),
+                                kind: UNCONNECTED_ANCHORS_ISSUE_UUID,
+                            },
+                            brief: format!(
+                                "Anchors are closer than {} m but unconnected",
+                                DISTANCE_THRESHOLD
+                            ),
+                            hint: ISSUE_HINT.to_string(),
+                        };
+                        let id = commands.spawn(issue).id();
+                        commands.entity(**root).add_child(id);
+                    }
+                }
+            }
+        }
+    }
+}

rmf_site_editor/src/site/deletion.rs

@@ -22,6 +22,7 @@ use crate::{
         Category, CurrentLevel, Dependents, LevelElevation, LevelProperties, NameInSite,
         SiteUpdateStage,
     },
+    Issue,
 };
 use bevy::{ecs::system::SystemParam, prelude::*};
 use rmf_site_format::{ConstraintDependents, Edge, MeshConstraint, Path, Point};
@@ -91,6 +92,7 @@ struct DeletionParams<'w, 's> {
     levels: Query<'w, 's, Entity, With<LevelElevation>>,
     select: EventWriter<'w, 's, Select>,
     log: EventWriter<'w, 's, Log>,
+    issues: Query<'w, 's, (Entity, &'static mut Issue)>,
 }
 
 pub struct DeletionPlugin;
@@ -225,6 +227,13 @@ fn cautious_delete(element: Entity, params: &mut DeletionParams) {
         }
     }
 
+    for (e, mut issue) in &mut params.issues {
+        issue.key.entities.remove(&element);
+        if issue.key.entities.is_empty() {
+            params.commands.entity(e).despawn_recursive();
+        }
+    }
+
     // Fetch the parent and delete this dependent
     // TODO(luca) should we add this snippet to the recursive delete also?
     if let Ok(parent) = params.parents.get(element) {

rmf_site_editor/src/site/door.rs

@@ -17,14 +17,17 @@
 
 use crate::{
     interaction::{Hovered, Selectable},
+    issue::*,
     shapes::*,
     site::*,
 };
 use bevy::{
     prelude::*,
     render::mesh::{Indices, PrimitiveTopology},
+    utils::Uuid,
 };
 use rmf_site_format::{Category, DoorType, Edge, DEFAULT_LEVEL_HEIGHT};
+use std::collections::{BTreeSet, HashMap};
 
 pub const DOOR_CUE_HEIGHT: f32 = 0.004;
 pub const DOOR_STOP_LINE_THICKNESS: f32 = 0.01;
@@ -471,3 +474,41 @@ pub fn update_door_for_moved_anchors(
         }
     }
 }
+
+/// Unique UUID to identify issue of duplicated door names
+pub const DUPLICATED_DOOR_NAME_ISSUE_UUID: Uuid =
+    Uuid::from_u128(0x73f641f2a08d4ffd90216eb9bacb4743u128);
+
+// When triggered by a validation request event, check if there are duplicated door names and
+// generate an issue if that is the case
+pub fn check_for_duplicated_door_names(
+    mut commands: Commands,
+    mut validate_events: EventReader<ValidateWorkspace>,
+    parents: Query<&Parent>,
+    door_names: Query<(Entity, &NameInSite), With<DoorMarker>>,
+) {
+    for root in validate_events.iter() {
+        let mut names: HashMap<String, BTreeSet<Entity>> = HashMap::new();
+        for (e, name) in &door_names {
+            if AncestorIter::new(&parents, e).any(|p| p == **root) {
+                let entities_with_name = names.entry(name.0.clone()).or_default();
+                entities_with_name.insert(e);
+            }
+        }
+        for (name, entities) in names.drain() {
+            if entities.len() > 1 {
+                let issue = Issue {
+                    key: IssueKey {
+                        entities: entities,
+                        kind: DUPLICATED_DOOR_NAME_ISSUE_UUID,
+                    },
+                    brief: format!("Multiple doors found with the same name {}", name),
+                    hint: "Doors use their names as identifiers with RMF and each door should have a unique \
+                           name, rename the affected doors".to_string()
+                };
+                let id = commands.spawn(issue).id();
+                commands.entity(**root).add_child(id);
+            }
+        }
+    }
+}

rmf_site_editor/src/site/fiducial.rs

@@ -17,7 +17,8 @@
 
 use crate::interaction::VisualCue;
 use crate::site::*;
-use bevy::prelude::*;
+use crate::{Issue, ValidateWorkspace};
+use bevy::{prelude::*, utils::Uuid};
 use std::collections::HashMap;
 
 #[derive(Component)]
@@ -290,3 +291,38 @@ pub fn update_fiducial_for_moved_anchors(
         }
     }
 }
+
+/// Unique UUID to identify issue of fiducials without affiliation
+pub const FIDUCIAL_WITHOUT_AFFILIATION_ISSUE_UUID: Uuid =
+    Uuid::from_u128(0x242a655f67cc4d4f9176ed5d64cd87f0u128);
+
+// When triggered by a validation request event, check if there are fiducials without affiliation,
+// generate an issue if that is the case
+pub fn check_for_fiducials_without_affiliation(
+    mut commands: Commands,
+    mut validate_events: EventReader<ValidateWorkspace>,
+    parents: Query<&Parent>,
+    fiducial_affiliations: Query<(Entity, &Affiliation<Entity>), With<FiducialMarker>>,
+) {
+    const ISSUE_HINT: &str = "Fiducial affiliations are used by the site editor to map matching \
+                            fiducials between different floors or drawings and calculate their \
+                            relative transform, fiducials without affiliation are ignored";
+    for root in validate_events.iter() {
+        for (e, affiliation) in &fiducial_affiliations {
+            if AncestorIter::new(&parents, e).any(|p| p == **root) {
+                if affiliation.0.is_none() {
+                    let issue = Issue {
+                        key: IssueKey {
+                            entities: [e].into(),
+                            kind: FIDUCIAL_WITHOUT_AFFILIATION_ISSUE_UUID,
+                        },
+                        brief: format!("Fiducial without affiliation found"),
+                        hint: ISSUE_HINT.to_string(),
+                    };
+                    let id = commands.spawn(issue).id();
+                    commands.entity(**root).add_child(id);
+                }
+            }
+        }
+    }
+}

rmf_site_editor/src/site/floor.rs

@@ -332,7 +332,9 @@ pub fn update_floors(
             .iter()
             .flat_map(|members| members.iter().cloned()),
     ) {
-        let Ok((segment, path, texture_source)) = floors.get(e) else { continue };
+        let Ok((segment, path, texture_source)) = floors.get(e) else {
+            continue;
+        };
         let (base_color_texture, texture) = from_texture_source(texture_source, &textures);
         if let Ok(mut mesh) = mesh_handles.get_mut(segment.mesh) {
             if let Ok(material) = material_handles.get(segment.mesh) {