Add support for spawning robots in locations

Signed-off-by: Luca Della Vedova <[email protected]>
open-rmf · May 31, 2023 · 479ba05 · 479ba05
1 parent f796e44
commit 479ba05
Show file tree

Hide file tree

Showing 3 changed files with 65 additions and 1 deletion.
diff --git a/rmf_site_editor/src/site/location.rs b/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>,
@@ -86,6 +89,66 @@ pub fn add_location_visuals(
     }
 }
 
+pub fn add_robot_to_spawn_location(
+    mut commands: Commands,
+    locations: Query<
+        (
+            Entity,
+            &Point<Entity>,
+            &LocationTags,
+            Option<&LocationRobotModel>,
+        ),
+        Changed<LocationTags>,
+    >,
+    levels: Query<(), With<LevelProperties>>,
+    mut models: Query<&mut AssetSource, With<ModelMarker>>,
+    anchors: AnchorParams,
+    parents: Query<&Parent>,
+) {
+    for (e, point, location_tags, location_model) in &locations {
+        let position = anchors
+            .point_in_parent_frame_of(point.0, Category::Location, e)
+            .unwrap();
+
+        let parent = AncestorIter::new(&parents, point.0).find(|p| levels.get(*p).is_ok());
+
+        let mut found_robot = false;
+        if let Some(parent) = parent {
+            for location_tag in location_tags.iter() {
+                if let LocationTag::SpawnRobot(m) = location_tag {
+                    if let Some(location_model) = location_model {
+                        // Update existing model
+                        if let Ok(mut source) = models.get_mut(location_model.0) {
+                            *source = m.source.clone();
+                        }
+                    } else {
+                        // Spawn new model
+                        let mut model = m.clone();
+                        model.pose = Pose {
+                            trans: position.into(),
+                            ..default()
+                        };
+                        // TODO(luca) there should be a marker component to denote this is a robot
+                        // as well as set it non static
+                        // Robots should probably be made non deletable, since their spawning is
+                        // controlled by the location property
+                        let child = commands.spawn(model).id();
+                        commands.entity(parent).push_children(&[child]);
+                        commands.entity(e).insert(LocationRobotModel(child));
+                    }
+                    found_robot = true;
+                }
+            }
+        }
+        if !found_robot {
+            if let Some(location_model) = location_model {
+                commands.entity(location_model.0).despawn_recursive();
+                commands.entity(e).remove::<LocationRobotModel>();
+            }
+        }
+    }
+}
+
 pub fn update_changed_location(
     mut locations: Query<
         (

diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs
@@ -234,6 +234,7 @@ impl Plugin for SitePlugin {
                     .with_system(update_floor_visibility)
                     .with_system(add_lane_visuals)
                     .with_system(add_location_visuals)
+                    .with_system(add_robot_to_spawn_location)
                     .with_system(update_level_visibility)
                     .with_system(update_changed_lane)
                     .with_system(update_lane_for_moved_anchor)

diff --git a/rmf_site_format/src/legacy/vertex.rs b/rmf_site_format/src/legacy/vertex.rs
@@ -60,7 +60,7 @@ impl Vertex {
         if !me.spawn_robot_name.is_empty() && !me.spawn_robot_type.is_empty() {
             tags.push(LocationTag::SpawnRobot(Model {
                 name: NameInSite(me.spawn_robot_name.1.clone()),
-                source: AssetSource::Search(me.spawn_robot_type.1.clone()),
+                source: AssetSource::Search("OpenRobotics/".to_string() + &me.spawn_robot_type.1),
                 pose: Pose::default(),
                 is_static: IsStatic(false),
                 constraints: ConstraintDependents::default(),