Introducing model descriptions and instances (#242)

Signed-off-by: Reuben Thomas <[email protected]> Signed-off-by: Xiyu Oh <[email protected]> Co-authored-by: Reuben Thomas <[email protected]>
open-rmf · Jan 3, 2025 · dcd6b51 · dcd6b51
1 parent 7ef0935
commit dcd6b51
Show file tree

Hide file tree

Showing 49 changed files with 3,763 additions and 403 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/assets/demo_maps/office.building.yaml b/assets/demo_maps/office.building.yaml
@@ -181,7 +181,7 @@ levels:
       - [814.828, 239.25, 0, "", {is_parking_spot: [4, false]}]
       - [1210.544, 365.254, 0, ""]
       - [1191.442, 824.457, 0, ""]
-      - [1232.421, 658.567, 0, tinyRobot1_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot1], spawn_robot_type: [1, TinyRobot]}]
+      - [1232.421, 658.567, 0, tinyRobot1_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot1], spawn_robot_type: [1, Open-RMF/TinyRobot]}]
       - [1991.121, 812.872, 0, ""]
       - [1990, 638.364, 0, pantry, {is_holding_point: [4, true], is_parking_spot: [4, false], pickup_dispenser: [1, coke_dispenser]}]
       - [2213.636, 812, 0, "", {is_parking_spot: [4, false]}]
@@ -206,7 +206,7 @@ levels:
       - [769.867, 618.148, 0, ""]
       - [2016.125, 1310.955, 0, ""]
       - [2468.096, 1217.693, 0, ""]
-      - [2412.581, 627.5, 0, tinyRobot2_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot2], spawn_robot_type: [1, TinyRobot]}]
+      - [2412.581, 627.5, 0, tinyRobot2_charger, {is_charger: [4, true], is_holding_point: [4, true], is_parking_spot: [4, true], spawn_robot_name: [1, tinyRobot2], spawn_robot_type: [1, Open-RMF/TinyRobot]}]
     walls:
       - [6, 7, {alpha: [3, 1], texture_name: [1, default]}]
       - [7, 12, {alpha: [3, 1], texture_name: [1, default]}]

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
@@ -49,6 +49,7 @@ pathdiff = "*"
 ehttp = { version = "0.4", features = ["native-async"] }
 nalgebra = "0.32.5"
 anyhow = "*"
+strum = "*"
 
 [target.'cfg(not(target_arch = "wasm32"))'.dependencies]
 clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] }

diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs
@@ -23,7 +23,7 @@ use crate::{
 use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow};
 use bevy_mod_raycast::primitives::{rays::Ray3d, Primitive3d};
 
-use rmf_site_format::{FloorMarker, Model, WallMarker};
+use rmf_site_format::{FloorMarker, ModelInstance, WallMarker};
 use std::collections::HashSet;
 
 /// A resource that keeps track of the unique entities that play a role in
@@ -120,23 +120,21 @@ impl Cursor {
         }
     }
 
-    pub fn set_model_preview(
+    pub fn set_model_instance_preview(
         &mut self,
         commands: &mut Commands,
         model_loader: &mut ModelLoader,
-        model: Option<Model>,
+        model_instance: Option<ModelInstance<Entity>>,
     ) {
         self.remove_preview(commands);
-        self.preview_model = if let Some(model) = model {
+        self.preview_model = model_instance.and_then(|model_instance| {
             Some(
                 model_loader
-                    .spawn_model(self.frame, model.clone())
+                    .spawn_model_instance(self.frame, model_instance)
                     .insert(Pending)
                     .id(),
             )
-        } else {
-            None
-        };
+        });
     }
 
     pub fn should_be_visible(&self) -> bool {

diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs
@@ -54,6 +54,9 @@ pub use lift::*;
 pub mod light;
 pub use light::*;
 
+pub mod model;
+pub use model::*;
+
 pub mod model_preview;
 pub use model_preview::*;
 
@@ -197,6 +200,7 @@ impl Plugin for InteractionPlugin {
                 .add_systems(
                     Update,
                     (
+                        update_model_instance_visual_cues.after(SelectionServiceStages::Select),
                         update_lane_visual_cues.after(SelectionServiceStages::Select),
                         update_edge_visual_cues.after(SelectionServiceStages::Select),
                         update_point_visual_cues.after(SelectionServiceStages::Select),

diff --git a/rmf_site_editor/src/interaction/model.rs b/rmf_site_editor/src/interaction/model.rs
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+use crate::{interaction::*, site::*};
+use bevy::prelude::*;
+
+pub fn update_model_instance_visual_cues(
+    model_descriptions: Query<
+        (Entity, &Selected, &Hovered),
+        (
+            With<ModelMarker>,
+            With<Group>,
+            Or<(Changed<Hovered>, Changed<Selected>)>,
+        ),
+    >,
+    mut model_instances: Query<
+        (
+            Entity,
+            &mut Selected,
+            &mut Hovered,
+            &mut Affiliation<Entity>,
+            Option<Ref<Tasks<Entity>>>,
+        ),
+        (With<ModelMarker>, Without<Group>),
+    >,
+    mut locations: Query<(&mut Selected, &mut Hovered), (With<LocationTags>, Without<ModelMarker>)>,
+    mut removed_components: RemovedComponents<Tasks<Entity>>,
+) {
+    for (instance_entity, mut instance_selected, mut instance_hovered, affiliation, tasks) in
+        &mut model_instances
+    {
+        let mut is_description_selected = false;
+        if let Some(description_entity) = affiliation.0 {
+            if let Ok((_, description_selected, description_hovered)) =
+                model_descriptions.get(description_entity)
+            {
+                if description_selected.cue() {
+                    instance_selected
+                        .support_selected
+                        .insert(description_entity);
+                    is_description_selected = true;
+                } else {
+                    instance_selected
+                        .support_selected
+                        .remove(&description_entity);
+                }
+                if description_hovered.cue() {
+                    instance_hovered.support_hovering.insert(description_entity);
+                } else {
+                    instance_hovered
+                        .support_hovering
+                        .remove(&description_entity);
+                }
+            }
+        }
+
+        // When an instance is selected, select all locations supporting it
+        if let Some(tasks) = tasks {
+            // When tasks for an instance have changed, reset all locations from supporting this instance
+            if tasks.is_changed() {
+                for (mut location_selected, mut location_hovered) in locations.iter_mut() {
+                    location_selected.support_selected.remove(&instance_entity);
+                    location_hovered.support_hovering.remove(&instance_entity);
+                }
+            }
+
+            if let Some(task_location) = tasks.0.first().and_then(|t| t.location()) {
+                if let Ok((mut location_selected, mut location_hovered)) =
+                    locations.get_mut(task_location.0)
+                {
+                    if instance_selected.cue() && !is_description_selected {
+                        location_selected.support_selected.insert(instance_entity);
+                    } else {
+                        location_selected.support_selected.remove(&instance_entity);
+                    }
+                    if instance_hovered.cue() {
+                        location_hovered.support_hovering.insert(instance_entity);
+                    } else {
+                        location_hovered.support_hovering.remove(&instance_entity);
+                    }
+                }
+            }
+        }
+    }
+
+    // When instances are removed, prevent any location from supporting them
+    for removed in removed_components.read() {
+        for (mut location_selected, mut location_hovered) in locations.iter_mut() {
+            location_selected.support_selected.remove(&removed);
+            location_hovered.support_hovering.remove(&removed);
+        }
+    }
+}
diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs
@@ -17,7 +17,7 @@
 
 use crate::{
     interaction::select::*,
-    site::{CurrentLevel, Model},
+    site::{CurrentLevel, ModelInstance},
 };
 use bevy::ecs::system::{Command, SystemParam, SystemState};
 
@@ -51,7 +51,7 @@ pub struct ObjectPlacement<'w, 's> {
 }
 
 impl<'w, 's> ObjectPlacement<'w, 's> {
-    pub fn place_object_2d(&mut self, object: Model) {
+    pub fn place_object_2d(&mut self, object: ModelInstance<Entity>) {
         let Some(level) = self.current_level.0 else {
             warn!("Unble to create [object:?] outside a level");
             return;
@@ -75,17 +75,17 @@ impl<'w, 's> ObjectPlacement<'w, 's> {
 
 /// Trait to be implemented to allow placing models with commands
 pub trait ObjectPlacementExt<'w, 's> {
-    fn place_object_2d(&mut self, object: Model);
+    fn place_object_2d(&mut self, object: ModelInstance<Entity>);
 }
 
 impl<'w, 's> ObjectPlacementExt<'w, 's> for Commands<'w, 's> {
-    fn place_object_2d(&mut self, object: Model) {
+    fn place_object_2d(&mut self, object: ModelInstance<Entity>) {
         self.add(ObjectPlaceCommand(object));
     }
 }
 
 #[derive(Deref, DerefMut)]
-pub struct ObjectPlaceCommand(Model);
+pub struct ObjectPlaceCommand(ModelInstance<Entity>);
 
 impl Command for ObjectPlaceCommand {
     fn apply(self, world: &mut World) {

diff --git a/rmf_site_editor/src/interaction/select/place_object_2d.rs b/rmf_site_editor/src/interaction/select/place_object_2d.rs
@@ -17,7 +17,7 @@
 
 use crate::{
     interaction::select::*,
-    site::{Model, ModelLoader},
+    site::{ModelInstance, ModelLoader},
 };
 use bevy::prelude::Input as UserInput;
 
@@ -104,7 +104,7 @@ pub fn build_place_object_2d_workflow(
 }
 
 pub struct PlaceObject2d {
-    pub object: Model,
+    pub object: ModelInstance<Entity>,
     pub level: Entity,
 }
 
@@ -121,7 +121,7 @@ pub fn place_object_2d_setup(
     let mut access = access.get_mut(&key).or_broken_buffer()?;
     let state = access.newest_mut().or_broken_buffer()?;
 
-    cursor.set_model_preview(&mut commands, &mut model_loader, Some(state.object.clone()));
+    cursor.set_model_instance_preview(&mut commands, &mut model_loader, Some(state.object.clone()));
     set_visibility(cursor.dagger, &mut visibility, false);
     set_visibility(cursor.halo, &mut visibility, false);
 
@@ -206,7 +206,7 @@ pub fn on_placement_chosen_2d(
 
     state.object.pose = placement.into();
     model_loader
-        .spawn_model(state.level, state.object)
+        .spawn_model_instance(state.level, state.object)
         .insert(Category::Model);
 
     Ok(())

diff --git a/rmf_site_editor/src/occupancy.rs b/rmf_site_editor/src/occupancy.rs
@@ -140,6 +140,8 @@ pub struct CalculateGrid {
     pub floor: f32,
     /// Ignore meshes above this height
     pub ceiling: f32,
+    // Ignore these entities
+    pub ignore: HashSet<Entity>,
 }
 
 enum Group {
@@ -180,6 +182,12 @@ fn calculate_grid(
         let physical_entities = collect_physical_entities(&bodies, &meta);
         info!("Checking {:?} physical entities", physical_entities.len());
         for e in &physical_entities {
+            if !request.ignore.is_empty() {
+                if AncestorIter::new(&parents, *e).any(|p| request.ignore.contains(&p)) {
+                    continue;
+                }
+            }
+
             let (_, mesh, aabb, tf) = match bodies.get(*e) {
                 Ok(body) => body,
                 Err(_) => continue,

diff --git a/rmf_site_editor/src/site/group.rs b/rmf_site_editor/src/site/group.rs
@@ -27,15 +27,9 @@ pub struct MergeGroups {
     pub into_group: Entity,
 }
 
-#[derive(Component, Deref)]
+#[derive(Component, Deref, DerefMut)]
 pub struct Members(Vec<Entity>);
 
-impl Members {
-    pub fn iter(&self) -> impl Iterator<Item = &Entity> {
-        self.0.iter()
-    }
-}
-
 #[derive(Component, Clone, Copy)]
 struct LastAffiliation(Option<Entity>);