Introducing scenarios concept

rmf_site_editor/src/site/load.rs

@@ -227,6 +227,27 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format::
                 id_to_entity.insert(*location_id, location);
                 consider_id(*location_id);
             }
+
+            for (robot_id, robot) in &site_data.models.mobile_robots {
+                let robot_entity = site.spawn(robot.clone()).id();
+                id_to_entity.insert(*robot_id, robot_entity);
+                consider_id(*robot_id);
+            }
+
+            for (robot_id, robot) in &site_data.models.workcells {
+                let robot_entity = site.spawn(robot.clone()).id();
+                id_to_entity.insert(*robot_id, robot_entity);
+                consider_id(*robot_id);
+            }
+
+            for (scenario_id, scenario) in &site_data.scenarios {
+                let scenario_entity = site
+                    .spawn(scenario.properties.clone())
+                    .insert(SiteID(*scenario_id))
+                    .id();
+                id_to_entity.insert(*scenario_id, scenario_entity);
+                consider_id(*scenario_id);
+            }
         });
 
     let nav_graph_rankings = match RecencyRanking::<NavGraphMarker>::from_u32(
@@ -236,18 +257,61 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format::
         Ok(r) => r,
         Err(id) => {
             error!(
-                "ERROR: Nav Graph ranking could not load because a graph with \
+                "Nav Graph ranking could not load because a graph with \
                 id {id} does not exist."
             );
             RecencyRanking::new()
         }
     };
 
-    site_cmd
-        .insert(nav_graph_rankings)
-        .insert(NextSiteID(highest_id + 1));
+    site_cmd.insert(nav_graph_rankings);
     let site_id = site_cmd.id();
 
+    // Construct instances separately so we can parent them correctly
+    for (scenario_id, scenario) in &site_data.scenarios {
+        let Some(scenario_entity) = id_to_entity.get(scenario_id).cloned() else {
+            error!(
+                "Failed to load scenario {scenario_id:?}, unable to find a \
+                loaded entity for it."
+            );
+            continue;
+        };
+
+        for (instance_id, instance) in &scenario.instances {
+            let Some(model_entity) = id_to_entity.get(&instance.model).cloned() else {
+                error!(
+                    "Failed to load instance {instance_id:?} for scenario \
+                    {scenario_id:?} because its model {:?} is missing.",
+                    instance.model,
+                );
+                continue;
+            };
+
+            let Some(parent_entity) = id_to_entity.get(&instance.parent).cloned() else {
+                error!(
+                    "Failed to load instance {instance_id:?} for scenario \
+                    {scenario_id:?} because its parent {:?} is missing.",
+                    instance.parent,
+                );
+                continue;
+            };
+
+            let instance_entity = commands
+                .spawn(instance.bundle.clone())
+                .insert(InScenario(scenario_entity))
+                .set_model_source(model_entity)
+                .insert(ModelMarker)
+                .insert(SiteID(*instance_id))
+                .id();
+            id_to_entity.insert(*instance_id, instance_entity);
+            consider_id(*instance_id);
+
+            commands.entity(parent_entity).add_child(instance_entity);
+        }
+    }
+
+    commands.entity(site_id).insert(NextSiteID(highest_id + 1));
+
     // Make the lift cabin anchors that are used by doors subordinate
     for (lift_id, lift_data) in &site_data.lifts {
         for (_, door) in &lift_data.cabin_doors {

rmf_site_editor/src/site/location.rs

@@ -22,6 +22,9 @@ use bevy::prelude::*;
 // experience z-fighting.
 const LOCATION_LAYER_HEIGHT: f32 = LANE_LAYER_LIMIT + SELECTED_LANE_OFFSET / 2.0;
 
+#[derive(Component, Debug, Clone)]
+pub struct LocationRobotModel(Entity);
+
 // TODO(MXG): Refactor this implementation with should_display_lane using traits and generics
 fn should_display_point(
     point: &Point<Entity>,
@@ -222,33 +225,3 @@ pub fn update_visibility_for_locations(
         }
     }
 }
-
-#[derive(Debug, Clone)]
-pub struct ConsiderLocationTag {
-    pub tag: Option<LocationTag>,
-    pub for_element: Entity,
-}
-
-impl ConsiderLocationTag {
-    pub fn new(tag: Option<LocationTag>, for_element: Entity) -> Self {
-        Self { tag, for_element }
-    }
-}
-
-// TODO(MXG): Consider refactoring into a generic plugin, alongside ConsiderAssociatedGraph
-pub fn handle_consider_location_tag(
-    mut recalls: Query<&mut RecallLocationTags>,
-    mut considerations: EventReader<ConsiderLocationTag>,
-) {
-    for consider in considerations.iter() {
-        if let Ok(mut recall) = recalls.get_mut(consider.for_element) {
-            recall.consider_tag = consider.tag.clone();
-            let r = recall.as_mut();
-            if let Some(LocationTag::SpawnRobot(model)) | Some(LocationTag::Workcell(model)) =
-                &r.consider_tag
-            {
-                r.consider_tag_asset_source_recall.remember(&model.source);
-            }
-        }
-    }
-}

rmf_site_editor/src/site/mod.rs

@@ -63,6 +63,9 @@ pub use measurement::*;
 pub mod model;
 pub use model::*;
 
+pub mod models;
+pub use models::*;
+
 pub mod nav_graph;
 pub use nav_graph::*;
 
@@ -149,7 +152,7 @@ impl Plugin for SitePlugin {
             .add_event::<ToggleLiftDoorAvailability>()
             .add_event::<ExportLights>()
             .add_event::<ConsiderAssociatedGraph>()
-            .add_event::<ConsiderLocationTag>()
+            .add_plugin(ModelSourcePlugin)
             .add_plugin(ChangePlugin::<AssociatedGraphs<Entity>>::default())
             .add_plugin(RecallPlugin::<RecallAssociatedGraphs<Entity>>::default())
             .add_plugin(ChangePlugin::<Motion>::default())
@@ -248,7 +251,6 @@ impl Plugin for SitePlugin {
                     .with_system(update_changed_location)
                     .with_system(update_location_for_moved_anchors)
                     .with_system(handle_consider_associated_graph)
-                    .with_system(handle_consider_location_tag)
                     .with_system(update_lift_for_moved_anchors)
                     .with_system(update_lift_door_availability)
                     .with_system(update_physical_lights)

rmf_site_editor/src/site/model.rs

@@ -260,7 +260,10 @@ pub fn update_model_scenes(
 
 pub fn update_model_tentative_formats(
     mut commands: Commands,
-    changed_models: Query<Entity, (Changed<AssetSource>, With<ModelMarker>)>,
+    changed_models: Query<
+        (Entity, &AssetSource, Option<&ModelScene>),
+        (Changed<AssetSource>, With<ModelMarker>),
+    >,
     mut loading_models: Query<
         (
             Entity,
@@ -272,9 +275,11 @@ pub fn update_model_tentative_formats(
     >,
     asset_server: Res<AssetServer>,
 ) {
-    for e in changed_models.iter() {
+    for (e, source, scene) in changed_models.iter() {
         // Reset to the first format
-        commands.entity(e).insert(TentativeModelFormat::default());
+        if !scene.is_some_and(|r| &r.source == source) {
+            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() {

rmf_site_editor/src/site/models.rs

@@ -0,0 +1,145 @@
+/*
+ * 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 bevy::{
+    ecs::system::{Command, EntityCommands},
+    prelude::*,
+};
+use rmf_site_format::{AssetSource, Scale};
+
+#[derive(Component, Clone, Copy)]
+pub struct InScenario(pub Entity);
+
+#[derive(Component, Clone, Copy)]
+pub struct ModelSource(Entity);
+impl ModelSource {
+    pub fn get(&self) -> Entity {
+        self.0
+    }
+}
+
+#[derive(Component, Clone)]
+pub struct ModelInstances(Vec<Entity>);
+impl ModelInstances {
+    pub fn iter(&self) -> impl Iterator<Item = &Entity> {
+        self.0.iter()
+    }
+}
+
+#[derive(Clone, Copy)]
+struct ChangeModelSource {
+    instance: Entity,
+    source: Entity,
+}
+
+impl Command for ChangeModelSource {
+    fn write(self, world: &mut World) {
+        let mut previous_source = None;
+        if let Some(mut e) = world.get_entity_mut(self.instance) {
+            if let Some(previous) = e.get::<ModelSource>() {
+                if previous.0 == self.source {
+                    // No need to change anything
+                    return;
+                }
+
+                previous_source = Some(previous.0);
+            }
+            e.insert(ModelSource(self.source));
+        } else {
+            error!(
+                "Cannot change model source of instance {:?} because it no \
+                longer exists",
+                self.instance,
+            );
+            return;
+        }
+
+        if let Some(mut e) = world.get_entity_mut(self.source) {
+            if let Some(mut instances) = e.get_mut::<ModelInstances>() {
+                instances.0.push(self.instance);
+            } else {
+                e.insert(ModelInstances(vec![self.instance]));
+            }
+        } else {
+            error!(
+                "Cannot change model source of instance {:?} to {:?} because \
+                that source no longer exists",
+                self.instance, self.source,
+            );
+
+            if let (Some(mut e), Some(prev)) =
+                (world.get_entity_mut(self.instance), previous_source)
+            {
+                e.insert(ModelSource(prev));
+            }
+            return;
+        }
+
+        if let Some(previous) = previous_source {
+            if let Some(mut e) = world.get_entity_mut(previous) {
+                if let Some(mut instances) = e.get_mut::<ModelInstances>() {
+                    instances.0.retain(|e| *e != self.instance);
+                }
+            }
+        }
+
+        world.send_event(self);
+    }
+}
+
+pub trait SetModelSourceExt {
+    fn set_model_source(&mut self, new_source: Entity) -> &mut Self;
+}
+
+impl<'w, 's, 'a> SetModelSourceExt for EntityCommands<'w, 's, 'a> {
+    fn set_model_source(&mut self, source: Entity) -> &mut Self {
+        let instance = self.id();
+        self.commands().add(ChangeModelSource { instance, source });
+        self
+    }
+}
+
+fn handle_changed_model_sources(
+    mut commands: Commands,
+    mut changed_instances: EventReader<ChangeModelSource>,
+    sources: Query<(&AssetSource, &Scale)>,
+    changed_sources: Query<
+        (&ModelInstances, &AssetSource, &Scale),
+        Or<(Changed<AssetSource>, Changed<Scale>)>,
+    >,
+) {
+    for change in changed_instances.iter() {
+        let Some(mut instance) = commands.get_entity(change.source) else { continue };
+        let Ok((source, scale)) = sources.get(change.source) else { continue };
+        instance.insert(source.clone()).insert(scale.clone());
+    }
+
+    for (instances, source, scale) in &changed_sources {
+        for instance in instances.iter() {
+            let Some(mut instance) = commands.get_entity(*instance) else { continue };
+            instance.insert(source.clone()).insert(scale.clone());
+        }
+    }
+}
+
+pub struct ModelSourcePlugin;
+impl Plugin for ModelSourcePlugin {
+    fn build(&self, app: &mut App) {
+        app.add_event::<ChangeModelSource>()
+            .add_system(handle_changed_model_sources);
+    }
+}