diff --git a/rmf_site_editor/src/site/location.rs b/rmf_site_editor/src/site/location.rs index 535bf5a4..d19e68f7 100644 --- 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, @@ -86,6 +89,66 @@ pub fn add_location_visuals( } } +pub fn add_robot_to_spawn_location( + mut commands: Commands, + locations: Query< + ( + Entity, + &Point, + &LocationTags, + Option<&LocationRobotModel>, + ), + Changed, + >, + levels: Query<(), With>, + mut models: Query<&mut AssetSource, With>, + 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::(); + } + } + } +} + 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 index fa526f75..6c3a3fc6 100644 --- 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 index d81d4742..8a74f69c 100644 --- 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(),