From 479ba050739912febc54b93ca8ecdafdd00e5fed Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 11 May 2023 16:20:34 +0800 Subject: [PATCH 1/2] Add support for spawning robots in locations Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/location.rs | 63 ++++++++++++++++++++++++++++ rmf_site_editor/src/site/mod.rs | 1 + rmf_site_format/src/legacy/vertex.rs | 2 +- 3 files changed, 65 insertions(+), 1 deletion(-) 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(), From f60ae2f3eb37249fec017a52d80a4a0dff60e4ca Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 28 Jun 2023 10:52:46 +0800 Subject: [PATCH 2/2] Update model names, cleanup logic Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/location.rs | 59 ++++++++++++++-------------- rmf_site_editor/src/site/model.rs | 11 ++++-- 2 files changed, 37 insertions(+), 33 deletions(-) diff --git a/rmf_site_editor/src/site/location.rs b/rmf_site_editor/src/site/location.rs index d19e68f7..3b5a0d22 100644 --- a/rmf_site_editor/src/site/location.rs +++ b/rmf_site_editor/src/site/location.rs @@ -101,7 +101,7 @@ pub fn add_robot_to_spawn_location( Changed, >, levels: Query<(), With>, - mut models: Query<&mut AssetSource, With>, + mut models: Query<(&mut NameInSite, &mut AssetSource), With>, anchors: AnchorParams, parents: Query<&Parent>, ) { @@ -112,38 +112,37 @@ pub fn add_robot_to_spawn_location( 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)); + if let Some(m) = location_tags.iter().find_map(|l| match l { + LocationTag::SpawnRobot(m) => Some(m), + _ => None, + }) { + if let Some(location_model) = location_model { + // Update existing model + if let Ok((mut name, mut source)) = models.get_mut(location_model.0) { + *name = m.name.clone(); + *source = m.source.clone(); } - found_robot = true; + } 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)); + } + } else { + if let Some(location_model) = location_model { + commands.entity(location_model.0).despawn_recursive(); + commands.entity(e).remove::(); } - } - } - if !found_robot { - if let Some(location_model) = location_model { - commands.entity(location_model.0).despawn_recursive(); - commands.entity(e).remove::(); } } } diff --git a/rmf_site_editor/src/site/model.rs b/rmf_site_editor/src/site/model.rs index e7d6aa25..af09a3be 100644 --- a/rmf_site_editor/src/site/model.rs +++ b/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, With)>, + changed_models: Query< + (Entity, &AssetSource, Option<&ModelScene>), + (Changed, With), + >, mut loading_models: Query< ( Entity, @@ -272,9 +275,11 @@ pub fn update_model_tentative_formats( >, asset_server: Res, ) { - 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() {