Refactor RobotProperty implementation #385
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,92 @@ | ||
| /* | ||
| * Copyright (C) 2025 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::site::{AssetSource, IsStatic, ModelProperty, Scale}; | ||
| use bevy::{ | ||
| ecs::{component::ComponentId, system::EntityCommands}, | ||
| prelude::*, | ||
| }; | ||
| use std::collections::HashMap; | ||
|
|
||
| /// Function that inserts a default property into an entity | ||
| pub type InsertModelPropertyFn = fn(EntityCommands); | ||
|
|
||
| pub fn get_insert_model_property_fn<T: Component + Default>() -> InsertModelPropertyFn { | ||
| |mut e_commands| { | ||
| e_commands.insert(T::default()); | ||
| } | ||
| } | ||
|
|
||
| /// Function that removes a property, if it exists, from an entity | ||
| pub type RemoveModelPropertyFn = fn(EntityCommands); | ||
|
|
||
| pub fn get_remove_model_property_fn<T: Component + Default>() -> RemoveModelPropertyFn { | ||
| |mut e_commands| { | ||
| e_commands.remove::<T>(); | ||
| } | ||
| } | ||
|
|
||
| /// This resource keeps track of all the properties that can be configured for a model description. | ||
| #[derive(Resource)] | ||
| pub struct ModelPropertyData { | ||
| pub required: HashMap<ComponentId, (String, InsertModelPropertyFn, RemoveModelPropertyFn)>, | ||
| pub optional: HashMap<ComponentId, (String, InsertModelPropertyFn, RemoveModelPropertyFn)>, | ||
| } | ||
|
|
||
| impl FromWorld for ModelPropertyData { | ||
| fn from_world(world: &mut World) -> Self { | ||
| let mut required = HashMap::new(); | ||
| world.register_component::<ModelProperty<AssetSource>>(); | ||
| required.insert( | ||
| world | ||
| .components() | ||
| .component_id::<ModelProperty<AssetSource>>() | ||
| .unwrap(), | ||
| ( | ||
| "Asset Source".to_string(), | ||
| get_insert_model_property_fn::<ModelProperty<AssetSource>>(), | ||
| get_remove_model_property_fn::<ModelProperty<AssetSource>>(), | ||
| ), | ||
| ); | ||
| world.register_component::<ModelProperty<Scale>>(); | ||
| required.insert( | ||
| world | ||
| .components() | ||
| .component_id::<ModelProperty<Scale>>() | ||
| .unwrap(), | ||
| ( | ||
| "Scale".to_string(), | ||
| get_insert_model_property_fn::<ModelProperty<Scale>>(), | ||
| get_remove_model_property_fn::<ModelProperty<Scale>>(), | ||
| ), | ||
| ); | ||
| world.register_component::<ModelProperty<IsStatic>>(); | ||
| required.insert( | ||
| world | ||
| .components() | ||
| .component_id::<ModelProperty<IsStatic>>() | ||
| .unwrap(), | ||
| ( | ||
| "Is Static".to_string(), | ||
| get_insert_model_property_fn::<IsStatic>(), | ||
| get_remove_model_property_fn::<IsStatic>(), | ||
| ), | ||
| ); | ||
| let optional = HashMap::new(); | ||
| Self { required, optional } | ||
| } | ||
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -43,11 +43,17 @@ impl Plugin for SlotcarSdfPlugin { | |
| } | ||
| } | ||
|
|
||
| /// When loading SDF models, some RobotPropertyKinds may be directly inserted into | ||
| /// a model instance descendant instead of the instance or affiliated description. | ||
| /// This system checks for such cases and inserts the respective components into | ||
| /// the affiliated model description (if there is no other RobotProperty data | ||
| /// present) and remove it from the original entity. | ||
| fn insert_slotcar_components( | ||
| mut commands: Commands, | ||
| mut change_robot_property: EventWriter<Change<ModelProperty<Robot>>>, | ||
| is_static: Query<&ModelProperty<IsStatic>, (With<ModelMarker>, With<Group>)>, | ||
| robot_property_kinds: Query< | ||
| // All 4 components will be present (see sdf_loader.rs) | ||
| ( | ||
| Entity, | ||
| &DifferentialDrive, | ||
|
|
@@ -57,7 +63,14 @@ fn insert_slotcar_components( | |
| ), | ||
| (Without<ModelMarker>, Without<Group>), | ||
| >, | ||
| robot_properties: Query< | ||
| ( | ||
| Option<&Mobility>, | ||
| Option<&PowerSource>, | ||
| Option<&PowerDissipation>, | ||
| ), | ||
| (With<ModelMarker>, With<Group>), | ||
| >, | ||
| model_descriptions: Query<&ModelProperty<Robot>, (With<ModelMarker>, With<Group>)>, | ||
| model_instances: ModelPropertyQuery<Robot>, | ||
| child_of: Query<&ChildOf>, | ||
|
|
@@ -66,18 +79,11 @@ fn insert_slotcar_components( | |
| robot_property_kinds.iter() | ||
| { | ||
| if !model_descriptions.get(e).is_ok() { | ||
luca-della-vedova marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| let mut description_entity: Option<Entity> = None; | ||
| let mut target_entity: Entity = e; | ||
| while let Ok(parent) = child_of.get(target_entity).map(|co| co.parent()) { | ||
| if let Some(desc) = model_instances.get(parent).ok().and_then(|a| a.0) { | ||
luca-della-vedova marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| description_entity = Some(desc); | ||
| break; | ||
| } | ||
| target_entity = parent; | ||
|
|
@@ -94,45 +100,61 @@ fn insert_slotcar_components( | |
| // and Robot will result in the later system run to overwrite changes | ||
| // from the earlier system. For now we will process data from Battery/ | ||
| // DifferentialDrive/MechanicalSystem/AmbientSystem in a single system. | ||
|
|
||
| // Only allow overwriting RobotProperties that are not present | ||
| let Ok((mobility, power_source, power_dissipation)) = robot_properties.get(desc) | ||
|
||
| else { | ||
| continue; | ||
| }; | ||
|
|
||
| if mobility.is_none() && is_static.get(desc).is_ok_and(|is| !is.0 .0) { | ||
xiyuoh marked this conversation as resolved.
Outdated
Show resolved
Hide resolved
|
||
| if let Ok(mobility_value) = serialize_robot_property_from_kind::< | ||
| Mobility, | ||
| DifferentialDrive, | ||
| >(differential_drive.clone()) | ||
| { | ||
| robot.properties.insert(Mobility::label(), mobility_value); | ||
| } | ||
| } | ||
|
|
||
| if power_source.is_none() { | ||
| if let Ok(power_source_value) = | ||
| serialize_robot_property_from_kind::<PowerSource, Battery>(battery.clone()) | ||
| { | ||
| robot | ||
| .properties | ||
| .insert(PowerSource::label(), power_source_value); | ||
| } | ||
| } | ||
|
|
||
| if power_dissipation.is_none() { | ||
| let mut power_dissipation_config = Map::new(); | ||
| if let Some(power_dissipation_map) = power_dissipation_config | ||
| .entry("config") | ||
| .or_insert(Value::Object(Map::new())) | ||
| .as_object_mut() | ||
luca-della-vedova marked this conversation as resolved.
Outdated
Show resolved
Hide resolved
|
||
| { | ||
| if let Ok(mechanical_system_value) = | ||
| serialize_robot_property_kind::<MechanicalSystem>( | ||
| mechanical_system.clone(), | ||
| ) | ||
| { | ||
| power_dissipation_map | ||
| .insert(MechanicalSystem::label(), mechanical_system_value); | ||
| } | ||
| if let Ok(ambient_system_value) = | ||
| serialize_robot_property_kind::<AmbientSystem>(ambient_system.clone()) | ||
| { | ||
| power_dissipation_map | ||
| .insert(AmbientSystem::label(), ambient_system_value); | ||
| } | ||
| } | ||
| if !power_dissipation_config.is_empty() { | ||
| robot.properties.insert( | ||
| PowerDissipation::label(), | ||
| Value::Object(power_dissipation_config), | ||
| ); | ||
| } | ||
| } | ||
|
|
||
| change_robot_property.write(Change::new(ModelProperty(robot), desc)); | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This looks a bit odd. What the SDF loader does is serialize the whole content of the world into a bevy::Scene then later we will spawn the
Scene.I think the scene is later spawned through write_to_world_with and that only spawns entities and components, not commands.
So I wonder, are you sure the command here is being applied?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
So this is the main change that fixes the "saved robot properties are being overwritten by the SDF loader" bug for me. If I undo this change
cargo testwould fail, and after some logging it turned out the original and destination JSONs are different, withRobotPropertybeing overwritten. This is what I think is happening withoutcommands():load.rsinserts theRobotcomponent into relevant model description entities. This would prepare the linked systems to insert appropriateRobotProperty(here) andRobotPropertyKind(here) components.Worldas where everything else is.RobotPropertyKindcomponents, and insert them into the appropriate affiliated description entities. This is done by writing aChangeevent with the updatedRobotserialized properties.ModelProperty<Robot>in the next cycle, and eventually propagates to the affiliated model instances via the update_model_instances system.TLDR I think steps (1) to (3) take place in cycle N, and step (4) takes place in cycle N+1. This means the inserted components in (1) are not known to
insert_slotcar_componentsin step (3) and will be overwritten. By usingcommands()we're essentially forcing step 3. to only find slotcar models with robot properties in cycle N+1, and be able to query components from step 1. to check if properties are already present.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
My assertion here is that the world you see here is not the
App's world, but a brand new one that is immediately serialized into a scene, and in this process only entities and components are saved, the commands are effectively dropped.I tested this by actually removing the whole statement that uses a command to add components altogether and verified that the unit test still passes. This makes me think that our unit test doesn't actually verify that this statement is working
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for pointing that out, I just tried opening the demo map and the tinyRobots do not have the properties inserted in. Let me think of a more reliable fix!