From b9a28975c8be57e349b482d7bb3c288fd300907a Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 24 Jul 2023 23:28:55 +0800 Subject: [PATCH] Loading and saving model instances in the editor Signed-off-by: Michael X. Grey --- rmf_site_editor/src/site/load.rs | 73 +++++++- rmf_site_editor/src/site/location.rs | 89 ---------- rmf_site_editor/src/site/mod.rs | 7 +- rmf_site_editor/src/site/models.rs | 152 ++++++++++++++++ rmf_site_editor/src/site/save.rs | 167 +++++++++++++++++- .../src/widgets/inspector/inspect_location.rs | 166 +++-------------- .../inspector/inspect_option_string.rs | 22 ++- rmf_site_editor/src/widgets/mod.rs | 3 +- rmf_site_format/src/instance.rs | 39 ++++ rmf_site_format/src/legacy/building_map.rs | 29 ++- rmf_site_format/src/legacy/nav_graph.rs | 10 +- rmf_site_format/src/legacy/vertex.rs | 80 +++++++-- rmf_site_format/src/lib.rs | 6 + rmf_site_format/src/location.rs | 154 +++------------- rmf_site_format/src/mobile_robot.rs | 19 +- rmf_site_format/src/scenario.rs | 23 ++- rmf_site_format/src/site.rs | 3 + rmf_site_format/src/stationary_robot.rs | 36 ++++ 18 files changed, 667 insertions(+), 411 deletions(-) create mode 100644 rmf_site_editor/src/site/models.rs create mode 100644 rmf_site_format/src/instance.rs create mode 100644 rmf_site_format/src/stationary_robot.rs diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index c7544d52..1ad20c18 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -226,6 +226,28 @@ fn generate_site_entities(commands: &mut Commands, site_data: &rmf_site_format:: .id(); 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); } }); @@ -236,18 +258,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 { diff --git a/rmf_site_editor/src/site/location.rs b/rmf_site_editor/src/site/location.rs index 3b5a0d22..5526f00f 100644 --- a/rmf_site_editor/src/site/location.rs +++ b/rmf_site_editor/src/site/location.rs @@ -89,65 +89,6 @@ 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 NameInSite, &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()); - - if let Some(parent) = parent { - 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(); - } - } 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::(); - } - } - } - } -} - pub fn update_changed_location( mut locations: Query< ( @@ -284,33 +225,3 @@ pub fn update_visibility_for_locations( } } } - -#[derive(Debug, Clone)] -pub struct ConsiderLocationTag { - pub tag: Option, - pub for_element: Entity, -} - -impl ConsiderLocationTag { - pub fn new(tag: Option, 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, -) { - 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); - } - } - } -} diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 6c3a3fc6..e4fb8c82 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/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::() .add_event::() .add_event::() - .add_event::() + .add_plugin(ModelSourcePlugin) .add_plugin(ChangePlugin::>::default()) .add_plugin(RecallPlugin::>::default()) .add_plugin(ChangePlugin::::default()) @@ -234,7 +237,6 @@ 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) @@ -249,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) diff --git a/rmf_site_editor/src/site/models.rs b/rmf_site_editor/src/site/models.rs new file mode 100644 index 00000000..45e97f97 --- /dev/null +++ b/rmf_site_editor/src/site/models.rs @@ -0,0 +1,152 @@ +/* + * 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 rmf_site_format::{AssetSource, Scale}; +use bevy::{ + prelude::*, + ecs::system::{Command, EntityCommands}, +}; + +#[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); +impl ModelInstances { + pub fn iter(&self) -> impl Iterator { + 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::() { + 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::() { + 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::() { + 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, + sources: Query<(&AssetSource, &Scale)>, + changed_sources: Query< + (&ModelInstances, &AssetSource, &Scale), + Or<(Changed, Changed)>, + >, +) { + 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::() + .add_system(handle_changed_model_sources); + } +} diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 3c047809..7c42b2eb 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -89,12 +89,20 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration >, Query, Without)>, Query>, Without)>, + Query< + (), + Or<( + With, + With, + With, + )>, + >, Query<&NextSiteID>, Query<&SiteID>, Query<&Children>, )> = SystemState::new(world); - let (level_children, nav_graph_elements, levels, lifts, sites, site_ids, children) = + let (level_children, nav_graph_elements, levels, lifts, scenarios, sites, site_ids, children) = state.get_mut(world); let mut new_entities = Vec::new(); @@ -147,6 +155,12 @@ fn assign_site_ids(world: &mut World, site: Entity) -> Result<(), SiteGeneration } } } + + if scenarios.contains(*site_child) { + if !site_ids.contains(*site_child) { + new_entities.push(*site_child); + } + } } let mut next_site_id = sites @@ -840,6 +854,152 @@ fn generate_locations( Ok(locations) } +fn generate_scenarios( + world: &mut World, + site: Entity, +) -> Result<(BTreeMap, Models), SiteGenerationError> { + let models = { + let mut state: SystemState<( + Query< + ( + &NameInSite, + &AssetSource, + &Scale, + &MobileRobotKinematics, + &SiteID, + &Parent, + ) + >, + Query< + ( + &NameInSite, + &AssetSource, + &Scale, + &SiteID, + &Parent, + ), + With, + >, + )> = SystemState::new(world); + + let (q_mobile, q_stationary) = state.get(world); + let mut models = Models::default(); + for (model_name, source, scale, kinematics, site_id, parent) in &q_mobile { + if parent.get() != site { + continue; + } + + models.mobile_robots.insert(site_id.0, MobileRobot { + model_name: model_name.clone(), + source: source.clone(), + scale: scale.clone(), + kinematics: kinematics.clone(), + }); + } + + for (model_name, source, scale, site_id, parent) in &q_stationary { + if parent.get() != site { + continue; + } + + models.workcells.insert(site_id.0, StationaryRobot { + model_name: model_name.clone(), + source: source.clone(), + scale: scale.clone(), + marker: StationaryRobotMarker, + }); + } + + models + }; + + let scenarios = { + let mut state: SystemState<( + Query< + ( + &ScenarioProperties, + &SiteID, + &Parent, + ) + >, + Query< + ( + &NameInSite, + &Pose, + &Parent, + &ModelSource, + &InScenario, + &SiteID, + ) + >, + Query<&SiteID>, + )> = SystemState::new(world); + + let (q_scenarios, q_instances, q_site_id) = state.get(world); + + let mut scenarios = BTreeMap::new(); + for (properties, site_id, parent) in &q_scenarios { + if parent.get() != site { + continue; + } + + scenarios.insert(site_id.0, Scenario { + properties: properties.clone(), + instances: BTreeMap::new(), + }); + } + + for (name, pose, parent, model, in_scenario, site_id) in &q_instances { + let Ok((_, scenario_id, _)) = q_scenarios.get(in_scenario.0) else { + continue; + }; + let Some(scenario) = scenarios.get_mut(&scenario_id.0) else { + continue; + }; + + let Ok(parent) = q_site_id.get(parent.get()) else { + error!( + "Unable to save instance {} because its parent does not \ + have a SiteID", + name.0, + ); + continue; + }; + let parent = parent.0; + + let Ok(model) = q_site_id.get(model.get()) else { + error!( + "Unable to save instance {} because its model does not \ + have a SiteID", + name.0, + ); + continue; + }; + let model = model.0; + if !models.contains_key(&model) { + error!( + "Unable to save instance {} because its model is not part \ + of the site that is being saved", + model, + ); + } + + scenario.instances.insert(site_id.0, Instance { + parent, + model, + bundle: InstanceBundle { + name: name.clone(), + pose: pose.clone() + }, + }); + } + + scenarios + }; + + Ok((scenarios, models)) +} + fn generate_graph_rankings( world: &mut World, site: Entity, @@ -876,6 +1036,7 @@ pub fn generate_site( let nav_graphs = generate_nav_graphs(world, site)?; let lanes = generate_lanes(world, site)?; let locations = generate_locations(world, site)?; + let (scenarios, models) = generate_scenarios(world, site)?; let graph_ranking = generate_graph_rankings(world, site)?; let props = match world.get::(site) { @@ -899,8 +1060,8 @@ pub fn generate_site( locations, }, }, - // TODO(MXG): Parse agent information once the spec is figured out - agents: Default::default(), + models, + scenarios, }); } diff --git a/rmf_site_editor/src/widgets/inspector/inspect_location.rs b/rmf_site_editor/src/widgets/inspector/inspect_location.rs index b3e93aa7..db84ce2d 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_location.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_location.rs @@ -16,12 +16,9 @@ */ use crate::{ - site::{ - ConsiderLocationTag, LocationTag, LocationTags, Model, RecallAssetSource, - RecallLocationTags, - }, + site::{LocationTags, Model, RecallAssetSource, RecallLocationTags}, widgets::{ - inspector::{InspectAssetSource, InspectName}, + inspector::{InspectAssetSource, InspectName, InspectOptionString}, AppEvents, Icons, }, }; @@ -56,144 +53,33 @@ impl<'a, 'w1, 'w2, 's2> InspectLocationWidget<'a, 'w1, 'w2, 's2> { pub fn show(self, ui: &mut Ui) -> Option { ui.label(RichText::new("Location Tags").size(18.0)); - let mut deleted_tag = None; - let mut changed_tag = None; - for (i, tag) in self.tags.0.iter().enumerate() { - ui.horizontal(|ui| { - if ui - .add(ImageButton::new(self.icons.trash.egui(), [18., 18.])) - .clicked() - { - deleted_tag = Some(i); - } - ui.label(tag.label()); - }); - match tag { - LocationTag::SpawnRobot(robot) => { - ui.push_id(i.to_string() + " spawn robot", |ui| { - if let Some(new_robot) = - self.inspect_model(ui, robot, &self.recall.robot_asset_source_recall) - { - changed_tag = Some((i, LocationTag::SpawnRobot(new_robot))); - } - }); - } - LocationTag::Workcell(cell) => { - ui.push_id(i.to_string() + " workcell", |ui| { - if let Some(new_cell) = - self.inspect_model(ui, cell, &self.recall.workcell_asset_source_recall) - { - changed_tag = Some((i, LocationTag::Workcell(new_cell))); - } - }); + let changed_charger = InspectOptionString::new( + "Charger", + &self.tags.charger, + &self.recall.recall_charger, + ).multiline().default("").show(ui); + let changed_parking = InspectOptionString::new( + "Parking", + &self.tags.parking, + &self.recall.recall_parking, + ).multiline().default("").show(ui); + let changed_holding = InspectOptionString::new( + "Holding", + &self.tags.holding, + &self.recall.recall_holding, + ).multiline().default("").show(ui); + + if changed_charger.is_some() || changed_parking.is_some() || changed_holding.is_some() { + return Some( + LocationTags { + charger: changed_charger.unwrap_or_else(|| self.tags.charger.clone()), + parking: changed_parking.unwrap_or_else(|| self.tags.parking.clone()), + holding: changed_holding.unwrap_or_else(|| self.tags.holding.clone()), } - _ => {} - } - ui.add_space(5.0); - ui.separator(); - ui.add_space(5.0); + ) } - let added_tag = ui - .collapsing("Add...", |ui| { - let (add, mut consider) = ui - .horizontal(|ui| { - let add = ui.button("Confirm").clicked(); - let mut consider = self.recall.assume_tag(self.tags); - let mut variants: SmallVec<[LocationTag; 5]> = SmallVec::new(); - if self.tags.iter().find(|t| t.is_charger()).is_none() { - variants.push(LocationTag::Charger); - } - if self.tags.iter().find(|t| t.is_parking_spot()).is_none() { - variants.push(LocationTag::ParkingSpot); - } - if self.tags.iter().find(|t| t.is_holding_point()).is_none() { - variants.push(LocationTag::HoldingPoint); - } - variants.push(self.recall.assume_spawn_robot()); - variants.push(self.recall.assume_workcell()); - - ComboBox::from_id_source("Add Location Tag") - .selected_text(consider.label()) - .show_ui(ui, |ui| { - for variant in variants { - let label = variant.label(); - ui.selectable_value(&mut consider, variant, label); - } - }); - - (add, consider) - }) - .inner; - - match &mut consider { - LocationTag::SpawnRobot(model) => { - ui.push_id("consider spawn robot", |ui| { - if let Some(new_model) = self.inspect_model( - ui, - model, - &self.recall.consider_tag_asset_source_recall, - ) { - *model = new_model; - } - }); - } - LocationTag::Workcell(model) => { - ui.push_id("consider workcell", |ui| { - if let Some(new_model) = self.inspect_model( - ui, - model, - &self.recall.consider_tag_asset_source_recall, - ) { - *model = new_model; - } - }); - } - _ => {} - } - - let consider_changed = if let Some(original) = &self.recall.consider_tag { - consider != *original - } else { - true - }; - if consider_changed { - self.events - .request - .consider_tag - .send(ConsiderLocationTag::new( - Some(consider.clone()), - self.entity, - )); - } - - if add { - Some(consider) - } else { - None - } - }) - .body_returned - .flatten(); - - if deleted_tag.is_some() || added_tag.is_some() || changed_tag.is_some() { - let mut new_tags = self.tags.clone(); - if let Some(i) = deleted_tag { - new_tags.remove(i); - } - - if let Some(new_tag) = added_tag { - new_tags.push(new_tag); - } - - if let Some((i, tag)) = changed_tag { - new_tags[i] = tag; - } - - Some(new_tags) - } else { - None - } + return None; } fn inspect_model( diff --git a/rmf_site_editor/src/widgets/inspector/inspect_option_string.rs b/rmf_site_editor/src/widgets/inspector/inspect_option_string.rs index 490b5948..4d4c6d74 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_option_string.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_option_string.rs @@ -21,6 +21,8 @@ pub struct InspectOptionString<'a> { title: &'a str, value: &'a Option, recall: &'a Option, + default: &'a str, + multiline: bool, } impl<'a> InspectOptionString<'a> { @@ -29,9 +31,21 @@ impl<'a> InspectOptionString<'a> { title, value, recall, + default: "", + multiline: false, } } + pub fn multiline(mut self) -> Self { + self.multiline = true; + self + } + + pub fn default(mut self, default: &'a str) -> Self { + self.default = default; + self + } + pub fn show(self, ui: &mut Ui) -> Option> { ui.horizontal(|ui| { let mut has_value = self.value.is_some(); @@ -42,9 +56,13 @@ impl<'a> InspectOptionString<'a> { self.recall .as_ref() .map(|x| x.clone()) - .unwrap_or_else(|| "".to_string()) + .unwrap_or_else(|| self.default.to_owned()) }); - ui.text_edit_singleline(&mut assumed_value); + if self.multiline { + ui.text_edit_multiline(&mut assumed_value); + } else { + ui.text_edit_singleline(&mut assumed_value); + } let new_value = Some(assumed_value); if new_value != *self.value { diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 3fef1efc..d0434c1e 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -23,7 +23,7 @@ use crate::{ occupancy::CalculateGrid, recency::ChangeRank, site::{ - AssociatedGraphs, Change, ConsiderAssociatedGraph, ConsiderLocationTag, CurrentLevel, + AssociatedGraphs, Change, ConsiderAssociatedGraph, CurrentLevel, Delete, ExportLights, FloorVisibility, PhysicalLightToggle, SaveNavGraphs, SiteState, ToggleLiftDoorAvailability, }, @@ -160,7 +160,6 @@ pub struct Requests<'w, 's> { pub export_lights: EventWriter<'w, 's, ExportLights>, pub save_nav_graphs: EventWriter<'w, 's, SaveNavGraphs>, pub calculate_grid: EventWriter<'w, 's, CalculateGrid>, - pub consider_tag: EventWriter<'w, 's, ConsiderLocationTag>, pub consider_graph: EventWriter<'w, 's, ConsiderAssociatedGraph>, } diff --git a/rmf_site_format/src/instance.rs b/rmf_site_format/src/instance.rs new file mode 100644 index 00000000..3bf81294 --- /dev/null +++ b/rmf_site_format/src/instance.rs @@ -0,0 +1,39 @@ +/* + * 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 crate::*; +#[cfg(feature = "bevy")] +use bevy::prelude::Bundle; +use serde::{Deserialize, Serialize}; + +#[derive(Serialize, Deserialize, Debug, Clone)] +pub struct Instance { + /// This may be the entity ID of a level or an anchor. + pub parent: u32, + /// The entity ID of the model (e.g. MobileRobot, StationaryRobot) + /// that this is instantiating. + pub model: u32, + #[serde(flatten)] + pub bundle: InstanceBundle, +} + +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct InstanceBundle { + pub name: NameInSite, + pub pose: Pose, +} diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs index 87a20e63..586e1653 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -5,7 +5,8 @@ use crate::{ Fiducial as SiteFiducial, FiducialMarker, Guided, Label, Lane as SiteLane, LaneMarker, Level as SiteLevel, LevelProperties as SiteLevelProperties, Motion, NameInSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose, RankingsInLevel, ReverseLane, - Rotation, Site, SiteProperties, DEFAULT_NAV_GRAPH_COLORS, + Rotation, Site, SiteProperties, DEFAULT_NAV_GRAPH_COLORS, Models, Instance, + Scenario, ScenarioProperties, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; @@ -126,6 +127,9 @@ impl BuildingMap { let mut level_name_to_id = BTreeMap::new(); let mut lanes = BTreeMap::>::new(); let mut locations = BTreeMap::new(); + let mut models = Models::default(); + let mut model_source_map: HashMap = HashMap::new(); + let mut instances: BTreeMap = BTreeMap::new(); let mut lift_cabin_anchors: BTreeMap> = BTreeMap::new(); @@ -168,6 +172,16 @@ impl BuildingMap { if let Some(location) = v.make_location(anchor_id) { locations.insert(site_id.next().unwrap(), location); } + + if let Some(instance) = v.make_instance( + &mut site_id, + &mut model_source_map, + &mut models, + anchor_id, + ) { + let instance_id = site_id.next().unwrap(); + instances.insert(instance_id, instance); + } } let mut doors = BTreeMap::new(); @@ -375,6 +389,16 @@ impl BuildingMap { ); } + let default_scenario_id = site_id.next().unwrap(); + let mut scenarios: BTreeMap = BTreeMap::new(); + scenarios.insert( + default_scenario_id, + Scenario { + properties: ScenarioProperties { name: "Default Scenario".to_owned() }, + instances, + } + ); + Ok(Site { format_version: Default::default(), anchors: site_anchors, @@ -391,7 +415,8 @@ impl BuildingMap { locations, }, }, - agents: Default::default(), + models, + scenarios, }) } } diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs index 11a1babe..f7b1b9b7 100644 --- a/rmf_site_format/src/legacy/nav_graph.rs +++ b/rmf_site_format/src/legacy/nav_graph.rs @@ -172,13 +172,9 @@ impl NavVertexProperties { None => return props, }; props.name = location.name.0.clone(); - props.is_charger = location.tags.iter().find(|t| t.is_charger()).is_some(); - props.is_holding_point = location - .tags - .iter() - .find(|t| t.is_holding_point()) - .is_some(); - props.is_parking_spot = location.tags.iter().find(|t| t.is_parking_spot()).is_some(); + props.is_charger = location.tags.charger.is_some(); + props.is_holding_point = location.tags.holding.is_some(); + props.is_parking_spot = location.tags.parking.is_some(); props } diff --git a/rmf_site_format/src/legacy/vertex.rs b/rmf_site_format/src/legacy/vertex.rs index 8a74f69c..7241476e 100644 --- a/rmf_site_format/src/legacy/vertex.rs +++ b/rmf_site_format/src/legacy/vertex.rs @@ -1,10 +1,13 @@ use super::rbmf::*; use crate::{ - is_default, AssetSource, AssociatedGraphs, ConstraintDependents, IsStatic, Location, - LocationTag, LocationTags, Model, ModelMarker, NameInSite, Pose, Scale, + is_default, AssetSource, AssociatedGraphs, ConstraintDependents, IsStatic, + Location, LocationTags, Model, ModelMarker, NameInSite, Pose, Scale, + Instance, InstanceBundle, Models, MobileRobot, MobileRobotKinematics, + DifferentialDrive, }; use glam::DVec2; use serde::{Deserialize, Serialize}; +use std::collections::HashMap; #[derive(Deserialize, Serialize, Clone, Default)] pub struct VertexProperties { @@ -43,30 +46,18 @@ impl Vertex { } pub fn make_location(&self, anchor: u32) -> Option> { - let mut tags = Vec::new(); + let mut tags = LocationTags::default(); let me = &self.4; if me.is_charger.1 { - tags.push(LocationTag::Charger); + tags.charger = Some(String::new()); } if me.is_parking_spot.1 { - tags.push(LocationTag::ParkingSpot); + tags.parking = Some(String::new()); } if me.is_holding_point.1 { - tags.push(LocationTag::HoldingPoint); - } - - 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("OpenRobotics/".to_string() + &me.spawn_robot_type.1), - pose: Pose::default(), - is_static: IsStatic(false), - constraints: ConstraintDependents::default(), - scale: Scale::default(), - marker: ModelMarker, - })) + tags.holding = Some(String::new()); } let name = if self.3.is_empty() { @@ -80,10 +71,61 @@ impl Vertex { } else { return Some(Location { anchor: anchor.into(), - tags: LocationTags(tags), + tags, name: NameInSite(name.unwrap_or("".to_string())), graphs: AssociatedGraphs::All, }); } } + + pub fn make_instance( + &self, + new_site_id: &mut std::ops::RangeFrom, + model_source_map: &mut HashMap, + models: &mut Models, + anchor: u32, + ) -> Option { + let me = &self.4; + if !me.spawn_robot_name.is_empty() && !me.spawn_robot_type.is_empty() { + if let Some(model_id) = model_source_map.get(&me.spawn_robot_type.1) { + return Some(Instance { + parent: anchor, + model: *model_id, + bundle: InstanceBundle { + name: NameInSite(me.spawn_robot_name.1.clone()), + pose: Pose::default(), + } + }); + } + + // Create a new model for this asset source that we have not seen + // before. + let model_id = new_site_id.next().unwrap(); + let mobile_robot = MobileRobot { + model_name: NameInSite(me.spawn_robot_type.1.clone()), + source: AssetSource::Search("OpenRobotics/".to_owned() + &me.spawn_robot_type.1.clone()), + scale: Scale::default(), + kinematics: MobileRobotKinematics::DifferentialDrive( + DifferentialDrive { + translational_speed: 0.5, + rotational_speed: 0.6, + bidirectional: false, + } + ) + }; + + models.mobile_robots.insert(model_id, mobile_robot); + model_source_map.insert(me.spawn_robot_type.1.clone(), model_id); + return Some(Instance { + parent: anchor, + model: model_id, + bundle: InstanceBundle { + name: NameInSite(me.spawn_robot_name.1.clone()), + pose: Pose::default(), + } + }); + } + + return None; + } } diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index b45ffdaf..eea3a1d8 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -45,6 +45,9 @@ pub use fiducial::*; pub mod floor; pub use floor::*; +pub mod instance; +pub use instance::*; + pub mod lane; pub use lane::*; @@ -99,6 +102,9 @@ pub use semver::*; pub mod site; pub use site::*; +pub mod stationary_robot; +pub use stationary_robot::*; + pub mod texture; pub use texture::*; diff --git a/rmf_site_format/src/location.rs b/rmf_site_format/src/location.rs index a694876c..b1b56298 100644 --- a/rmf_site_format/src/location.rs +++ b/rmf_site_format/src/location.rs @@ -19,67 +19,32 @@ use crate::*; #[cfg(feature = "bevy")] use bevy::prelude::{Bundle, Component, Deref, DerefMut, Entity}; use serde::{Deserialize, Serialize}; - -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] -pub enum LocationTag { - Charger, - ParkingSpot, - HoldingPoint, - SpawnRobot(Model), - Workcell(Model), -} - -impl LocationTag { - pub fn label(&self) -> &'static str { - match self { - Self::Charger => "Charger", - Self::ParkingSpot => "Parking Spot", - Self::HoldingPoint => "Holding Point", - Self::SpawnRobot(_) => "Spawn Robot", - Self::Workcell(_) => "Workcell", - } - } - - pub fn is_charger(&self) -> bool { - matches!(self, Self::Charger) - } - pub fn is_parking_spot(&self) -> bool { - matches!(self, Self::ParkingSpot) - } - pub fn is_holding_point(&self) -> bool { - matches!(self, Self::HoldingPoint) - } - pub fn spawn_robot(&self) -> Option<&Model> { - match self { - Self::SpawnRobot(model) => Some(model), - _ => None, - } - } - pub fn workcell(&self) -> Option<&Model> { - match self { - Self::Workcell(model) => Some(model), - _ => None, - } - } -} +use std::collections::{BTreeSet, HashSet, HashMap}; #[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Bundle))] pub struct Location { pub anchor: Point, + #[serde(flatten)] pub tags: LocationTags, pub name: NameInSite, pub graphs: AssociatedGraphs, } -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] -#[serde(transparent)] -#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] -pub struct LocationTags(pub Vec); +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Default)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct LocationTags { + #[serde(default, skip_serializing_if = "Option::is_none")] + pub charger: Option, + #[serde(default, skip_serializing_if = "Option::is_none")] + pub parking: Option, + #[serde(default, skip_serializing_if = "Option::is_none")] + pub holding: Option, +} -impl Default for LocationTags { - fn default() -> Self { - LocationTags(Vec::new()) +impl LocationTags { + pub fn is_empty(&self) -> bool { + self.charger.is_none() && self.parking.is_none() && self.holding.is_none() } } @@ -112,88 +77,25 @@ impl From> for Location { #[derive(Default, Debug, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Component))] pub struct RecallLocationTags { - pub robot_asset_source_recall: RecallAssetSource, - pub robot_asset_source: Option, - pub workcell_asset_source_recall: RecallAssetSource, - pub workcell_asset_source: Option, - pub robot_name: Option, - pub workcell_name: Option, - pub consider_tag: Option, - pub consider_tag_asset_source_recall: RecallAssetSource, -} - -impl RecallLocationTags { - pub fn assume_tag(&self, current: &LocationTags) -> LocationTag { - if let Some(tag) = &self.consider_tag { - match tag { - LocationTag::Charger | LocationTag::HoldingPoint | LocationTag::ParkingSpot => { - // If the tag to consider is one of these three values, then - // only accept it if it does not already exist in the current - // tag list. - if current.0.iter().find(|t| **t == *tag).is_none() { - return tag.clone(); - } - } - _ => return tag.clone(), - } - } - if current.0.iter().find(|t| t.is_charger()).is_none() { - return LocationTag::Charger; - } - if current.0.iter().find(|t| t.is_parking_spot()).is_none() { - return LocationTag::ParkingSpot; - } - self.assume_spawn_robot() - } - pub fn assume_spawn_robot(&self) -> LocationTag { - let model = self - .consider_tag - .as_ref() - .map(|t| t.spawn_robot()) - .flatten() - .cloned() - .unwrap_or_else(|| Model { - name: self.robot_name.clone().unwrap_or_default(), - source: self.robot_asset_source.clone().unwrap_or_default(), - ..Default::default() - }); - LocationTag::SpawnRobot(model) - } - pub fn assume_workcell(&self) -> LocationTag { - let model = self - .consider_tag - .as_ref() - .map(|t| t.spawn_robot()) - .flatten() - .cloned() - .unwrap_or_else(|| Model { - name: self.workcell_name.clone().unwrap_or_default(), - source: self.workcell_asset_source.clone().unwrap_or_default(), - ..Default::default() - }); - LocationTag::Workcell(model) - } + pub recall_charger: Option, + pub recall_parking: Option, + pub recall_holding: Option, } impl Recall for RecallLocationTags { type Source = LocationTags; fn remember(&mut self, source: &Self::Source) { - for tag in &source.0 { - // TODO(MXG): Consider isolating this memory per element - match tag { - LocationTag::SpawnRobot(robot) => { - self.robot_asset_source_recall.remember(&robot.source); - self.robot_asset_source = Some(robot.source.clone()); - self.robot_name = Some(robot.name.clone()); - } - LocationTag::Workcell(cell) => { - self.workcell_asset_source_recall.remember(&cell.source); - self.workcell_asset_source = Some(cell.source.clone()); - self.workcell_name = Some(cell.name.clone()); - } - _ => {} - } + if let Some(charger) = &source.charger { + self.recall_charger = Some(charger.clone()); + } + + if let Some(parking) = &source.parking { + self.recall_parking = Some(parking.clone()); + } + + if let Some(holding) = &source.holding { + self.recall_holding = Some(holding.clone()); } } } diff --git a/rmf_site_format/src/mobile_robot.rs b/rmf_site_format/src/mobile_robot.rs index 4f283dc0..adb2d045 100644 --- a/rmf_site_format/src/mobile_robot.rs +++ b/rmf_site_format/src/mobile_robot.rs @@ -17,16 +17,13 @@ use crate::*; #[cfg(feature = "bevy")] -use bevy::prelude::{Component, Entity}; +use bevy::prelude::{Component, Bundle}; use serde::{Deserialize, Serialize}; use std::collections::BTreeMap; +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] pub struct MobileRobot { - pub properties: MobileRobotProperties, - pub instances: BTreeMap, -} - -pub struct MobileRobotProperties { pub model_name: NameInSite, pub source: AssetSource, #[serde(default, skip_serializing_if = "is_default")] @@ -34,19 +31,15 @@ pub struct MobileRobotProperties { pub kinematics: MobileRobotKinematics, } +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] pub enum MobileRobotKinematics { DifferentialDrive(DifferentialDrive), } +#[derive(Serialize, Deserialize, Debug, Clone)] pub struct DifferentialDrive { pub translational_speed: f32, pub rotational_speed: f32, pub bidirectional: bool, } - -pub struct MobileRobotInstance { - pub name: NameInSite, - pub pose: Pose, - #[serde(default, skip_serializing_if = "Option::is_none")] - pub anchor: Option>, -} diff --git a/rmf_site_format/src/scenario.rs b/rmf_site_format/src/scenario.rs index 9ed1b0f1..8d6da99d 100644 --- a/rmf_site_format/src/scenario.rs +++ b/rmf_site_format/src/scenario.rs @@ -35,14 +35,35 @@ impl Default for ScenarioProperties { } } -#[derive(Serailize, Deserialize, Debug, Clone, Default)] +#[derive(Serialize, Deserialize, Debug, Clone, Default)] pub struct Scenario { /// Basic properties of the scenario pub properties: ScenarioProperties, + /// Instances of models contained in the scenario + pub instances: BTreeMap, +} + +#[derive(Serialize, Deserialize, Debug, Clone, Default)] +pub struct Models { /// What mobile robots exist in the scenario #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub mobile_robots: BTreeMap, + /// What workcells exist in the scenario + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub workcells: BTreeMap, /// What agents exist in the scenario #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub agents: BTreeMap, } + +impl Models { + pub fn is_empty(&self) -> bool { + self.mobile_robots.is_empty() && self.workcells.is_empty() && self.agents.is_empty() + } + + pub fn contains_key(&self, key: &u32) -> bool { + self.mobile_robots.contains_key(key) + || self.workcells.contains_key(key) + || self.agents.contains_key(key) + } +} diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 91d4b45c..efd9abee 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -57,6 +57,9 @@ pub struct Site { /// Data related to navigation #[serde(default, skip_serializing_if = "Navigation::is_empty")] pub navigation: Navigation, + /// Models associated with this site + #[serde(default, skip_serializing_if = "Models::is_empty")] + pub models: Models, /// Scenarios associated with this site #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] pub scenarios: BTreeMap, diff --git a/rmf_site_format/src/stationary_robot.rs b/rmf_site_format/src/stationary_robot.rs new file mode 100644 index 00000000..c5e41995 --- /dev/null +++ b/rmf_site_format/src/stationary_robot.rs @@ -0,0 +1,36 @@ +/* + * 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 crate::*; +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Bundle}; +use serde::{Deserialize, Serialize}; +use std::collections::BTreeMap; + +#[derive(Serialize, Deserialize, Debug, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct StationaryRobot { + pub model_name: NameInSite, + pub source: AssetSource, + pub scale: Scale, + #[serde(skip)] + pub marker: StationaryRobotMarker, +} + +#[derive(Clone, Copy, Debug, Default, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct StationaryRobotMarker;