From f796e44ce5a1d13a2878b5cc2cd7d93de0bbee0b Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 12 May 2023 20:46:44 +0800 Subject: [PATCH] Workcell editor mode (#120) Signed-off-by: Luca Della Vedova Co-authored-by: Michael X. Grey Co-authored-by: Arjo Chakravarty --- .github/workflows/ci_web.yaml | 4 +- assets/demo_workcells/demo.workcell.json | 79 +++ rmf_site_editor/.cargo/config.toml | 3 +- rmf_site_editor/Cargo.toml | 20 +- rmf_site_editor/rust-toolchain.toml | 2 +- rmf_site_editor/src/demo_world.rs | 12 +- rmf_site_editor/src/interaction/anchor.rs | 80 ++- rmf_site_editor/src/interaction/assets.rs | 168 +++++- rmf_site_editor/src/interaction/cursor.rs | 157 +++++- rmf_site_editor/src/interaction/gizmo.rs | 150 ++--- rmf_site_editor/src/interaction/mod.rs | 4 + rmf_site_editor/src/interaction/mode.rs | 7 + rmf_site_editor/src/interaction/outline.rs | 18 +- rmf_site_editor/src/interaction/picking.rs | 9 +- rmf_site_editor/src/interaction/select.rs | 8 + .../src/interaction/select_anchor.rs | 528 +++++++++++++++++- rmf_site_editor/src/interaction/visual_cue.rs | 5 + rmf_site_editor/src/keyboard.rs | 25 + rmf_site_editor/src/lib.rs | 31 +- rmf_site_editor/src/main_menu.rs | 209 +------ rmf_site_editor/src/save.rs | 141 +++++ rmf_site_editor/src/sdf_loader.rs | 88 +++ rmf_site_editor/src/shapes.rs | 140 ++++- rmf_site_editor/src/site/anchor.rs | 9 +- rmf_site_editor/src/site/assets.rs | 8 +- rmf_site_editor/src/site/change_plugin.rs | 19 +- rmf_site_editor/src/site/deletion.rs | 29 +- rmf_site_editor/src/site/drawing.rs | 23 +- rmf_site_editor/src/site/lane.rs | 8 +- rmf_site_editor/src/site/level.rs | 21 +- rmf_site_editor/src/site/lift.rs | 7 +- rmf_site_editor/src/site/load.rs | 46 +- rmf_site_editor/src/site/mod.rs | 21 +- rmf_site_editor/src/site/model.rs | 299 +++++++--- rmf_site_editor/src/site/recall_plugin.rs | 7 + rmf_site_editor/src/site/save.rs | 27 +- rmf_site_editor/src/site/sdf.rs | 205 +++++++ rmf_site_editor/src/site/site.rs | 69 +-- rmf_site_editor/src/site/util.rs | 9 +- rmf_site_editor/src/site_asset_io.rs | 69 ++- rmf_site_editor/src/urdf_loader.rs | 73 +++ rmf_site_editor/src/widgets/create.rs | 192 +++++-- .../src/widgets/inspector/inspect_anchor.rs | 149 ++++- .../widgets/inspector/inspect_asset_source.rs | 10 +- .../inspector/inspect_mesh_constraint.rs | 98 ++++ .../inspector/inspect_mesh_primitive.rs | 66 +++ .../src/widgets/inspector/inspect_name.rs | 26 +- .../src/widgets/inspector/inspect_pose.rs | 35 +- .../src/widgets/inspector/inspect_scale.rs | 63 +++ rmf_site_editor/src/widgets/inspector/mod.rs | 65 ++- rmf_site_editor/src/widgets/mod.rs | 180 +++++- rmf_site_editor/src/widgets/view_levels.rs | 5 + .../src/widgets/view_nav_graphs.rs | 21 +- rmf_site_editor/src/workcell/keyboard.rs | 59 ++ rmf_site_editor/src/workcell/load.rs | 160 ++++++ .../src/workcell/mesh_constraint.rs | 65 +++ rmf_site_editor/src/workcell/mod.rs | 125 +++++ rmf_site_editor/src/workcell/save.rs | 271 +++++++++ rmf_site_editor/src/workcell/urdf.rs | 128 +++++ rmf_site_editor/src/workcell/workcell.rs | 69 +++ rmf_site_editor/src/workspace.rs | 337 +++++++++++ rmf_site_format/Cargo.toml | 5 +- rmf_site_format/src/anchor.rs | 47 +- rmf_site_format/src/asset_source.rs | 54 +- rmf_site_format/src/category.rs | 2 + rmf_site_format/src/legacy/model.rs | 5 +- rmf_site_format/src/legacy/optimization.rs | 39 +- rmf_site_format/src/legacy/vertex.rs | 6 +- rmf_site_format/src/lib.rs | 3 + rmf_site_format/src/misc.rs | 13 +- rmf_site_format/src/model.rs | 9 + rmf_site_format/src/site.rs | 10 +- rmf_site_format/src/workcell.rs | 392 +++++++++++++ 73 files changed, 4807 insertions(+), 739 deletions(-) create mode 100644 assets/demo_workcells/demo.workcell.json create mode 100644 rmf_site_editor/src/save.rs create mode 100644 rmf_site_editor/src/sdf_loader.rs create mode 100644 rmf_site_editor/src/site/sdf.rs create mode 100644 rmf_site_editor/src/urdf_loader.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_mesh_constraint.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_mesh_primitive.rs create mode 100644 rmf_site_editor/src/widgets/inspector/inspect_scale.rs create mode 100644 rmf_site_editor/src/workcell/keyboard.rs create mode 100644 rmf_site_editor/src/workcell/load.rs create mode 100644 rmf_site_editor/src/workcell/mesh_constraint.rs create mode 100644 rmf_site_editor/src/workcell/mod.rs create mode 100644 rmf_site_editor/src/workcell/save.rs create mode 100644 rmf_site_editor/src/workcell/urdf.rs create mode 100644 rmf_site_editor/src/workcell/workcell.rs create mode 100644 rmf_site_editor/src/workspace.rs create mode 100644 rmf_site_format/src/workcell.rs diff --git a/.github/workflows/ci_web.yaml b/.github/workflows/ci_web.yaml index b7106cbf..0a78bd59 100644 --- a/.github/workflows/ci_web.yaml +++ b/.github/workflows/ci_web.yaml @@ -18,8 +18,6 @@ jobs: - name: rust-wasm-target run: | - rustup toolchain install nightly - rustup default nightly rustup target add wasm32-unknown-unknown - name: apt-deps @@ -29,7 +27,7 @@ jobs: - name: cargo-deps run: | - cargo install wasm-bindgen-cli + cargo install wasm-bindgen-cli --vers =0.2.84 - uses: actions/checkout@v3 diff --git a/assets/demo_workcells/demo.workcell.json b/assets/demo_workcells/demo.workcell.json new file mode 100644 index 00000000..fbbd9c84 --- /dev/null +++ b/assets/demo_workcells/demo.workcell.json @@ -0,0 +1,79 @@ +{ + "name": "test_workcell", + "id": 0, + "frames": { + "1": { + "parent": 0, + "Pose3D": { + "trans": [ + 5.0, + 0.0, + 0.0 + ], + "rot": { + "euler_xyz": [ + { + "deg": 45.0 + }, + { + "deg": 30.0 + }, + { + "deg": 90.0 + } + ] + } + } + }, + "2": { + "parent": 0, + "Pose3D": { + "trans": [ + 0.0, + 5.0, + 0.0 + ], + "rot": { + "euler_xyz": [ + { + "deg": 45.0 + }, + { + "deg": 30.0 + }, + { + "deg": 90.0 + } + ] + } + } + }, + "3": { + "parent": 1, + "Pose3D": { + "trans": [ + 1.0, + 0.31817698, + 0.60250926 + ], + "rot": { + "euler_xyz": [ + { + "deg": 92.758385 + }, + { + "deg": -73.071754 + }, + { + "deg": -171.64337 + } + ] + } + } + } + }, + "visuals": { + }, + "collisions": { + } +} diff --git a/rmf_site_editor/.cargo/config.toml b/rmf_site_editor/.cargo/config.toml index 8d0604ff..fa507733 100644 --- a/rmf_site_editor/.cargo/config.toml +++ b/rmf_site_editor/.cargo/config.toml @@ -1,2 +1,3 @@ [target.x86_64-pc-windows-msvc] -rustflags = ["-Zshare-generics=off"] +linker = "clang" +rustflags = ["-C", "link-arg=-fuse-ld=/usr/bin/mold", "-Zshare-generics=off"] diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index b81ab96e..5737d7da 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -13,14 +13,21 @@ name = "rmf_site_editor" [dependencies] bevy_egui = "0.19" -bevy_mod_picking = "0.10" +bevy_mod_picking = "0.11" bevy_mod_raycast = "0.7" bevy_mod_outline = "0.3.3" +bevy_infinite_grid = "0.6" +bevy_polyline = "0.4" +bevy_stl = "0.7.0" +bevy_obj = { git = "https://github.com/luca-della-vedova/bevy_obj", branch = "luca/scene_0.9", features = ["scene"]} +bevy_rapier3d = "0.20.0" +crossbeam-channel = "0.5.0" smallvec = "*" serde = { version = "1.0", features = ["derive"] } serde_yaml = "0.8.23" serde_json = "1.0" -wasm-bindgen = "0.2" +# wasm-bindgen 0.2.85 introduces a compile error in stdweb +wasm-bindgen = "=0.2.84" web-sys = { version = "0.3.56", features = ["console"] } futures-lite = "1.12.0" bevy = "0.9" @@ -31,19 +38,22 @@ thiserror = "*" rmf_site_format = { path = "../rmf_site_format", features = ["bevy"] } itertools = "*" bitfield = "*" +rfd = "0.11" +urdf-rs = "0.7" +# sdformat_rs = { path = "../../sdf_rust_experimental/sdformat_rs"} +sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "f86344f"} # only enable the 'dynamic' feature if we're not building for web or windows [target.'cfg(all(not(target_arch = "wasm32"), not(target_os = "windows")))'.dependencies] -bevy = { version = "0.9", features = ["dynamic"] } +bevy = { version = "0.9", features = ["dynamic", "jpeg", "tga"] } surf = { version = "2.3" } [target.'cfg(not(target_arch = "wasm32"))'.dependencies] -rfd = "0.8.2" clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] } # windows doesnt work well with dynamic feature yet [target.'cfg(target_os = "windows")'.dependencies] -bevy = "0.9" +bevy = { version = "0.9", features = ["jpeg", "tga"] } surf = { version = "2.3" } [target.'cfg(target_arch = "wasm32")'.dependencies] diff --git a/rmf_site_editor/rust-toolchain.toml b/rmf_site_editor/rust-toolchain.toml index 5d56faf9..292fe499 100644 --- a/rmf_site_editor/rust-toolchain.toml +++ b/rmf_site_editor/rust-toolchain.toml @@ -1,2 +1,2 @@ [toolchain] -channel = "nightly" +channel = "stable" diff --git a/rmf_site_editor/src/demo_world.rs b/rmf_site_editor/src/demo_world.rs index b7e6127b..ebb55754 100644 --- a/rmf_site_editor/src/demo_world.rs +++ b/rmf_site_editor/src/demo_world.rs @@ -1,3 +1,11 @@ -pub fn demo_office() -> String { - return include_str!("../../assets/demo_maps/office.building.yaml").to_string(); +pub fn demo_office() -> Vec { + return include_str!("../../assets/demo_maps/office.building.yaml") + .as_bytes() + .to_vec(); +} + +pub fn demo_workcell() -> Vec { + return include_str!("../../assets/demo_workcells/demo.workcell.json") + .as_bytes() + .to_vec(); } diff --git a/rmf_site_editor/src/interaction/anchor.rs b/rmf_site_editor/src/interaction/anchor.rs index c5e840d0..5c8d9a9c 100644 --- a/rmf_site_editor/src/interaction/anchor.rs +++ b/rmf_site_editor/src/interaction/anchor.rs @@ -32,20 +32,23 @@ pub struct AnchorVisualization { pub fn add_anchor_visual_cues( mut commands: Commands, - new_anchors: Query<(Entity, &Parent, Option<&Subordinate>), (Added, Without)>, + new_anchors: Query< + (Entity, &Parent, Option<&Subordinate>, &Anchor), + (Added, Without), + >, categories: Query<&Category>, site_assets: Res, interaction_assets: Res, ) { - for (e, parent, subordinate) in &new_anchors { + for (e, parent, subordinate, anchor) in &new_anchors { let body_mesh = match categories.get(parent.get()).unwrap() { Category::Level => site_assets.level_anchor_mesh.clone(), Category::Lift => site_assets.lift_anchor_mesh.clone(), _ => site_assets.site_anchor_mesh.clone(), }; - let mut commands = commands.entity(e); - let body = commands.add_children(|parent| { + let mut entity_commands = commands.entity(e); + let body = entity_commands.add_children(|parent| { let mut body = parent.spawn(PbrBundle { mesh: body_mesh, material: site_assets.passive_anchor_material.clone(), @@ -60,10 +63,16 @@ pub fn add_anchor_visual_cues( body }); - commands + entity_commands .insert(AnchorVisualization { body, drag: None }) - .insert(OutlineVisualization::Anchor) - .insert(VisualCue::outline().irregular()); + .insert(OutlineVisualization::Anchor { body }); + + // 3D anchors should always be visible with arrow cue meshes + if anchor.is_3D() { + entity_commands.insert(VisualCue::outline()); + } else { + entity_commands.insert(VisualCue::outline().irregular()); + } } } @@ -166,15 +175,17 @@ pub fn update_anchor_cues_for_mode( } pub fn update_anchor_visual_cues( - mut command: Commands, + mut commands: Commands, mut anchors: Query< ( Entity, + &Anchor, &Hovered, &Selected, &mut AnchorVisualization, &mut VisualCue, Option<&Subordinate>, + ChangeTrackers, ChangeTrackers, ), Or<(Changed, Changed, Changed)>, @@ -187,7 +198,18 @@ pub fn update_anchor_visual_cues( interaction_assets: Res, debug_mode: Option>, ) { - for (a, hovered, selected, mut shapes, mut cue, subordinate, select_tracker) in &mut anchors { + for ( + a, + anchor, + hovered, + selected, + mut shapes, + mut cue, + subordinate, + hover_tracker, + select_tracker, + ) in &mut anchors + { if debug_mode.as_ref().filter(|d| d.0).is_some() { // NOTE(MXG): I have witnessed a scenario where a lane is deleted // and then the anchors that supported it are permanently stuck as @@ -247,20 +269,36 @@ pub fn update_anchor_visual_cues( ); } - if select_tracker.is_changed() { - if selected.cue() { - if shapes.drag.is_none() && subordinate.is_none() { - interaction_assets.add_anchor_draggable_arrows( - &mut command, - a, - shapes.as_mut(), - ); + if anchor.is_3D() { + if select_tracker.is_changed() || hover_tracker.is_changed() { + if selected.cue() || hovered.cue() { + if shapes.drag.is_none() { + interaction_assets.add_anchor_gizmos_3D( + &mut commands, + a, + shapes.as_mut(), + subordinate.is_none(), + ); + } + } else { + if let Some(drag) = shapes.drag { + commands.entity(drag).despawn_recursive(); + } + shapes.drag = None; } - } else { - if let Some(drag) = shapes.drag { - command.entity(drag).despawn_recursive(); + } + } else { + if select_tracker.is_changed() { + if selected.cue() { + if shapes.drag.is_none() && subordinate.is_none() { + interaction_assets.add_anchor_gizmos_2D(&mut commands, a, shapes.as_mut()); + } + } else { + if let Some(drag) = shapes.drag { + commands.entity(drag).despawn_recursive(); + } + shapes.drag = None; } - shapes.drag = None; } } } diff --git a/rmf_site_editor/src/interaction/assets.rs b/rmf_site_editor/src/interaction/assets.rs index b68d854f..c66f3fa2 100644 --- a/rmf_site_editor/src/interaction/assets.rs +++ b/rmf_site_editor/src/interaction/assets.rs @@ -16,7 +16,11 @@ */ use crate::{interaction::*, shapes::*}; -use bevy::{math::Affine3A, prelude::*}; +use bevy::{math::Affine3A, prelude::*, render::view::visibility::RenderLayers}; +use bevy_polyline::{ + material::PolylineMaterial, + polyline::{Polyline, PolylineBundle}, +}; #[derive(Clone, Debug, Resource)] pub struct InteractionAssets { @@ -35,17 +39,33 @@ pub struct InteractionAssets { pub direction_light_cover_material: Handle, pub x_axis_materials: GizmoMaterialSet, pub y_axis_materials: GizmoMaterialSet, + pub z_axis_materials: GizmoMaterialSet, pub z_plane_materials: GizmoMaterialSet, pub lift_doormat_available_materials: GizmoMaterialSet, pub lift_doormat_unavailable_materials: GizmoMaterialSet, + pub centimeter_finite_grid: Vec<(Handle, Handle)>, } impl InteractionAssets { - pub fn make_draggable_axis( + pub fn make_orientation_cue_meshes(&self, commands: &mut Commands, parent: Entity, scale: f32) { + // The arrows should originate in the mesh origin + let pos = Vec3::splat(0.0); + let rot_x = Quat::from_rotation_y(90_f32.to_radians()); + let rot_y = Quat::from_rotation_x(-90_f32.to_radians()); + let rot_z = Quat::default(); + let x_mat = self.x_axis_materials.clone(); + let y_mat = self.y_axis_materials.clone(); + let z_mat = self.z_axis_materials.clone(); + self.make_axis(commands, None, parent, x_mat, pos, rot_x, scale); + self.make_axis(commands, None, parent, y_mat, pos, rot_y, scale); + self.make_axis(commands, None, parent, z_mat, pos, rot_z, scale); + } + + pub fn make_axis( &self, command: &mut Commands, // What entity will be moved when this gizmo is dragged - for_entity: Entity, + for_entity_opt: Option, // What entity should be the parent frame of this gizmo parent: Entity, material_set: GizmoMaterialSet, @@ -54,34 +74,61 @@ impl InteractionAssets { scale: f32, ) -> Entity { return command.entity(parent).add_children(|parent| { - parent - .spawn(PbrBundle { - transform: Transform::from_rotation(rotation) - .with_translation(offset) - .with_scale(Vec3::splat(scale)), - mesh: self.arrow_mesh.clone(), - material: material_set.passive.clone(), - ..default() - }) - .insert(DragAxisBundle::new(for_entity, Vec3::Z).with_materials(material_set)) - .id() + let mut child_entity = parent.spawn(PbrBundle { + transform: Transform::from_rotation(rotation) + .with_translation(offset) + .with_scale(Vec3::splat(scale)), + mesh: self.arrow_mesh.clone(), + material: material_set.passive.clone(), + ..default() + }); + + if let Some(for_entity) = for_entity_opt { + child_entity + .insert(DragAxisBundle::new(for_entity, Vec3::Z).with_materials(material_set)); + } + child_entity.id() }); } - pub fn add_anchor_draggable_arrows( + pub fn make_draggable_axis( &self, command: &mut Commands, + // What entity will be moved when this gizmo is dragged + for_entity: Entity, + // What entity should be the parent frame of this gizmo + parent: Entity, + material_set: GizmoMaterialSet, + offset: Vec3, + rotation: Quat, + scale: f32, + ) -> Entity { + self.make_axis( + command, + Some(for_entity), + parent, + material_set, + offset, + rotation, + scale, + ) + } + + #[allow(non_snake_case)] + pub fn add_anchor_gizmos_2D( + &self, + commands: &mut Commands, anchor: Entity, cue: &mut AnchorVisualization, ) { - let drag_parent = command.entity(anchor).add_children(|parent| { + let drag_parent = commands.entity(anchor).add_children(|parent| { parent .spawn(SpatialBundle::default()) - .insert(VisualCue::no_outline()) + .insert(VisualCue::no_outline().irregular().always_xray()) .id() }); - let height = 0.01; + let height = 0.0; let scale = 0.2; let offset = 0.15; for (m, p, r) in [ @@ -106,12 +153,63 @@ impl InteractionAssets { Quat::from_rotation_x(90_f32.to_radians()), ), ] { - self.make_draggable_axis(command, anchor, drag_parent, m, p, r, scale); + self.make_draggable_axis(commands, anchor, drag_parent, m, p, r, scale); } cue.drag = Some(drag_parent); } + #[allow(non_snake_case)] + pub fn add_anchor_gizmos_3D( + &self, + commands: &mut Commands, + anchor: Entity, + cue: &mut AnchorVisualization, + draggable: bool, + ) { + let drag_parent = commands.entity(anchor).add_children(|parent| { + parent + .spawn(SpatialBundle::default()) + .insert(VisualCue::no_outline().irregular().always_xray()) + .id() + }); + + let for_entity = if draggable { Some(anchor) } else { None }; + let scale = 0.2; + let offset = 0.15; + for (m, p, r) in [ + ( + self.x_axis_materials.clone(), + Vec3::new(offset, 0., 0.), + Quat::from_rotation_y(90_f32.to_radians()), + ), + ( + self.y_axis_materials.clone(), + Vec3::new(0., offset, 0.), + Quat::from_rotation_x(-90_f32.to_radians()), + ), + ( + self.z_axis_materials.clone(), + Vec3::new(0., 0., offset), + Quat::IDENTITY, + ), + ] { + self.make_axis(commands, for_entity, drag_parent, m, p, r, scale); + } + + commands.entity(drag_parent).add_children(|parent| { + for (polyline, material) in &self.centimeter_finite_grid { + parent.spawn(PolylineBundle { + polyline: polyline.clone(), + material: material.clone(), + ..default() + }); + } + }); + + cue.drag = Some(drag_parent); + } + pub fn lift_doormat_materials(&self, available: bool) -> GizmoMaterialSet { if available { self.lift_doormat_available_materials.clone() @@ -128,7 +226,7 @@ impl FromWorld for InteractionAssets { let halo_mesh = meshes.add(make_halo_mesh()); let arrow_mesh = meshes.add(make_cylinder_arrow_mesh()); let point_light_socket_mesh = meshes.add( - make_cylinder(0.03, 0.02) + make_cylinder(0.06, 0.02) .transform_by(Affine3A::from_translation(0.04 * Vec3::Z)) .into(), ); @@ -174,14 +272,14 @@ impl FromWorld for InteractionAssets { ); let directional_light_cover_mesh = meshes.add( Mesh::from( - make_cylinder(0.01, 0.1).transform_by(Affine3A::from_translation(0.01 * Vec3::Z)), + make_cylinder(0.02, 0.1).transform_by(Affine3A::from_translation(0.01 * Vec3::Z)), ) .with_generated_outline_normals() .unwrap(), ); let directional_light_shine_mesh = meshes.add( Mesh::from( - make_cylinder(0.01, 0.1).transform_by(Affine3A::from_translation(-0.01 * Vec3::Z)), + make_cylinder(0.02, 0.1).transform_by(Affine3A::from_translation(-0.01 * Vec3::Z)), ) .with_generated_outline_normals() .unwrap(), @@ -212,6 +310,7 @@ impl FromWorld for InteractionAssets { }); let x_axis_materials = GizmoMaterialSet::make_x_axis(&mut materials); let y_axis_materials = GizmoMaterialSet::make_y_axis(&mut materials); + let z_axis_materials = GizmoMaterialSet::make_z_axis(&mut materials); let z_plane_materials = GizmoMaterialSet::make_z_plane(&mut materials); let lift_doormat_available_materials = GizmoMaterialSet { passive: materials.add(StandardMaterial { @@ -254,6 +353,29 @@ impl FromWorld for InteractionAssets { }), }; + let centimeter_finite_grid = { + let (polylines, polyline_mats): (Vec<_>, Vec<_>) = + make_metric_finite_grid(0.01, 100, Color::WHITE) + .into_iter() + .unzip(); + let mut polyline_assets = world.get_resource_mut::>().unwrap(); + let polylines: Vec> = polylines + .into_iter() + .map(|p| polyline_assets.add(p)) + .collect(); + let mut polyline_mat_assets = world + .get_resource_mut::>() + .unwrap(); + let polyline_mats: Vec> = polyline_mats + .into_iter() + .map(|m| polyline_mat_assets.add(m)) + .collect(); + polylines + .into_iter() + .zip(polyline_mats.into_iter()) + .collect() + }; + Self { dagger_mesh, dagger_material, @@ -270,9 +392,11 @@ impl FromWorld for InteractionAssets { direction_light_cover_material, x_axis_materials, y_axis_materials, + z_axis_materials, z_plane_materials, lift_doormat_available_materials, lift_doormat_unavailable_materials, + centimeter_finite_grid, } } } diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs index 4fbe797a..f661d2e5 100644 --- a/rmf_site_editor/src/interaction/cursor.rs +++ b/rmf_site_editor/src/interaction/cursor.rs @@ -23,7 +23,7 @@ use crate::{ use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_mod_picking::PickingRaycastSet; use bevy_mod_raycast::{Intersection, Ray3d}; -use rmf_site_format::{FloorMarker, WallMarker}; +use rmf_site_format::{FloorMarker, Model, ModelMarker, WallMarker, WorkcellModel}; use std::collections::HashSet; /// A resource that keeps track of the unique entities that play a role in @@ -36,6 +36,8 @@ pub struct Cursor { // TODO(MXG): Switch the anchor preview when the anchor enters a lift pub level_anchor_placement: Entity, pub site_anchor_placement: Entity, + pub frame_placement: Entity, + pub preview_model: Option, dependents: HashSet, /// Use a &str to label each mode that might want to turn the cursor on modes: HashSet<&'static str>, @@ -100,6 +102,44 @@ impl Cursor { } } + fn remove_preview(&mut self, commands: &mut Commands) { + if let Some(current_preview) = self.preview_model { + commands + .entity(self.frame) + .remove_children(&[current_preview]); + commands.entity(current_preview).despawn_recursive(); + } + } + + // TODO(luca) reduce duplication here + pub fn set_model_preview(&mut self, commands: &mut Commands, model: Option) { + self.remove_preview(commands); + self.preview_model = if let Some(model) = model { + let e = commands.spawn(model).insert(Pending).id(); + commands.entity(self.frame).push_children(&[e]); + Some(e) + } else { + None + } + } + + pub fn set_workcell_model_preview( + &mut self, + commands: &mut Commands, + model: Option, + ) { + self.remove_preview(commands); + self.preview_model = if let Some(model) = model { + let cmd = commands.spawn(Pending); + let e = cmd.id(); + model.add_bevy_components(cmd); + commands.entity(self.frame).push_children(&[e]); + Some(e) + } else { + None + } + } + pub fn should_be_visible(&self) -> bool { (!self.dependents.is_empty() || !self.modes.is_empty()) && self.blockers.is_empty() } @@ -122,7 +162,9 @@ impl FromWorld for Cursor { let dagger_material = interaction_assets.dagger_material.clone(); let level_anchor_mesh = site_assets.level_anchor_mesh.clone(); let site_anchor_mesh = site_assets.site_anchor_mesh.clone(); + let frame_mesh = interaction_assets.arrow_mesh.clone(); let preview_anchor_material = site_assets.preview_anchor_material.clone(); + let preview_frame_material = site_assets.preview_anchor_material.clone(); let halo = world .spawn(PbrBundle { @@ -176,9 +218,30 @@ impl FromWorld for Cursor { }) .id(); + let frame_placement = world + .spawn(AnchorBundle::new([0., 0.].into()).visible(false)) + .insert(Pending) + .insert(Preview) + .insert(VisualCue::no_outline()) + .with_children(|parent| { + parent.spawn(PbrBundle { + mesh: frame_mesh, + material: preview_frame_material, + transform: Transform::from_scale(Vec3::new(0.2, 0.2, 0.2)), + ..default() + }); + }) + .id(); + let cursor = world .spawn(VisualCue::no_outline()) - .push_children(&[halo, dagger, level_anchor_placement, site_anchor_placement]) + .push_children(&[ + halo, + dagger, + level_anchor_placement, + site_anchor_placement, + frame_placement, + ]) .insert(SpatialBundle { visibility: Visibility { is_visible: false }, ..default() @@ -191,6 +254,8 @@ impl FromWorld for Cursor { dagger, level_anchor_placement, site_anchor_placement, + frame_placement, + preview_model: None, dependents: Default::default(), modes: Default::default(), blockers: Default::default(), @@ -236,10 +301,13 @@ pub fn update_cursor_transform( mode: Res, cursor: Res, intersections: Query<&Intersection>, + models: Query<(), With>, mut transforms: Query<&mut Transform>, + hovering: Res, intersect_ground_params: IntersectGroundPlaneParams, + mut visibility: Query<&mut Visibility>, ) { - match *mode { + match &*mode { InteractionMode::Inspect => { let intersection = match intersections.iter().last() { Some(intersection) => intersection, @@ -281,6 +349,77 @@ pub fn update_cursor_transform( *transform = Transform::from_translation(intersection); } + // TODO(luca) snap to features of meshes + InteractionMode::SelectAnchor3D(_mode) => { + let mut transform = match transforms.get_mut(cursor.frame) { + Ok(transform) => transform, + Err(_) => { + println!("No cursor transform found"); + return; + } + }; + // Check if there is an intersection to a mesh, if there isn't fallback to ground plane + // TODO(luca) Clean this messy statement, the API for intersections is not too friendly + if let Some((Some(triangle), Some(position), Some(normal))) = intersections + .iter() + .last() + .and_then(|data| Some((data.world_triangle(), data.position(), data.normal()))) + { + // Make sure we are hovering over a model and not anything else (i.e. anchor) + match cursor.preview_model { + None => { + if hovering.0.and_then(|e| models.get(e).ok()).is_some() { + // Find the closest triangle vertex + // TODO(luca) Also snap to edges of triangles or just disable altogether and snap + // to area, then populate a MeshConstraint component to be used by downstream + // spawning methods + // TODO(luca) there must be a better way to find a minimum given predicate in Rust + let triangle_vecs = vec![triangle.v1, triangle.v2]; + let mut closest_vertex = triangle.v0; + let mut closest_dist = position.distance(triangle.v0.into()); + for v in triangle_vecs { + let dist = position.distance(v.into()); + if dist < closest_dist { + closest_dist = dist; + closest_vertex = v; + } + } + //closest_vertex = *triangle_vecs.iter().min_by(|position, ver| position.distance(**ver).cmp(closest_dist)).unwrap(); + let ray = Ray3d::new(closest_vertex.into(), normal); + *transform = Transform::from_matrix( + ray.to_aligned_transform([0., 0., 1.].into()), + ); + set_visibility(cursor.frame, &mut visibility, true); + } else { + // Hide the cursor + set_visibility(cursor.frame, &mut visibility, false); + } + } + Some(_) => { + // If we are placing a model avoid snapping to faced and just project to + // ground plane + let intersection = match intersect_ground_params.ground_plane_intersection() + { + Some(intersection) => intersection, + None => { + return; + } + }; + set_visibility(cursor.frame, &mut visibility, true); + *transform = Transform::from_translation(intersection); + } + } + } else { + let intersection = match intersect_ground_params.ground_plane_intersection() { + Some(intersection) => intersection, + None => { + return; + } + }; + set_visibility(cursor.frame, &mut visibility, true); + *transform = Transform::from_translation(intersection); + } + } } } @@ -321,3 +460,15 @@ pub fn update_cursor_hover_visualization( cursor.remove_dependent(e, &mut visibility); } } + +// This system makes sure model previews are not picked up by raycasting +pub fn make_model_previews_not_selectable( + mut commands: Commands, + new_models: Query, Added)>, + cursor: Res, +) { + if let Some(e) = cursor.preview_model.and_then(|m| new_models.get(m).ok()) { + commands.entity(e).remove::(); + commands.entity(e).remove::(); + } +} diff --git a/rmf_site_editor/src/interaction/gizmo.rs b/rmf_site_editor/src/interaction/gizmo.rs index 63f8e6c8..02f011eb 100644 --- a/rmf_site_editor/src/interaction/gizmo.rs +++ b/rmf_site_editor/src/interaction/gizmo.rs @@ -16,15 +16,16 @@ */ use crate::interaction::*; -use bevy::prelude::*; -use bevy_mod_picking::{PickableBundle, PickingRaycastSet}; +use bevy::{math::Affine3A, prelude::*}; +use bevy_mod_picking::{PickableBundle, PickableMesh, PickingRaycastSet}; use bevy_mod_raycast::{Intersection, Ray3d}; use rmf_site_format::Pose; #[derive(Debug, Clone, Copy)] pub struct InitialDragConditions { click_point: Vec3, - entity_tf: Transform, + tf_for_entity_global: Transform, + tf_for_entity_parent_inv: Affine3A, } #[derive(Debug, Clone)] @@ -51,6 +52,14 @@ impl GizmoMaterialSet { } } + pub fn make_z_axis(materials: &mut Mut>) -> Self { + Self { + passive: materials.add(Color::rgb(0., 0., 0.9).into()), + hover: materials.add(Color::rgb(0.5, 0.5, 1.0).into()), + drag: materials.add(Color::rgb(0., 0., 0.6).into()), + } + } + pub fn make_z_plane(materials: &mut Mut>) -> Self { Self { passive: materials.add(Color::rgba(0., 0., 1., 0.6).into()), @@ -233,7 +242,7 @@ pub fn update_gizmo_click_start( mut visibility: Query<&mut Visibility>, mouse_button_input: Res>, touch_input: Res, - transforms: Query<&GlobalTransform>, + transforms: Query<(&Transform, &GlobalTransform)>, intersections: Query<&Intersection>, mut cursor: ResMut, mut gizmo_state: ResMut, @@ -281,11 +290,14 @@ pub fn update_gizmo_click_start( click.send(GizmoClicked(e)); if let Ok(Some(intersection)) = intersections.get_single().map(|i| i.position()) { if let Ok((gizmo, Some(mut draggable), mut material)) = gizmos.get_mut(e) { - if let Ok(tf) = transforms.get(draggable.for_entity) { + if let Ok((local_tf, global_tf)) = transforms.get(draggable.for_entity) { selection_blocker.dragging = true; + let tf_for_entity_parent_inv = + local_tf.compute_affine() * global_tf.affine().inverse(); draggable.drag = Some(InitialDragConditions { click_point: intersection.clone(), - entity_tf: tf.compute_transform(), + tf_for_entity_global: global_tf.compute_transform(), + tf_for_entity_parent_inv, }); if let Some(drag_materials) = &gizmo.materials { *material = drag_materials.drag.clone(); @@ -375,82 +387,70 @@ pub fn update_drag_motions( if let Ok((axis, draggable, drag_tf)) = drag_axis.get(dragging) { if let Some(initial) = &draggable.drag { - if let Some((for_local_tf, for_global_tf)) = - transforms.get(draggable.for_entity).ok() - { - let n = if axis.frame.is_local() { - drag_tf - .affine() - .transform_vector3(axis.along) - .normalize_or_zero() - } else { - axis.along.normalize_or_zero() - }; - let dp = ray.origin() - initial.click_point; - let a = ray.direction().dot(n); - let b = ray.direction().dot(dp); - let c = n.dot(dp); - - let denom = a.powi(2) - 1.; - if denom.abs() < 1e-3 { - // The rays are nearly parallel, so we should not attempt moving - // because the motion will be too extreme - return; - } - - let t = (a * b - c) / denom; - let delta = t * n; - let tf_goal = initial - .entity_tf - .with_translation(initial.entity_tf.translation + delta); - let tf_parent_inv = - for_local_tf.compute_affine() * for_global_tf.affine().inverse(); - move_to.send(MoveTo { - entity: draggable.for_entity, - transform: Transform::from_matrix( - (tf_parent_inv * tf_goal.compute_affine()).into(), - ), - }); + let n = if axis.frame.is_local() { + drag_tf + .affine() + .transform_vector3(axis.along) + .normalize_or_zero() + } else { + axis.along.normalize_or_zero() + }; + let dp = ray.origin() - initial.click_point; + let a = ray.direction().dot(n); + let b = ray.direction().dot(dp); + let c = n.dot(dp); + + let denom = a.powi(2) - 1.; + if denom.abs() < 1e-3 { + // The rays are nearly parallel, so we should not attempt moving + // because the motion will be too extreme + return; } + + let t = (a * b - c) / denom; + let delta = t * n; + let tf_goal = initial + .tf_for_entity_global + .with_translation(initial.tf_for_entity_global.translation + delta); + move_to.send(MoveTo { + entity: draggable.for_entity, + transform: Transform::from_matrix( + (initial.tf_for_entity_parent_inv * tf_goal.compute_affine()).into(), + ), + }); } } if let Ok((plane, draggable, drag_tf)) = drag_plane.get(dragging) { if let Some(initial) = &draggable.drag { - if let Some((for_local_tf, for_global_tf)) = - transforms.get(draggable.for_entity).ok() - { - let n_p = if plane.frame.is_local() { - drag_tf - .affine() - .transform_vector3(plane.in_plane) - .normalize_or_zero() - } else { - plane.in_plane.normalize_or_zero() - }; - - let n_r = ray.direction(); - let denom = n_p.dot(n_r); - if denom.abs() < 1e-3 { - // The rays are nearly parallel so we should not attempt - // moving because the motion will be too extreme - return; - } - - let t = (initial.click_point - ray.origin()).dot(n_p) / denom; - let delta = ray.position(t) - initial.click_point; - let tf_goal = initial - .entity_tf - .with_translation(initial.entity_tf.translation + delta); - let tf_parent_inv = - for_local_tf.compute_affine() * for_global_tf.affine().inverse(); - move_to.send(MoveTo { - entity: draggable.for_entity, - transform: Transform::from_matrix( - (tf_parent_inv * tf_goal.compute_affine()).into(), - ), - }); + let n_p = if plane.frame.is_local() { + drag_tf + .affine() + .transform_vector3(plane.in_plane) + .normalize_or_zero() + } else { + plane.in_plane.normalize_or_zero() + }; + + let n_r = ray.direction(); + let denom = n_p.dot(n_r); + if denom.abs() < 1e-3 { + // The rays are nearly parallel so we should not attempt + // moving because the motion will be too extreme + return; } + + let t = (initial.click_point - ray.origin()).dot(n_p) / denom; + let delta = ray.position(t) - initial.click_point; + let tf_goal = initial + .tf_for_entity_global + .with_translation(initial.tf_for_entity_global.translation + delta); + move_to.send(MoveTo { + entity: draggable.for_entity, + transform: Transform::from_matrix( + (initial.tf_for_entity_parent_inv * tf_goal.compute_affine()).into(), + ), + }); } } } diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 50905bce..ff1484dd 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -74,6 +74,7 @@ pub use visual_cue::*; use bevy::prelude::*; use bevy_mod_outline::OutlinePlugin; use bevy_mod_picking::{PickingPlugin, PickingSystem}; +use bevy_polyline::PolylinePlugin; #[derive(Default)] pub struct InteractionPlugin; @@ -115,6 +116,7 @@ impl Plugin for InteractionPlugin { InteractionState::Disable, ) .add_state_to_stage(CoreStage::PostUpdate, InteractionState::Disable) + .add_plugin(PolylinePlugin) .init_resource::() .init_resource::() .init_resource::() @@ -147,11 +149,13 @@ impl Plugin for InteractionPlugin { .with_system(maintain_hovered_entities.after(handle_selection_picking)) .with_system(maintain_selected_entities.after(maintain_hovered_entities)) .with_system(handle_select_anchor_mode.after(maintain_selected_entities)) + .with_system(handle_select_anchor_3d_mode.after(maintain_selected_entities)) .with_system(update_anchor_visual_cues.after(maintain_selected_entities)) .with_system(update_unassigned_anchor_cues) .with_system(update_anchor_cues_for_mode) .with_system(update_anchor_proximity_xray.after(update_cursor_transform)) .with_system(remove_deleted_supports_from_visual_cues) + .with_system(make_model_previews_not_selectable) .with_system(update_lane_visual_cues.after(maintain_selected_entities)) .with_system(update_edge_visual_cues.after(maintain_selected_entities)) .with_system(update_point_visual_cues.after(maintain_selected_entities)) diff --git a/rmf_site_editor/src/interaction/mode.rs b/rmf_site_editor/src/interaction/mode.rs index 951c3443..d37113ea 100644 --- a/rmf_site_editor/src/interaction/mode.rs +++ b/rmf_site_editor/src/interaction/mode.rs @@ -27,6 +27,9 @@ pub enum InteractionMode { Inspect, /// The user must select an SelectAnchor(SelectAnchor), + /// 3D version of SelectAnchor + // TODO(anyone) rename above SelectAnchor2D and this SelectAnchor + SelectAnchor3D(SelectAnchor3D), } impl Default for InteractionMode { @@ -40,6 +43,7 @@ impl InteractionMode { match self { Self::Inspect => true, Self::SelectAnchor(_) => true, + Self::SelectAnchor3D(_) => true, // _ => false, } } @@ -65,6 +69,9 @@ impl InteractionMode { Self::SelectAnchor(select_anchor) => { Some(select_anchor.backout(&mut params.select_anchor)) } + Self::SelectAnchor3D(select_anchor) => { + Some(select_anchor.backout(&mut params.select_anchor)) + } }; if let Some(change_mode) = change_mode { diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs index 287e6d5c..1d1a699e 100644 --- a/rmf_site_editor/src/interaction/outline.rs +++ b/rmf_site_editor/src/interaction/outline.rs @@ -29,7 +29,7 @@ use smallvec::SmallVec; #[derive(Component)] pub enum OutlineVisualization { Ordinary, - Anchor, + Anchor { body: Entity }, } impl Default for OutlineVisualization { @@ -54,7 +54,7 @@ impl OutlineVisualization { Some(Color::WHITE) } } - OutlineVisualization::Anchor => { + OutlineVisualization::Anchor { .. } => { if hovered.is_hovered { Some(Color::WHITE) } else { @@ -75,7 +75,7 @@ impl OutlineVisualization { OutlineRenderLayers(RenderLayers::none()) } } - OutlineVisualization::Anchor => { + OutlineVisualization::Anchor { .. } => { OutlineRenderLayers(RenderLayers::layer(XRAY_RENDER_LAYER)) } } @@ -94,6 +94,15 @@ impl OutlineVisualization { model_origin: Vec3::ZERO, } } + + /// If this element should use a different entity as its root for + /// highlighting then that will be given here. + pub fn root(&self) -> Option { + match self { + OutlineVisualization::Ordinary => None, + OutlineVisualization::Anchor { body } => Some(*body), + } + } } pub fn add_outline_visualization( @@ -133,9 +142,10 @@ pub fn update_outline_visualization( let color = vis.color(hovered, selected); let layers = vis.layers(hovered, selected); let depth = vis.depth(); + let root = vis.root().unwrap_or(e); let mut queue: SmallVec<[Entity; 10]> = SmallVec::new(); - queue.push(e); + queue.push(root); while let Some(top) = queue.pop() { if let Ok((children, cue)) = descendants.get(top) { if let Some(cue) = cue { diff --git a/rmf_site_editor/src/interaction/picking.rs b/rmf_site_editor/src/interaction/picking.rs index 1db1e56c..ae06ace7 100644 --- a/rmf_site_editor/src/interaction/picking.rs +++ b/rmf_site_editor/src/interaction/picking.rs @@ -15,10 +15,7 @@ * */ -use crate::{ - interaction::*, - site::{Anchor, CurrentSite}, -}; +use crate::{interaction::*, site::Anchor, CurrentWorkspace}; use bevy::prelude::*; use bevy_mod_picking::{PickableMesh, PickingCamera, PickingCameraBundle}; @@ -130,7 +127,7 @@ pub fn update_picked( visual_cues: Query<&ComputedVisualCue>, mut picked: ResMut, mut change_pick: EventWriter, - current_site: Res, + current_workspace: Res, ) { if let Some(blockers) = blockers { if blockers.blocking() { @@ -147,7 +144,7 @@ pub fn update_picked( } } - let current_site = match current_site.0 { + let current_site = match current_workspace.root { Some(current_site) => current_site, None => return, }; diff --git a/rmf_site_editor/src/interaction/select.rs b/rmf_site_editor/src/interaction/select.rs index ce3c9208..e6e5aeb4 100644 --- a/rmf_site_editor/src/interaction/select.rs +++ b/rmf_site_editor/src/interaction/select.rs @@ -203,6 +203,7 @@ pub fn maintain_hovered_entities( mouse_button_input: Res>, touch_input: Res, mut select: EventWriter, mut hover: EventWriter, blockers: Option>, - site: Res, + workspace: Res, + open_sites: Query>, ) { let mut request = match &*mode { InteractionMode::SelectAnchor(request) => request.clone(), @@ -1706,7 +1972,7 @@ pub fn handle_select_anchor_mode( let new_anchor = params.commands.spawn(AnchorBundle::at_transform(tf)).id(); if request.scope.is_site() { - if let Some(site) = site.0 { + if let Some(site) = workspace.to_site(&open_sites) { params.commands.entity(site).add_child(new_anchor); } else { panic!("No current site??"); @@ -1739,6 +2005,9 @@ pub fn handle_select_anchor_mode( PreviewResult::Updated(next) => { *mode = InteractionMode::SelectAnchor(next); } + PreviewResult::Updated3D(next) => { + *mode = InteractionMode::SelectAnchor3D(next); + } PreviewResult::Unchanged => { // Do nothing, the mode has not changed } @@ -1771,8 +2040,255 @@ pub fn handle_select_anchor_mode( } } +fn compute_parent_inverse_pose( + tf: &GlobalTransform, + transforms: &Query<&GlobalTransform>, + parent: Entity, +) -> Pose { + let parent_tf = transforms + .get(parent) + .expect("Failed in fetching parent transform"); + + let inv_tf = parent_tf.affine().inverse(); + let goal_tf = tf.affine(); + let mut pose = Pose::default(); + pose.rot = pose.rot.as_euler_extrinsic_xyz(); + pose.align_with(&Transform::from_matrix((inv_tf * goal_tf).into())) +} + +fn find_mesh_element( + params: &SelectAnchorPlacementParams, + cursor_tf: &GlobalTransform, + hovered: Entity, +) -> MeshElement { + // TODO(luca) Assign a proper vertex id, will need mesh lookup based on + // hover status and (expensive) iteration on vertices + MeshElement::Vertex(0) +} + +pub fn handle_select_anchor_3d_mode( + mut mode: ResMut, + anchors: Query<(), With>, + transforms: Query<&GlobalTransform>, + hovering: Res, + mouse_button_input: Res>, + touch_input: Res, + mut params: SelectAnchorPlacementParams, + selection: Res, + mut select: EventReader>, pub move_to: EventWriter<'w, 's, MoveTo>, pub current_level: ResMut<'w, CurrentLevel>, - pub current_site: ResMut<'w, CurrentSite>, + pub current_workspace: ResMut<'w, CurrentWorkspace>, pub change_mode: ResMut<'w, Events>, pub delete: EventWriter<'w, 's, Delete>, pub toggle_door_levels: EventWriter<'w, 's, ToggleLiftDoorAvailability>, @@ -156,14 +176,20 @@ pub struct LayerEvents<'w, 's> { pub struct AppEvents<'w, 's> { pub commands: Commands<'w, 's>, pub change: ChangeEvents<'w, 's>, + pub workcell_change: WorkcellChangeEvents<'w, 's>, pub display: PanelResources<'w, 's>, pub request: Requests<'w, 's>, + pub file_events: FileEvents<'w, 's>, pub layers: LayerEvents<'w, 's>, + pub app_state: Res<'w, State>, + pub pending_asset_sources: + Query<'w, 's, (Entity, &'static AssetSource, &'static Scale), With>, } -fn standard_ui_layout( +fn site_ui_layout( mut egui_context: ResMut, mut picking_blocker: Option>, + open_sites: Query>, inspector_params: InspectorParams, levels: LevelParams, lights: LightParams, @@ -187,7 +213,7 @@ fn standard_ui_layout( CollapsingHeader::new("Navigation Graphs") .default_open(true) .show(ui, |ui| { - ViewNavGraphs::new(&nav_graphs, &mut events).show(ui); + ViewNavGraphs::new(&nav_graphs, &mut events).show(ui, &open_sites); }); ui.separator(); // TODO(MXG): Consider combining Nav Graphs and Layers @@ -224,6 +250,142 @@ fn standard_ui_layout( }); }); + egui::TopBottomPanel::top("top_panel").show(egui_context.ctx_mut(), |ui| { + egui::menu::bar(ui, |ui| { + ui.menu_button("File", |ui| { + if ui.add(Button::new("New").shortcut_text("Ctrl+N")).clicked() { + events.file_events.new_workspace.send(CreateNewWorkspace); + } + #[cfg(not(target_arch = "wasm32"))] + { + if ui + .add(Button::new("Save").shortcut_text("Ctrl+S")) + .clicked() + { + events + .file_events + .save + .send(SaveWorkspace::new().to_default_file()); + } + if ui + .add(Button::new("Save As").shortcut_text("Ctrl+Shift+S")) + .clicked() + { + events + .file_events + .save + .send(SaveWorkspace::new().to_dialog()); + } + } + if ui + .add(Button::new("Open").shortcut_text("Ctrl+O")) + .clicked() + { + events + .file_events + .load_workspace + .send(LoadWorkspace::Dialog); + } + }); + }); + }); + + let egui_context = egui_context.ctx_mut(); + let ui_has_focus = egui_context.wants_pointer_input() + || egui_context.wants_keyboard_input() + || egui_context.is_pointer_over_area(); + + if let Some(picking_blocker) = &mut picking_blocker { + picking_blocker.ui = ui_has_focus; + } + + if ui_has_focus { + // If the UI has focus and there were no hover events emitted by the UI, + // then we should emit a None hover event + if events.request.hover.is_empty() { + events.request.hover.send(Hover(None)); + } + } +} + +fn workcell_ui_layout( + mut egui_context: ResMut, + mut picking_blocker: Option>, + inspector_params: InspectorParams, + mut events: AppEvents, +) { + egui::SidePanel::right("right_panel") + .resizable(true) + .show(egui_context.ctx_mut(), |ui| { + egui::ScrollArea::both() + .auto_shrink([false, false]) + .show(ui, |ui| { + ui.vertical(|ui| { + CollapsingHeader::new("Inspect") + .default_open(true) + .show(ui, |ui| { + InspectorWidget::new(&inspector_params, &mut events).show(ui); + }); + ui.separator(); + CollapsingHeader::new("Create") + .default_open(true) + .show(ui, |ui| { + CreateWidget::new(&mut events).show(ui); + }); + ui.separator(); + }); + }); + }); + + egui::TopBottomPanel::top("top_panel").show(egui_context.ctx_mut(), |ui| { + egui::menu::bar(ui, |ui| { + ui.menu_button("File", |ui| { + if ui.add(Button::new("New").shortcut_text("Ctrl+N")).clicked() { + events.file_events.new_workspace.send(CreateNewWorkspace); + } + #[cfg(not(target_arch = "wasm32"))] + { + if ui + .add(Button::new("Save").shortcut_text("Ctrl+S")) + .clicked() + { + events + .file_events + .save + .send(SaveWorkspace::new().to_default_file()); + } + if ui + .add(Button::new("Save As").shortcut_text("Ctrl+Shift+S")) + .clicked() + { + events + .file_events + .save + .send(SaveWorkspace::new().to_dialog()); + } + if ui + .add(Button::new("Export urdf").shortcut_text("Ctrl+E")) + .clicked() + { + events + .file_events + .save + .send(SaveWorkspace::new().to_dialog().to_urdf()); + } + } + if ui + .add(Button::new("Open").shortcut_text("Ctrl+O")) + .clicked() + { + events + .file_events + .load_workspace + .send(LoadWorkspace::Dialog); + } + }); + }); + }); + let egui_context = egui_context.ctx_mut(); let ui_has_focus = egui_context.wants_pointer_input() || egui_context.wants_keyboard_input() diff --git a/rmf_site_editor/src/widgets/view_levels.rs b/rmf_site_editor/src/widgets/view_levels.rs index 50b2ce1c..f8c87a6b 100644 --- a/rmf_site_editor/src/widgets/view_levels.rs +++ b/rmf_site_editor/src/widgets/view_levels.rs @@ -47,6 +47,7 @@ impl Default for LevelDisplay { #[derive(SystemParam)] pub struct LevelParams<'w, 's> { pub levels: Query<'w, 's, (Entity, &'static LevelProperties)>, + pub parents: Query<'w, 's, &'static Parent>, pub icons: Res<'w, Icons>, } @@ -94,6 +95,10 @@ impl<'a, 'w1, 's1, 'w2, 's2> ViewLevels<'a, 'w1, 's1, 'w2, 's2> { .params .levels .iter() + .filter(|(e, _props)| { + AncestorIter::new(&self.params.parents, *e) + .any(|e| Some(e) == self.events.request.current_workspace.root) + }) .map(|(e, props)| (Reverse(props.elevation), e)) .collect(); diff --git a/rmf_site_editor/src/widgets/view_nav_graphs.rs b/rmf_site_editor/src/widgets/view_nav_graphs.rs index cf59838d..7a86f38c 100644 --- a/rmf_site_editor/src/widgets/view_nav_graphs.rs +++ b/rmf_site_editor/src/widgets/view_nav_graphs.rs @@ -18,11 +18,11 @@ use crate::{ recency::RecencyRanking, site::{ - Change, CurrentSite, Delete, DisplayColor, ImportNavGraphs, NameInSite, NavGraph, - NavGraphMarker, SaveNavGraphs, DEFAULT_NAV_GRAPH_COLORS, + Change, Delete, DisplayColor, ImportNavGraphs, NameInSite, NavGraph, NavGraphMarker, + SaveNavGraphs, SiteProperties, DEFAULT_NAV_GRAPH_COLORS, }, widgets::{inspector::color_edit, AppEvents, Icons, MoveLayer}, - Autoload, + Autoload, CurrentWorkspace, }; use bevy::{ ecs::system::SystemParam, @@ -88,8 +88,8 @@ impl<'a, 'w1, 's1, 'w2, 's2> ViewNavGraphs<'a, 'w1, 's1, 'w2, 's2> { Self { params, events } } - pub fn show(self, ui: &mut Ui) { - let ranking = match self.events.request.current_site.0 { + pub fn show(self, ui: &mut Ui, open_sites: &Query>) { + let ranking = match self.events.request.current_workspace.root { Some(c) => match self.params.ranking.get(c) { Ok(r) => r, Err(_) => return, @@ -210,7 +210,7 @@ impl<'a, 'w1, 's1, 'w2, 's2> ViewNavGraphs<'a, 'w1, 's1, 'w2, 's2> { { ui.separator(); if ui.button("Import Graphs...").clicked() { - match self.events.request.current_site.0 { + match self.events.request.current_workspace.to_site(open_sites) { Some(into_site) => { match &self.events.display.nav_graph.choosing_file_to_import { Some(_) => { @@ -251,7 +251,9 @@ impl<'a, 'w1, 's1, 'w2, 's2> ViewNavGraphs<'a, 'w1, 's1, 'w2, 's2> { ui.horizontal(|ui| { if let Some(export_file) = &self.events.display.nav_graph.export_file { if ui.button("Export").clicked() { - if let Some(current_site) = self.events.request.current_site.0 { + if let Some(current_site) = + self.events.request.current_workspace.to_site(open_sites) + { self.events.request.save_nav_graphs.send(SaveNavGraphs { site: current_site, to_file: export_file.clone(), @@ -295,13 +297,14 @@ pub fn resolve_nav_graph_import_export_files( mut nav_graph_display: ResMut, mut save_nav_graphs: EventWriter, mut import_nav_graphs: EventWriter, - current_site: Res, + open_sites: Query>, + current_workspace: Res, ) { if 'resolved: { if let Some(task) = &mut nav_graph_display.choosing_file_for_export { if let Some(result) = future::block_on(future::poll_once(task)) { if let Some(result) = result { - if let Some(current_site) = current_site.0 { + if let Some(current_site) = current_workspace.to_site(&open_sites) { save_nav_graphs.send(SaveNavGraphs { site: current_site, to_file: result.clone(), diff --git a/rmf_site_editor/src/workcell/keyboard.rs b/rmf_site_editor/src/workcell/keyboard.rs new file mode 100644 index 00000000..c1e1e54d --- /dev/null +++ b/rmf_site_editor/src/workcell/keyboard.rs @@ -0,0 +1,59 @@ +/* + * 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 bevy::prelude::*; +use bevy_egui::EguiContext; + +use rmf_site_format::{WorkcellCollisionMarker, WorkcellVisualMarker}; + +pub fn handle_workcell_keyboard_input( + keyboard_input: Res>, + mut egui_context: ResMut, + mut visuals: Query< + &mut Visibility, + (With, Without), + >, + mut collisions: Query< + &mut Visibility, + (With, Without), + >, +) { + let egui_context = egui_context.ctx_mut(); + let ui_has_focus = egui_context.wants_pointer_input() + || egui_context.wants_keyboard_input() + || egui_context.is_pointer_over_area(); + + if ui_has_focus { + return; + } + + if keyboard_input.any_pressed([KeyCode::LShift, KeyCode::RShift]) { + if keyboard_input.just_pressed(KeyCode::V) { + println!("Toggling visuals"); + for mut v in visuals.iter_mut() { + v.is_visible = !v.is_visible; + } + } + + if keyboard_input.just_pressed(KeyCode::C) { + println!("Toggling collisions"); + for mut c in collisions.iter_mut() { + c.is_visible = !c.is_visible; + } + } + } +} diff --git a/rmf_site_editor/src/workcell/load.rs b/rmf_site_editor/src/workcell/load.rs new file mode 100644 index 00000000..04557cb8 --- /dev/null +++ b/rmf_site_editor/src/workcell/load.rs @@ -0,0 +1,160 @@ +/* + * 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 std::collections::HashMap; +use std::path::PathBuf; + +use crate::site::{AnchorBundle, DefaultFile, Dependents, PreventDeletion, SiteState}; +use crate::workcell::ChangeCurrentWorkcell; +use bevy::prelude::*; +use std::collections::HashSet; + +use rmf_site_format::{ + Category, ConstraintDependents, MeshConstraint, NameInWorkcell, SiteID, + WorkcellCollisionMarker, WorkcellVisualMarker, +}; + +pub struct LoadWorkcell { + /// The site data to load + pub workcell: rmf_site_format::Workcell, + /// Should the application switch focus to this new site + pub focus: bool, + /// Set if the workcell was loaded from a file + pub default_file: Option, +} + +fn generate_workcell_entities( + commands: &mut Commands, + workcell: &rmf_site_format::Workcell, +) -> Entity { + // Create hashmap of ids to entity to correctly generate hierarchy + let mut id_to_entity = HashMap::new(); + // Hashmap of parent id to list of its children entities + let mut parent_to_child_entities = HashMap::new(); + // Hashmap of parent model entity to constraint dependent entity + let mut model_to_constraint_dependent_entities = HashMap::new(); + + let root = commands + .spawn(SpatialBundle::VISIBLE_IDENTITY) + .insert(workcell.properties.clone()) + .insert(NameInWorkcell(workcell.properties.name.clone())) + .insert(SiteID(workcell.id)) + .insert(Category::Workcell) + .insert(PreventDeletion::because( + "Workcell root cannot be deleted".to_string(), + )) + .id(); + id_to_entity.insert(&workcell.id, root); + + for (id, parented_visual) in &workcell.visuals { + let cmd = commands.spawn((SiteID(*id), WorkcellVisualMarker)); + let e = cmd.id(); + parented_visual.bundle.add_bevy_components(cmd); + // TODO(luca) this hashmap update is duplicated, refactor into function + let child_entities: &mut Vec = parent_to_child_entities + .entry(parented_visual.parent) + .or_default(); + child_entities.push(e); + id_to_entity.insert(id, e); + } + + for (id, parented_collision) in &workcell.collisions { + let cmd = commands.spawn((SiteID(*id), WorkcellCollisionMarker)); + let e = cmd.id(); + parented_collision.bundle.add_bevy_components(cmd); + // TODO(luca) this hashmap update is duplicated, refactor into function + let child_entities: &mut Vec = parent_to_child_entities + .entry(parented_collision.parent) + .or_default(); + child_entities.push(e); + id_to_entity.insert(id, e); + } + + for (id, parented_anchor) in &workcell.frames { + let e = commands + .spawn(AnchorBundle::new(parented_anchor.bundle.anchor.clone()).visible(true)) + .insert(SiteID(*id)) + .id(); + if let Some(c) = &parented_anchor.bundle.mesh_constraint { + let model_entity = *id_to_entity + .get(&c.entity) + .expect("Mesh constraint refers to non existing model"); + commands.entity(e).insert(MeshConstraint { + entity: model_entity, + element: c.element.clone(), + relative_pose: c.relative_pose, + }); + let constraint_dependents: &mut HashSet = + model_to_constraint_dependent_entities + .entry(model_entity) + .or_default(); + constraint_dependents.insert(e); + } + if let Some(name) = &parented_anchor.bundle.name { + commands.entity(e).insert(name.clone()); + } + let child_entities: &mut Vec = parent_to_child_entities + .entry(parented_anchor.parent) + .or_default(); + child_entities.push(e); + id_to_entity.insert(id, e); + } + + // Add constraint dependents to models + for (model, dependents) in model_to_constraint_dependent_entities { + commands + .entity(model) + .insert(ConstraintDependents(dependents)); + } + + for (parent, children) in parent_to_child_entities { + if let Some(parent) = id_to_entity.get(&parent) { + commands + .entity(*parent) + .insert(Dependents(HashSet::from_iter(children.clone()))) + .push_children(&children); + } else { + println!("DEV error, didn't find matching entity for id {}", parent); + continue; + } + } + root +} + +pub fn load_workcell( + mut commands: Commands, + mut load_workcells: EventReader, + mut change_current_workcell: EventWriter, + mut site_display_state: ResMut>, +) { + for cmd in load_workcells.iter() { + println!("Loading workcell"); + let root = generate_workcell_entities(&mut commands, &cmd.workcell); + if let Some(path) = &cmd.default_file { + commands.entity(root).insert(DefaultFile(path.clone())); + } + + if cmd.focus { + change_current_workcell.send(ChangeCurrentWorkcell { root }); + + // TODO(luca) get rid of SiteState + if *site_display_state.current() == SiteState::Display { + site_display_state.set(SiteState::Off).ok(); + } + } + } +} diff --git a/rmf_site_editor/src/workcell/mesh_constraint.rs b/rmf_site_editor/src/workcell/mesh_constraint.rs new file mode 100644 index 00000000..e93c3120 --- /dev/null +++ b/rmf_site_editor/src/workcell/mesh_constraint.rs @@ -0,0 +1,65 @@ +/* + * 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::site::AnchorBundle; +use bevy::prelude::*; +use rmf_site_format::{Anchor, ConstraintDependents, MeshConstraint, ModelMarker, Pose}; + +pub fn update_constraint_dependents( + updated_models: Query< + (&ConstraintDependents, &Transform), + (Changed, With), + >, + mut transforms: Query<&mut Transform, Without>, + mesh_constraints: Query<&MeshConstraint>, +) { + // TODO(luca) Add widget for parent reassignment in models, otherwise Changed will + // never trigger + // When a mesh constraint is added we need to remove the Pose component and + // set the transform of the entity according to the entity contained in the MeshConstraint + // component + for (deps, model_tf) in updated_models.iter() { + for dep in deps.iter() { + if let Ok(mut anchor_tf) = transforms.get_mut(*dep) { + if let Ok(constraint) = mesh_constraints.get(*dep) { + // TODO(luca) should relative_pose be relative to model origin instead? + // constraint.relative_pose = tf.into(); + // Set the transform to be a combination of model's and constraint's relative_pose + *anchor_tf = *model_tf * constraint.relative_pose.transform(); + } + } + } + } +} + +pub fn add_anchors_for_new_mesh_constraints( + mut commands: Commands, + changed_constraints: Query<(Entity, &MeshConstraint), Changed>>, + transforms: Query<&Transform>, +) { + for (e, constraint) in changed_constraints.iter() { + if let Ok(model_tf) = transforms.get(constraint.entity) { + let tf = *model_tf * constraint.relative_pose.transform(); + let pose = Pose::default().align_with(&tf); + // TODO(luca) is this OK performance wise or should we detect if the component is + // already present and change its value? + commands + .entity(e) + .insert(AnchorBundle::new(Anchor::Pose3D(pose))); + } + } +} diff --git a/rmf_site_editor/src/workcell/mod.rs b/rmf_site_editor/src/workcell/mod.rs new file mode 100644 index 00000000..d74aced0 --- /dev/null +++ b/rmf_site_editor/src/workcell/mod.rs @@ -0,0 +1,125 @@ +/* + * 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. + * +*/ + +pub mod load; +pub use load::*; + +pub mod keyboard; +pub use keyboard::*; + +pub mod mesh_constraint; +pub use mesh_constraint::*; + +pub mod save; +pub use save::*; + +pub mod workcell; +pub use workcell::*; + +pub mod urdf; +pub use urdf::*; + +use bevy::pbr::wireframe::{Wireframe, WireframePlugin}; +use bevy::render::{render_resource::WgpuFeatures, settings::WgpuSettings}; +use bevy::{prelude::*, render::view::visibility::VisibilitySystems, transform::TransformSystem}; +use bevy_infinite_grid::{InfiniteGrid, InfiniteGridBundle, InfiniteGridPlugin}; + +use crate::interaction::Gizmo; +use crate::AppState; +use crate::{ + shapes::make_infinite_grid, + site::{ + handle_new_mesh_primitives, make_models_selectable, update_anchor_transforms, + update_model_scenes, update_model_tentative_formats, update_transforms_for_changed_poses, + }, +}; + +use rmf_site_format::ModelMarker; + +use bevy_rapier3d::prelude::*; + +#[derive(Default)] +pub struct WorkcellEditorPlugin; + +fn spawn_grid(mut commands: Commands) { + commands.spawn(make_infinite_grid(1.0, 100.0, None)); +} + +fn delete_grid(mut commands: Commands, grids: Query>) { + for grid in grids.iter() { + commands.entity(grid).despawn_recursive(); + } +} + +fn add_wireframe_to_meshes( + mut commands: Commands, + new_meshes: Query>>, + parents: Query<&Parent>, + models: Query>, +) { + for e in new_meshes.iter() { + for ancestor in AncestorIter::new(&parents, e) { + if let Ok(_) = models.get(ancestor) { + println!("Adding wireframe to mesh {:?}", e); + commands.entity(e).insert(Wireframe); + } + } + } +} + +impl Plugin for WorkcellEditorPlugin { + fn build(&self, app: &mut App) { + app.add_plugin(InfiniteGridPlugin) + .insert_resource(WgpuSettings { + features: WgpuFeatures::POLYGON_MODE_LINE, + ..default() + }) + .add_plugin(WireframePlugin) + .add_plugin(RapierPhysicsPlugin::::default()) + .add_plugin(RapierDebugRenderPlugin::default()) + .add_event::() + .add_event::() + .add_event::() + .add_system_set(SystemSet::on_enter(AppState::WorkcellEditor).with_system(spawn_grid)) + .add_system_set(SystemSet::on_exit(AppState::WorkcellEditor).with_system(delete_grid)) + .add_system_set( + SystemSet::on_update(AppState::WorkcellEditor) + .with_system(add_wireframe_to_meshes) + .with_system(update_constraint_dependents) + .with_system(update_model_scenes) + .with_system(update_model_tentative_formats) + .with_system(make_models_selectable) + .with_system(handle_workcell_keyboard_input) + .with_system(handle_new_mesh_primitives) + .with_system(change_workcell.before(load_workcell)) + .with_system(handle_new_urdf_roots), + ) + .add_system(load_workcell) + .add_system(save_workcell) + .add_system(add_workcell_visualization) + .add_system_set( + SystemSet::on_update(AppState::WorkcellEditor) + .before(TransformSystem::TransformPropagate) + .after(VisibilitySystems::VisibilityPropagate) + .with_system(update_anchor_transforms) + .with_system( + add_anchors_for_new_mesh_constraints.before(update_anchor_transforms), + ) + .with_system(update_transforms_for_changed_poses), + ); + } +} diff --git a/rmf_site_editor/src/workcell/save.rs b/rmf_site_editor/src/workcell/save.rs new file mode 100644 index 00000000..cd54ebc7 --- /dev/null +++ b/rmf_site_editor/src/workcell/save.rs @@ -0,0 +1,271 @@ +/* + * 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 bevy::ecs::system::SystemState; +use bevy::prelude::*; +use std::path::PathBuf; + +use crate::site::Pending; +use crate::ExportFormat; + +use thiserror::Error as ThisError; + +use rmf_site_format::*; + +/// Event used to trigger saving of the workcell +pub struct SaveWorkcell { + pub root: Entity, + pub to_file: PathBuf, + pub format: ExportFormat, +} + +#[derive(ThisError, Debug, Clone)] +pub enum WorkcellGenerationError { + #[error("the specified entity [{0:?}] does not refer to a workcell")] + InvalidWorkcellEntity(Entity), +} + +fn parent_in_workcell(q_parents: &Query<&Parent>, entity: Entity, root: Entity) -> bool { + AncestorIter::new(q_parents, entity) + .find(|p| *p == root) + .is_some() +} + +// This is mostly duplicated with the function in site/save.rs, however this case +// is a lot simpler, also site/save.rs checks for children of levels but there are no levels here +fn assign_site_ids(world: &mut World, workcell: Entity) { + // TODO(luca) actually keep site IDs instead of always generating them from scratch + // (as it is done in site editor) + let mut state: SystemState<( + Query< + Entity, + ( + Or<( + With, + With, + With, + With, + )>, + Without, + ), + >, + Query<&Children>, + )> = SystemState::new(world); + let (q_used_entities, q_children) = state.get(&world); + + let mut new_entities = vec![workcell]; + for e in q_children.iter_descendants(workcell) { + if let Ok(_) = q_used_entities.get(e) { + new_entities.push(e); + } + } + + for (idx, entity) in new_entities.iter().enumerate() { + world + .entity_mut(*entity) + .insert(SiteID(idx.try_into().unwrap())); + } +} + +pub fn generate_workcell( + world: &mut World, + root: Entity, +) -> Result { + assign_site_ids(world, root); + let mut state: SystemState<( + Query< + ( + Entity, + &Anchor, + Option<&NameInWorkcell>, + &SiteID, + &Parent, + Option<&MeshConstraint>, + ), + Without, + >, + Query< + ( + Entity, + &NameInWorkcell, + Option<&AssetSource>, + Option<&MeshPrimitive>, + &Pose, + &SiteID, + &Parent, + &Scale, + ), + ( + Or<(With, With)>, + Without, + ), + >, + Query<&WorkcellVisualMarker>, + Query<&WorkcellCollisionMarker>, + Query<&SiteID>, + Query<&WorkcellProperties>, + Query<&Parent>, + )> = SystemState::new(world); + let (q_anchors, q_models, q_visuals, q_collisions, q_site_id, q_properties, q_parents) = + state.get(world); + + let mut workcell = Workcell::default(); + match q_properties.get(root) { + Ok(properties) => { + workcell.properties = properties.clone(); + } + Err(_) => { + return Err(WorkcellGenerationError::InvalidWorkcellEntity(root)); + } + } + + // Visuals + for (e, name, source, primitive, pose, id, parent, scale) in &q_models { + if !parent_in_workcell(&q_parents, e, root) { + continue; + } + // Get the parent SiteID + let parent = match q_site_id.get(parent.get()) { + Ok(parent) => parent.0, + Err(_) => { + println!("DEV Error: Parent not found for visual {:?}", parent.get()); + continue; + } + }; + let geom = if let Some(source) = source { + // It's a model + Geometry::Mesh { + filename: String::from(source), + scale: Some(**scale), + } + } else if let Some(primitive) = primitive { + Geometry::Primitive(primitive.clone()) + } else { + println!("DEV Error, visual without primitive or mesh"); + continue; + }; + if q_visuals.get(e).is_ok() { + workcell.visuals.insert( + id.0, + Parented { + parent: parent, + bundle: WorkcellModel { + name: name.0.clone(), + geometry: geom, + pose: pose.clone(), + }, + }, + ); + } else if q_collisions.get(e).is_ok() { + // TODO(luca) reduce duplication with above branch + workcell.collisions.insert( + id.0, + Parented { + parent: parent, + bundle: WorkcellModel { + name: name.0.clone(), + geometry: geom, + pose: pose.clone(), + }, + }, + ); + } + } + + // Anchors + for (e, anchor, name, id, parent, constraint) in &q_anchors { + if !parent_in_workcell(&q_parents, e, root) { + continue; + } + let parent = match q_site_id.get(parent.get()) { + Ok(parent) => parent.0, + Err(_) => { + println!("DEV Error: Parent not found for anchor {:?}", parent.get()); + continue; + } + }; + // TODO(luca) is duplication here OK? same information is contained in mesh constraint and + // anchor + let constraint = if let Some(c) = constraint { + Some(MeshConstraint { + entity: **q_site_id.get(c.entity).unwrap(), + element: c.element.clone(), + relative_pose: c.relative_pose, + }) + } else { + None + }; + + workcell.frames.insert( + id.0, + Parented { + parent: parent, + bundle: Frame { + anchor: anchor.clone(), + name: name.cloned(), + mesh_constraint: constraint, + marker: FrameMarker, + }, + }, + ); + } + + Ok(workcell) +} + +pub fn save_workcell(world: &mut World) { + let save_events: Vec<_> = world + .resource_mut::>() + .drain() + .collect(); + for save_event in save_events { + let path = save_event.to_file; + println!( + "Saving to {}", + path.to_str().unwrap_or("") + ); + let f = match std::fs::File::create(path) { + Ok(f) => f, + Err(err) => { + println!("Unable to save file: {err}"); + continue; + } + }; + + let workcell = match generate_workcell(world, save_event.root) { + Ok(root) => root, + Err(err) => { + println!("Unable to compile workcell: {err}"); + continue; + } + }; + + match save_event.format { + ExportFormat::Default => match workcell.to_writer(f) { + Ok(()) => { + println!("Save successful"); + } + Err(err) => { + println!("Save failed: {err}"); + } + }, + ExportFormat::Urdf => { + println!("Saving to urdf"); + } + } + } +} diff --git a/rmf_site_editor/src/workcell/urdf.rs b/rmf_site_editor/src/workcell/urdf.rs new file mode 100644 index 00000000..4fd41e4a --- /dev/null +++ b/rmf_site_editor/src/workcell/urdf.rs @@ -0,0 +1,128 @@ +/* + * 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 bevy::prelude::*; +use std::collections::{HashMap, HashSet}; + +use rmf_site_format::{ + Anchor, Angle, Category, Link, MeshPrimitive, Pose, Rotation, UrdfRoot, + WorkcellCollisionMarker, WorkcellModel, WorkcellVisualMarker, +}; + +use urdf_rs::JointType; + +use bevy_rapier3d::na::geometry::Rotation as RapierRotation; +use bevy_rapier3d::prelude::*; +use bevy_rapier3d::rapier::math::Isometry; + +pub fn handle_new_urdf_roots(mut commands: Commands, new_urdfs: Query<(Entity, &UrdfRoot)>) { + let mut link_name_to_entity = HashMap::new(); + // Keep track of which links have a parent, add the ones that don't as a root child + let mut root_links = HashSet::new(); + for (e, urdf) in new_urdfs.iter() { + commands.entity(e).insert(RigidBody::KinematicVelocityBased); + // Populate here + for link in &urdf.links { + // TODO*luca) link as child of anchor + let link_entity = commands + .spawn(SpatialBundle::VISIBLE_IDENTITY) + .insert(Link::from(link)) + .insert(RigidBody::KinematicVelocityBased) + .insert(Category::Workcell) + .id(); + println!("Found link {:?} - {}", link_entity, link.name); + link_name_to_entity.insert(link.name.clone(), link_entity); + root_links.insert(link_entity); + for visual in &link.visual { + let model = WorkcellModel::from(visual); + let cmd = commands.spawn((SpatialBundle::VISIBLE_IDENTITY, WorkcellVisualMarker)); + let id = cmd.id(); + model.add_bevy_components(cmd); + commands.entity(link_entity).add_child(id); + } + for collision in &link.collision { + let model = WorkcellModel::from(collision); + let cmd = + commands.spawn((SpatialBundle::VISIBLE_IDENTITY, WorkcellCollisionMarker)); + let id = cmd.id(); + model.add_bevy_components(cmd); + commands.entity(link_entity).add_child(id); + } + } + for joint in &urdf.joints { + if let Some(parent) = link_name_to_entity.get(&joint.parent.link) { + if let Some(child) = link_name_to_entity.get(&joint.child.link) { + let trans = Vec3::from_array(joint.origin.xyz.map(|t| t as f32)); + let rot = Vec3::from_array(joint.origin.rpy.map(|t| t as f32)); + let rot = RapierRotation::from_euler_angles(rot[0], rot[1], rot[2]); + // TODO(luca) invert the above since it's in joint coordinates + let frame = Isometry::::from_parts(trans.into(), rot.into()); + let joint_data = match joint.joint_type { + JointType::Revolute => { + let axis = Vec3::from_array(joint.axis.xyz.map(|t| t as f32)); + let joint = RevoluteJointBuilder::new(axis) + //.local_anchor2(trans) + .limits([joint.limit.lower as f32, joint.limit.upper as f32]); + ImpulseJoint::new(*parent, joint) + } + JointType::Prismatic => { + let axis = Vec3::from_array(joint.axis.xyz.map(|t| t as f32)); + let joint = PrismaticJointBuilder::new(axis) + //.local_anchor2(trans) + .local_axis2(axis) + .limits([joint.limit.lower as f32, joint.limit.upper as f32]); + ImpulseJoint::new(*parent, joint) + } + JointType::Fixed => { + let joint = FixedJointBuilder::new() + .local_anchor1(trans) + //.local_anchor2(trans) + //.local_basis2(rot.into()) + ; + ImpulseJoint::new(*parent, joint) + } + _ => { + todo!("Unimplemented joint type {:?}", joint.joint_type); + } + }; + let trans = joint.origin.xyz.map(|t| t as f32); + let rot = Rotation::EulerExtrinsicXYZ( + joint.origin.rpy.map(|angle| Angle::Rad(angle as f32)), + ); + //commands.entity(*child).insert(AnchorBundle::new(Anchor::Pose3D(Pose {trans, rot}))); + commands.entity(*parent).add_child(*child); + root_links.remove(child); + println!( + "Adding joint between {:?} - {} and {:?} - {}", + *parent, &joint.parent.link, *child, &joint.child.link + ); + commands.entity(*child).with_children(|children| { + children + .spawn(SpatialBundle::VISIBLE_IDENTITY) + .insert(joint_data) + .insert(Anchor::Pose3D(Pose { trans, rot })); + }); + } + } + } + for link in root_links.iter() { + println!("Found root entity {:?}", link); + commands.entity(e).add_child(*link); + } + commands.entity(e).remove::(); + } +} diff --git a/rmf_site_editor/src/workcell/workcell.rs b/rmf_site_editor/src/workcell/workcell.rs new file mode 100644 index 00000000..9ddbca4a --- /dev/null +++ b/rmf_site_editor/src/workcell/workcell.rs @@ -0,0 +1,69 @@ +/* + * 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::interaction::{InteractionAssets, Selectable}; +use crate::site::SiteAssets; +use crate::CurrentWorkspace; +use bevy::prelude::*; +use rmf_site_format::WorkcellProperties; + +/// Used as an event to command that a new workcell should be made the current one +#[derive(Clone, Copy, Debug)] +pub struct ChangeCurrentWorkcell { + /// What should the current workcell root be + pub root: Entity, +} + +pub fn change_workcell( + mut current_workspace: ResMut, + mut change_current_workcell: EventReader, + open_workcells: Query>, +) { + if let Some(cmd) = change_current_workcell.iter().last() { + if open_workcells.get(cmd.root).is_err() { + println!( + "Requested workspace change to an entity that is not an open workcell: {:?}", + cmd.root + ); + return; + } + + current_workspace.root = Some(cmd.root); + current_workspace.display = true; + } +} + +pub fn add_workcell_visualization( + mut commands: Commands, + new_workcells: Query>, + site_assets: Res, + interaction_assets: Res, +) { + for e in new_workcells.iter() { + let body_mesh = site_assets.site_anchor_mesh.clone(); + let mut entity_commands = commands.entity(e); + entity_commands.add_children(|parent| { + let mut body = parent.spawn(PbrBundle { + mesh: body_mesh, + material: site_assets.passive_anchor_material.clone(), + ..default() + }); + body.insert(Selectable::new(e)); + }); + interaction_assets.make_orientation_cue_meshes(&mut commands, e, 1.0); + } +} diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs new file mode 100644 index 00000000..96a923cc --- /dev/null +++ b/rmf_site_editor/src/workspace.rs @@ -0,0 +1,337 @@ +/* + * 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 bevy::{prelude::*, tasks::AsyncComputeTaskPool}; +use rfd::AsyncFileDialog; +use std::path::PathBuf; + +use crate::interaction::InteractionState; +use crate::site::LoadSite; +use crate::workcell::LoadWorkcell; +use crate::AppState; +use rmf_site_format::legacy::building_map::BuildingMap; +use rmf_site_format::{Site, SiteProperties, Workcell}; + +use crossbeam_channel::{Receiver, Sender}; + +/// Used as an event to command that a new workspace should be made the current one +#[derive(Clone, Copy, Debug)] +pub struct ChangeCurrentWorkspace { + /// What should the current site be + pub root: Entity, +} + +/// Used as an event to command that a new workspace should be created, behavior will depend on +/// what app mode the editor is currently in +pub struct CreateNewWorkspace; + +/// Used as an event to command that a workspace should be loaded. This will spawn a file open +/// dialog (in non-wasm) with allowed extensions depending on the app state +// TODO(luca) Encapsulate a list of optional filters, for example to allow users to only load +// workcells or sites +// Dialog will spawn a RFD dialog, Path will open a specific path, the others will parse embedded +// data +pub enum LoadWorkspace { + Dialog, + Path(PathBuf), + Data(WorkspaceData), +} + +pub enum WorkspaceData { + LegacyBuilding(Vec), + Site(Vec), + Workcell(Vec), +} + +impl WorkspaceData { + pub fn new(path: &PathBuf, data: Vec) -> Option { + let filename = path.file_name().and_then(|f| f.to_str())?; + if filename.ends_with(".building.yaml") { + Some(WorkspaceData::LegacyBuilding(data)) + } else if filename.ends_with("site.ron") { + Some(WorkspaceData::Site(data)) + } else if filename.ends_with("workcell.json") { + Some(WorkspaceData::Workcell(data)) + } else { + println!("Unrecognized file type {:?}", filename); + None + } + } +} + +/// Used as a resource that keeps track of the current workspace +#[derive(Clone, Copy, Debug, Default, Resource)] +pub struct CurrentWorkspace { + pub root: Option, + pub display: bool, +} + +pub struct LoadWorkspaceFile(pub std::path::PathBuf, pub Vec); + +/// Using channels instead of events to allow usage in wasm since, unlike event writers, they can +/// be cloned and moved into async functions therefore don't have lifetime issues +#[derive(Debug, Resource)] +pub struct LoadWorkspaceChannels { + pub sender: Sender, + pub receiver: Receiver, +} + +impl Default for LoadWorkspaceChannels { + fn default() -> Self { + let (sender, receiver) = crossbeam_channel::unbounded(); + Self { sender, receiver } + } +} + +/// Used to keep track of visibility when switching workspace +#[derive(Debug, Default, Resource)] +pub struct RecallWorkspace(Option); + +impl CurrentWorkspace { + pub fn to_site(self, open_sites: &Query>) -> Option { + let site_entity = self.root?; + open_sites.get(site_entity).ok() + } +} + +pub struct WorkspacePlugin; + +impl Plugin for WorkspacePlugin { + fn build(&self, app: &mut App) { + app.add_event::() + .add_event::() + .add_event::() + .init_resource::() + .init_resource::() + .init_resource::() + .add_system(dispatch_new_workspace_events) + .add_system(workspace_file_load_complete) + .add_system(sync_workspace_visibility) + .add_system(dispatch_load_workspace_events); + } +} + +pub fn dispatch_new_workspace_events( + state: Res>, + mut new_workspace: EventReader, + mut load_site: EventWriter, + mut load_workcell: EventWriter, +) { + if let Some(_cmd) = new_workspace.iter().last() { + match state.current() { + AppState::MainMenu => { + println!("DEV ERROR: Sent generic change workspace while in main menu"); + } + AppState::SiteEditor => { + load_site.send(LoadSite { + site: Site::default(), + focus: true, + default_file: None, + }); + } + AppState::WorkcellEditor => { + load_workcell.send(LoadWorkcell { + workcell: Workcell::default(), + focus: true, + default_file: None, + }); + } + } + } +} + +pub fn dispatch_load_workspace_events( + mut commands: Commands, + mut app_state: ResMut>, + mut interaction_state: ResMut>, + mut load_channels: ResMut, + mut load_site: EventWriter, + mut load_workcell: EventWriter, + mut load_workspace: EventReader, +) { + if let Some(cmd) = load_workspace.iter().last() { + match cmd { + LoadWorkspace::Dialog => { + let sender = load_channels.sender.clone(); + AsyncComputeTaskPool::get() + .spawn(async move { + if let Some(file) = AsyncFileDialog::new().pick_file().await { + let data = file.read().await; + #[cfg(not(target_arch = "wasm32"))] + sender + .send(LoadWorkspaceFile(file.path().to_path_buf(), data)) + .expect("Failed sending file event"); + #[cfg(target_arch = "wasm32")] + sender + .send(LoadWorkspaceFile(PathBuf::from(file.file_name()), data)) + .expect("Failed sending file event"); + } + }) + .detach(); + } + LoadWorkspace::Path(path) => { + if let Some(data) = std::fs::read(&path) + .ok() + .and_then(|d| WorkspaceData::new(&path, d)) + { + handle_workspace_data( + Some(path.clone()), + &data, + &mut app_state, + &mut interaction_state, + &mut load_site, + &mut load_workcell, + ); + } + } + LoadWorkspace::Data(data) => { + // Do a sync load and state update + handle_workspace_data( + None, + &data, + &mut app_state, + &mut interaction_state, + &mut load_site, + &mut load_workcell, + ); + } + } + } +} + +fn handle_workspace_data( + file: Option, + workspace_data: &WorkspaceData, + app_state: &mut ResMut>, + interaction_state: &mut ResMut>, + load_site: &mut EventWriter, + load_workcell: &mut EventWriter, +) { + match workspace_data { + WorkspaceData::LegacyBuilding(data) => { + println!("Opening legacy building map file"); + match BuildingMap::from_bytes(&data) { + Ok(building) => { + match building.to_site() { + Ok(site) => { + // Switch state + app_state.set(AppState::SiteEditor).ok(); + load_site.send(LoadSite { + site, + focus: true, + default_file: file, + }); + interaction_state.set(InteractionState::Enable).ok(); + } + Err(err) => { + println!("Failed converting to site {:?}", err); + } + } + } + Err(err) => { + println!("Failed loading legacy building {:?}", err); + } + } + } + WorkspaceData::Site(data) => { + println!("Opening site file"); + match Site::from_bytes(&data) { + Ok(site) => { + // Switch state + app_state.set(AppState::SiteEditor).ok(); + load_site.send(LoadSite { + site, + focus: true, + default_file: file, + }); + interaction_state.set(InteractionState::Enable).ok(); + } + Err(err) => { + println!("Failed loading site {:?}", err); + } + } + } + WorkspaceData::Workcell(data) => { + println!("Opening workcell file"); + match Workcell::from_bytes(&data) { + Ok(workcell) => { + // Switch state + app_state.set(AppState::WorkcellEditor).ok(); + load_workcell.send(LoadWorkcell { + workcell, + focus: true, + default_file: file, + }); + interaction_state.set(InteractionState::Enable).ok(); + } + Err(err) => { + println!("Failed loading workcell {:?}", err); + } + } + } + } +} + +/// Handles the file opening events +fn workspace_file_load_complete( + mut commands: Commands, + mut app_state: ResMut>, + mut interaction_state: ResMut>, + mut load_site: EventWriter, + mut load_workcell: EventWriter, + mut load_channels: ResMut, +) { + if let Ok(result) = load_channels.receiver.try_recv() { + let LoadWorkspaceFile(file, data) = result; + if let Some(workspace_data) = WorkspaceData::new(&file, data) { + handle_workspace_data( + Some(file), + &workspace_data, + &mut app_state, + &mut interaction_state, + &mut load_site, + &mut load_workcell, + ); + } + } +} + +pub fn sync_workspace_visibility( + current_workspace: Res, + mut recall: ResMut, + mut visibility: Query<&mut Visibility>, +) { + if !current_workspace.is_changed() { + return; + } + + if recall.0 != current_workspace.root { + // Set visibility of current to target + if let Some(current_workspace_entity) = current_workspace.root { + if let Ok(mut v) = visibility.get_mut(current_workspace_entity) { + v.is_visible = current_workspace.display; + } + } + // Disable visibility in recall + if let Some(recall) = recall.0 { + if let Ok(mut v) = visibility.get_mut(recall) { + v.is_visible = false; + } + } + recall.0 = current_workspace.root; + } +} diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml index de1832b6..fcbe24eb 100644 --- a/rmf_site_format/Cargo.toml +++ b/rmf_site_format/Cargo.toml @@ -9,7 +9,7 @@ crate-type = ["rlib"] [dependencies] serde = { version = "1.0", features = ["derive"] } -optimization_engine = "0.7" +optimization_engine = "0.7.7" serde_yaml = "0.8.23" serde_json = "*" ron = "0.7" @@ -17,6 +17,7 @@ thiserror = "*" glam = "0.22" # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features bevy = { version = "0.9", optional = true } +urdf-rs = "0.7" [target.'cfg(target_arch = "wasm")'.dependencies] -optimization_engine = { version = "0.7", features = ["wasm"] } +optimization_engine = { version = "0.7.7", features = ["wasm"] } diff --git a/rmf_site_format/src/anchor.rs b/rmf_site_format/src/anchor.rs index 15daf3da..1fcaff8f 100644 --- a/rmf_site_format/src/anchor.rs +++ b/rmf_site_format/src/anchor.rs @@ -15,7 +15,7 @@ * */ -use crate::{Categorized, Category}; +use crate::{Categorized, Category, Pose}; #[cfg(feature = "bevy")] use bevy::{ ecs::{query::QueryEntityError, system::SystemParam}, @@ -32,6 +32,7 @@ use serde::{Deserialize, Serialize}; pub enum Anchor { Translate2D([f32; 2]), CategorizedTranslate2D(Categorized<[f32; 2]>), + Pose3D(Pose), } impl From<[f32; 2]> for Anchor { @@ -40,11 +41,16 @@ impl From<[f32; 2]> for Anchor { } } +fn to_slice(p: &[f32]) -> &[f32; 2] { + p.try_into().expect("Wrong array size") +} + impl Anchor { pub fn translation_for_category(&self, category: Category) -> &[f32; 2] { match self { Self::Translate2D(v) => v, Self::CategorizedTranslate2D(v) => v.for_category(category), + Self::Pose3D(p) => to_slice(&p.trans[0..2]), } } @@ -66,6 +72,10 @@ impl Anchor { } return true; } + Self::Pose3D(p) => { + let p_right = Vec2::from_array(*to_slice(&p.trans[0..2])); + return (p_left - p_right).length() <= dist; + } } } Self::CategorizedTranslate2D(left_categories) => match other { @@ -84,9 +94,37 @@ impl Anchor { } return true; } + Self::Pose3D(p) => { + let p_left = Vec2::from_array(*left_categories.for_general()); + let p_right = Vec2::from_array(*to_slice(&p.trans[0..2])); + return (p_left - p_right).length() <= dist; + } + }, + Self::Pose3D(p_left) => match other { + Self::Translate2D(p_right) => { + let p_left = Vec3::from_array(p_left.trans); + let p_right = Vec3::from_array([p_right[0], p_right[1], 0.0]); + return (p_left - p_right).length() <= dist; + } + Self::CategorizedTranslate2D(p_right) => { + let p_right = p_right.for_general(); + let p_left = Vec3::from_array(p_left.trans); + let p_right = Vec3::from_array([p_right[0], p_right[1], 0.0]); + return (p_left - p_right).length() <= dist; + } + Self::Pose3D(p_right) => { + let p_left = Vec3::from_array(p_left.trans); + let p_right = Vec3::from_array(p_right.trans); + return (p_left - p_right).length() <= dist; + } }, } } + + #[allow(non_snake_case)] + pub fn is_3D(&self) -> bool { + matches!(self, Anchor::Pose3D { .. }) + } } #[cfg(feature = "bevy")] @@ -109,6 +147,7 @@ impl Anchor { let p = categorized.for_category(category); Transform::from_translation([p[0], p[1], 0.0].into()) } + Anchor::Pose3D(p) => p.transform(), } } @@ -125,6 +164,12 @@ impl Anchor { *v = (Vec2::from(*v) + delta).into(); } } + Anchor::Pose3D(p) => { + p.trans[0] = tf.translation.x; + p.trans[1] = tf.translation.y; + p.trans[2] = tf.translation.z; + p.align_with(tf); + } } } } diff --git a/rmf_site_format/src/asset_source.rs b/rmf_site_format/src/asset_source.rs index 7f880c24..0ba87cc0 100644 --- a/rmf_site_format/src/asset_source.rs +++ b/rmf_site_format/src/asset_source.rs @@ -28,6 +28,7 @@ pub enum AssetSource { Remote(String), Search(String), Bundled(String), + Package(String), } impl AssetSource { @@ -37,6 +38,7 @@ impl AssetSource { Self::Remote(_) => "Remote", Self::Search(_) => "Search", Self::Bundled(_) => "Bundled", + Self::Package(_) => "Package", } } } @@ -50,23 +52,28 @@ impl Default for AssetSource { // Utility functions to add / strip prefixes for using AssetSource in AssetIo objects impl From<&Path> for AssetSource { fn from(path: &Path) -> Self { - // TODO pattern matching here would make sure unimplemented variants are a compile error - if path.starts_with("rmf-server://") { - let without_prefix = path - .to_str() - .unwrap() - .strip_prefix("rmf-server://") - .unwrap(); - return AssetSource::Remote(String::from(without_prefix)); - } else if path.starts_with("file://") { - let without_prefix = path.to_str().unwrap().strip_prefix("file://").unwrap(); - return AssetSource::Local(String::from(without_prefix)); - } else if path.starts_with("search://") { - let without_prefix = path.to_str().unwrap().strip_prefix("search://").unwrap(); - return AssetSource::Search(String::from(without_prefix)); - } else if path.starts_with("bundled://") { - let without_prefix = path.to_str().unwrap().strip_prefix("bundled://").unwrap(); - return AssetSource::Bundled(String::from(without_prefix)); + if let Some(path) = path.to_str().and_then(|p| Some(String::from(p))) { + AssetSource::from(&path) + } else { + AssetSource::default() + } + } +} + +// Utility functions to add / strip prefixes for using AssetSource in AssetIo objects +impl From<&String> for AssetSource { + fn from(path: &String) -> Self { + // TODO(luca) pattern matching here would make sure unimplemented variants are a compile error + if let Some(path) = path.strip_prefix("rmf-server://").map(|p| p.to_string()) { + return AssetSource::Remote(path); + } else if let Some(path) = path.strip_prefix("file://").map(|p| p.to_string()) { + return AssetSource::Local(path); + } else if let Some(path) = path.strip_prefix("search://").map(|p| p.to_string()) { + return AssetSource::Search(path); + } else if let Some(path) = path.strip_prefix("bundled://").map(|p| p.to_string()) { + return AssetSource::Bundled(path); + } else if let Some(path) = path.strip_prefix("package://").map(|p| p.to_string()) { + return AssetSource::Package(path); } AssetSource::default() } @@ -75,10 +82,11 @@ impl From<&Path> for AssetSource { impl From<&AssetSource> for String { fn from(asset_source: &AssetSource) -> String { match asset_source { - AssetSource::Remote(uri) => String::from("rmf-server://") + &uri, - AssetSource::Local(filename) => String::from("file://") + &filename, - AssetSource::Search(name) => String::from("search://") + &name, - AssetSource::Bundled(name) => String::from("bundled://") + &name, + AssetSource::Remote(uri) => String::from("rmf-server://") + uri, + AssetSource::Local(filename) => String::from("file://") + filename, + AssetSource::Search(name) => String::from("search://") + name, + AssetSource::Bundled(name) => String::from("bundled://") + name, + AssetSource::Package(path) => String::from("package://") + path, } } } @@ -90,6 +98,7 @@ pub struct RecallAssetSource { pub remote_uri: Option, pub search_name: Option, pub bundled_name: Option, + pub package_path: Option, } impl Recall for RecallAssetSource { @@ -109,6 +118,9 @@ impl Recall for RecallAssetSource { AssetSource::Bundled(name) => { self.bundled_name = Some(name.clone()); } + AssetSource::Package(path) => { + self.package_path = Some(path.clone()); + } } } } diff --git a/rmf_site_format/src/category.rs b/rmf_site_format/src/category.rs index 208863b1..7fb5351f 100644 --- a/rmf_site_format/src/category.rs +++ b/rmf_site_format/src/category.rs @@ -42,6 +42,7 @@ pub enum Category { Model, Camera, Drawing, + Workcell, } impl Category { @@ -62,6 +63,7 @@ impl Category { Self::Model => "Model", Self::Camera => "Camera", Self::Drawing => "Drawing", + Self::Workcell => "Workcell", } } diff --git a/rmf_site_format/src/legacy/model.rs b/rmf_site_format/src/legacy/model.rs index 51062f0e..d4603d6b 100644 --- a/rmf_site_format/src/legacy/model.rs +++ b/rmf_site_format/src/legacy/model.rs @@ -1,5 +1,6 @@ use crate::{ - Angle, AssetSource, IsStatic, Model as SiteModel, ModelMarker, NameInSite, Pose, Rotation, + Angle, AssetSource, ConstraintDependents, IsStatic, Model as SiteModel, ModelMarker, + NameInSite, Pose, Rotation, Scale, }; use glam::DVec2; use serde::{Deserialize, Serialize}; @@ -32,6 +33,8 @@ impl Model { rot: Rotation::Yaw(Angle::Deg(self.yaw.to_degrees() as f32)), }, is_static: IsStatic(self.static_), + constraints: ConstraintDependents::default(), + scale: Scale::default(), marker: ModelMarker, } } diff --git a/rmf_site_format/src/legacy/optimization.rs b/rmf_site_format/src/legacy/optimization.rs index 75c9ce72..9e80ac35 100644 --- a/rmf_site_format/src/legacy/optimization.rs +++ b/rmf_site_format/src/legacy/optimization.rs @@ -51,31 +51,20 @@ pub fn align_building(building: &BuildingMap) -> HashMap { max_vals.extend([inf, inf, 45_f64.to_radians(), 1000.0]); } - // There seems to be an issue in optimization_engine's wasm support: - // https://github.com/alphaville/optimization-engine/issues/248#issuecomment-986323803 - // As a result this optimization doesn't work when compiled in wasm. We'll - // disable it for wasm architecture, which means the scaling fit won't be - // globally optimized across the different levels, instead levels will only - // be scaled according to their individual measurements. This likely means - // that scaling won't be quite as accurate overall, but everything should - // basically work as long as each level has one or more measurements. - #[cfg(not(target_arch = "wasm32"))] - { - let constraints = constraints::Rectangle::new(Some(&min_vals), Some(&max_vals)); - let mut panoc_cache = PANOCCache::new(u.len(), 1e-6, 10); - let f = |u: &[f64], c: &mut f64| -> Result<(), SolverError> { - *c = calculate_scale_cost(&measurements, &fiducials, u); - Ok(()) - }; - - let df = |u: &[f64], gradient: &mut [f64]| -> Result<(), SolverError> { - calculate_scale_gradient(&measurements, &fiducials, u, gradient); - Ok(()) - }; - let problem = Problem::new(&constraints, df, f); - let mut panoc = PANOCOptimizer::new(problem, &mut panoc_cache).with_max_iter(1000); - panoc.solve(&mut u).ok(); - } + let constraints = constraints::Rectangle::new(Some(&min_vals), Some(&max_vals)); + let mut panoc_cache = PANOCCache::new(u.len(), 1e-6, 10); + let f = |u: &[f64], c: &mut f64| -> Result<(), SolverError> { + *c = calculate_scale_cost(&measurements, &fiducials, u); + Ok(()) + }; + + let df = |u: &[f64], gradient: &mut [f64]| -> Result<(), SolverError> { + calculate_scale_gradient(&measurements, &fiducials, u, gradient); + Ok(()) + }; + let problem = Problem::new(&constraints, df, f); + let mut panoc = PANOCOptimizer::new(problem, &mut panoc_cache).with_max_iter(1000); + panoc.solve(&mut u).ok(); calculate_yaw_adjustment(&fiducials, &mut u); calculate_displacement_adjustment(&fiducials, &mut u); diff --git a/rmf_site_format/src/legacy/vertex.rs b/rmf_site_format/src/legacy/vertex.rs index 6f0aafe0..d81d4742 100644 --- a/rmf_site_format/src/legacy/vertex.rs +++ b/rmf_site_format/src/legacy/vertex.rs @@ -1,7 +1,7 @@ use super::rbmf::*; use crate::{ - is_default, AssetSource, AssociatedGraphs, IsStatic, Location, LocationTag, LocationTags, - Model, ModelMarker, NameInSite, Pose, + is_default, AssetSource, AssociatedGraphs, ConstraintDependents, IsStatic, Location, + LocationTag, LocationTags, Model, ModelMarker, NameInSite, Pose, Scale, }; use glam::DVec2; use serde::{Deserialize, Serialize}; @@ -63,6 +63,8 @@ impl Vertex { source: AssetSource::Search(me.spawn_robot_type.1.clone()), pose: Pose::default(), is_static: IsStatic(false), + constraints: ConstraintDependents::default(), + scale: Scale::default(), marker: ModelMarker, })) } diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index 2694082d..c166bd0a 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -99,6 +99,9 @@ pub use texture::*; pub mod wall; pub use wall::*; +pub mod workcell; +pub use workcell::*; + mod is_default; pub(crate) use is_default::*; diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index d760f021..fb8f8b6c 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -125,6 +125,16 @@ impl RectFace { } } +#[derive(Serialize, Deserialize, Deref, DerefMut, PartialEq, Clone, Debug)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct Scale(pub Vec3); + +impl Default for Scale { + fn default() -> Self { + Self(Vec3::ONE) + } +} + #[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, PartialOrd)] #[serde(rename_all = "snake_case")] pub enum Angle { @@ -315,7 +325,7 @@ impl Pose { } } - pub fn align_with(&mut self, tf: &Transform) { + pub fn align_with(&mut self, tf: &Transform) -> Self { self.trans = tf.translation.into(); match self.rot { @@ -345,6 +355,7 @@ impl Pose { self.rot = Rotation::Quat(tf.rotation.to_array()); } } + *self } } diff --git a/rmf_site_format/src/model.rs b/rmf_site_format/src/model.rs index 6b1d506c..3e4539a4 100644 --- a/rmf_site_format/src/model.rs +++ b/rmf_site_format/src/model.rs @@ -32,6 +32,13 @@ pub struct Model { #[serde(default, skip_serializing_if = "is_default")] /// Whether this model should be able to move in simulation pub is_static: IsStatic, + #[serde(skip)] + /// List of mesh constraints that apply to this model + /// Skipped in serialization since the information is already contained in MeshConstraints + pub constraints: ConstraintDependents, + /// Scale to be applied to the model + #[serde(default, skip_serializing_if = "is_default")] + pub scale: Scale, /// Only relevant for bevy #[serde(skip)] pub marker: ModelMarker, @@ -48,6 +55,8 @@ impl Default for Model { source: AssetSource::default(), pose: Pose::default(), is_static: IsStatic(false), + constraints: ConstraintDependents::default(), + scale: Scale::default(), marker: ModelMarker, } } diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index 80c289f0..21ad67fb 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -29,7 +29,15 @@ pub struct SiteProperties { pub name: String, } -#[derive(Serialize, Deserialize, Debug, Clone)] +impl Default for SiteProperties { + fn default() -> Self { + Self { + name: "new_site".to_string(), + } + } +} + +#[derive(Serialize, Deserialize, Debug, Clone, Default)] pub struct Site { /// The site data format that is being used pub format_version: SemVer, diff --git a/rmf_site_format/src/workcell.rs b/rmf_site_format/src/workcell.rs new file mode 100644 index 00000000..0d331acb --- /dev/null +++ b/rmf_site_format/src/workcell.rs @@ -0,0 +1,392 @@ +/* + * 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 std::collections::{BTreeMap, HashSet}; +use std::io; + +use crate::*; +#[cfg(feature = "bevy")] +use bevy::ecs::system::EntityCommands; +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component, Deref, DerefMut, Entity}; +#[cfg(feature = "bevy")] +use bevy::reflect::TypeUuid; +use glam::Vec3; +use serde::{Deserialize, Serialize}; +use urdf_rs::Robot; + +/// Helper structure to serialize / deserialize entities with parents +#[derive(Serialize, Deserialize, Clone, Debug)] +pub struct Parented { + pub parent: P, + #[serde(flatten)] + pub bundle: T, +} + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct FrameMarker; + +#[derive(Serialize, Deserialize, Debug, Clone)] +pub struct Frame { + #[serde(flatten)] + pub anchor: Anchor, + #[serde(default, skip_serializing_if = "is_default")] + pub name: Option, + #[serde(default, skip_serializing_if = "is_default")] + pub mesh_constraint: Option>, + #[serde(skip)] + pub marker: FrameMarker, +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct MeshConstraint { + pub entity: T, + pub element: MeshElement, + pub relative_pose: Pose, +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +pub enum MeshElement { + Vertex(u32), + // TODO(luca) edge and vertices +} + +/// Attached to Model entities to keep track of constraints attached to them, +/// for change detection and hierarchy propagation +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] +pub struct ConstraintDependents(pub HashSet); + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct WorkcellProperties { + pub name: String, +} + +#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] +pub struct NameInWorkcell(pub String); + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] +pub struct Mass(f32); + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct Inertia {} + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct Inertial { + pub origin: Pose, + pub mass: Mass, + pub inertia: Inertia, +} + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct Link { + pub name: NameInWorkcell, + pub inertial: Inertial, + #[serde(skip)] + pub marker: LinkMarker, +} + +#[derive(Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct LinkMarker; + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct Joint { + pub name: NameInWorkcell, +} + +#[derive(Serialize, Deserialize, Debug, Clone)] +pub enum Geometry { + //#[serde(flatten)] + Primitive(MeshPrimitive), + Mesh { + filename: String, + #[serde(default, skip_serializing_if = "is_default")] + scale: Option, + }, +} + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub enum MeshPrimitive { + Box { size: [f32; 3] }, + Cylinder { radius: f32, length: f32 }, + Capsule { radius: f32, length: f32 }, + Sphere { radius: f32 }, +} + +impl MeshPrimitive { + pub fn label(&self) -> String { + match &self { + MeshPrimitive::Box { .. } => "Box", + MeshPrimitive::Cylinder { .. } => "Cylinder", + MeshPrimitive::Capsule { .. } => "Capsule", + MeshPrimitive::Sphere { .. } => "Sphere", + } + .to_string() + } +} + +#[derive(Clone, Debug, Default, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct RecallMeshPrimitive { + pub box_size: Option<[f32; 3]>, + pub cylinder_radius: Option, + pub cylinder_length: Option, + pub capsule_radius: Option, + pub capsule_length: Option, + pub sphere_radius: Option, +} + +impl Recall for RecallMeshPrimitive { + type Source = MeshPrimitive; + + fn remember(&mut self, source: &MeshPrimitive) { + match source { + MeshPrimitive::Box { size } => { + self.box_size = Some(*size); + } + MeshPrimitive::Cylinder { radius, length } => { + self.cylinder_radius = Some(*radius); + self.cylinder_length = Some(*length); + } + MeshPrimitive::Capsule { radius, length } => { + self.capsule_radius = Some(*radius); + self.capsule_length = Some(*length); + } + MeshPrimitive::Sphere { radius } => { + self.sphere_radius = Some(*radius); + } + } + } +} + +impl RecallMeshPrimitive { + pub fn assume_box(&self, current: &MeshPrimitive) -> MeshPrimitive { + MeshPrimitive::Box { + size: self.box_size.unwrap_or_default(), + } + } + + pub fn assume_cylinder(&self, current: &MeshPrimitive) -> MeshPrimitive { + MeshPrimitive::Cylinder { + radius: self.cylinder_radius.unwrap_or_default(), + length: self.cylinder_length.unwrap_or_default(), + } + } + + pub fn assume_capsule(&self, current: &MeshPrimitive) -> MeshPrimitive { + MeshPrimitive::Capsule { + radius: self.capsule_radius.unwrap_or_default(), + length: self.capsule_length.unwrap_or_default(), + } + } + + pub fn assume_sphere(&self, current: &MeshPrimitive) -> MeshPrimitive { + MeshPrimitive::Sphere { + radius: self.sphere_radius.unwrap_or_default(), + } + } +} + +impl Default for Geometry { + fn default() -> Self { + Geometry::Primitive(MeshPrimitive::Box { size: [0.0; 3] }) + } +} + +#[derive(Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct WorkcellVisualMarker; + +#[derive(Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct WorkcellCollisionMarker; + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +pub struct WorkcellModel { + pub name: String, + pub geometry: Geometry, + pub pose: Pose, +} + +#[cfg(feature = "bevy")] +impl WorkcellModel { + pub fn add_bevy_components(&self, mut commands: EntityCommands) { + match &self.geometry { + Geometry::Primitive(primitive) => { + commands.insert(( + primitive.clone(), + self.pose.clone(), + NameInWorkcell(self.name.clone()), + )); + } + Geometry::Mesh { filename, scale } => { + println!("Setting pose of {:?} to {:?}", filename, self.pose); + let scale = Scale(scale.unwrap_or_default()); + // TODO(luca) Make a bundle for workcell models to avoid manual insertion here + commands.insert(( + NameInWorkcell(self.name.clone()), + AssetSource::from(filename), + self.pose.clone(), + ConstraintDependents::default(), + scale, + ModelMarker, + )); + } + } + } +} + +// TODO(luca) we might need a different bundle to denote a workcell included in site +// editor mode to deal with serde of workcells there (that would only have an asset source?) +/// Container for serialization / deserialization of workcells +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +pub struct Workcell { + /// Workcell specific properties + #[serde(flatten)] + pub properties: WorkcellProperties, + /// Site ID, used for entities to set their parent to the root workcell + pub id: u32, + /// Frames, key is their id, used for hierarchy + pub frames: BTreeMap>, + /// Visuals, key is their id, used for hierarchy + pub visuals: BTreeMap>, + /// Collisions, key is their id, used for hierarchy + pub collisions: BTreeMap>, + // TODO(luca) Joints +} + +impl Workcell { + pub fn to_writer(&self, writer: W) -> serde_json::Result<()> { + serde_json::ser::to_writer_pretty(writer, self) + } + + pub fn to_string(&self) -> serde_json::Result { + serde_json::ser::to_string_pretty(self) + } + + pub fn from_reader(reader: R) -> serde_json::Result { + serde_json::de::from_reader(reader) + } + + pub fn from_str<'a>(s: &'a str) -> serde_json::Result { + serde_json::de::from_str(s) + } + + pub fn from_bytes<'a>(s: &'a [u8]) -> serde_json::Result { + serde_json::from_slice(s) + } +} + +#[cfg_attr( + feature = "bevy", + derive(Component, Clone, Debug, Deref, DerefMut, TypeUuid) +)] +#[cfg_attr(feature = "bevy", uuid = "fe707f9e-c6f3-11ed-afa1-0242ac120002")] +pub struct UrdfRoot(pub Robot); + +// TODO(luca) feature gate urdf support +impl From<&urdf_rs::Geometry> for Geometry { + fn from(geom: &urdf_rs::Geometry) -> Self { + match geom { + urdf_rs::Geometry::Box { size } => Geometry::Primitive(MeshPrimitive::Box { + size: (**size).map(|f| f as f32), + }), + urdf_rs::Geometry::Cylinder { radius, length } => { + Geometry::Primitive(MeshPrimitive::Cylinder { + radius: *radius as f32, + length: *length as f32, + }) + } + urdf_rs::Geometry::Capsule { radius, length } => { + Geometry::Primitive(MeshPrimitive::Capsule { + radius: *radius as f32, + length: *length as f32, + }) + } + urdf_rs::Geometry::Sphere { radius } => Geometry::Primitive(MeshPrimitive::Sphere { + radius: *radius as f32, + }), + urdf_rs::Geometry::Mesh { filename, scale } => { + let scale = scale + .clone() + .and_then(|s| Some(Vec3::from_array(s.map(|v| v as f32)))); + Geometry::Mesh { + filename: filename.clone(), + scale, + } + } + } + } +} + +impl From<&urdf_rs::Link> for Link { + fn from(link: &urdf_rs::Link) -> Self { + Self { + name: NameInWorkcell(link.name.clone()), + inertial: Inertial { + origin: Pose { + trans: link.inertial.origin.xyz.0.map(|v| v as f32), + rot: Rotation::EulerExtrinsicXYZ( + link.inertial.origin.rpy.map(|v| Angle::Rad(v as f32)), + ), + }, + mass: Mass(link.inertial.mass.value as f32), + inertia: Inertia::default(), + }, + marker: LinkMarker, + } + } +} + +impl WorkcellModel { + fn from_urdf_data( + pose: &urdf_rs::Pose, + name: &Option, + geometry: &urdf_rs::Geometry, + ) -> Self { + let trans = pose.xyz.map(|t| t as f32); + let rot = Rotation::EulerExtrinsicXYZ(pose.rpy.map(|t| Angle::Rad(t as f32))); + WorkcellModel { + name: name.clone().unwrap_or_default(), + geometry: geometry.into(), + pose: Pose { trans, rot }, + } + } +} + +impl From<&urdf_rs::Visual> for WorkcellModel { + fn from(visual: &urdf_rs::Visual) -> Self { + WorkcellModel::from_urdf_data(&visual.origin, &visual.name, &visual.geometry) + } +} + +impl From<&urdf_rs::Collision> for WorkcellModel { + fn from(collision: &urdf_rs::Collision) -> Self { + WorkcellModel::from_urdf_data(&collision.origin, &collision.name, &collision.geometry) + } +}