Urdf import, export and workcell editor fixes (#177)

Signed-off-by: Audrow Nash <[email protected]> Signed-off-by: Luca Della Vedova <[email protected]> Co-authored-by: Audrow Nash <[email protected]>
open-rmf · Jan 23, 2024 · e852b34 · e852b34
1 parent 0768528
commit e852b34
Show file tree

Hide file tree

Showing 50 changed files with 3,393 additions and 1,121 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/assets/demo_workcells/demo.workcell.json b/assets/demo_workcells/demo.workcell.json
@@ -75,5 +75,9 @@
   "visuals": {
   },
   "collisions": {
+  },
+  "inertias": {
+  },
+  "joints": {
   }
 }
diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
@@ -17,13 +17,12 @@ path = "examples/extending_menu.rs"
 
 [dependencies]
 bevy_egui = "0.21"
-bevy_mod_raycast = "0.13"
+bevy_mod_raycast = "0.15"
 bevy_mod_outline = "0.5"
 bevy_infinite_grid = "0.8"
 bevy_polyline = "0.7"
 bevy_stl = "0.11"
 bevy_obj = { version = "0.11", features = ["scene"] }
-bevy_rapier3d = "0.22.0"
 smallvec = "*"
 serde = { version = "1.0", features = ["derive"] }
 serde_yaml = "0.8.23"
@@ -33,7 +32,7 @@ wasm-bindgen = "=0.2.84"
 futures-lite = "1.12.0"
 bevy = { version = "0.11", features = ["pnm", "jpeg", "tga"] }
 bevy_utils = "0.11"
-dirs = "4.0"
+dirs = "5.0"
 thread_local = "*"
 lyon = "1"
 thiserror = "*"
@@ -46,9 +45,10 @@ tracing-subscriber = "0.3.1"
 rfd = "0.12"
 urdf-rs = "0.7"
 utm = "0.1.6"
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "f86344f"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "a5daef0"}
 gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "first_implementation" }
 pathdiff = "*"
+tera = "1.19.1"
 
 # only enable the 'dynamic_linking' feature if we're not building for web or windows
 # TODO(luca) it seems this can be enabled on windows, check

diff --git a/rmf_site_editor/src/interaction/cursor.rs b/rmf_site_editor/src/interaction/cursor.rs
@@ -18,11 +18,11 @@
 use crate::{
     animate::*,
     interaction::*,
-    site::{AnchorBundle, Pending, SiteAssets},
+    site::{AnchorBundle, Pending, SiteAssets, Trashcan},
 };
 use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow};
-use bevy_mod_raycast::{Ray3d, RaycastMesh, RaycastSource};
-use rmf_site_format::{FloorMarker, Model, ModelMarker, WallMarker, WorkcellModel};
+use bevy_mod_raycast::{deferred::RaycastMesh, deferred::RaycastSource, primitives::rays::Ray3d};
+use rmf_site_format::{FloorMarker, Model, ModelMarker, PrimitiveShape, WallMarker, WorkcellModel};
 use std::collections::HashSet;
 
 /// A resource that keeps track of the unique entities that play a role in
@@ -36,6 +36,7 @@ pub struct Cursor {
     pub level_anchor_placement: Entity,
     pub site_anchor_placement: Entity,
     pub frame_placement: Entity,
+    pub trashcan: Entity,
     pub preview_model: Option<Entity>,
     dependents: HashSet<Entity>,
     /// Use a &str to label each mode that might want to turn the cursor on
@@ -107,10 +108,7 @@ 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();
+            commands.entity(current_preview).set_parent(self.trashcan);
         }
     }
 
@@ -133,9 +131,9 @@ impl Cursor {
     ) {
         self.remove_preview(commands);
         self.preview_model = if let Some(model) = model {
-            let cmd = commands.spawn(Pending);
+            let mut cmd = commands.spawn(Pending);
             let e = cmd.id();
-            model.add_bevy_components(cmd);
+            model.add_bevy_components(&mut cmd);
             commands.entity(self.frame).push_children(&[e]);
             Some(e)
         } else {
@@ -251,13 +249,16 @@ impl FromWorld for Cursor {
             })
             .id();
 
+        let trashcan = world.spawn(Trashcan).id();
+
         Self {
             frame: cursor,
             halo,
             dagger,
             level_anchor_placement,
             site_anchor_placement,
             frame_placement,
+            trashcan,
             preview_model: None,
             dependents: Default::default(),
             modes: Default::default(),
@@ -278,6 +279,7 @@ pub struct IntersectGroundPlaneParams<'w, 's> {
     camera_controls: Res<'w, CameraControls>,
     cameras: Query<'w, 's, &'static Camera>,
     global_transforms: Query<'w, 's, &'static GlobalTransform>,
+    primary_window: Query<'w, 's, &'static Window, With<PrimaryWindow>>,
 }
 
 impl<'w, 's> IntersectGroundPlaneParams<'w, 's> {
@@ -287,7 +289,9 @@ impl<'w, 's> IntersectGroundPlaneParams<'w, 's> {
         let e_active_camera = self.camera_controls.active_camera();
         let active_camera = self.cameras.get(e_active_camera).ok()?;
         let camera_tf = self.global_transforms.get(e_active_camera).ok()?;
-        let ray = Ray3d::from_screenspace(cursor_position, active_camera, camera_tf)?;
+        let primary_window = self.primary_window.get_single().ok()?;
+        let ray =
+            Ray3d::from_screenspace(cursor_position, active_camera, camera_tf, primary_window)?;
         let n_p = Vec3::Z;
         let n_r = ray.direction();
         let denom = n_p.dot(n_r);
@@ -304,7 +308,7 @@ pub fn update_cursor_transform(
     mode: Res<InteractionMode>,
     cursor: Res<Cursor>,
     raycast_sources: Query<&RaycastSource<SiteRaycastSet>>,
-    models: Query<(), With<ModelMarker>>,
+    models: Query<(), Or<(With<ModelMarker>, With<PrimitiveShape>)>>,
     mut transforms: Query<&mut Transform>,
     hovering: Res<Hovering>,
     intersect_ground_params: IntersectGroundPlaneParams,

diff --git a/rmf_site_editor/src/interaction/gizmo.rs b/rmf_site_editor/src/interaction/gizmo.rs
@@ -16,8 +16,8 @@
 */
 
 use crate::interaction::*;
-use bevy::{math::Affine3A, prelude::*};
-use bevy_mod_raycast::{Ray3d, RaycastMesh, RaycastSource};
+use bevy::{math::Affine3A, prelude::*, window::PrimaryWindow};
+use bevy_mod_raycast::{deferred::RaycastMesh, deferred::RaycastSource, primitives::rays::Ray3d};
 use rmf_site_format::Pose;
 
 #[derive(Debug, Clone, Copy)]
@@ -363,6 +363,7 @@ pub fn update_drag_motions(
     drag_state: Res<GizmoState>,
     mut cursor_motion: EventReader<CursorMoved>,
     mut move_to: EventWriter<MoveTo>,
+    primary_window: Query<&Window, With<PrimaryWindow>>,
 ) {
     if let GizmoState::Dragging(dragging) = *drag_state {
         let cursor_position = match cursor_motion.iter().last() {
@@ -381,7 +382,10 @@ pub fn update_drag_motions(
                 }
             };
 
-            match Ray3d::from_screenspace(cursor_position, camera, &camera_tf) {
+            let Ok(primary_window) = primary_window.get_single() else {
+                return;
+            };
+            match Ray3d::from_screenspace(cursor_position, camera, &camera_tf, primary_window) {
                 Some(ray) => ray,
                 None => {
                     return;

diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs
@@ -90,7 +90,7 @@ pub use visual_cue::*;
 
 use bevy::prelude::*;
 use bevy_mod_outline::OutlinePlugin;
-use bevy_mod_raycast::{DefaultRaycastingPlugin, RaycastSystem};
+use bevy_mod_raycast::deferred::DeferredRaycastingPlugin;
 use bevy_polyline::PolylinePlugin;
 
 #[derive(Reflect)]
@@ -136,7 +136,7 @@ impl Plugin for InteractionPlugin {
                 apply_deferred.in_set(InteractionUpdateSet::CommandFlush),
             )
             .add_plugins(PolylinePlugin)
-            .add_plugins(DefaultRaycastingPlugin::<SiteRaycastSet>::default())
+            .add_plugins(DeferredRaycastingPlugin::<SiteRaycastSet>::default())
             .init_resource::<InteractionAssets>()
             .init_resource::<Cursor>()
             .init_resource::<CameraControls>()
@@ -254,14 +254,7 @@ impl Plugin for InteractionPlugin {
                 )
                     .run_if(in_state(InteractionState::Enable)),
             )
-            .add_systems(
-                First,
-                (
-                    update_picked,
-                    update_interaction_mode,
-                    update_raycast_with_cursor.before(RaycastSystem::BuildRays::<SiteRaycastSet>),
-                ),
-            );
+            .add_systems(First, (update_picked, update_interaction_mode));
     }
 }
 

diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs
@@ -20,7 +20,7 @@ use bevy::render::view::RenderLayers;
 use bevy_mod_outline::{OutlineBundle, OutlineRenderLayers, OutlineVolume, SetOutlineDepth};
 use rmf_site_format::{
     ConstraintMarker, DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags,
-    MeasurementMarker, ModelMarker, PhysicalCameraProperties, WallMarker,
+    MeasurementMarker, ModelMarker, PhysicalCameraProperties, PrimitiveShape, WallMarker,
 };
 use smallvec::SmallVec;
 
@@ -118,6 +118,7 @@ pub fn add_outline_visualization(
             Added<PhysicalCameraProperties>,
             Added<LightKind>,
             Added<LocationTags>,
+            Added<PrimitiveShape>,
         )>,
     >,
 ) {

diff --git a/rmf_site_editor/src/interaction/picking.rs b/rmf_site_editor/src/interaction/picking.rs
@@ -17,7 +17,9 @@
 
 use crate::{interaction::*, site::Anchor, CurrentWorkspace};
 use bevy::prelude::*;
-use bevy_mod_raycast::{RaycastMethod, RaycastSource};
+use bevy_mod_raycast::{
+    deferred::RaycastMethod, deferred::RaycastSource, immediate::RaycastVisibility,
+};
 
 /// A resource to track what kind of picking blockers are currently active
 #[derive(Resource)]
@@ -72,9 +74,11 @@ pub fn update_picking_cam(
                     .remove::<RaycastSource<SiteRaycastSet>>();
             }
 
-            commands
-                .entity(camera_controls.active_camera())
-                .insert(RaycastSource::<SiteRaycastSet>::new().with_early_exit(false));
+            commands.entity(camera_controls.active_camera()).insert(
+                RaycastSource::<SiteRaycastSet>::new_cursor()
+                    .with_early_exit(false)
+                    .with_visibility(RaycastVisibility::MustBeVisible),
+            );
         }
     }
 }
@@ -115,20 +119,6 @@ fn pick_topmost(
     return None;
 }
 
-// Update our `RaycastSource` with the current cursor position every frame.
-pub fn update_raycast_with_cursor(
-    mut cursor: EventReader<CursorMoved>,
-    mut query: Query<&mut RaycastSource<SiteRaycastSet>>,
-) {
-    // Grab the most recent cursor event if it exists:
-    let Some(cursor_moved) = cursor.iter().last() else {
-        return;
-    };
-    for mut pick_source in &mut query {
-        pick_source.cast_method = RaycastMethod::Screenspace(cursor_moved.position);
-    }
-}
-
 pub fn update_picked(
     mode: Res<InteractionMode>,
     selectable: Query<&Selectable>,

diff --git a/rmf_site_editor/src/interaction/select.rs b/rmf_site_editor/src/interaction/select.rs
@@ -17,7 +17,7 @@
 
 use crate::{interaction::*, site::Anchor};
 use bevy::prelude::*;
-use bevy_mod_raycast::RaycastMesh;
+use bevy_mod_raycast::deferred::RaycastMesh;
 use std::collections::HashSet;
 
 /// This component is put on entities with meshes to mark them as items that can
@@ -236,9 +236,7 @@ pub fn maintain_hovered_entities(
             // TODO(luca) refactor to remove this hack
             // Skip if we are in SelectAnchor3D mode
             if let InteractionMode::SelectAnchor3D(mode) = &*mode {
-                if mode.begin_creating() {
-                    return;
-                }
+                return;
             }
             select.send(Select(Some(current_hovered)));
         }