From bb43fcbffe39a7e59a8018fa4fd93b84e1cc9dd3 Mon Sep 17 00:00:00 2001 From: Grey Date: Fri, 14 Jun 2024 16:17:16 +0800 Subject: [PATCH] Use site ID to ensure unique file names (#219) * Use site ID to ensure unique file names Signed-off-by: Michael X. Grey * Fix style Signed-off-by: Michael X. Grey * Fix door names for toggle floors plugin Signed-off-by: Luca Della Vedova --------- Signed-off-by: Michael X. Grey Signed-off-by: Luca Della Vedova Co-authored-by: Luca Della Vedova --- rmf_site_editor/src/site/save.rs | 21 +++-- rmf_site_editor/src/site/sdf_exporter.rs | 80 ++++++++++++---- rmf_site_editor/src/workspace.rs | 2 + rmf_site_format/src/sdf.rs | 115 +++++++++++------------ 4 files changed, 133 insertions(+), 85 deletions(-) diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 0ab2ec9c..06e5bf0f 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -1278,6 +1278,19 @@ pub fn save_site(world: &mut World) { } ExportFormat::Sdf => { // TODO(luca) reduce code duplication with default exporting + + // Make sure to generate the site before anything else, because + // generating the site will ensure that all items are assigned a + // SiteID, and the SDF export process will not work correctly if + // any are unassigned. + let site = match generate_site(world, save_event.site) { + Ok(site) => site, + Err(err) => { + error!("Unable to compile site: {err}"); + continue; + } + }; + info!("Saving to {}", new_path.display()); let Some(parent_folder) = new_path.parent() else { error!("Unable to save SDF. Please select a save path that has a parent directory."); @@ -1309,14 +1322,6 @@ pub fn save_site(world: &mut World) { } migrate_relative_paths(save_event.site, &new_path, world); - - let site = match generate_site(world, save_event.site) { - Ok(site) => site, - Err(err) => { - error!("Unable to compile site: {err}"); - continue; - } - }; let graphs = legacy::nav_graph::NavGraph::from_site(&site); let sdf = match site.to_sdf() { Ok(sdf) => sdf, diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs index 5fa1da71..d3c14e74 100644 --- a/rmf_site_editor/src/site/sdf_exporter.rs +++ b/rmf_site_editor/src/site/sdf_exporter.rs @@ -53,7 +53,10 @@ pub fn headless_sdf_export( return; } if sites.is_empty() { - warn!("Site loading failed, aborting"); + warn!( + "Unable to load site from file [{}] so we cannot export an SDF from it", + export_state.target_path, + ); exit.send(bevy::app::AppExit); } if !missing_models.is_empty() { @@ -128,6 +131,13 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re std::fs::write(filename, bytes).map_err(|e| e.to_string()) }; + let get_site_id = |e: Entity| -> Result { + q_site_ids.get(e).map(|id| id.0).map_err(|_| { + let backtrace = std::backtrace::Backtrace::force_capture(); + format!("Site ID was not available for entity {e:?}. Backtrace:\n{backtrace}") + }) + }; + let get_mesh_and_material = |entity: Entity| -> Option<(&Mesh, &StandardMaterial)> { let Ok((mesh, material)) = q_pbr.get(entity) else { return None; @@ -241,14 +251,22 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re visual_data.extend(model_visuals); } else { // Create a new mesh for it - let filename = format!("{}/{}_collision.glb", folder.display(), **name); + let filename = format!( + "{}/model_{}_collision.glb", + folder.display(), + get_site_id(*child)?, + ); write_meshes_to_file( model_collisions, None, CompressGltfOptions::skip_materials(), filename, )?; - let filename = format!("{}/{}_visual.glb", folder.display(), **name); + let filename = format!( + "{}/model_{}_visual.glb", + folder.display(), + get_site_id(*child)?, + ); write_meshes_to_file( model_visuals, Some(format!("{}_visual", **name)), @@ -270,20 +288,22 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re let Ok(tf) = q_tfs.get(*entity) else { continue; }; - let Some(door_name) = door_name else { - continue; - }; let data = MeshData { mesh, material: Some(material), transform: Some(tf.clone()), }; - let filename = - format!("{}/{}_{}.glb", folder.display(), **door_name, segment_name); + let filename = format!( + "{}/door_{}_{}.glb", + folder.display(), + get_site_id(*child)?, + segment_name, + ); + let door_name = door_name.map(|n| n.0.as_str()).unwrap_or(""); write_meshes_to_file( vec![data], - None, + Some(format!("door_{}_{}", door_name, segment_name)), CompressGltfOptions::default(), filename, )?; @@ -292,17 +312,25 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re continue; }; } - let filename = format!("{}/level_{}_collision.glb", folder.display(), **level_name); + let filename = format!( + "{}/level_{}_collision.glb", + folder.display(), + get_site_id(*site_child)?, + ); write_meshes_to_file( collision_data, - None, + Some(format!("level_{}_collision", **level_name)), CompressGltfOptions::skip_materials(), filename, )?; - let filename = format!("{}/level_{}_visual.glb", folder.display(), **level_name); + let filename = format!( + "{}/level_{}_visual.glb", + folder.display(), + get_site_id(*site_child)?, + ); write_meshes_to_file( visual_data, - Some(format!("level_{}_visuals", **level_name)), + Some(format!("level_{}_visual", **level_name)), CompressGltfOptions::default(), filename, )?; @@ -327,8 +355,17 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re transform: None, }); } - let filename = format!("{}/{}.glb", folder.display(), **lift_name); - write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename)?; + let filename = format!( + "{}/lift_{}.glb", + folder.display(), + get_site_id(*site_child)? + ); + write_meshes_to_file( + lift_data, + Some(format!("lift_{}", **lift_name)), + CompressGltfOptions::default(), + filename, + )?; // Now generate the lift doors let LiftCabin::Rect(cabin) = cabin; for (face, door) in cabin.doors().iter() { @@ -357,15 +394,20 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re transform: Some(tf.clone()), }; let filename = format!( - "{}/{}_{}_{}.glb", + "{}/lift_{}_{}_{}.glb", folder.display(), - **lift_name, + get_site_id(*site_child)?, face.label(), - segment_name + segment_name, ); write_meshes_to_file( vec![data], - None, + Some(format!( + "lift_{}_{}_{}", + **lift_name, + face.label(), + segment_name + )), CompressGltfOptions::default(), filename, )?; diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index b18bc527..e1dfcd9d 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -299,6 +299,8 @@ pub fn dispatch_load_workspace_events( .send(LoadWorkspaceFile(Some(path.clone()), data)) .expect("Failed sending load event"); } + } else { + warn!("Unable to read file [{path:?}] so it cannot be loaded"); } } LoadWorkspace::Data(data) => { diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs index 6ade88bb..3414f0e6 100644 --- a/rmf_site_format/src/sdf.rs +++ b/rmf_site_format/src/sdf.rs @@ -65,13 +65,13 @@ impl Pose { } } -fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink { +fn make_sdf_door_link(mesh_prefix: &str, link_name: &str) -> SdfLink { SdfLink { name: link_name.to_string(), collision: vec![SdfCollision { - name: format!("{}_collision", link_name), + name: format!("{link_name}_collision"), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/{}_{}.glb", door_name, link_name), + uri: format!("meshes/{mesh_prefix}_{link_name}.glb"), ..Default::default() }), surface: Some(SdfSurface { @@ -84,9 +84,9 @@ fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink { ..Default::default() }], visual: vec![SdfVisual { - name: format!("{}_visual", link_name), + name: format!("{link_name}_visual"), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/{}_{}.glb", door_name, link_name), + uri: format!("meshes/{mesh_prefix}_{link_name}.glb"), ..Default::default() }), ..Default::default() @@ -100,9 +100,9 @@ fn make_sdf_door( right_anchor: Anchor, offset: Vec3, ros_interface: bool, - name_override: Option<&str>, kind: &DoorType, - door_name: &str, + mesh_prefix: &str, + model_name: &str, ) -> Result { let left_trans = left_anchor.translation_for_category(Category::Door); let right_trans = right_anchor.translation_for_category(Category::Door); @@ -114,7 +114,6 @@ fn make_sdf_door( let dy = left_trans[1] - right_trans[1]; let door_length = (dx * dx + dy * dy).sqrt(); let yaw = -dx.atan2(dy); - let prefix = name_override.unwrap_or_default().to_string(); let labels = match kind { DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => { Vec::from(["body"]) @@ -138,9 +137,9 @@ fn make_sdf_door( let mut component_data = ElementMap::default(); door_plugin_inner .attributes - .insert("name".to_string(), door_name.to_string()); + .insert("name".to_string(), model_name.to_owned()); let mut door_model = SdfModel { - name: door_name.to_string(), + name: model_name.to_owned(), pose: Some( Pose { trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(), @@ -151,13 +150,8 @@ fn make_sdf_door( r#static: Some(false), ..Default::default() }; - let link_name = if let Some(name_override) = name_override.as_ref() { - name_override - } else { - &door_name - }; for label in labels.iter() { - door_model.link.push(make_sdf_door_link(link_name, label)); + door_model.link.push(make_sdf_door_link(mesh_prefix, label)); } let mut door_motion_params = vec![]; let joints = match kind { @@ -170,7 +164,7 @@ fn make_sdf_door( .insert("left_joint_name".into(), "empty_joint".into()); door_plugin_inner .attributes - .insert("right_joint_name".into(), prefix.clone() + "joint"); + .insert("right_joint_name".into(), model_name.to_owned() + "_joint"); door_motion_params.push(("v_max_door", "0.2")); door_motion_params.push(("a_max_door", "0.2")); door_motion_params.push(("a_nom_door", "0.08")); @@ -182,7 +176,7 @@ fn make_sdf_door( } .to_sdf(); vec![SdfJoint { - name: prefix.clone() + "joint", + name: model_name.to_owned() + "_joint", parent: "world".into(), child: "body".into(), r#type: "prismatic".into(), @@ -222,7 +216,8 @@ fn make_sdf_door( ..Default::default() } .to_sdf(); - let (left_joint_name, right_joint_name) = ("empty_joint", prefix.clone() + "joint"); + let (left_joint_name, right_joint_name) = + ("empty_joint", model_name.to_owned() + "_joint"); door_plugin_inner .attributes .insert("left_joint_name".into(), left_joint_name.into()); @@ -230,7 +225,7 @@ fn make_sdf_door( .attributes .insert("right_joint_name".into(), right_joint_name); vec![SdfJoint { - name: prefix.clone() + "joint", + name: model_name.to_owned() + "_joint", parent: "world".into(), child: "body".into(), r#type: "revolute".into(), @@ -251,12 +246,14 @@ fn make_sdf_door( door_plugin_inner .attributes .insert("type".into(), "DoubleSlidingDoor".into()); - door_plugin_inner - .attributes - .insert("left_joint_name".into(), prefix.clone() + "left_joint"); - door_plugin_inner - .attributes - .insert("right_joint_name".into(), prefix.clone() + "right_joint"); + door_plugin_inner.attributes.insert( + "left_joint_name".into(), + model_name.to_owned() + "_left_joint", + ); + door_plugin_inner.attributes.insert( + "right_joint_name".into(), + model_name.to_owned() + "_right_joint", + ); door_motion_params.push(("v_max_door", "0.2")); door_motion_params.push(("a_max_door", "0.2")); door_motion_params.push(("a_nom_door", "0.08")); @@ -276,7 +273,7 @@ fn make_sdf_door( let right_length = door_length - left_length; vec![ SdfJoint { - name: prefix.clone() + "right_joint", + name: model_name.to_owned() + "_right_joint", parent: "world".into(), child: "right".into(), r#type: "prismatic".into(), @@ -293,7 +290,7 @@ fn make_sdf_door( ..Default::default() }, SdfJoint { - name: prefix.clone() + "left_joint", + name: model_name.to_owned() + "_left_joint", parent: "world".into(), child: "left".into(), r#type: "prismatic".into(), @@ -315,12 +312,14 @@ fn make_sdf_door( door_plugin_inner .attributes .insert("type".into(), "DoubleSwingDoor".into()); - door_plugin_inner - .attributes - .insert("left_joint_name".into(), prefix.clone() + "left_joint"); - door_plugin_inner - .attributes - .insert("right_joint_name".into(), prefix.clone() + "right_joint"); + door_plugin_inner.attributes.insert( + "left_joint_name".into(), + model_name.to_owned() + "_left_joint", + ); + door_plugin_inner.attributes.insert( + "right_joint_name".into(), + model_name.to_owned() + "_right_joint", + ); door_motion_params.push(("v_max_door", "0.5")); door_motion_params.push(("a_max_door", "0.3")); door_motion_params.push(("a_nom_door", "0.15")); @@ -345,7 +344,7 @@ fn make_sdf_door( .to_sdf(); vec![ SdfJoint { - name: prefix.clone() + "right_joint", + name: model_name.to_owned() + "_right_joint", parent: "world".into(), child: "right".into(), r#type: "revolute".into(), @@ -362,7 +361,7 @@ fn make_sdf_door( ..Default::default() }, SdfJoint { - name: prefix.clone() + "left_joint", + name: model_name.to_owned() + "_left_joint", parent: "world".into(), child: "left".into(), r#type: "revolute".into(), @@ -388,7 +387,7 @@ fn make_sdf_door( } .to_sdf(); vec![SdfJoint { - name: prefix.clone() + "joint", + name: model_name.to_owned() + "_joint", parent: "world".into(), child: "body".into(), r#type: "fixed".into(), @@ -435,7 +434,7 @@ impl Site { filename: "toggle_floors".into(), ..Default::default() }; - for level in self.levels.values() { + for (level_id, level) in &self.levels { let mut level_model_names = vec![]; let mut model_element_map = ElementMap::default(); max_elevation = max_elevation.max(level.properties.elevation.0); @@ -444,7 +443,7 @@ impl Site { name: "floor".into(), ..Default::default() }; - let level_model_name = format!("level_{}", level.properties.name.0); + let level_model_name = &level.properties.name.0; floor_models_ele .attributes .insert("name".into(), level.properties.name.0.clone()); @@ -453,14 +452,14 @@ impl Site { .insert("model_name".into(), level_model_name.clone()); // Floors walls and static models are included in the level mesh world.model.push(SdfModel { - name: level_model_name, + name: level_model_name.clone(), r#static: Some(true), link: vec![SdfLink { name: "link".into(), collision: vec![SdfCollision { name: "collision".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/level_{}_collision.glb", level.properties.name.0), + uri: format!("meshes/level_{}_collision.glb", level_id), ..Default::default() }), surface: Some(SdfSurface { @@ -475,7 +474,7 @@ impl Site { visual: vec![SdfVisual { name: "visual".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/level_{}_visual.glb", level.properties.name.0), + uri: format!("meshes/level_{}_visual.glb", level_id), ..Default::default() }), ..Default::default() @@ -486,7 +485,7 @@ impl Site { }); // TODO(luca) We need this because there is no concept of ingestor or dispenser in // rmf_site yet. Remove when there is - for model in level.models.values() { + for (model_id, model) in &level.models { let mut added = false; if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) { @@ -521,7 +520,7 @@ impl Site { collision: vec![SdfCollision { name: "collision".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/{}_collision.glb", model.name.0), + uri: format!("meshes/model_{}_collision.glb", model_id), ..Default::default() }), ..Default::default() @@ -529,7 +528,7 @@ impl Site { visual: vec![SdfVisual { name: "visual".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/{}_visual.glb", model.name.0), + uri: format!("meshes/model_{}_visual.glb", model_id), ..Default::default() }), ..Default::default() @@ -545,20 +544,20 @@ impl Site { } } // Now add all the doors - for door in level.doors.values() { - // TODO(luca) doors into toggle floors + for (door_id, door) in &level.doors { let left_anchor = get_anchor(door.anchors.left())?; let right_anchor = get_anchor(door.anchors.right())?; - world.model.push(make_sdf_door( + let door_model = make_sdf_door( left_anchor, right_anchor, Vec3::new(0.0, 0.0, level.properties.elevation.0), true, - None, &door.kind, - &door.name.0, - )?); - level_model_names.push(door.name.0.clone()); + format!("door_{door_id}").as_str(), + door.name.0.as_str(), + )?; + level_model_names.push(door_model.name.clone()); + world.model.push(door_model); } for model_name in level_model_names.into_iter() { let model_element = XmlElement { @@ -571,7 +570,7 @@ impl Site { floor_models_ele.data = ElementData::Nested(model_element_map); toggle_floors_plugin.elements.push(floor_models_ele); } - for lift in self.lifts.values() { + for (lift_id, lift) in &self.lifts { let get_lift_anchor = |id: u32| -> Result { lift.cabin_anchors .get(&id) @@ -640,7 +639,7 @@ impl Site { // TODO(luca) remove unwrap let door = lift.cabin_doors.get(&door_placement.door).unwrap(); let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, face.label()); - let cabin_mesh_prefix = format!("{}_{}", lift_name, face.label()); + let cabin_mesh_prefix = format!("lift_{}_{}", lift_id, face.label()); let left_anchor = get_lift_anchor(door.reference_anchors.left())?; let right_anchor = get_lift_anchor(door.reference_anchors.right())?; let x_offset = -face.u() @@ -653,8 +652,8 @@ impl Site { right_anchor, x_offset, false, - Some(&cabin_mesh_prefix), &door.kind, + &cabin_mesh_prefix, &cabin_door_name, )?; for mut joint in cabin_door.joint.drain(..) { @@ -679,8 +678,8 @@ impl Site { right_anchor, Vec3::from(pose.trans) + Vec3::new(0.0, 0.0, level.properties.elevation.0), false, - Some(&cabin_mesh_prefix), &door.kind, + &cabin_mesh_prefix, &shaft_door_name, )?; // Add the pose of the lift to have world coordinates @@ -744,7 +743,7 @@ impl Site { collision: vec![SdfCollision { name: "collision".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/{}.glb", lift.properties.name.0), + uri: format!("meshes/lift_{}.glb", lift_id), ..Default::default() }), surface: Some(SdfSurface { @@ -759,7 +758,7 @@ impl Site { visual: vec![SdfVisual { name: "visual".into(), geometry: SdfGeometry::Mesh(SdfMeshShape { - uri: format!("meshes/{}.glb", lift.properties.name.0), + uri: format!("meshes/lift_{}.glb", lift_id), ..Default::default() }), ..Default::default()