From 56c548cd68f90657117a5ce432e58a06529b79d3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 5 Sep 2024 17:54:48 +0800 Subject: [PATCH 01/25] Cleanup pre-workcell editor migration Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/deletion.rs | 20 +-- rmf_site_editor/src/site/mod.rs | 1 - .../src/widgets/inspector/inspect_anchor.rs | 37 ++--- .../inspector/inspect_mesh_constraint.rs | 78 ---------- .../inspector/inspect_workcell_parent.rs | 7 +- rmf_site_editor/src/widgets/inspector/mod.rs | 4 - rmf_site_editor/src/workcell/load.rs | 29 +--- rmf_site_editor/src/workcell/save.rs | 27 +--- rmf_site_format/src/lib.rs | 3 + rmf_site_format/src/primitive_shape.rs | 130 ++++++++++++++++ rmf_site_format/src/workcell.rs | 139 +----------------- 11 files changed, 149 insertions(+), 326 deletions(-) delete mode 100644 rmf_site_editor/src/widgets/inspector/inspect_mesh_constraint.rs create mode 100644 rmf_site_format/src/primitive_shape.rs diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index d57f9af2..6dbcd38f 100644 --- a/rmf_site_editor/src/site/deletion.rs +++ b/rmf_site_editor/src/site/deletion.rs @@ -25,7 +25,7 @@ use crate::{ AppState, Issue, }; use bevy::{ecs::system::SystemParam, prelude::*}; -use rmf_site_format::{ConstraintDependents, Edge, MeshConstraint, Path, Point}; +use rmf_site_format::{Edge, Path, Point}; use std::collections::HashSet; // TODO(MXG): Use this module to implement the deletion buffer. The role of the @@ -84,8 +84,6 @@ struct DeletionParams<'w, 's> { paths: Query<'w, 's, &'static Path>, parents: Query<'w, 's, &'static mut Parent>, dependents: Query<'w, 's, &'static mut Dependents>, - constraint_dependents: Query<'w, 's, &'static mut ConstraintDependents>, - mesh_constraints: Query<'w, 's, &'static mut MeshConstraint>, children: Query<'w, 's, &'static Children>, selection: Res<'w, Selection>, current_level: ResMut<'w, CurrentLevel>, @@ -211,22 +209,6 @@ fn cautious_delete(element: Entity, params: &mut DeletionParams) { } } - if let Ok(dependents) = params.constraint_dependents.get(e) { - for dep in dependents.iter() { - // Remove MeshConstraint component from dependent - params - .commands - .entity(*dep) - .remove::>(); - } - } - - if let Ok(constraint) = params.mesh_constraints.get(e) { - if let Ok(mut parent) = params.constraint_dependents.get_mut(constraint.entity) { - parent.remove(&e); - } - } - if **params.selection == Some(e) { params.select.send(Select(None)); } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 94b07d91..f58e1730 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -226,7 +226,6 @@ impl Plugin for SitePlugin { ChangePlugin::::default(), ChangePlugin::::default(), ChangePlugin::::default(), - ChangePlugin::>::default(), ChangePlugin::::default(), ChangePlugin::::default(), )) diff --git a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs index 1b472973..4c56b707 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs @@ -17,7 +17,7 @@ use crate::{ interaction::{Hover, MoveTo}, - site::{Anchor, Category, Change, Dependents, JointProperties, MeshConstraint, Subordinate}, + site::{Anchor, Category, Dependents, JointProperties, Subordinate}, widgets::{ inspector::{Inspect, InspectPoseComponent}, prelude::*, @@ -39,14 +39,12 @@ pub struct InspectAnchor<'w, 's> { &'static Transform, Option<&'static Subordinate>, &'static Parent, - Option<&'static MeshConstraint>, ), >, icons: Res<'w, Icons>, joints: Query<'w, 's, Entity, With>, hover: EventWriter<'w, Hover>, move_to: EventWriter<'w, MoveTo>, - mesh_constraints: EventWriter<'w, Change>>, create_joint: EventWriter<'w, CreateJoint>, } @@ -127,7 +125,7 @@ fn impl_inspect_anchor( let mut params = state.get_mut(world); - if let Ok((anchor, tf, subordinate, parent, mesh_constraint)) = params.anchors.get(id) { + if let Ok((anchor, tf, subordinate, parent)) = params.anchors.get(id) { if let Some(subordinate) = subordinate.map(|s| s.0) { panel.orthogonal(ui, |ui| { if let Some(boss) = subordinate { @@ -168,30 +166,13 @@ fn impl_inspect_anchor( } Anchor::Pose3D(pose) => { panel.align(ui, |ui| { - if let Some(c) = mesh_constraint { - if let Some(new_pose) = - InspectPoseComponent::new(&c.relative_pose).for_rotation().show(ui) - { - // TODO(luca) Using moveto doesn't allow switching between - // variants of Pose3D - params.mesh_constraints.send(Change::new( - MeshConstraint { - entity: c.entity, - element: c.element.clone(), - relative_pose: new_pose, - }, - id, - )); - } - } else { - if let Some(new_pose) = InspectPoseComponent::new(pose).show(ui) { - // TODO(luca) Using moveto doesn't allow switching between variants of - // Pose3D - params.move_to.send(MoveTo { - entity: id, - transform: new_pose.transform(), - }); - } + if let Some(new_pose) = InspectPoseComponent::new(pose).show(ui) { + // TODO(luca) Using moveto doesn't allow switching between variants of + // Pose3D + params.move_to.send(MoveTo { + entity: id, + transform: new_pose.transform(), + }); } // If the parent is not a joint, add a joint creation widget if params.joints.get(parent.get()).is_err() { diff --git a/rmf_site_editor/src/widgets/inspector/inspect_mesh_constraint.rs b/rmf_site_editor/src/widgets/inspector/inspect_mesh_constraint.rs deleted file mode 100644 index 9197373f..00000000 --- a/rmf_site_editor/src/widgets/inspector/inspect_mesh_constraint.rs +++ /dev/null @@ -1,78 +0,0 @@ -/* - * 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::widgets::{prelude::*, Inspect, SelectorWidget}; -use bevy::prelude::*; -use rmf_site_format::*; -use std::collections::{BTreeMap, BTreeSet}; - -use crate::site::Category; - -#[derive(SystemParam)] -pub struct InspectModelDependents<'w, 's> { - dependents: Query<'w, 's, &'static ConstraintDependents, With>, - categories: Query<'w, 's, &'static Category>, - selector: SelectorWidget<'w, 's>, -} - -impl<'w, 's> WidgetSystem for InspectModelDependents<'w, 's> { - fn show( - Inspect { selection, .. }: Inspect, - ui: &mut Ui, - state: &mut SystemState, - world: &mut World, - ) { - let mut params = state.get_mut(world); - params.show_widget(selection, ui); - } -} - -impl<'w, 's> InspectModelDependents<'w, 's> { - pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { - let Ok(dependents) = self.dependents.get(id) else { - return; - }; - - ui.vertical(|ui| { - if dependents.0.is_empty() { - ui.label("No dependents"); - } else { - ui.heading("Constraint Dependents"); - let mut category_map: BTreeMap> = BTreeMap::new(); - for e in &dependents.0 { - if let Ok(category) = self.categories.get(*e) { - category_map.entry(*category).or_default().insert(*e); - } else { - ui.label(format!("ERROR: Broken reference to entity {e:?}")); - } - } - - for (category, entities) in &category_map { - ui.label(category.label()); - - for e in entities { - ui.horizontal(|ui| { - self.selector.show_widget(*e, ui); - }); - } - } - } - }); - - ui.add_space(10.0); - } -} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs b/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs index 6244c057..6bd0f40e 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs @@ -17,7 +17,7 @@ use crate::{ interaction::{Hover, ObjectPlacement}, - site::{FrameMarker, MeshConstraint, NameInWorkcell, NameOfWorkcell}, + site::{FrameMarker, NameInWorkcell, NameOfWorkcell}, widgets::{prelude::*, Icons, Inspect, SelectorWidget}, CurrentWorkspace, }; @@ -37,7 +37,6 @@ pub struct InspectWorkcellParent<'w, 's> { With, )>, >, - mesh_constraints: Query<'w, 's, &'static MeshConstraint>, icons: Res<'w, Icons>, selector: SelectorWidget<'w, 's>, object_placement: ObjectPlacement<'w, 's>, @@ -65,10 +64,6 @@ impl<'w, 's> InspectWorkcellParent<'w, 's> { .and_then(|p| self.workcell_elements.get(**p)) { ui.vertical(|ui| { - if let Ok(c) = self.mesh_constraints.get(id) { - ui.label("Mesh Parent"); - self.selector.show_widget(c.entity, ui); - } ui.label("Parent Frame"); self.selector.show_widget(parent, ui); let assign_response = ui.add(ImageButton::new(self.icons.edit.egui())); diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 79bb99ab..cb254d65 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -66,9 +66,6 @@ pub use inspect_light::*; pub mod inspect_location; pub use inspect_location::*; -pub mod inspect_mesh_constraint; -pub use inspect_mesh_constraint::*; - pub mod inspect_point; pub use inspect_point::*; @@ -202,7 +199,6 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), diff --git a/rmf_site_editor/src/workcell/load.rs b/rmf_site_editor/src/workcell/load.rs index 14bdf68d..3bba1099 100644 --- a/rmf_site_editor/src/workcell/load.rs +++ b/rmf_site_editor/src/workcell/load.rs @@ -30,8 +30,8 @@ use bevy::prelude::*; use std::collections::HashSet; use rmf_site_format::{ - Category, ConstraintDependents, FrameMarker, Geometry, MeshConstraint, ModelMarker, - NameInWorkcell, Parented, Scale, SiteID, Workcell, WorkcellModel, + Category, FrameMarker, Geometry, ModelMarker, NameInWorkcell, Parented, Scale, SiteID, + Workcell, WorkcellModel, }; #[derive(Event)] @@ -49,8 +49,6 @@ fn generate_workcell_entities(commands: &mut Commands, workcell: &Workcell) -> E 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::INHERITED_IDENTITY) @@ -76,7 +74,6 @@ fn generate_workcell_entities(commands: &mut Commands, workcell: &Workcell) -> E } Geometry::Mesh { source, scale } => { commands.entity(e).insert(( - ConstraintDependents::default(), NameInWorkcell(parented.bundle.name.clone()), parented.bundle.pose.clone(), Scale(scale.unwrap_or(Vec3::ONE)), @@ -110,21 +107,6 @@ fn generate_workcell_entities(commands: &mut Commands, workcell: &Workcell) -> E .insert(SiteID(*id)) .insert(FrameMarker) .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()); } @@ -161,13 +143,6 @@ fn generate_workcell_entities(commands: &mut Commands, workcell: &Workcell) -> 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 diff --git a/rmf_site_editor/src/workcell/save.rs b/rmf_site_editor/src/workcell/save.rs index 33e15d49..d0e11222 100644 --- a/rmf_site_editor/src/workcell/save.rs +++ b/rmf_site_editor/src/workcell/save.rs @@ -90,17 +90,7 @@ pub fn generate_workcell( ) -> Result { assign_site_ids(world, root); let mut state: SystemState<( - Query< - ( - Entity, - &Anchor, - Option<&NameInWorkcell>, - &SiteID, - &Parent, - Option<&MeshConstraint>, - ), - Without, - >, + Query<(Entity, &Anchor, Option<&NameInWorkcell>, &SiteID, &Parent), Without>, Query<(Entity, &Pose, &Mass, &Moment, &SiteID, &Parent), Without>, Query< ( @@ -199,7 +189,7 @@ pub fn generate_workcell( } // Anchors - for (e, anchor, name, id, parent, constraint) in &q_anchors { + for (e, anchor, name, id, parent) in &q_anchors { if !parent_in_workcell(&q_parents, e, root) { continue; } @@ -210,18 +200,6 @@ pub fn generate_workcell( 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 { @@ -229,7 +207,6 @@ pub fn generate_workcell( bundle: Frame { anchor: anchor.clone(), name: name.cloned(), - mesh_constraint: constraint, marker: FrameMarker, }, }, diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index cf8f0e49..abab4614 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -98,6 +98,9 @@ pub use physical_camera::*; pub mod point; pub use point::*; +pub mod primitive_shape; +pub use primitive_shape::*; + pub mod recall; pub use recall::*; diff --git a/rmf_site_format/src/primitive_shape.rs b/rmf_site_format/src/primitive_shape.rs new file mode 100644 index 00000000..db60d9b5 --- /dev/null +++ b/rmf_site_format/src/primitive_shape.rs @@ -0,0 +1,130 @@ +/* + * Copyright (C) 2024 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::Recall; + +#[cfg(feature = "bevy")] +use bevy::prelude::{Component, Reflect, ReflectComponent}; + +use serde::{Deserialize, Serialize}; + +#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component, Reflect))] +#[cfg_attr(feature = "bevy", reflect(Component))] +pub enum PrimitiveShape { + Box { size: [f32; 3] }, + Cylinder { radius: f32, length: f32 }, + Capsule { radius: f32, length: f32 }, + Sphere { radius: f32 }, +} + +impl Default for PrimitiveShape { + fn default() -> Self { + Self::Box { + size: [1.0, 1.0, 1.0], + } + } +} + +impl PrimitiveShape { + pub fn label(&self) -> String { + match &self { + PrimitiveShape::Box { .. } => "Box", + PrimitiveShape::Cylinder { .. } => "Cylinder", + PrimitiveShape::Capsule { .. } => "Capsule", + PrimitiveShape::Sphere { .. } => "Sphere", + } + .to_string() + } +} + +#[derive(Clone, Debug, Default, PartialEq)] +#[cfg_attr(feature = "bevy", derive(Component))] +pub struct RecallPrimitiveShape { + 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 RecallPrimitiveShape { + type Source = PrimitiveShape; + + fn remember(&mut self, source: &PrimitiveShape) { + match source { + PrimitiveShape::Box { size } => { + self.box_size = Some(*size); + } + PrimitiveShape::Cylinder { radius, length } => { + self.cylinder_radius = Some(*radius); + self.cylinder_length = Some(*length); + } + PrimitiveShape::Capsule { radius, length } => { + self.capsule_radius = Some(*radius); + self.capsule_length = Some(*length); + } + PrimitiveShape::Sphere { radius } => { + self.sphere_radius = Some(*radius); + } + } + } +} + +impl RecallPrimitiveShape { + pub fn assume_box(&self, current: &PrimitiveShape) -> PrimitiveShape { + if matches!(current, PrimitiveShape::Box { .. }) { + current.clone() + } else { + PrimitiveShape::Box { + size: self.box_size.unwrap_or_default(), + } + } + } + + pub fn assume_cylinder(&self, current: &PrimitiveShape) -> PrimitiveShape { + if matches!(current, PrimitiveShape::Cylinder { .. }) { + current.clone() + } else { + PrimitiveShape::Cylinder { + radius: self.cylinder_radius.unwrap_or_default(), + length: self.cylinder_length.unwrap_or_default(), + } + } + } + + pub fn assume_capsule(&self, current: &PrimitiveShape) -> PrimitiveShape { + if matches!(current, PrimitiveShape::Capsule { .. }) { + current.clone() + } else { + PrimitiveShape::Capsule { + radius: self.capsule_radius.unwrap_or_default(), + length: self.capsule_length.unwrap_or_default(), + } + } + } + + pub fn assume_sphere(&self, current: &PrimitiveShape) -> PrimitiveShape { + if matches!(current, PrimitiveShape::Sphere { .. }) { + current.clone() + } else { + PrimitiveShape::Sphere { + radius: self.sphere_radius.unwrap_or_default(), + } + } + } +} diff --git a/rmf_site_format/src/workcell.rs b/rmf_site_format/src/workcell.rs index 0ca6d883..13611b8c 100644 --- a/rmf_site_format/src/workcell.rs +++ b/rmf_site_format/src/workcell.rs @@ -17,9 +17,6 @@ use std::collections::{BTreeMap, HashMap}; -#[cfg(feature = "bevy")] -use std::collections::HashSet; - use std::io; use crate::misc::Rotation; @@ -27,9 +24,7 @@ use crate::*; #[cfg(feature = "bevy")] use bevy::ecs::system::EntityCommands; #[cfg(feature = "bevy")] -use bevy::prelude::{ - Bundle, Component, Deref, DerefMut, Entity, Reflect, ReflectComponent, SpatialBundle, -}; +use bevy::prelude::{Bundle, Component, Deref, DerefMut, SpatialBundle}; #[cfg(feature = "bevy")] use bevy::reflect::{TypePath, TypeUuid}; use glam::{EulerRot, Vec3}; @@ -54,32 +49,10 @@ pub struct Frame { 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 -#[cfg(feature = "bevy")] -#[derive(Component, Deref, DerefMut, Serialize, Deserialize, Debug, Default, Clone, PartialEq)] -pub struct ConstraintDependents(pub HashSet); - #[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] #[cfg_attr(feature = "bevy", derive(Component))] pub struct NameOfWorkcell(pub String); @@ -289,114 +262,6 @@ pub enum Geometry { }, } -#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)] -#[cfg_attr(feature = "bevy", derive(Component, Reflect))] -#[cfg_attr(feature = "bevy", reflect(Component))] -pub enum PrimitiveShape { - Box { size: [f32; 3] }, - Cylinder { radius: f32, length: f32 }, - Capsule { radius: f32, length: f32 }, - Sphere { radius: f32 }, -} - -impl Default for PrimitiveShape { - fn default() -> Self { - Self::Box { - size: [1.0, 1.0, 1.0], - } - } -} - -impl PrimitiveShape { - pub fn label(&self) -> String { - match &self { - PrimitiveShape::Box { .. } => "Box", - PrimitiveShape::Cylinder { .. } => "Cylinder", - PrimitiveShape::Capsule { .. } => "Capsule", - PrimitiveShape::Sphere { .. } => "Sphere", - } - .to_string() - } -} - -#[derive(Clone, Debug, Default, PartialEq)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub struct RecallPrimitiveShape { - 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 RecallPrimitiveShape { - type Source = PrimitiveShape; - - fn remember(&mut self, source: &PrimitiveShape) { - match source { - PrimitiveShape::Box { size } => { - self.box_size = Some(*size); - } - PrimitiveShape::Cylinder { radius, length } => { - self.cylinder_radius = Some(*radius); - self.cylinder_length = Some(*length); - } - PrimitiveShape::Capsule { radius, length } => { - self.capsule_radius = Some(*radius); - self.capsule_length = Some(*length); - } - PrimitiveShape::Sphere { radius } => { - self.sphere_radius = Some(*radius); - } - } - } -} - -impl RecallPrimitiveShape { - pub fn assume_box(&self, current: &PrimitiveShape) -> PrimitiveShape { - if matches!(current, PrimitiveShape::Box { .. }) { - current.clone() - } else { - PrimitiveShape::Box { - size: self.box_size.unwrap_or_default(), - } - } - } - - pub fn assume_cylinder(&self, current: &PrimitiveShape) -> PrimitiveShape { - if matches!(current, PrimitiveShape::Cylinder { .. }) { - current.clone() - } else { - PrimitiveShape::Cylinder { - radius: self.cylinder_radius.unwrap_or_default(), - length: self.cylinder_length.unwrap_or_default(), - } - } - } - - pub fn assume_capsule(&self, current: &PrimitiveShape) -> PrimitiveShape { - if matches!(current, PrimitiveShape::Capsule { .. }) { - current.clone() - } else { - PrimitiveShape::Capsule { - radius: self.capsule_radius.unwrap_or_default(), - length: self.capsule_length.unwrap_or_default(), - } - } - } - - pub fn assume_sphere(&self, current: &PrimitiveShape) -> PrimitiveShape { - if matches!(current, PrimitiveShape::Sphere { .. }) { - current.clone() - } else { - PrimitiveShape::Sphere { - radius: self.sphere_radius.unwrap_or_default(), - } - } - } -} - impl Default for Geometry { fn default() -> Self { Geometry::Primitive(PrimitiveShape::Box { size: [0.0; 3] }) @@ -538,7 +403,6 @@ impl Workcell { bundle: Frame { anchor: Anchor::Pose3D(Pose::default()), name: Some(NameInWorkcell(link.name.clone())), - mesh_constraint: Default::default(), marker: Default::default(), }, }, @@ -692,7 +556,6 @@ impl Workcell { name: Some(NameInWorkcell(String::from( self.properties.name.0.clone() + "_workcell_link", ))), - mesh_constraint: None, marker: FrameMarker, }; frames.insert( From d3ff2c68440695007c3435e0a066b4e793f1ea71 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 5 Sep 2024 18:31:45 +0800 Subject: [PATCH 02/25] Move pose implementation to right module Signed-off-by: Luca Della Vedova --- rmf_site_format/src/misc.rs | 25 +++++++++++++++++++++++++ rmf_site_format/src/workcell.rs | 27 +-------------------------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 635f6d0e..c8f014b6 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -353,6 +353,31 @@ impl Default for Pose { } } +impl From for urdf_rs::Pose { + fn from(pose: Pose) -> Self { + urdf_rs::Pose { + rpy: match pose.rot { + Rotation::EulerExtrinsicXYZ(arr) => urdf_rs::Vec3(arr.map(|v| v.radians().into())), + Rotation::Yaw(v) => urdf_rs::Vec3([0.0, 0.0, v.radians().into()]), + Rotation::Quat([x, y, z, w]) => { + let (z, y, x) = glam::quat(x, y, z, w).to_euler(EulerRot::ZYX); + urdf_rs::Vec3([x as f64, y as f64, z as f64]) + } + }, + xyz: urdf_rs::Vec3(pose.trans.map(|v| v as f64)), + } + } +} + +impl From<&urdf_rs::Pose> for Pose { + fn from(pose: &urdf_rs::Pose) -> Self { + Pose { + trans: pose.xyz.map(|t| t as f32), + rot: Rotation::EulerExtrinsicXYZ(pose.rpy.map(|t| Angle::Rad(t as f32))), + } + } +} + #[cfg(feature = "bevy")] impl Pose { pub fn transform(&self) -> Transform { diff --git a/rmf_site_format/src/workcell.rs b/rmf_site_format/src/workcell.rs index 13611b8c..519152e2 100644 --- a/rmf_site_format/src/workcell.rs +++ b/rmf_site_format/src/workcell.rs @@ -27,7 +27,7 @@ use bevy::ecs::system::EntityCommands; use bevy::prelude::{Bundle, Component, Deref, DerefMut, SpatialBundle}; #[cfg(feature = "bevy")] use bevy::reflect::{TypePath, TypeUuid}; -use glam::{EulerRot, Vec3}; +use glam::Vec3; use serde::{Deserialize, Serialize}; use thiserror::Error as ThisError; @@ -307,31 +307,6 @@ pub enum UrdfImportError { UnsupportedJointType, } -impl From for urdf_rs::Pose { - fn from(pose: Pose) -> Self { - urdf_rs::Pose { - rpy: match pose.rot { - Rotation::EulerExtrinsicXYZ(arr) => urdf_rs::Vec3(arr.map(|v| v.radians().into())), - Rotation::Yaw(v) => urdf_rs::Vec3([0.0, 0.0, v.radians().into()]), - Rotation::Quat([x, y, z, w]) => { - let (z, y, x) = glam::quat(x, y, z, w).to_euler(EulerRot::ZYX); - urdf_rs::Vec3([x as f64, y as f64, z as f64]) - } - }, - xyz: urdf_rs::Vec3(pose.trans.map(|v| v as f64)), - } - } -} - -impl From<&urdf_rs::Pose> for Pose { - fn from(pose: &urdf_rs::Pose) -> Self { - Pose { - trans: pose.xyz.map(|t| t as f32), - rot: Rotation::EulerExtrinsicXYZ(pose.rpy.map(|t| Angle::Rad(t as f32))), - } - } -} - impl From for urdf_rs::Geometry { fn from(geometry: Geometry) -> Self { match geometry { From 260d26384170300574f997c27b11b79f9cfaca92 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Fri, 6 Sep 2024 16:19:59 +0800 Subject: [PATCH 03/25] WIP removed bulk of workcell editor Signed-off-by: Luca Della Vedova --- Cargo.lock | 451 +------- assets/demo_workcells/demo.workcell.json | 83 -- rmf_site_editor/Cargo.toml | 6 +- rmf_site_editor/src/demo_world.rs | 6 - rmf_site_editor/src/interaction/mod.rs | 8 +- rmf_site_editor/src/interaction/outline.rs | 3 +- rmf_site_editor/src/interaction/select.rs | 6 - .../src/interaction/select/place_object.rs | 44 - .../src/interaction/select/place_object_3d.rs | 521 ---------- .../interaction/select/replace_parent_3d.rs | 309 ------ rmf_site_editor/src/lib.rs | 8 +- rmf_site_editor/src/main_menu.rs | 15 - rmf_site_editor/src/shapes.rs | 23 - .../src/site/drawing_editor/mod.rs | 3 +- rmf_site_editor/src/site/mod.rs | 1 + rmf_site_editor/src/view_menu.rs | 13 +- rmf_site_editor/src/widgets/creation.rs | 16 +- .../src/widgets/fuel_asset_browser.rs | 8 +- .../src/widgets/inspector/inspect_anchor.rs | 13 +- .../src/widgets/inspector/inspect_joint.rs | 3 +- .../src/widgets/inspector/inspect_name.rs | 3 +- .../inspector/inspect_workcell_parent.rs | 4 +- rmf_site_editor/src/widgets/inspector/mod.rs | 6 +- rmf_site_editor/src/workcell/frame.rs | 33 - rmf_site_editor/src/workcell/joint.rs | 83 -- rmf_site_editor/src/workcell/keyboard.rs | 52 - rmf_site_editor/src/workcell/load.rs | 176 ---- rmf_site_editor/src/workcell/menu.rs | 59 -- rmf_site_editor/src/workcell/mod.rs | 113 -- rmf_site_editor/src/workcell/model.rs | 102 -- rmf_site_editor/src/workcell/save.rs | 342 ------ .../urdf_package_exporter/generate_package.rs | 136 --- .../src/workcell/urdf_package_exporter/mod.rs | 5 - .../urdf_package_exporter/template.rs | 27 - .../templates/CMakeLists.txt.j2 | 20 - .../templates/display.launch.py.j2 | 31 - .../templates/package.xml.j2 | 25 - .../templates/urdf.rviz.j2 | 35 - rmf_site_editor/src/workcell/workcell.rs | 75 -- rmf_site_editor/src/workspace.rs | 27 +- rmf_site_format/src/constraint.rs | 53 - rmf_site_format/src/lib.rs | 6 - rmf_site_format/src/workcell.rs | 983 ------------------ 43 files changed, 98 insertions(+), 3838 deletions(-) delete mode 100644 assets/demo_workcells/demo.workcell.json delete mode 100644 rmf_site_editor/src/interaction/select/place_object_3d.rs delete mode 100644 rmf_site_editor/src/interaction/select/replace_parent_3d.rs delete mode 100644 rmf_site_editor/src/workcell/frame.rs delete mode 100644 rmf_site_editor/src/workcell/joint.rs delete mode 100644 rmf_site_editor/src/workcell/keyboard.rs delete mode 100644 rmf_site_editor/src/workcell/load.rs delete mode 100644 rmf_site_editor/src/workcell/menu.rs delete mode 100644 rmf_site_editor/src/workcell/mod.rs delete mode 100644 rmf_site_editor/src/workcell/model.rs delete mode 100644 rmf_site_editor/src/workcell/save.rs delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/generate_package.rs delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/mod.rs delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/template.rs delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 delete mode 100644 rmf_site_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 delete mode 100644 rmf_site_editor/src/workcell/workcell.rs delete mode 100644 rmf_site_format/src/constraint.rs diff --git a/Cargo.lock b/Cargo.lock index bc6e5a4d..15207015 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -159,12 +159,6 @@ version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fc7eb209b1518d6bb87b283c20095f5228ecda460da70b44f0802523dea6da04" -[[package]] -name = "android-tzdata" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e999941b234f3131b00bc13c22d06e8c5ff726d1b6318ac7eb276997bbb4fef0" - [[package]] name = "android_log-sys" version = "0.3.1" @@ -435,6 +429,7 @@ version = "0.12.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e4bc7e09282a82a48d70ade0c4c1154b0fd7882a735a39c66766a5d0f4718ea9" dependencies = [ + "bevy_dylib", "bevy_internal", ] @@ -610,6 +605,15 @@ dependencies = [ "sysinfo", ] +[[package]] +name = "bevy_dylib" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "45b99001eb4837c78d9c63cc8b32fda61ea96b194a2cda54b569aeee69a9853c" +dependencies = [ + "bevy_internal", +] + [[package]] name = "bevy_ecs" version = "0.12.1" @@ -795,15 +799,7 @@ version = "0.0.1" source = "git+https://github.com/open-rmf/bevy_impulse?branch=main#2d86d5c58aaf18e5c9be933d3c5155ce1dc80dcc" dependencies = [ "quote", - "syn 1.0.109", -] - -[[package]] -name = "bevy_infinite_grid" -version = "0.10.0" -source = "git+https://github.com/ForesightMiningSoftwareCorporation/bevy_infinite_grid?rev=86018dd#86018dd5708357067323931241ebc835fa73bf30" -dependencies = [ - "bevy", + "syn 2.0.52", ] [[package]] @@ -1397,15 +1393,6 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0d8c1fef690941d3e7788d328517591fecc684c084084702d6ff1641e993699a" -[[package]] -name = "block-buffer" -version = "0.10.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71" -dependencies = [ - "generic-array", -] - [[package]] name = "block-sys" version = "0.1.0-beta.1" @@ -1441,16 +1428,6 @@ dependencies = [ "tracing", ] -[[package]] -name = "bstr" -version = "1.9.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "05efc5cfd9110c8416e471df0e96702d58690178e206e61b7173706673c93706" -dependencies = [ - "memchr", - "serde", -] - [[package]] name = "bumpalo" version = "3.15.3" @@ -1546,40 +1523,6 @@ version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fd16c4719339c4530435d38e511904438d07cce7950afa3718a84ac36c10e89e" -[[package]] -name = "chrono" -version = "0.4.34" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5bc015644b92d5890fab7489e49d21f879d5c990186827d42ec511919404f38b" -dependencies = [ - "android-tzdata", - "iana-time-zone", - "num-traits", - "windows-targets 0.52.4", -] - -[[package]] -name = "chrono-tz" -version = "0.8.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d59ae0466b83e838b81a54256c39d5d7c20b9d7daa10510a242d9b75abd5936e" -dependencies = [ - "chrono", - "chrono-tz-build", - "phf", -] - -[[package]] -name = "chrono-tz-build" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "433e39f13c9a060046954e0592a8d0a4bcb1040125cbf91cb8ee58964cfb350f" -dependencies = [ - "parse-zoneinfo", - "phf", - "phf_codegen", -] - [[package]] name = "clang-sys" version = "1.7.0" @@ -1829,15 +1772,6 @@ dependencies = [ "windows 0.54.0", ] -[[package]] -name = "cpufeatures" -version = "0.2.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "53fe5e26ff1b7aef8bca9c6080520cfb8d9333c7568e1829cef191a9723e5504" -dependencies = [ - "libc", -] - [[package]] name = "crc32fast" version = "1.4.0" @@ -1893,16 +1827,6 @@ version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7" -[[package]] -name = "crypto-common" -version = "0.1.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3" -dependencies = [ - "generic-array", - "typenum", -] - [[package]] name = "d3d12" version = "0.7.0" @@ -1926,22 +1850,6 @@ version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7e962a19be5cfc3f3bf6dd8f61eb50107f356ad6270fbb3ed41476571db78be5" -[[package]] -name = "deunicode" -version = "1.4.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b6e854126756c496b8c81dec88f9a706b15b875c5849d4097a3854476b9fdf94" - -[[package]] -name = "digest" -version = "0.10.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292" -dependencies = [ - "block-buffer", - "crypto-common", -] - [[package]] name = "dirs" version = "5.0.1" @@ -2469,16 +2377,6 @@ dependencies = [ "system-deps", ] -[[package]] -name = "generic-array" -version = "0.14.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a" -dependencies = [ - "typenum", - "version_check", -] - [[package]] name = "geo" version = "0.27.0" @@ -2629,30 +2527,6 @@ version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" -[[package]] -name = "globset" -version = "0.4.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57da3b9b5b85bd66f31093f8c408b90a74431672542466497dcbdfdc02034be1" -dependencies = [ - "aho-corasick", - "bstr", - "log", - "regex-automata 0.4.6", - "regex-syntax 0.8.2", -] - -[[package]] -name = "globwalk" -version = "0.8.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "93e3af942408868f6934a7b85134a3230832b9977cf66125df2f9edcfce4ddcc" -dependencies = [ - "bitflags 1.3.2", - "ignore", - "walkdir", -] - [[package]] name = "glow" version = "0.12.3" @@ -2927,38 +2801,6 @@ dependencies = [ "windows-sys 0.52.0", ] -[[package]] -name = "humansize" -version = "2.1.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6cb51c9a029ddc91b07a787f1d86b53ccfa49b0e86688c946ebe8d3555685dd7" -dependencies = [ - "libm", -] - -[[package]] -name = "iana-time-zone" -version = "0.1.60" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e7ffbb5a1b541ea2561f8c41c087286cc091e21e556a4f09a8f6cbf17b69b141" -dependencies = [ - "android_system_properties", - "core-foundation-sys", - "iana-time-zone-haiku", - "js-sys", - "wasm-bindgen", - "windows-core 0.52.0", -] - -[[package]] -name = "iana-time-zone-haiku" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" -dependencies = [ - "cc", -] - [[package]] name = "idna" version = "0.5.0" @@ -2969,22 +2811,6 @@ dependencies = [ "unicode-normalization", ] -[[package]] -name = "ignore" -version = "0.4.22" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b46810df39e66e925525d6e38ce1e7f6e1d208f72dc39757880fcb66e2c58af1" -dependencies = [ - "crossbeam-deque", - "globset", - "log", - "memchr", - "regex-automata 0.4.6", - "same-file", - "walkdir", - "winapi-util", -] - [[package]] name = "image" version = "0.24.9" @@ -3873,15 +3699,6 @@ dependencies = [ "windows-targets 0.48.5", ] -[[package]] -name = "parse-zoneinfo" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c705f256449c60da65e11ff6626e0c16a0a0b96aaa348de61376b249bc340f41" -dependencies = [ - "regex", -] - [[package]] name = "paste" version = "1.0.14" @@ -3900,51 +3717,6 @@ version = "2.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e3148f5046208a5d56bcfc03053e3ca6334e51da8dfb19b6cdc8b306fae3283e" -[[package]] -name = "pest" -version = "2.7.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "56f8023d0fb78c8e03784ea1c7f3fa36e68a723138990b8d5a47d916b651e7a8" -dependencies = [ - "memchr", - "thiserror", - "ucd-trie", -] - -[[package]] -name = "pest_derive" -version = "2.7.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0d24f72393fd16ab6ac5738bc33cdb6a9aa73f8b902e8fe29cf4e67d7dd1026" -dependencies = [ - "pest", - "pest_generator", -] - -[[package]] -name = "pest_generator" -version = "2.7.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fdc17e2a6c7d0a492f0158d7a4bd66cc17280308bbaff78d5bef566dca35ab80" -dependencies = [ - "pest", - "pest_meta", - "proc-macro2", - "quote", - "syn 2.0.52", -] - -[[package]] -name = "pest_meta" -version = "2.7.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "934cd7631c050f4674352a6e835d5f6711ffbfb9345c2fc0107155ac495ae293" -dependencies = [ - "once_cell", - "pest", - "sha2", -] - [[package]] name = "petgraph" version = "0.6.4" @@ -3955,44 +3727,6 @@ dependencies = [ "indexmap 2.2.5", ] -[[package]] -name = "phf" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ade2d8b8f33c7333b51bcf0428d37e217e9f32192ae4772156f65063b8ce03dc" -dependencies = [ - "phf_shared", -] - -[[package]] -name = "phf_codegen" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e8d39688d359e6b34654d328e262234662d16cc0f60ec8dcbe5e718709342a5a" -dependencies = [ - "phf_generator", - "phf_shared", -] - -[[package]] -name = "phf_generator" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "48e4cc64c2ad9ebe670cb8fd69dd50ae301650392e81c05f9bfcb2d5bdbc24b0" -dependencies = [ - "phf_shared", - "rand", -] - -[[package]] -name = "phf_shared" -version = "0.11.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90fcb95eef784c2ac79119d1dd819e162b5da872ce6f3c3abe1e8ca1c082f72b" -dependencies = [ - "siphasher", -] - [[package]] name = "pin-project-lite" version = "0.2.13" @@ -4044,12 +3778,6 @@ dependencies = [ "unicode-xid", ] -[[package]] -name = "ppv-lite86" -version = "0.2.17" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" - [[package]] name = "proc-macro-crate" version = "1.3.1" @@ -4108,36 +3836,6 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17fd96390ed3feda12e1dfe2645ed587e0bea749e319333f104a33ff62f77a0b" -[[package]] -name = "rand" -version = "0.8.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" -dependencies = [ - "libc", - "rand_chacha", - "rand_core", -] - -[[package]] -name = "rand_chacha" -version = "0.3.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" -dependencies = [ - "ppv-lite86", - "rand_core", -] - -[[package]] -name = "rand_core" -version = "0.6.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" -dependencies = [ - "getrandom", -] - [[package]] name = "range-alloc" version = "0.1.3" @@ -4314,7 +4012,6 @@ dependencies = [ "bevy_egui", "bevy_gltf_export", "bevy_impulse", - "bevy_infinite_grid", "bevy_mod_outline", "bevy_mod_raycast", "bevy_obj", @@ -4334,12 +4031,12 @@ dependencies = [ "pathdiff", "rfd", "rmf_site_format", + "rmf_workcell_format", "sdformat_rs", "serde", "serde_json", "serde_yaml", "smallvec", - "tera", "thiserror", "thread_local", "tracing", @@ -4370,6 +4067,25 @@ dependencies = [ "yaserde", ] +[[package]] +name = "rmf_workcell_format" +version = "0.0.1" +dependencies = [ + "bevy", + "glam", + "once_cell", + "pathdiff", + "rmf_site_format", + "ron", + "serde", + "serde_json", + "serde_yaml", + "thiserror", + "urdf-rs", + "uuid", + "yaserde", +] + [[package]] name = "robust" version = "1.1.0" @@ -4585,17 +4301,6 @@ dependencies = [ "yaml-rust", ] -[[package]] -name = "sha2" -version = "0.10.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "793db75ad2bcafc3ffa7c68b215fee268f537982cd901d132f89c6343f3a3dc8" -dependencies = [ - "cfg-if", - "cpufeatures", - "digest", -] - [[package]] name = "sharded-slab" version = "0.1.7" @@ -4630,12 +4335,6 @@ version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d66dc143e6b11c1eddc06d5c423cfc97062865baf299914ab64caa38182078fe" -[[package]] -name = "siphasher" -version = "0.3.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38b58827f4464d87d377d175e90bf58eb00fd8716ff0a62f80356b5e61555d0d" - [[package]] name = "slab" version = "0.4.9" @@ -4654,16 +4353,6 @@ dependencies = [ "version_check", ] -[[package]] -name = "slug" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3bd94acec9c8da640005f8e135a39fc0372e74535e6b368b7a04b875f784c8c4" -dependencies = [ - "deunicode", - "wasm-bindgen", -] - [[package]] name = "smallvec" version = "1.13.1" @@ -4820,28 +4509,6 @@ version = "0.12.14" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e1fc403891a21bcfb7c37834ba66a547a8f402146eba7265b5a6d88059c9ff2f" -[[package]] -name = "tera" -version = "1.19.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "970dff17c11e884a4a09bc76e3a17ef71e01bb13447a11e85226e254fe6d10b8" -dependencies = [ - "chrono", - "chrono-tz", - "globwalk", - "humansize", - "lazy_static", - "percent-encoding", - "pest", - "pest_derive", - "rand", - "regex", - "serde", - "serde_json", - "slug", - "unic-segment", -] - [[package]] name = "termcolor" version = "1.4.1" @@ -5119,62 +4786,6 @@ version = "1.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" -[[package]] -name = "ucd-trie" -version = "0.1.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed646292ffc8188ef8ea4d1e0e0150fb15a5c2e12ad9b8fc191ae7a8a7f3c4b9" - -[[package]] -name = "unic-char-property" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8c57a407d9b6fa02b4795eb81c5b6652060a15a7903ea981f3d723e6c0be221" -dependencies = [ - "unic-char-range", -] - -[[package]] -name = "unic-char-range" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0398022d5f700414f6b899e10b8348231abf9173fa93144cbc1a43b9793c1fbc" - -[[package]] -name = "unic-common" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "80d7ff825a6a654ee85a63e80f92f054f904f21e7d12da4e22f9834a4aaa35bc" - -[[package]] -name = "unic-segment" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e4ed5d26be57f84f176157270c112ef57b86debac9cd21daaabbe56db0f88f23" -dependencies = [ - "unic-ucd-segment", -] - -[[package]] -name = "unic-ucd-segment" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2079c122a62205b421f499da10f3ee0f7697f012f55b675e002483c73ea34700" -dependencies = [ - "unic-char-property", - "unic-char-range", - "unic-ucd-version", -] - -[[package]] -name = "unic-ucd-version" -version = "0.9.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "96bd2f2237fe450fcd0a1d2f5f4e91711124f7857ba2e964247776ebeeb7b0c4" -dependencies = [ - "unic-common", -] - [[package]] name = "unicode-bidi" version = "0.3.15" diff --git a/assets/demo_workcells/demo.workcell.json b/assets/demo_workcells/demo.workcell.json deleted file mode 100644 index 91a8e549..00000000 --- a/assets/demo_workcells/demo.workcell.json +++ /dev/null @@ -1,83 +0,0 @@ -{ - "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": { - }, - "inertias": { - }, - "joints": { - } -} diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index 69839f84..393911cb 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -19,8 +19,6 @@ path = "examples/extending_menu.rs" bevy_egui = "0.23" bevy_mod_raycast = "0.16" bevy_mod_outline = "0.6" -# PR merged after 0.10 but not released yet, bump to 0.10.1 once merged -bevy_infinite_grid = { git = "https://github.com/ForesightMiningSoftwareCorporation/bevy_infinite_grid", rev = "86018dd" } bevy_gltf_export = { git = "https://github.com/luca-della-vedova/bevy_gltf_export", branch = "luca/transform_api"} bevy_polyline = "0.8.1" bevy_stl = "0.12" @@ -31,12 +29,13 @@ serde_yaml = "0.8.23" serde_json = "1.0" wasm-bindgen = "0.2.87" futures-lite = "1.12.0" -bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } +bevy = { version = "0.12", features = ["pnm", "jpeg", "tga", "dynamic_linking"] } dirs = "5.0" thread_local = "*" geo = "0.27" thiserror = "*" rmf_site_format = { path = "../rmf_site_format", features = ["bevy"] } +rmf_workcell_format = { path = "../../rmf_workcell/rmf_workcell_format", features = ["bevy"] } itertools = "*" bitfield = "*" crossbeam-channel = "0.5" @@ -49,7 +48,6 @@ utm = "0.1.6" sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "9fc35f2"} gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "main" } pathdiff = "*" -tera = "1.19.1" ehttp = { version = "0.4", features = ["native-async"] } nalgebra = "0.32.5" anyhow = "*" diff --git a/rmf_site_editor/src/demo_world.rs b/rmf_site_editor/src/demo_world.rs index ebb55754..2272eb88 100644 --- a/rmf_site_editor/src/demo_world.rs +++ b/rmf_site_editor/src/demo_world.rs @@ -3,9 +3,3 @@ pub fn demo_office() -> Vec { .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/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 2a57893b..f21b0e44 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -16,11 +16,12 @@ */ use crate::site::{ - update_anchor_transforms, CollisionMeshMarker, ConstraintMarker, DoorMarker, FiducialMarker, + update_anchor_transforms, CollisionMeshMarker, DoorMarker, FiducialMarker, FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, MeasurementMarker, SiteUpdateSet, VisualMeshMarker, WallMarker, }; -use crate::workcell::WorkcellVisualizationMarker; +// TODO(luca) restore +// use crate::workcell::WorkcellVisualizationMarker; pub mod anchor; pub use anchor::*; @@ -165,12 +166,11 @@ impl Plugin for InteractionPlugin { CategoryVisibilityPlugin::::visible(true), CategoryVisibilityPlugin::::visible(true), CategoryVisibilityPlugin::::visible(true), - CategoryVisibilityPlugin::::visible(true), CategoryVisibilityPlugin::::visible(true), CategoryVisibilityPlugin::::visible(false), CategoryVisibilityPlugin::::visible(true), CategoryVisibilityPlugin::::visible(true), - CategoryVisibilityPlugin::::visible(true), + // CategoryVisibilityPlugin::::visible(true), )) .add_plugins((CameraControlsPlugin, ModelPreviewPlugin)); diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs index ef1f010e..3872ff25 100644 --- a/rmf_site_editor/src/interaction/outline.rs +++ b/rmf_site_editor/src/interaction/outline.rs @@ -19,7 +19,7 @@ use crate::{interaction::*, site::DrawingMarker}; use bevy::render::view::RenderLayers; use bevy_mod_outline::{OutlineBundle, OutlineMode, OutlineRenderLayers, OutlineVolume}; use rmf_site_format::{ - ConstraintMarker, DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, + DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, MeasurementMarker, ModelMarker, PhysicalCameraProperties, PrimitiveShape, WallMarker, }; use smallvec::SmallVec; @@ -110,7 +110,6 @@ pub fn add_outline_visualization( Added, Added>, Added, - Added, Added, Added, Added, diff --git a/rmf_site_editor/src/interaction/select.rs b/rmf_site_editor/src/interaction/select.rs index e471a13b..c9e810e3 100644 --- a/rmf_site_editor/src/interaction/select.rs +++ b/rmf_site_editor/src/interaction/select.rs @@ -52,12 +52,6 @@ pub use place_object::*; pub mod place_object_2d; pub use place_object_2d::*; -pub mod place_object_3d; -pub use place_object_3d::*; - -pub mod replace_parent_3d; -use replace_parent_3d::*; - pub mod replace_point; use replace_point::*; diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs index 96a9f346..16d5b788 100644 --- a/rmf_site_editor/src/interaction/select/place_object.rs +++ b/rmf_site_editor/src/interaction/select/place_object.rs @@ -31,26 +31,13 @@ impl Plugin for ObjectPlacementPlugin { #[derive(Resource, Clone, Copy)] pub struct ObjectPlacementServices { pub place_object_2d: Service, ()>, - pub place_object_3d: Service, ()>, - pub replace_parent_3d: Service, ()>, - pub hover_service_object_3d: Service<(), (), Hover>, } impl ObjectPlacementServices { pub fn from_app(app: &mut App) -> Self { - let hover_service_object_3d = app.spawn_continuous_service( - Update, - hover_service:: - .configure(|config: SystemConfigs| config.in_set(SelectionServiceStages::Hover)), - ); let place_object_2d = spawn_place_object_2d_workflow(app); - let place_object_3d = spawn_place_object_3d_workflow(hover_service_object_3d, app); - let replace_parent_3d = spawn_replace_parent_3d_workflow(hover_service_object_3d, app); Self { place_object_2d, - place_object_3d, - replace_parent_3d, - hover_service_object_3d, } } } @@ -73,37 +60,6 @@ impl<'w, 's> ObjectPlacement<'w, 's> { }); } - pub fn place_object_3d( - &mut self, - object: PlaceableObject, - parent: Option, - workspace: Entity, - ) { - let state = self - .commands - .spawn(SelectorInput(PlaceObject3d { - object, - parent, - workspace, - })) - .id(); - self.send(RunSelector { - selector: self.services.place_object_3d, - input: Some(state), - }); - } - - pub fn replace_parent_3d(&mut self, object: Entity, workspace: Entity) { - let state = self - .commands - .spawn(SelectorInput(ReplaceParent3d { object, workspace })) - .id(); - self.send(RunSelector { - selector: self.services.replace_parent_3d, - input: Some(state), - }); - } - fn send(&mut self, run: RunSelector) { self.commands.add(move |world: &mut World| { world.send_event(run); diff --git a/rmf_site_editor/src/interaction/select/place_object_3d.rs b/rmf_site_editor/src/interaction/select/place_object_3d.rs deleted file mode 100644 index a7b129d0..00000000 --- a/rmf_site_editor/src/interaction/select/place_object_3d.rs +++ /dev/null @@ -1,521 +0,0 @@ -/* - * Copyright (C) 2024 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::select::*, - site::{ - Anchor, AnchorBundle, Dependents, FrameMarker, Model, ModelLoadingRequest, - ModelSpawningExt, NameInWorkcell, Pending, SiteID, - }, - widgets::canvas_tooltips::CanvasTooltips, - workcell::flatten_loaded_model_hierarchy, -}; -use bevy::{ - ecs::system::{EntityCommands, SystemParam}, - prelude::Input as UserInput, -}; -use bevy_mod_raycast::deferred::RaycastSource; -use std::borrow::Cow; - -pub const PLACE_OBJECT_3D_MODE_LABEL: &'static str = "place_object_3d"; - -pub fn spawn_place_object_3d_workflow( - hover_service: Service<(), (), Hover>, - app: &mut App, -) -> Service, ()> { - let setup = app.spawn_service(place_object_3d_setup); - let find_position = app.spawn_continuous_service(Update, place_object_3d_find_placement); - let placement_chosen = app.spawn_service(on_placement_chosen_3d.into_blocking_service()); - let handle_key_code = app.spawn_service(on_keyboard_for_place_object_3d); - let cleanup = app.spawn_service(place_object_3d_cleanup.into_blocking_service()); - let selection_update = app.world.resource::().selection_update; - let keyboard_just_pressed = app - .world - .resource::() - .keyboard_just_pressed; - - app.world.spawn_io_workflow(build_place_object_3d_workflow( - setup, - find_position, - placement_chosen, - handle_key_code, - cleanup, - hover_service.optional_stream_cast(), - selection_update, - keyboard_just_pressed, - )) -} - -pub fn build_place_object_3d_workflow( - setup: Service, SelectionNodeResult, Select>, - find_placement: Service, Transform, Select>, - placement_chosen: Service<(Transform, BufferKey), SelectionNodeResult>, - handle_key_code: Service<(KeyCode, BufferKey), SelectionNodeResult, Select>, - cleanup: Service, SelectionNodeResult>, - // Used to manage highlighting prospective parent frames - hover_service: Service<(), ()>, - // Used to manage highlighting the current parent frame - selection_update: Service, - keyboard_just_pressed: Service<(), (), StreamOf>, -) -> impl FnOnce(Scope, ()>, &mut Builder) { - move |scope, builder| { - let buffer = builder.create_buffer::(BufferSettings::keep_last(1)); - let selection_update_node = builder.create_node(selection_update); - let setup_node = scope - .input - .chain(builder) - .then(extract_selector_input::.into_blocking_callback()) - .branch_for_err(|err| err.connect(scope.terminate)) - .cancel_on_none() - .then_push(buffer) - .then_access(buffer) - .then_node(setup); - - builder.connect(setup_node.streams, selection_update_node.input); - - let begin_input_services = setup_node - .output - .chain(builder) - .branch_for_err(|err| err.map_block(print_if_err).connect(scope.terminate)) - .output() - .fork_clone(builder); - - let find_placement_node = begin_input_services - .clone_chain(builder) - .then_access(buffer) - .then_node(find_placement); - - find_placement_node - .output - .chain(builder) - .with_access(buffer) - .then(placement_chosen) - .fork_result( - |ok| ok.connect(scope.terminate), - |err| err.map_block(print_if_err).connect(scope.terminate), - ); - - builder.connect(find_placement_node.streams, selection_update_node.input); - - begin_input_services - .clone_chain(builder) - .then(hover_service) - .connect(scope.terminate); - - let keyboard = begin_input_services - .clone_chain(builder) - .then_node(keyboard_just_pressed); - let handle_key_node = keyboard - .streams - .chain(builder) - .inner() - .with_access(buffer) - .then_node(handle_key_code); - - handle_key_node - .output - .chain(builder) - .dispose_on_ok() - .map_block(print_if_err) - .connect(scope.terminate); - - builder.connect(handle_key_node.streams, selection_update_node.input); - - builder.on_cleanup(buffer, move |scope, builder| { - scope.input.chain(builder).then(cleanup).fork_result( - |ok| ok.connect(scope.terminate), - |err| err.map_block(print_if_err).connect(scope.terminate), - ); - }); - } -} - -pub struct PlaceObject3d { - pub object: PlaceableObject, - pub parent: Option, - pub workspace: Entity, -} - -#[derive(Clone, Debug)] -pub enum PlaceableObject { - Model(Model), - Anchor, - VisualMesh(Model), - CollisionMesh(Model), -} - -pub fn place_object_3d_setup( - In(srv): BlockingServiceInput, Select>, - mut access: BufferAccessMut, - mut cursor: ResMut, - mut visibility: Query<&mut Visibility>, - mut commands: Commands, - mut highlight: ResMut, - mut filter: PlaceObject3dFilter, - mut gizmo_blockers: ResMut, -) -> SelectionNodeResult { - let mut access = access.get_mut(&srv.request).or_broken_buffer()?; - let state = access.newest_mut().or_broken_buffer()?; - - match &state.object { - PlaceableObject::Anchor => { - // Make the anchor placement component of the cursor visible - set_visibility(cursor.frame_placement, &mut visibility, true); - set_visibility(cursor.dagger, &mut visibility, true); - set_visibility(cursor.halo, &mut visibility, true); - } - PlaceableObject::Model(m) - | PlaceableObject::VisualMesh(m) - | PlaceableObject::CollisionMesh(m) => { - // Spawn the model as a child of the cursor - cursor.set_model_preview(&mut commands, Some(m.clone())); - set_visibility(cursor.dagger, &mut visibility, false); - set_visibility(cursor.halo, &mut visibility, false); - } - } - - if let Some(parent) = state.parent { - let parent = filter.filter_select(parent); - state.parent = parent; - } - srv.streams.send(Select::new(state.parent)); - - highlight.0 = true; - gizmo_blockers.selecting = true; - - cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - - Ok(()) -} - -pub fn place_object_3d_cleanup( - In(_): In>, - mut cursor: ResMut, - mut visibility: Query<&mut Visibility>, - mut commands: Commands, - mut highlight: ResMut, - mut gizmo_blockers: ResMut, -) -> SelectionNodeResult { - cursor.remove_preview(&mut commands); - cursor.remove_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - set_visibility(cursor.frame_placement, &mut visibility, false); - highlight.0 = false; - gizmo_blockers.selecting = false; - - Ok(()) -} - -pub fn place_object_3d_find_placement( - In(ContinuousService { key: srv_key }): ContinuousServiceInput< - BufferKey, - Transform, - Select, - >, - mut orders: ContinuousQuery, Transform, Select>, - mut buffer: BufferAccessMut, - mut cursor: ResMut, - raycast_sources: Query<&RaycastSource>, - mut transforms: Query<&mut Transform>, - intersect_ground_params: IntersectGroundPlaneParams, - mut visibility: Query<&mut Visibility>, - mut tooltips: ResMut, - keyboard_input: Res>, - mut hover: EventWriter, - hovering: Res, - mouse_button_input: Res>, - blockers: Option>, - meta: Query<(Option<&'static NameInWorkcell>, Option<&'static SiteID>)>, - mut filter: PlaceObject3dFilter, -) { - let Some(mut orders) = orders.get_mut(&srv_key) else { - return; - }; - - let Some(order) = orders.get_mut(0) else { - return; - }; - - let key = order.request(); - let Ok(mut buffer) = buffer.get_mut(key) else { - error!("Unable to retrieve buffer in place_object_3d_cursor_transform"); - return; - }; - let Some(state) = buffer.newest_mut() else { - error!("Missing state in place_object_3d_cursor_transform"); - return; - }; - - if state.parent.is_some() { - tooltips.add(Cow::Borrowed("Esc: deselect current parent")); - } - - let project_to_plane = keyboard_input.any_pressed([KeyCode::ShiftLeft, KeyCode::ShiftRight]); - - let mut transform = match transforms.get_mut(cursor.frame) { - Ok(transform) => transform, - Err(err) => { - error!("No cursor transform found: {err}"); - return; - } - }; - - let Ok(source) = raycast_sources.get_single() else { - return; - }; - - // Check if there is an intersection with a mesh - let mut intersection: Option = None; - let mut new_hover = None; - let mut select_new_parent = false; - if !project_to_plane { - for (e, i) in source.intersections() { - let Some(e) = filter.filter_pick(*e) else { - continue; - }; - - if let Some(parent) = state.parent { - if e == parent { - new_hover = Some(parent); - cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - tooltips.add(Cow::Borrowed("Click to place")); - tooltips.add(Cow::Borrowed("+Shift: Project to parent frame")); - - // Don't use the intersection with the parent if the parent - // is an anchor because that results in silly orientations - // which the user probably does not want. - if !filter.anchors.contains(e) { - intersection = Some( - Transform::from_translation(i.position()) - .with_rotation(aligned_z_axis(i.normal())), - ); - } - break; - } - } else { - new_hover = Some(e); - select_new_parent = true; - cursor.remove_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - tooltips.add(Cow::Borrowed("Click to set as parent")); - tooltips.add(Cow::Borrowed("+Shift: Project to ground plane")); - break; - } - } - } else { - cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - } - - if new_hover != hovering.0 { - hover.send(Hover(new_hover)); - } - - if !select_new_parent { - intersection = intersection.or_else(|| { - if let Some(parent) = state.parent { - tooltips.add(Cow::Borrowed("Click to place")); - cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - intersect_ground_params.frame_plane_intersection(parent) - } else { - tooltips.add(Cow::Borrowed("Click to place")); - cursor.add_mode(PLACE_OBJECT_3D_MODE_LABEL, &mut visibility); - intersect_ground_params.ground_plane_intersection() - } - }); - - if let Some(intersection) = intersection { - *transform = intersection; - } - } - - let clicked = mouse_button_input.just_pressed(MouseButton::Left); - let blocked = blockers.filter(|x| x.blocking()).is_some(); - if clicked && !blocked { - if select_new_parent { - if let Some(new_parent) = new_hover { - state.parent = Some(new_parent); - order.streams().send(Select::new(Some(new_parent))); - if let Ok((name, id)) = meta.get(new_parent) { - let id = id.map(|id| id.0.to_string()); - info!( - "Placing object in the frame of [{}], id: {}", - name.map(|name| name.0.as_str()).unwrap_or(""), - id.as_ref().map(|id| id.as_str()).unwrap_or("*"), - ); - } - } - } else { - if let Some(intersection) = intersection { - // The user is choosing a location to place the object. - order.respond(intersection); - } else { - warn!("Unable to find a placement position. Try adjusting your camera angle."); - } - } - } -} - -#[derive(SystemParam)] -pub struct PlaceObject3dFilter<'w, 's> { - inspect: InspectorFilter<'w, 's>, - ignore: Query<'w, 's, (), Or<(With, With)>>, - // We aren't using this in the filter functions, we're sneaking this query - // into this system param to skirt around the 16-parameter limit for - // place_object_3d_find_placement - anchors: Query<'w, 's, (), With>, -} - -impl<'w, 's> SelectionFilter for PlaceObject3dFilter<'w, 's> { - fn filter_pick(&mut self, target: Entity) -> Option { - let e = self.inspect.filter_pick(target); - - if let Some(e) = e { - if self.ignore.contains(e) { - return None; - } - } - e - } - - fn filter_select(&mut self, target: Entity) -> Option { - self.inspect.filter_select(target) - } - - fn on_click(&mut self, _: Hover) -> Option, -) { - let Some(mut orders) = orders.get_mut(&key) else { - selected.clear(); - return; - }; - - let Some(order) = orders.get_mut(0) else { - // Clear the selected reader so we don't mistake an earlier signal as - // being intended for this workflow. - selected.clear(); - return; - }; - - let object = *order.request(); - for s in selected.read() { - // Allow users to signal the choice of parent by means other than clicking - match s.0 { - Some(s) => { - if let Some(e) = filter.filter_pick(s.candidate) { - order.respond(Some(e)); - return; - } - - info!( - "Received parent replacement selection signal for an invalid parent candidate" - ); - } - None => { - // The user has sent a signal to remove the object from its parent - order.respond(None); - return; - } - } - } - - let Ok(source) = raycast_sources.get_single() else { - return; - }; - - let mut hovered: Option = None; - let mut ignore_click = false; - for (e, _) in source.intersections() { - let Some(e) = filter.filter_pick(*e) else { - continue; - }; - - if AncestorIter::new(&parents, e) - .filter(|e| *e == object) - .next() - .is_some() - { - ignore_click = true; - tooltips.add(Cow::Borrowed( - "Cannot select a child of the object to be its parent", - )); - break; - } - - if e == object { - ignore_click = true; - tooltips.add(Cow::Borrowed( - "Cannot select an object to be its own parent", - )); - break; - } - - hovered = Some(e); - } - - if hovered.is_some() { - tooltips.add(Cow::Borrowed("Click to select this as the parent")); - } else if !ignore_click { - tooltips.add(Cow::Borrowed("Click to remove parent")); - } - - if hovered != hovering.0 { - hover.send(Hover(hovered)); - } - - let clicked = mouse_button_input.just_pressed(MouseButton::Left); - let blocked = blockers.filter(|x| x.blocking()).is_some(); - if clicked && !blocked && !ignore_click { - order.respond(hovered); - } -} - -pub fn replace_parent_3d_parent_chosen( - In((parent, key)): In<(Option, BufferKey)>, - access: BufferAccess, - mut dependents: Query<&mut Dependents>, - mut poses: Query<&mut Pose>, - global_tfs: Query<&GlobalTransform>, - parents: Query<&Parent>, - frames: Query<(), With>, - mut commands: Commands, - mut anchors: Query<&mut Anchor>, -) -> SelectionNodeResult { - let access = access.get(&key).or_broken_buffer()?; - let state = access.newest().or_broken_state()?; - - let parent = parent - .and_then(|p| { - if frames.contains(p) { - Some(p) - } else { - // The selected parent is not a frame, so find its first ancestor - // that contains a FrameMarker - AncestorIter::new(&parents, p).find(|e| frames.contains(*e)) - } - }) - .unwrap_or(state.workspace); - - let previous_parent = parents.get(state.object).or_broken_query()?.get(); - if parent == previous_parent { - info!("Object's parent remains the same"); - return Ok(()); - } - - let object_tf = global_tfs.get(state.object).or_broken_query()?.affine(); - let inv_parent_tf = global_tfs.get(parent).or_broken_query()?.affine().inverse(); - let relative_pose: Pose = Transform::from_matrix((inv_parent_tf * object_tf).into()).into(); - - let [mut previous_deps, mut new_deps] = dependents - .get_many_mut([previous_parent, parent]) - .or_broken_query()?; - - if let Ok(mut pose_mut) = poses.get_mut(state.object) { - *pose_mut = relative_pose; - } else { - let mut anchor = anchors.get_mut(state.object).or_broken_query()?; - *anchor = Anchor::Pose3D(relative_pose); - } - - // Do all mutations after everything is successfully queried so we don't - // risk an inconsistent/broken world due to a query failing. - commands - .get_entity(state.object) - .or_broken_query()? - .set_parent(parent); - previous_deps.remove(&state.object); - new_deps.insert(state.object); - - Ok(()) -} - -pub fn on_keyboard_for_replace_parent_3d(In(code): In) -> SelectionNodeResult { - if matches!(code, KeyCode::Escape) { - // Simply exit the workflow if the user presses esc - return Err(None); - } - - Ok(()) -} diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index fc1f8541..ea80b534 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -1,3 +1,4 @@ +#[allow(warnings)] use bevy::{app::ScheduleRunnerPlugin, log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*}; use bevy_egui::EguiPlugin; use main_menu::MainMenuPlugin; @@ -34,9 +35,6 @@ use log::LogHistoryPlugin; pub mod main_menu; pub mod site; -// mod warehouse_generator; -pub mod workcell; -use workcell::WorkcellEditorPlugin; pub mod interaction; pub mod workspace; @@ -45,7 +43,6 @@ use workspace::*; pub mod sdf_loader; pub mod site_asset_io; -//pub mod urdf_loader; use sdf_loader::*; pub mod view_menu; @@ -87,7 +84,7 @@ pub enum AppState { MainMenu, SiteEditor, SiteVisualizer, - //WarehouseGenerator, + // TODO(luca) remove this WorkcellEditor, SiteDrawingEditor, } @@ -237,7 +234,6 @@ impl Plugin for SiteEditor { app.add_plugins(( StandardUiPlugin::default(), MainMenuPlugin, - WorkcellEditorPlugin, )) // Note order matters, plugins that edit the menus must be initialized after the UI .add_plugins((ViewMenuPlugin, OSMViewPlugin, SiteWireframePlugin)); diff --git a/rmf_site_editor/src/main_menu.rs b/rmf_site_editor/src/main_menu.rs index abc8502b..dc9ddaa2 100644 --- a/rmf_site_editor/src/main_menu.rs +++ b/rmf_site_editor/src/main_menu.rs @@ -67,21 +67,6 @@ fn egui_ui( if ui.button("Create new file").clicked() { workspace_loader.create_empty_from_dialog(); } - - // TODO(@mxgrey): Bring this back when we have finished developing - // the key features for workcell editing. - // if ui.button("Workcell Editor").clicked() { - // workspace_loader.send(LoadWorkspace::Data(WorkspaceData::Workcell( - // demo_workcell(), - // ))); - // } - - // TODO(@mxgrey): Bring this back when we have time to fix the - // warehouse generator. - // if ui.button("Warehouse generator").clicked() { - // info!("Entering warehouse generator"); - // _app_state.overwrite_set(AppState::WarehouseGenerator).unwrap(); - // } }); #[cfg(not(target_arch = "wasm32"))] diff --git a/rmf_site_editor/src/shapes.rs b/rmf_site_editor/src/shapes.rs index 1d173766..d5f993f6 100644 --- a/rmf_site_editor/src/shapes.rs +++ b/rmf_site_editor/src/shapes.rs @@ -23,7 +23,6 @@ use bevy::{ primitives::Aabb, }, }; -use bevy_infinite_grid::{InfiniteGridBundle, InfiniteGridSettings}; use bevy_mod_outline::ATTRIBUTE_OUTLINE_NORMAL; use bevy_mod_outline::{GenerateOutlineNormalsError, OutlineMeshExt}; use bevy_polyline::{material::PolylineMaterial, polyline::Polyline}; @@ -1241,28 +1240,6 @@ const Y_AXIS_COLOR: Color = Color::rgb(0.2, 1.0, 0.2); const NEG_X_AXIS_COLOR: Color = Color::rgb(0.5, 0.0, 0.0); const NEG_Y_AXIS_COLOR: Color = Color::rgb(0.0, 0.5, 0.0); -pub(crate) fn make_infinite_grid( - scale: f32, - fadeout_distance: f32, - shadow_color: Option, -) -> InfiniteGridBundle { - let mut settings = InfiniteGridSettings::default(); - // The upstream bevy_infinite_grid developers use an x-z plane grid but we - // use an x-y plane grid, so we need to make some tweaks. - settings.x_axis_color = X_AXIS_COLOR; - settings.z_axis_color = Y_AXIS_COLOR; - settings.fadeout_distance = fadeout_distance; - settings.shadow_color = shadow_color; - let transform = Transform::from_rotation(Quat::from_rotation_x(90_f32.to_radians())) - .with_scale(Vec3::splat(scale)); - - InfiniteGridBundle { - settings, - transform, - ..Default::default() - } -} - const POLYLINE_SEPARATOR: Vec3 = Vec3::splat(std::f32::NAN); pub(crate) fn make_finite_grid( diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs index f96d9090..ae30d7fc 100644 --- a/rmf_site_editor/src/site/drawing_editor/mod.rs +++ b/rmf_site_editor/src/site/drawing_editor/mod.rs @@ -24,11 +24,12 @@ use crate::AppState; use crate::{ interaction::{ChangeProjectionMode, Selection, SuppressHighlight}, site::{ - DrawingMarker, Edge, MeasurementMarker, NameOfSite, NameOfWorkcell, Pending, + DrawingMarker, Edge, MeasurementMarker, NameOfSite, Pending, PreventDeletion, }, CurrentWorkspace, WorkspaceMarker, }; +use rmf_workcell_format::NameOfWorkcell; #[derive(Clone, Copy, Event)] pub struct BeginEditDrawing(pub Entity); diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index f58e1730..aa67e34d 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -120,6 +120,7 @@ pub use wall::*; use crate::recency::{RecencyRank, RecencyRankingPlugin}; use crate::{AppState, RegisterIssueType}; pub use rmf_site_format::{DirectionalLight, PointLight, SpotLight, Style, *}; +use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell, JointProperties}; use bevy::{prelude::*, render::view::visibility::VisibilitySystems, transform::TransformSystem}; diff --git a/rmf_site_editor/src/view_menu.rs b/rmf_site_editor/src/view_menu.rs index a4586b16..12ec7534 100644 --- a/rmf_site_editor/src/view_menu.rs +++ b/rmf_site_editor/src/view_menu.rs @@ -21,7 +21,7 @@ use crate::site::{ LiftCabinDoorMarker, LocationTags, MeasurementMarker, VisualMeshMarker, WallMarker, }; use crate::widgets::menu_bar::{MenuEvent, MenuItem, MenuVisualizationStates, ViewMenu}; -use crate::workcell::WorkcellVisualizationMarker; +// use crate::workcell::WorkcellVisualizationMarker; use crate::AppState; use bevy::ecs::system::SystemParam; use bevy::prelude::*; @@ -40,7 +40,7 @@ struct VisibilityEvents<'w> { walls: EventWriter<'w, SetCategoryVisibility>, visuals: EventWriter<'w, SetCategoryVisibility>, collisions: EventWriter<'w, SetCategoryVisibility>, - origin_axis: EventWriter<'w, SetCategoryVisibility>, + // origin_axis: EventWriter<'w, SetCategoryVisibility>, } #[derive(Default)] @@ -58,7 +58,7 @@ pub struct ViewMenuItems { collisions: Entity, visuals: Entity, walls: Entity, - origin_axis: Entity, + // origin_axis: Entity, } impl FromWorld for ViewMenuItems { @@ -68,7 +68,6 @@ impl FromWorld for ViewMenuItems { AppState::SiteDrawingEditor, AppState::SiteVisualizer, ]); - let workcell_states = HashSet::from([AppState::WorkcellEditor]); let mut active_states = site_states.clone(); active_states.insert(AppState::WorkcellEditor); let view_header = world.resource::().get(); @@ -162,6 +161,7 @@ impl FromWorld for ViewMenuItems { .insert(MenuVisualizationStates(site_states)) .set_parent(view_header) .id(); + /* let default_visibility = world.resource::>(); let origin_axis = world @@ -172,6 +172,7 @@ impl FromWorld for ViewMenuItems { .insert(MenuVisualizationStates(workcell_states)) .set_parent(view_header) .id(); + */ ViewMenuItems { doors, @@ -184,7 +185,7 @@ impl FromWorld for ViewMenuItems { collisions, visuals, walls, - origin_axis, + // origin_axis, } } } @@ -224,8 +225,10 @@ fn handle_view_menu_events( events.visuals.send(toggle(event.source()).into()); } else if event.clicked() && event.source() == view_menu.walls { events.walls.send(toggle(event.source()).into()); + /* } else if event.clicked() && event.source() == view_menu.origin_axis { events.origin_axis.send(toggle(event.source()).into()); + */ } } } diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 60e5ff0b..286d2cec 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -17,7 +17,7 @@ use crate::{ inspector::{InspectAssetSourceComponent, InspectScaleComponent}, - interaction::{AnchorSelection, ObjectPlacement, PlaceableObject, Selection}, + interaction::{AnchorSelection, ObjectPlacement}, site::{ AssetSource, CurrentLevel, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale, }, @@ -53,7 +53,6 @@ struct Creation<'w, 's> { commands: Commands<'w, 's>, anchor_selection: AnchorSelection<'w, 's>, object_placement: ObjectPlacement<'w, 's>, - selection: Res<'w, Selection>, } impl<'w, 's> WidgetSystem for Creation<'w, 's> { @@ -145,9 +144,11 @@ impl<'w, 's> Creation<'w, 's> { } } AppState::WorkcellEditor => { + /* if ui.button("Frame").clicked() { self.place_object(PlaceableObject::Anchor); } + */ } } match self.app_state.get() { @@ -198,6 +199,7 @@ impl<'w, 's> Creation<'w, 's> { } } AppState::WorkcellEditor => { + /* if ui.button("Browse fuel").clicked() { asset_gallery.show = true; } @@ -220,6 +222,7 @@ impl<'w, 's> Creation<'w, 's> { )); } ui.add_space(10.0); + */ } } } @@ -229,15 +232,6 @@ impl<'w, 's> Creation<'w, 's> { }); } - pub fn place_object(&mut self, object: PlaceableObject) { - if let Some(workspace) = self.current_workspace.root { - self.object_placement - .place_object_3d(object, self.selection.0, workspace); - } else { - warn!("Unable to create [{object:?}] outside of a workspace"); - } - } - pub fn spawn_model_2d(&mut self, object: Model) { if let Some(level) = self.current_level.0 { self.object_placement.place_object_2d(object, level); diff --git a/rmf_site_editor/src/widgets/fuel_asset_browser.rs b/rmf_site_editor/src/widgets/fuel_asset_browser.rs index 29c93a86..dad3f826 100644 --- a/rmf_site_editor/src/widgets/fuel_asset_browser.rs +++ b/rmf_site_editor/src/widgets/fuel_asset_browser.rs @@ -16,13 +16,13 @@ */ use crate::{ - interaction::{ModelPreviewCamera, ObjectPlacement, PlaceableObject, Selection}, + interaction::{ModelPreviewCamera, ObjectPlacement}, site::{ AssetSource, CurrentLevel, FuelClient, Model, ModelSpawningExt, SetFuelApiKey, UpdateFuelCache, }, widgets::prelude::*, - AppState, CurrentWorkspace, + AppState, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{self, Button, ComboBox, ImageSource, RichText, ScrollArea, Ui, Window}; @@ -82,8 +82,6 @@ pub struct FuelAssetBrowser<'w, 's> { set_api_key: EventWriter<'w, SetFuelApiKey>, commands: Commands<'w, 's>, place_object: ObjectPlacement<'w, 's>, - current_workspace: Res<'w, CurrentWorkspace>, - current_selection: Res<'w, Selection>, current_level: Res<'w, CurrentLevel>, app_state: Res<'w, State>, } @@ -294,6 +292,7 @@ impl<'w, 's> FuelAssetBrowser<'w, 's> { } } AppState::WorkcellEditor => { + /* if let Some(workspace) = self.current_workspace.root { self.place_object.place_object_3d( PlaceableObject::Model(model), @@ -303,6 +302,7 @@ impl<'w, 's> FuelAssetBrowser<'w, 's> { } else { warn!("Cannot spawn a model outside of a workspace"); } + */ } _ => { warn!("Invalid mode for spawning a model: {:?}", &self.app_state); diff --git a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs index 4c56b707..01cb1ec0 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs @@ -17,14 +17,17 @@ use crate::{ interaction::{Hover, MoveTo}, - site::{Anchor, Category, Dependents, JointProperties, Subordinate}, + site::{Anchor, Category, Dependents, Subordinate}, widgets::{ inspector::{Inspect, InspectPoseComponent}, prelude::*, Icons, SelectorWidget, }, - workcell::CreateJoint, + // TODO(luca) restore create joint feature by (temporarily) adding it to rmf_workcell_format? + // otherwise consider creating a new widget for anchor inspection + // workcell::CreateJoint, }; +use rmf_workcell_format::JointProperties; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{DragValue, ImageButton, Ui}; use std::collections::{BTreeMap, BTreeSet}; @@ -45,7 +48,8 @@ pub struct InspectAnchor<'w, 's> { joints: Query<'w, 's, Entity, With>, hover: EventWriter<'w, Hover>, move_to: EventWriter<'w, MoveTo>, - create_joint: EventWriter<'w, CreateJoint>, + // TODO(luca) restore + // create_joint: EventWriter<'w, CreateJoint>, } impl<'w, 's> ShareableWidget for InspectAnchor<'w, 's> {} @@ -177,10 +181,13 @@ fn impl_inspect_anchor( // If the parent is not a joint, add a joint creation widget if params.joints.get(parent.get()).is_err() { if ui.button("Create joint").on_hover_text("Create a fixed joint and place it between the parent frame and this frame").clicked() { + // TODO(luca) restore + /* params.create_joint.send(CreateJoint { parent: parent.get(), child: id, }); + */ } } }); diff --git a/rmf_site_editor/src/widgets/inspector/inspect_joint.rs b/rmf_site_editor/src/widgets/inspector/inspect_joint.rs index 8f3686eb..f1e7bb6d 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_joint.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_joint.rs @@ -16,9 +16,10 @@ */ use crate::{ - site::{Dependents, FrameMarker, JointProperties}, + site::Dependents, widgets::{prelude::*, Inspect, SelectorWidget}, }; +use rmf_workcell_format::{FrameMarker, JointProperties}; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::Ui; diff --git a/rmf_site_editor/src/widgets/inspector/inspect_name.rs b/rmf_site_editor/src/widgets/inspector/inspect_name.rs index 3b5af089..dca60646 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_name.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_name.rs @@ -21,7 +21,8 @@ use crate::{ }; use bevy::prelude::*; use bevy_egui::egui::Ui; -use rmf_site_format::{NameInSite, NameInWorkcell, NameOfWorkcell}; +use rmf_site_format::NameInSite; +use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell}; #[derive(SystemParam)] pub struct InspectName<'w, 's> { diff --git a/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs b/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs index 6bd0f40e..734e8144 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs @@ -17,10 +17,10 @@ use crate::{ interaction::{Hover, ObjectPlacement}, - site::{FrameMarker, NameInWorkcell, NameOfWorkcell}, widgets::{prelude::*, Icons, Inspect, SelectorWidget}, CurrentWorkspace, }; +use rmf_workcell_format::{FrameMarker, NameInWorkcell, NameOfWorkcell}; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{ImageButton, Ui}; @@ -75,11 +75,13 @@ impl<'w, 's> InspectWorkcellParent<'w, 's> { assign_response.on_hover_text("Reassign"); if parent_replace { + /* if let Some(workspace) = self.current_workspace.root { self.object_placement.replace_parent_3d(id, workspace) } else { warn!("Cannot replace a parent when no workspace is active"); } + */ } }); } diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index cb254d65..a616f83f 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -105,8 +105,8 @@ pub use inspect_texture::*; pub mod inspect_value; pub use inspect_value::*; -pub mod inspect_workcell_parent; -pub use inspect_workcell_parent::*; +//pub mod inspect_workcell_parent; +//pub use inspect_workcell_parent::*; use crate::{ interaction::Selection, @@ -199,7 +199,7 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectionPlugin::::new(), + //InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), diff --git a/rmf_site_editor/src/workcell/frame.rs b/rmf_site_editor/src/workcell/frame.rs deleted file mode 100644 index f9de49a2..00000000 --- a/rmf_site_editor/src/workcell/frame.rs +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2024 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::AnchorVisualization; -use bevy::prelude::*; -use rmf_site_format::FrameMarker; - -// TODO(luca) We should probably have a different mesh altogether for workcell anchors, rather than -// a scaled down version of site anchors. -pub fn scale_workcell_anchors( - new_frames: Query<&AnchorVisualization, With>, - mut transforms: Query<&mut Transform>, -) { - for frame in new_frames.iter() { - if let Ok(mut tf) = transforms.get_mut(frame.body) { - tf.scale = Vec3::splat(0.25); - } - } -} diff --git a/rmf_site_editor/src/workcell/joint.rs b/rmf_site_editor/src/workcell/joint.rs deleted file mode 100644 index e205b267..00000000 --- a/rmf_site_editor/src/workcell/joint.rs +++ /dev/null @@ -1,83 +0,0 @@ -/* - * 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::{Delete, Dependents}; -use bevy::prelude::*; -use rmf_site_format::{FrameMarker, Joint, JointProperties, NameInWorkcell}; - -/// Event used to request the creation of a joint between a parent and a child frame -#[derive(Event)] -pub struct CreateJoint { - pub parent: Entity, - pub child: Entity, - // TODO(luca) Add different properties here such as JointType -} - -pub fn handle_create_joint_events( - mut commands: Commands, - mut events: EventReader, - mut dependents: Query<&mut Dependents>, - frames: Query<&NameInWorkcell, With>, -) { - for req in events.read() { - let Ok(parent_name) = frames.get(req.parent) else { - error!( - "Requested to create a joint with a parent that is not a frame, \ - this is not valid and will be ignored" - ); - continue; - }; - let Ok(child_name) = frames.get(req.child) else { - error!( - "Requested to create a joint with a child that is not a frame, \ - this is not valid and will be ignored" - ); - continue; - }; - let joint_name = if !parent_name.is_empty() && !child_name.is_empty() { - format!("joint-{}-{}", **parent_name, **child_name) - } else { - "new_joint".into() - }; - let joint = Joint { - name: NameInWorkcell(joint_name), - properties: JointProperties::Fixed, - }; - let mut cmd = commands.spawn(Dependents::single(req.child)); - let joint_id = cmd.id(); - joint.add_bevy_components(&mut cmd); - // Now place the joint between the parent and child in the hierarchy - commands.entity(req.child).set_parent(joint_id); - commands.entity(joint_id).set_parent(req.parent); - if let Ok(mut deps) = dependents.get_mut(req.parent) { - deps.remove(&req.child); - deps.insert(joint_id); - } - } -} - -/// This system cleans up joints which don't have a child anymore because it was despawned -pub fn cleanup_orphaned_joints( - changed_joints: Query<(Entity, &Children), (Changed, With)>, - mut delete: EventWriter, -) { - for (e, children) in &changed_joints { - if children.is_empty() { - delete.send(Delete::new(e)); - } - } -} diff --git a/rmf_site_editor/src/workcell/keyboard.rs b/rmf_site_editor/src/workcell/keyboard.rs deleted file mode 100644 index 0832f076..00000000 --- a/rmf_site_editor/src/workcell/keyboard.rs +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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::{ExportFormat, SaveWorkspace, SaveWorkspaceDestination}; -use bevy::{prelude::*, window::PrimaryWindow}; -use bevy_egui::EguiContexts; - -pub fn handle_workcell_keyboard_input( - keyboard_input: Res>, - mut egui_context: EguiContexts, - mut save_events: EventWriter, - primary_windows: Query>, -) { - let Some(egui_context) = primary_windows - .get_single() - .ok() - .and_then(|w| egui_context.try_ctx_for_window_mut(w)) - else { - return; - }; - - 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::ControlLeft, KeyCode::ControlRight]) { - if keyboard_input.just_pressed(KeyCode::E) { - save_events.send(SaveWorkspace { - destination: SaveWorkspaceDestination::Dialog, - format: ExportFormat::Urdf, - }); - } - } -} diff --git a/rmf_site_editor/src/workcell/load.rs b/rmf_site_editor/src/workcell/load.rs deleted file mode 100644 index 3bba1099..00000000 --- a/rmf_site_editor/src/workcell/load.rs +++ /dev/null @@ -1,176 +0,0 @@ -/* - * 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, CollisionMeshMarker, DefaultFile, Dependents, ModelSpawningExt, - PreventDeletion, VisualMeshMarker, - }, - workcell::ChangeCurrentWorkcell, - WorkspaceMarker, -}; -use bevy::prelude::*; -use std::collections::HashSet; - -use rmf_site_format::{ - Category, FrameMarker, Geometry, ModelMarker, NameInWorkcell, Parented, Scale, SiteID, - Workcell, WorkcellModel, -}; - -#[derive(Event)] -pub struct LoadWorkcell { - /// The site data to load - pub workcell: 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: &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(); - - let root = commands - .spawn(SpatialBundle::INHERITED_IDENTITY) - .insert(workcell.properties.clone()) - .insert(SiteID(workcell.id)) - .insert(Category::Workcell) - .insert(WorkspaceMarker) - .insert(PreventDeletion::because( - "Workcell root cannot be deleted".to_string(), - )) - .id(); - id_to_entity.insert(workcell.id, root); - - let mut add_model = - |parented: &Parented, id: u32, e: Entity, commands: &mut Commands| { - match &parented.bundle.geometry { - Geometry::Primitive(primitive) => { - commands.entity(e).insert(( - primitive.clone(), - parented.bundle.pose.clone(), - NameInWorkcell(parented.bundle.name.clone()), - )); - } - Geometry::Mesh { source, scale } => { - commands.entity(e).insert(( - NameInWorkcell(parented.bundle.name.clone()), - parented.bundle.pose.clone(), - Scale(scale.unwrap_or(Vec3::ONE)), - ModelMarker, - )); - commands.spawn_model((e, source.clone()).into()); - } - }; - commands.entity(e).insert(SiteID(id)); - let child_entities: &mut Vec = - parent_to_child_entities.entry(parented.parent).or_default(); - child_entities.push(e); - id_to_entity.insert(id, e); - }; - - for (id, visual) in &workcell.visuals { - let e = commands.spawn((VisualMeshMarker, Category::Visual)).id(); - add_model(visual, *id, e, commands); - } - - for (id, collision) in &workcell.collisions { - let e = commands - .spawn((CollisionMeshMarker, Category::Collision)) - .id(); - add_model(collision, *id, e, commands); - } - - for (id, parented_anchor) in &workcell.frames { - let e = commands - .spawn(AnchorBundle::new(parented_anchor.bundle.anchor.clone()).visible(true)) - .insert(SiteID(*id)) - .insert(FrameMarker) - .id(); - 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); - } - - for (id, parented_inertia) in &workcell.inertias { - let e = commands - .spawn(SpatialBundle::INHERITED_IDENTITY) - .insert(parented_inertia.bundle.clone()) - .insert(Category::Inertia) - .insert(SiteID(*id)) - .id(); - let child_entities: &mut Vec = parent_to_child_entities - .entry(parented_inertia.parent) - .or_default(); - child_entities.push(e); - id_to_entity.insert(*id, e); - } - - for (id, parented_joint) in &workcell.joints { - let joint = &parented_joint.bundle; - let mut cmd = commands.spawn(SiteID(*id)); - let e = cmd.id(); - joint.add_bevy_components(&mut cmd); - let child_entities: &mut Vec = parent_to_child_entities - .entry(parented_joint.parent) - .or_default(); - child_entities.push(e); - id_to_entity.insert(*id, e); - } - - 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 { - error!("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, -) { - for cmd in load_workcells.read() { - info!("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 }); - } - } -} diff --git a/rmf_site_editor/src/workcell/menu.rs b/rmf_site_editor/src/workcell/menu.rs deleted file mode 100644 index cbf7370e..00000000 --- a/rmf_site_editor/src/workcell/menu.rs +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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::menu_bar::{FileMenu, MenuEvent, MenuItem, MenuVisualizationStates}; -use crate::{AppState, ExportFormat, SaveWorkspace, SaveWorkspaceDestination}; -use bevy::prelude::*; -use std::collections::HashSet; - -/// Keeps track of which entity is associated to the export urdf button. -#[derive(Resource)] -pub struct ExportUrdfMenu { - export_urdf: Entity, -} - -impl FromWorld for ExportUrdfMenu { - fn from_world(world: &mut World) -> Self { - let workcell_states = HashSet::from([AppState::WorkcellEditor]); - // TODO(luca) add shortcut text for Ctrl-E - let file_header = world.resource::().get(); - let export_urdf = world - .spawn(( - MenuItem::Text("Export Urdf".to_string()), - MenuVisualizationStates(workcell_states), - )) - .set_parent(file_header) - .id(); - - ExportUrdfMenu { export_urdf } - } -} - -pub fn handle_export_urdf_menu_events( - mut menu_events: EventReader, - urdf_menu: Res, - mut save_events: EventWriter, -) { - for event in menu_events.read() { - if event.clicked() && event.source() == urdf_menu.export_urdf { - save_events.send(SaveWorkspace { - destination: SaveWorkspaceDestination::Dialog, - format: ExportFormat::Urdf, - }); - } - } -} diff --git a/rmf_site_editor/src/workcell/mod.rs b/rmf_site_editor/src/workcell/mod.rs deleted file mode 100644 index 2b4d5c0d..00000000 --- a/rmf_site_editor/src/workcell/mod.rs +++ /dev/null @@ -1,113 +0,0 @@ -/* - * 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 frame; -pub use frame::*; - -pub mod joint; -pub use joint::*; - -pub mod load; -pub use load::*; - -pub mod keyboard; -pub use keyboard::*; - -pub mod menu; -pub use menu::*; - -pub mod model; -pub use model::*; - -pub mod save; -pub use save::*; - -pub mod urdf_package_exporter; - -pub mod workcell; -pub use workcell::*; - -use bevy::prelude::*; -use bevy_infinite_grid::{InfiniteGrid, InfiniteGridPlugin}; - -use crate::AppState; -use crate::{ - shapes::make_infinite_grid, - site::{ - handle_new_primitive_shapes, handle_update_fuel_cache_requests, - read_update_fuel_cache_results, reload_failed_models_with_new_api_key, - update_anchor_transforms, update_model_scales, update_transforms_for_changed_poses, - }, -}; - -#[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(); - } -} - -impl Plugin for WorkcellEditorPlugin { - fn build(&self, app: &mut App) { - app.add_plugins(InfiniteGridPlugin) - .add_event::() - .add_event::() - .add_systems(OnEnter(AppState::WorkcellEditor), spawn_grid) - .add_systems(OnExit(AppState::WorkcellEditor), delete_grid) - .add_systems( - Update, - ( - update_model_scales, - handle_new_primitive_shapes, - handle_create_joint_events, - cleanup_orphaned_joints, - handle_update_fuel_cache_requests, - read_update_fuel_cache_results, - reload_failed_models_with_new_api_key, - handle_workcell_keyboard_input, - change_workcell.before(load_workcell), - handle_export_urdf_menu_events, - ) - .run_if(in_state(AppState::WorkcellEditor)), - ) - .add_systems( - Update, - (load_workcell, save_workcell, add_workcell_visualization), - ) - // TODO(luca) restore doing this before transform propagation - .add_systems( - Update, - ( - scale_workcell_anchors, - update_anchor_transforms, - update_transforms_for_changed_poses, - ) - .run_if(in_state(AppState::WorkcellEditor)), - ); - } - - // Put the UI dependent plugins in `finish` to make sure the interaction is initialized first - fn finish(&self, app: &mut App) { - app.init_resource::(); - } -} diff --git a/rmf_site_editor/src/workcell/model.rs b/rmf_site_editor/src/workcell/model.rs deleted file mode 100644 index 41bf2a16..00000000 --- a/rmf_site_editor/src/workcell/model.rs +++ /dev/null @@ -1,102 +0,0 @@ -/* - * 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::{Preview, Selectable, VisualCue}, - site::Dependents, -}; -use bevy::prelude::*; -use rmf_site_format::{ModelMarker, NameInSite, NameInWorkcell, Pose, PrimitiveShape}; - -/// SDFs loaded through site editor wrap all the collisions and visuals into a single Model entity. -/// This doesn't quite work for URDF / workcells since we need to export and edit single visuals -/// and collisions, hence we process the loaded models to flatten them here -pub fn flatten_loaded_model_hierarchy( - In(old_parent): In, - mut commands: Commands, - cues: Query<&VisualCue>, - previews: Query<&Preview>, - mut poses: Query<&mut Pose>, - mut dependents: Query<&mut Dependents>, - parents: Query<&Parent>, - children: Query<&Children>, - meshes: Query<(), With>>, - models: Query<(), Or<(With, With)>>, - site_names: Query<&NameInSite>, -) { - let Ok(new_parent) = parents.get(old_parent) else { - warn!( - "Failed flattening model hierarchy, model {:?} has no parent", - old_parent - ); - return; - }; - let Ok(parent_pose) = poses.get(old_parent).cloned() else { - return; - }; - for c in DescendantIter::new(&children, old_parent) { - if meshes.get(c).is_ok() { - // Set its selectable to the first parent model, or to itself if none is found - let mut parent_found = false; - for p in AncestorIter::new(&parents, c) { - if models.get(p).is_ok() { - commands.entity(c).insert(Selectable::new(p)); - parent_found = true; - break; - } - } - if !parent_found { - commands.entity(c).insert(Selectable::new(c)); - } - } - // Change site names to workcell names - if let Ok(name) = site_names.get(c) { - commands - .entity(c) - .insert(NameInWorkcell(name.0.clone())) - .remove::(); - } - let Ok(mut child_pose) = poses.get_mut(c) else { - continue; - }; - commands.entity(**new_parent).add_child(c); - if let Ok(mut deps) = dependents.get_mut(**new_parent) { - deps.insert(c); - } - let tf_child = child_pose.transform(); - let tf_parent = parent_pose.transform(); - *child_pose = (tf_parent * tf_child).into(); - - // Note: This is wiping out properties that we might try to apply to the - // original model entity. Because of this, we need to manually push those - // properties (e.g. VisualCue, Preview) along to the flattened entities. - // This might not scale well in the long run. - let mut c_mut = commands.entity(c); - if let Ok(cue) = cues.get(old_parent) { - c_mut.insert(cue.clone()); - } - if let Ok(preview) = previews.get(old_parent) { - c_mut.insert(preview.clone()); - } - } - if let Ok(mut parent_dependents) = dependents.get_mut(**new_parent) { - parent_dependents.remove(&old_parent); - } - - // Now despawn the unnecessary model - commands.entity(old_parent).despawn_recursive(); -} diff --git a/rmf_site_editor/src/workcell/save.rs b/rmf_site_editor/src/workcell/save.rs deleted file mode 100644 index d0e11222..00000000 --- a/rmf_site_editor/src/workcell/save.rs +++ /dev/null @@ -1,342 +0,0 @@ -/* - * 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::{CollisionMeshMarker, Pending, VisualMeshMarker}; -use crate::workcell::urdf_package_exporter::{generate_package, PackageContext, Person}; -use crate::ExportFormat; - -use thiserror::Error as ThisError; - -use rmf_site_format::*; - -/// Event used to trigger saving of the workcell -#[derive(Event)] -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, - 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), Without>, - Query<(Entity, &Pose, &Mass, &Moment, &SiteID, &Parent), Without>, - Query< - ( - Entity, - &NameInWorkcell, - Option<&AssetSource>, - Option<&PrimitiveShape>, - &Pose, - &SiteID, - &Parent, - Option<&Scale>, - ), - ( - Or<(With, With)>, - Without, - ), - >, - Query<(Entity, &JointProperties, &NameInWorkcell, &SiteID, &Parent), Without>, - Query<&VisualMeshMarker>, - Query<&CollisionMeshMarker>, - Query<&SiteID>, - Query<&NameOfWorkcell>, - Query<&Parent>, - )> = SystemState::new(world); - let ( - q_anchors, - q_inertials, - q_models, - q_joints, - 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(name) => workcell.properties.name = name.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(_) => { - error!("Parent not found for visual {:?}", parent.get()); - continue; - } - }; - let geom = if let Some(source) = source { - // It's a model - Geometry::Mesh { - source: source.clone(), - scale: scale.map(|s| **s), - } - } else if let Some(primitive) = primitive { - Geometry::Primitive(primitive.clone()) - } else { - error!("DEV Error, visual without primitive or mesh"); - continue; - }; - if q_visuals.get(e).is_ok() { - workcell.visuals.insert( - id.0, - Parented { - 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, - bundle: WorkcellModel { - name: name.0.clone(), - geometry: geom, - pose: pose.clone(), - }, - }, - ); - } - } - - // Anchors - for (e, anchor, name, id, parent) 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(_) => { - error!("Parent not found for anchor {:?}", parent.get()); - continue; - } - }; - workcell.frames.insert( - id.0, - Parented { - parent, - bundle: Frame { - anchor: anchor.clone(), - name: name.cloned(), - marker: FrameMarker, - }, - }, - ); - } - - for (e, pose, mass, moment, id, parent) in &q_inertials { - if !parent_in_workcell(&q_parents, e, root) { - continue; - } - let parent = match q_site_id.get(parent.get()) { - Ok(parent) => parent.0, - Err(_) => { - error!("Parent not found for inertial {:?}", parent.get()); - continue; - } - }; - - workcell.inertias.insert( - id.0, - Parented { - parent, - bundle: Inertia { - center: pose.clone(), - mass: mass.clone(), - moment: moment.clone(), - }, - }, - ); - } - - for (e, properties, name, id, parent) in &q_joints { - if !parent_in_workcell(&q_parents, e, root) { - continue; - } - let parent = match q_site_id.get(parent.get()) { - Ok(parent) => parent.0, - Err(_) => { - error!("Parent not found for joint {:?}", parent.get()); - continue; - } - }; - - workcell.joints.insert( - id.0, - Parented { - parent, - bundle: Joint { - name: name.clone(), - properties: properties.clone(), - }, - }, - ); - } - - 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 workcell = match generate_workcell(world, save_event.root) { - Ok(root) => root, - Err(err) => { - error!("Unable to compile workcell: {err}"); - continue; - } - }; - - let path = save_event.to_file; - match save_event.format { - ExportFormat::Default => { - info!( - "Saving to {}", - path.to_str().unwrap_or("") - ); - let f = match std::fs::File::create(path) { - Ok(f) => f, - Err(err) => { - error!("Unable to save file: {err}"); - continue; - } - }; - match workcell.to_writer(f) { - Ok(()) => { - info!("Save successful"); - } - Err(err) => { - error!("Save failed: {err}"); - } - } - } - ExportFormat::Urdf => { - // TODO(luca) File name is ignored, change to pick folder instead of pick file - match export_package(&path, workcell) { - Ok(()) => { - info!("Successfully exported package"); - } - Err(err) => { - error!("Failed to export package: {err}"); - } - }; - } - ExportFormat::Sdf => { - warn!("Exporting workcells to sdf is not supported."); - } - } - } -} - -fn export_package(path: &PathBuf, workcell: Workcell) -> Result<(), Box> { - let output_directory = path.parent().ok_or("Not able to get parent")?; - - let package_context = PackageContext { - license: "TODO".to_string(), - maintainers: vec![Person { - name: "TODO".to_string(), - email: "todo@todo.com".to_string(), - }], - project_name: workcell.properties.name.0.clone() + "_description", - fixed_frame: "world".to_string(), - dependencies: vec![], - project_description: "TODO".to_string(), - project_version: "0.0.1".to_string(), - urdf_file_name: "robot.urdf".to_string(), - }; - - generate_package(workcell, package_context, &output_directory)?; - Ok(()) -} diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/generate_package.rs b/rmf_site_editor/src/workcell/urdf_package_exporter/generate_package.rs deleted file mode 100644 index e9ea3fa1..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/generate_package.rs +++ /dev/null @@ -1,136 +0,0 @@ -use crate::site_asset_io::cache_path; -use crate::workcell::urdf_package_exporter::template::PackageContext; -use rmf_site_format::{AssetSource, Geometry, Workcell}; -use std::error::Error; -use std::io::{Error as IoError, ErrorKind as IoErrorKind}; -use std::path::{Path, PathBuf}; -use tera::Tera; - -pub fn generate_package( - workcell: Workcell, - package_context: PackageContext, - output_directory_path: &Path, -) -> Result<(), Box> { - let new_package_name = &package_context.project_name; - - let output_package_path = output_directory_path.join(new_package_name); - std::fs::create_dir_all(&output_package_path)?; - - // Create the package - write_urdf_and_copy_mesh_files(workcell, &new_package_name, &output_package_path)?; - generate_templates(package_context, &output_package_path)?; - - Ok(()) -} - -fn write_urdf_and_copy_mesh_files( - mut workcell: Workcell, - new_package_name: &str, - output_package_path: &Path, -) -> Result<(), Box> { - convert_and_copy_meshes(&mut workcell, new_package_name, output_package_path)?; - - let urdf_robot = workcell.to_urdf()?; - let urdf_directory_path = output_package_path.join("urdf"); - std::fs::create_dir_all(&urdf_directory_path)?; - let urdf_file_path = urdf_directory_path.join("robot.urdf"); - let urdf_string = urdf_rs::write_to_string(&urdf_robot)?; - std::fs::write(urdf_file_path, urdf_string)?; - - Ok(()) -} - -fn convert_and_copy_meshes( - workcell: &mut Workcell, - package_name: &str, - output_package_path: &Path, -) -> Result<(), Box> { - let meshes_directory_path = output_package_path.join("meshes"); - std::fs::create_dir_all(&meshes_directory_path)?; - for (_, model) in &mut workcell - .visuals - .iter_mut() - .chain(workcell.collisions.iter_mut()) - { - if let Geometry::Mesh { - source: asset_source, - .. - } = &mut model.bundle.geometry - { - let path = get_path_to_asset_file(&asset_source)?; - - let file_name = path - .file_name() - .ok_or(IoError::new( - IoErrorKind::InvalidInput, - "Unable to get file name from path", - ))? - .to_str() - .ok_or(IoError::new( - IoErrorKind::InvalidInput, - "Unable to convert file name to str", - ))?; - - std::fs::copy(&path, &meshes_directory_path.join(&file_name))?; - let package_path = format!("{}/meshes/{}", package_name, file_name); - *asset_source = AssetSource::Package(package_path); - } - } - Ok(()) -} - -fn get_path_to_asset_file(asset_source: &AssetSource) -> Result> { - match asset_source { - AssetSource::Package(_) => Ok(urdf_rs::utils::expand_package_path( - &(String::try_from(asset_source)?), - None, - ) - .to_string() - .into()), - AssetSource::Remote(asset_name) => { - let mut asset_path = cache_path(); - asset_path.push(&asset_name); - Ok(asset_path) - } - AssetSource::Local(path) => Ok(path.into()), - AssetSource::Search(_) => Err(IoError::new( - IoErrorKind::Unsupported, - "Not a supported asset type for exporting a workcell to a package", - ))?, - } -} - -fn generate_templates( - package_context: PackageContext, - package_directory: &Path, -) -> Result<(), Box> { - let context = tera::Context::from_serialize(package_context)?; - let mut tera = Tera::default(); - tera.add_raw_template("package.xml", include_str!("templates/package.xml.j2"))?; - tera.add_raw_template( - "CMakeLists.txt", - include_str!("templates/CMakeLists.txt.j2"), - )?; - tera.add_raw_template("urdf.rviz", include_str!("templates/urdf.rviz.j2"))?; - tera.add_raw_template( - "display.launch.py", - include_str!("templates/display.launch.py.j2"), - )?; - let f = std::fs::File::create(package_directory.join("package.xml"))?; - tera.render_to("package.xml", &context, f)?; - - let f = std::fs::File::create(package_directory.join("CMakeLists.txt"))?; - tera.render_to("CMakeLists.txt", &context, f)?; - - let rviz_directory = package_directory.join("rviz"); - std::fs::create_dir_all(&rviz_directory)?; - let f = std::fs::File::create(rviz_directory.join("urdf.rviz"))?; - tera.render_to("urdf.rviz", &context, f)?; - - let launch_directory = package_directory.join("launch"); - std::fs::create_dir_all(&launch_directory)?; - let f = std::fs::File::create(launch_directory.join("display.launch.py"))?; - tera.render_to("display.launch.py", &context, f)?; - - Ok(()) -} diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/mod.rs b/rmf_site_editor/src/workcell/urdf_package_exporter/mod.rs deleted file mode 100644 index 5d3e09b3..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/mod.rs +++ /dev/null @@ -1,5 +0,0 @@ -pub mod generate_package; -pub use generate_package::generate_package; - -pub use template::{PackageContext, Person}; -pub mod template; diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/template.rs b/rmf_site_editor/src/workcell/urdf_package_exporter/template.rs deleted file mode 100644 index f84eca0a..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/template.rs +++ /dev/null @@ -1,27 +0,0 @@ -use serde::Serialize; -use std::path::PathBuf; - -#[derive(Debug, Serialize)] -pub struct PackageContext { - pub project_name: String, - pub project_description: String, - pub project_version: String, - pub license: String, - pub maintainers: Vec, - pub dependencies: Vec, - pub fixed_frame: String, - pub urdf_file_name: String, -} - -#[derive(Debug, Serialize)] -pub struct Person { - pub name: String, - pub email: String, -} - -#[derive(Debug)] -pub struct Template { - pub name: String, - pub path: String, - pub output_path: PathBuf, -} diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 b/rmf_site_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 deleted file mode 100644 index 063522fb..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.16) - -project({{project_name}}) - -find_package(ament_cmake REQUIRED) -{%- for dependency in dependencies %} -find_package({{dependency}} REQUIRED) -{%- endfor %} - -install( - DIRECTORY launch meshes rviz urdf - DESTINATION share/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 b/rmf_site_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 deleted file mode 100644 index 8e8c9efd..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 +++ /dev/null @@ -1,31 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(): - ld = LaunchDescription() - - package_path = FindPackageShare('{{project_name}}') - default_rviz_config_path = PathJoinSubstitution([package_path, 'rviz', 'urdf.rviz']) - - gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'], - description='Flag to enable joint_state_publisher_gui') - ld.add_action(gui_arg) - - rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, - description='Absolute path to rviz config file') - ld.add_action(rviz_arg) - - ld.add_action(IncludeLaunchDescription( - PathJoinSubstitution( - [FindPackageShare('urdf_launch'), 'launch', 'display.launch.py']), - launch_arguments={ - 'urdf_package': '{{project_name}}', - 'urdf_package_path': 'urdf/{{urdf_file_name}}', - 'rviz_config': LaunchConfiguration('rvizconfig'), - 'jsp_gui': LaunchConfiguration('gui'), - }.items() - )) - - return ld diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 b/rmf_site_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 deleted file mode 100644 index 737b2792..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 +++ /dev/null @@ -1,25 +0,0 @@ - - - - {{project_name}} - {{project_version}} - {{project_description}} - - {%- for maintainer in maintainers %} - {{maintainer.name}} - {%- endfor %} - - {{license}} - - ament_cmake - urdf_launch - ament_lint_auto - {%- for dependency in dependencies %} - {{dependency}} - {%- endfor %} - - - ament_cmake - - - diff --git a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 b/rmf_site_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 deleted file mode 100644 index bb8cc092..00000000 --- a/rmf_site_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 +++ /dev/null @@ -1,35 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Name: Displays - - Class: rviz_common/Views - Name: Views -Visualization Manager: - Displays: - - Class: rviz_default_plugins/Grid - Name: Grid - Value: true - - Alpha: 0.8 - Class: rviz_default_plugins/RobotModel - Description Topic: - Value: /robot_description - Name: RobotModel - Value: true - - Class: rviz_default_plugins/TF - Name: TF - Value: true - Global Options: - Fixed Frame: {{fixed_frame}} - Tools: - - Class: rviz_default_plugins/MoveCamera - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 1.7 - Name: Current View - Pitch: 0.33 - Value: Orbit (rviz) - Yaw: 5.5 -Window Geometry: - Height: 800 - Width: 1200 diff --git a/rmf_site_editor/src/workcell/workcell.rs b/rmf_site_editor/src/workcell/workcell.rs deleted file mode 100644 index c03dac6f..00000000 --- a/rmf_site_editor/src/workcell/workcell.rs +++ /dev/null @@ -1,75 +0,0 @@ -/* - * 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::NameOfWorkcell; - -/// Used as an event to command that a new workcell should be made the current one -#[derive(Clone, Copy, Debug, Event)] -pub struct ChangeCurrentWorkcell { - /// What should the current workcell root be - pub root: Entity, -} - -/// Marker component used to mark the visualization of the workcell (its origin axis). -#[derive(Component, Debug, Clone)] -pub struct WorkcellVisualizationMarker; - -pub fn change_workcell( - mut current_workspace: ResMut, - mut change_current_workcell: EventReader, - open_workcells: Query>, -) { - if let Some(cmd) = change_current_workcell.read().last() { - if open_workcells.get(cmd.root).is_err() { - error!( - "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 = commands - .spawn(( - PbrBundle { - mesh: site_assets.site_anchor_mesh.clone(), - material: site_assets.passive_anchor_material.clone(), - ..default() - }, - WorkcellVisualizationMarker, - Selectable::new(e), - )) - .set_parent(e) - .id(); - interaction_assets.make_orientation_cue_meshes(&mut commands, body, 1.0); - } -} diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index 08ac7c7f..08fb72d8 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -22,10 +22,11 @@ use std::path::PathBuf; use crate::interaction::InteractionState; use crate::site::{DefaultFile, LoadSite, SaveSite}; -use crate::workcell::{LoadWorkcell, SaveWorkcell}; +// use crate::workcell::{LoadWorkcell, SaveWorkcell}; use crate::AppState; use rmf_site_format::legacy::building_map::BuildingMap; -use rmf_site_format::{NameOfSite, Site, Workcell}; +use rmf_site_format::{NameOfSite, Site}; +use rmf_workcell_format::Workcell; use crossbeam_channel::{Receiver, Sender}; @@ -185,8 +186,8 @@ impl Plugin for WorkspacePlugin { app.add_event::() .add_event::() .add_event::() - .add_event::() - .add_event::() + //.add_event::() + //.add_event::() .init_resource::() .init_resource::() .init_resource::() @@ -208,7 +209,7 @@ pub fn dispatch_new_workspace_events( state: Res>, mut new_workspace: EventReader, mut load_site: EventWriter, - mut load_workcell: EventWriter, + // mut load_workcell: EventWriter, ) { if let Some(_cmd) = new_workspace.read().last() { match state.get() { @@ -223,11 +224,13 @@ pub fn dispatch_new_workspace_events( }); } AppState::WorkcellEditor => { + /* load_workcell.send(LoadWorkcell { workcell: Workcell::default(), focus: true, default_file: None, }); + */ } } } @@ -239,7 +242,7 @@ pub fn process_load_workspace_files( mut app_state: ResMut>, mut interaction_state: ResMut>, mut load_site: EventWriter, - mut load_workcell: EventWriter, + // mut load_workcell: EventWriter, ) { let LoadWorkspaceFile(default_file, data) = request; match data { @@ -307,14 +310,16 @@ pub fn process_load_workspace_files( WorkspaceData::Workcell(data) => { info!("Opening workcell file"); match Workcell::from_bytes(&data) { - Ok(workcell) => { + Ok(_workcell) => { // Switch state app_state.set(AppState::WorkcellEditor); + /* load_workcell.send(LoadWorkcell { workcell, focus: true, default_file, }); + */ interaction_state.set(InteractionState::Enable); } Err(err) => { @@ -332,14 +337,16 @@ pub fn process_load_workspace_files( Ok(urdf) => { // TODO(luca) make this function return a result and this a match statement match Workcell::from_urdf(&urdf) { - Ok(workcell) => { + Ok(_workcell) => { // Switch state app_state.set(AppState::WorkcellEditor); + /* load_workcell.send(LoadWorkcell { workcell, focus: true, default_file, }); + */ interaction_state.set(InteractionState::Enable); } Err(err) => { @@ -574,17 +581,19 @@ fn dispatch_save_workspace_events( fn workspace_file_save_complete( app_state: Res>, mut save_site: EventWriter, - mut save_workcell: EventWriter, + // mut save_workcell: EventWriter, save_channels: Res, ) { if let Ok(result) = save_channels.receiver.try_recv() { match app_state.get() { AppState::WorkcellEditor => { + /* save_workcell.send(SaveWorkcell { root: result.root, to_file: result.path, format: result.format, }); + */ } AppState::SiteEditor | AppState::SiteDrawingEditor | AppState::SiteVisualizer => { save_site.send(SaveSite { diff --git a/rmf_site_format/src/constraint.rs b/rmf_site_format/src/constraint.rs deleted file mode 100644 index a52e8dcf..00000000 --- a/rmf_site_format/src/constraint.rs +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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::{Edge, RefTrait}; -#[cfg(feature = "bevy")] -use bevy::prelude::{Bundle, Component}; -use serde::{Deserialize, Serialize}; -use std::collections::HashMap; - -#[derive(Serialize, Deserialize, Debug, Clone)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct Constraint { - pub edge: Edge, - /// Marker that tells bevy the entity is a Constraint-type - #[serde(skip)] - pub marker: ConstraintMarker, -} - -#[derive(Clone, Debug, Default)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub struct ConstraintMarker; - -impl Constraint { - pub fn convert(&self, id_map: &HashMap) -> Result, T> { - Ok(Constraint { - edge: self.edge.convert(id_map)?, - marker: Default::default(), - }) - } -} - -impl From> for Constraint { - fn from(edge: Edge) -> Self { - Constraint { - edge, - marker: Default::default(), - } - } -} diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs index abab4614..03d15ea0 100644 --- a/rmf_site_format/src/lib.rs +++ b/rmf_site_format/src/lib.rs @@ -32,9 +32,6 @@ pub use category::*; pub mod camera_poses; pub use camera_poses::*; -pub mod constraint; -pub use constraint::*; - pub mod dock; pub use dock::*; @@ -119,9 +116,6 @@ pub use texture::*; pub mod wall; pub use wall::*; -pub mod workcell; -pub use workcell::*; - pub mod georeference; pub use georeference::*; diff --git a/rmf_site_format/src/workcell.rs b/rmf_site_format/src/workcell.rs index 519152e2..e69de29b 100644 --- a/rmf_site_format/src/workcell.rs +++ b/rmf_site_format/src/workcell.rs @@ -1,983 +0,0 @@ -/* - * 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, HashMap}; - -use std::io; - -use crate::misc::Rotation; -use crate::*; -#[cfg(feature = "bevy")] -use bevy::ecs::system::EntityCommands; -#[cfg(feature = "bevy")] -use bevy::prelude::{Bundle, Component, Deref, DerefMut, SpatialBundle}; -#[cfg(feature = "bevy")] -use bevy::reflect::{TypePath, TypeUuid}; -use glam::Vec3; -use serde::{Deserialize, Serialize}; -use thiserror::Error as ThisError; - -/// 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(skip)] - pub marker: FrameMarker, -} - -#[derive(Serialize, Deserialize, Debug, Default, Clone, PartialEq)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub struct NameOfWorkcell(pub String); - -#[derive(Serialize, Deserialize, Debug, Default, Clone)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct WorkcellProperties { - pub name: NameOfWorkcell, -} - -#[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 Moment { - pub ixx: f32, - pub ixy: f32, - pub ixz: f32, - pub iyy: f32, - pub iyz: f32, - pub izz: f32, -} - -#[derive(Serialize, Deserialize, Debug, Default, Clone)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct Inertia { - pub center: Pose, - pub mass: Mass, - pub moment: Moment, -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -pub struct JointAxis([f32; 3]); - -impl From<&urdf_rs::Axis> for JointAxis { - fn from(axis: &urdf_rs::Axis) -> Self { - Self(axis.xyz.map(|t| t as f32)) - } -} - -impl From<&JointAxis> for urdf_rs::Axis { - fn from(axis: &JointAxis) -> Self { - Self { - xyz: urdf_rs::Vec3(axis.0.map(|v| v as f64)), - } - } -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -enum RangeLimits { - None, - Symmetric(f32), - Asymmetric { - lower: Option, - upper: Option, - }, -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -pub struct JointLimits { - position: RangeLimits, - effort: RangeLimits, - velocity: RangeLimits, -} - -impl From<&urdf_rs::JointLimit> for JointLimits { - fn from(limit: &urdf_rs::JointLimit) -> Self { - Self { - position: RangeLimits::Asymmetric { - lower: Some(limit.lower as f32), - upper: Some(limit.upper as f32), - }, - effort: RangeLimits::Symmetric(limit.effort as f32), - velocity: RangeLimits::Symmetric(limit.velocity as f32), - } - } -} - -impl From<&JointLimits> for urdf_rs::JointLimit { - fn from(limits: &JointLimits) -> Self { - const DEFAULT_EFFORT_LIMIT: f64 = 1e3; - const DEFAULT_VELOCITY_LIMIT: f64 = 10.0; - fn min_or_default(slice: [Option; 2], default: f64) -> f64 { - let mut vec = slice - .iter() - .filter_map(|v| v.map(|m| m as f64)) - .collect::>(); - vec.sort_by(|a, b| a.total_cmp(b)); - vec.first().cloned().unwrap_or(default) - } - // 0.0 is a valid default in urdf for lower and upper limits - let (lower, upper) = match limits.position { - RangeLimits::None => (0.0, 0.0), - RangeLimits::Symmetric(l) => (l as f64, l as f64), - RangeLimits::Asymmetric { lower, upper } => ( - lower.map(|v| v as f64).unwrap_or_default(), - upper.map(|v| v as f64).unwrap_or_default(), - ), - }; - let effort = match limits.effort { - RangeLimits::None => { - println!( - "No effort limit found when exporting to urdf, setting to {}", - DEFAULT_EFFORT_LIMIT - ); - DEFAULT_EFFORT_LIMIT - } - RangeLimits::Symmetric(l) => l as f64, - RangeLimits::Asymmetric { lower, upper } => { - let limit = min_or_default([lower, upper], DEFAULT_EFFORT_LIMIT); - println!( - "Asymmetric effort limit found when exporting to urdf, setting to {}", - limit - ); - limit - } - }; - let velocity = match limits.velocity { - RangeLimits::None => { - println!( - "No velocity limit found when exporting to urdf, setting to {}", - DEFAULT_VELOCITY_LIMIT - ); - DEFAULT_VELOCITY_LIMIT - } - RangeLimits::Symmetric(l) => l as f64, - RangeLimits::Asymmetric { lower, upper } => { - let limit = min_or_default([lower, upper], DEFAULT_VELOCITY_LIMIT); - println!( - "Asymmetric velocity limit found when exporting to urdf, setting to {}", - limit - ); - limit - } - }; - Self { - lower, - upper, - effort, - velocity, - } - } -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -#[cfg_attr(feature = "bevy", derive(Bundle))] -pub struct Joint { - pub name: NameInWorkcell, - pub properties: JointProperties, -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -#[cfg_attr(feature = "bevy", derive(Component))] -pub enum JointProperties { - Fixed, - Prismatic(SingleDofJoint), - Revolute(SingleDofJoint), - Continuous(SingleDofJoint), -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -pub struct SingleDofJoint { - pub limits: JointLimits, - pub axis: JointAxis, -} - -impl JointProperties { - pub fn label(&self) -> String { - match &self { - JointProperties::Fixed => "Fixed", - JointProperties::Revolute(_) => "Revolute", - JointProperties::Prismatic(_) => "Prismatic", - JointProperties::Continuous(_) => "Continuous", - } - .to_string() - } -} - -// TODO(luca) should commands implementation be in rmf_site_editor instead of rmf_site_format? -/// Custom spawning implementation since bundles don't allow options -#[cfg(feature = "bevy")] -impl Joint { - pub fn add_bevy_components(&self, commands: &mut EntityCommands) { - commands.insert(( - SpatialBundle::INHERITED_IDENTITY, - Category::Joint, - self.name.clone(), - self.properties.clone(), - )); - } -} - -#[derive(Serialize, Deserialize, Debug, Clone)] -pub enum Geometry { - //#[serde(flatten)] - Primitive(PrimitiveShape), - Mesh { - source: AssetSource, - #[serde(default, skip_serializing_if = "is_default")] - scale: Option, - }, -} - -impl Default for Geometry { - fn default() -> Self { - Geometry::Primitive(PrimitiveShape::Box { size: [0.0; 3] }) - } -} - -#[derive(Serialize, Deserialize, Debug, Default, Clone)] -pub struct WorkcellModel { - pub name: String, - pub geometry: Geometry, - pub pose: Pose, -} - -// 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>, - /// Inertias, key is their id, used for hierarchy - pub inertias: BTreeMap>, - /// Joints, key is their id, used for hierarchy. They must have a frame as a parent and a frame - /// as a child - pub joints: BTreeMap>, -} - -#[derive(Debug, ThisError)] -pub enum UrdfImportError { - #[error("a joint refers to a non existing link [{0}]")] - BrokenJointReference(String), - // TODO(luca) Add urdf_rs::JointType to this error, it doesn't implement Display - #[error("unsupported joint type found")] - UnsupportedJointType, -} - -impl From for urdf_rs::Geometry { - fn from(geometry: Geometry) -> Self { - match geometry { - Geometry::Mesh { source, scale } => urdf_rs::Geometry::Mesh { - // SAFETY: We don't need to validate the syntax of the asset - // path because that will be done later when we attempt to load - // this as an asset. - filename: unsafe { (&source).as_unvalidated_asset_path() }, - scale: scale.map(|v| urdf_rs::Vec3([v.x as f64, v.y as f64, v.z as f64])), - }, - Geometry::Primitive(PrimitiveShape::Box { size: [x, y, z] }) => { - urdf_rs::Geometry::Box { - size: urdf_rs::Vec3([x as f64, y as f64, z as f64]), - } - } - Geometry::Primitive(PrimitiveShape::Cylinder { radius, length }) => { - urdf_rs::Geometry::Cylinder { - radius: radius as f64, - length: length as f64, - } - } - Geometry::Primitive(PrimitiveShape::Capsule { radius, length }) => { - urdf_rs::Geometry::Capsule { - radius: radius as f64, - length: length as f64, - } - } - Geometry::Primitive(PrimitiveShape::Sphere { radius }) => urdf_rs::Geometry::Sphere { - radius: radius as f64, - }, - } - } -} - -#[derive(Debug, ThisError)] -pub enum WorkcellToUrdfError { - #[error("Invalid anchor type {0:?}")] - InvalidAnchorType(Anchor), - #[error("Urdf error: {0}")] - WriteToStringError(#[from] urdf_rs::UrdfError), - #[error("Broken reference: {0}")] - BrokenReference(u32), - #[error("Frame {0} referred by joint {1} has no name, this is not allowed in URDF")] - MissingJointFrameName(u32, u32), -} - -impl Workcell { - pub fn from_urdf(urdf: &urdf_rs::Robot) -> Result { - let mut frame_name_to_id = HashMap::new(); - let root_id = 0_u32; - let mut cur_id = 1u32..; - let mut frames = BTreeMap::new(); - let mut visuals = BTreeMap::new(); - let mut collisions = BTreeMap::new(); - let mut inertias = BTreeMap::new(); - let mut joints = BTreeMap::new(); - // Populate here - for link in &urdf.links { - let inertia = Inertia::from(&link.inertial); - // Add a frame with the link's name, then the inertia data as a child - let frame_id = cur_id.next().unwrap(); - let inertia_id = cur_id.next().unwrap(); - frame_name_to_id.insert(link.name.clone(), frame_id); - // Pose and parent will be overwritten by joints, if needed - frames.insert( - frame_id, - Parented { - parent: root_id, - bundle: Frame { - anchor: Anchor::Pose3D(Pose::default()), - name: Some(NameInWorkcell(link.name.clone())), - marker: Default::default(), - }, - }, - ); - inertias.insert( - inertia_id, - Parented { - parent: frame_id, - bundle: inertia, - }, - ); - for visual in &link.visual { - let model = WorkcellModel::from(visual); - let visual_id = cur_id.next().unwrap(); - visuals.insert( - visual_id, - Parented { - parent: frame_id, - bundle: model, - }, - ); - } - for collision in &link.collision { - let model = WorkcellModel::from(collision); - let collision_id = cur_id.next().unwrap(); - collisions.insert( - collision_id, - Parented { - parent: frame_id, - bundle: model, - }, - ); - } - } - for joint in &urdf.joints { - let parent = frame_name_to_id.get(&joint.parent.link).ok_or( - UrdfImportError::BrokenJointReference(joint.parent.link.clone()), - )?; - let child = frame_name_to_id.get(&joint.child.link).ok_or( - UrdfImportError::BrokenJointReference(joint.child.link.clone()), - )?; - let properties = match joint.joint_type { - urdf_rs::JointType::Revolute => JointProperties::Revolute(SingleDofJoint { - axis: (&joint.axis).into(), - limits: (&joint.limit).into(), - }), - urdf_rs::JointType::Prismatic => JointProperties::Prismatic(SingleDofJoint { - axis: (&joint.axis).into(), - limits: (&joint.limit).into(), - }), - urdf_rs::JointType::Fixed => JointProperties::Fixed, - urdf_rs::JointType::Continuous => JointProperties::Continuous(SingleDofJoint { - axis: (&joint.axis).into(), - limits: (&joint.limit).into(), - }), - _ => { - return Err(UrdfImportError::UnsupportedJointType); - } - }; - let joint_id = cur_id.next().unwrap(); - // Reassign the child parenthood and pose to the joint - // If the frame didn't exist we would have returned an error when populating child - // hence this is safe. - let child_frame = frames.get_mut(child).unwrap(); - child_frame.parent = joint_id; - // In urdf, joint origin represents the coordinates of the joint in the - // parent frame. The child is always in the origin of the joint - child_frame.bundle.anchor = Anchor::Pose3D((&joint.origin).into()); - joints.insert( - joint_id, - Parented { - parent: *parent, - bundle: Joint { - name: NameInWorkcell(joint.name.clone()), - properties, - }, - }, - ); - } - - Ok(Workcell { - properties: WorkcellProperties { - name: NameOfWorkcell(urdf.name.clone()), - }, - id: root_id, - frames, - visuals, - collisions, - inertias, - joints, - }) - } - 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 to_urdf(&self) -> Result { - let mut parent_to_visuals = HashMap::new(); - for (_, visual) in self.visuals.iter() { - let parent = visual.parent; - let visual = &visual.bundle; - let visual = urdf_rs::Visual { - name: Some(visual.name.clone()), - origin: visual.pose.into(), - geometry: visual.geometry.clone().into(), - material: None, - }; - parent_to_visuals - .entry(parent) - .or_insert_with(Vec::new) - .push(visual); - } - - let mut parent_to_collisions = HashMap::new(); - for (_, collision) in self.collisions.iter() { - let parent = collision.parent; - let collision = &collision.bundle; - let collision = urdf_rs::Collision { - name: Some(collision.name.clone()), - origin: collision.pose.into(), - geometry: collision.geometry.clone().into(), - }; - parent_to_collisions - .entry(parent) - .or_insert_with(Vec::new) - .push(collision); - } - - // If the workcell has a single frame child we can use the child as the base link. - // Otherwise, we will need to spawn a new base link to contain all the workcell children - let workcell_child_frames = self - .frames - .iter() - .filter(|(_, frame)| frame.parent == self.id); - let num_children = workcell_child_frames.clone().count(); - let frames = if num_children != 1 { - // TODO(luca) remove hardcoding of base link name, it might in some cases create - // duplicates - let mut frames = self.frames.clone(); - let dummy_frame = Frame { - anchor: Anchor::Pose3D(Pose { - rot: Rotation::Quat([0.0, 0.0, 0.0, 0.0]), - trans: [0.0, 0.0, 0.0], - }), - // As per Industrial Workcell Coordinate Conventions, the name of the workcell - // datum link shall be "_workcell_link". - name: Some(NameInWorkcell(String::from( - self.properties.name.0.clone() + "_workcell_link", - ))), - marker: FrameMarker, - }; - frames.insert( - self.id, - Parented { - // Root has no parent, use placeholder of max u32 - parent: u32::MAX, - bundle: dummy_frame, - }, - ); - frames - } else { - // Flatten the hierarchy by making the only child the new workcell base link - self.frames.clone() - }; - - let mut parent_to_inertials = HashMap::new(); - for (_, inertia) in self.inertias.iter() { - let parent = inertia.parent; - let inertia = &inertia.bundle; - let inertial = urdf_rs::Inertial::from(inertia); - parent_to_inertials.insert(parent, inertial); - } - - // TODO(luca) combine multiple frames without a joint inbetween into a single link. - // For now as soon as a joint is missing the hierarchy will be broken - let links = frames - .iter() - .map(|(frame_id, parented_frame)| { - let name = match &parented_frame.bundle.name { - Some(name) => name.0.clone(), - None => format!("frame_{}", &frame_id), - }; - - let inertial = parent_to_inertials.remove(&frame_id).unwrap_or_default(); - let collision = parent_to_collisions.remove(&frame_id).unwrap_or_default(); - let visual = parent_to_visuals.remove(&frame_id).unwrap_or_default(); - - urdf_rs::Link { - name, - inertial, - collision, - visual, - } - }) - .collect::>(); - - let joints = self - .joints - .iter() - .map(|(joint_id, parented_joint)| { - let joint_parent = parented_joint.parent; - let joint = &parented_joint.bundle; - // The pose of the joint is the pose of the frame that has it as its parent - let parent_frame = self - .frames - .get(&joint_parent) - .ok_or(WorkcellToUrdfError::BrokenReference(joint_parent))?; - let (child_frame_id, child_frame) = self - .frames - .iter() - .find(|(_, frame)| frame.parent == *joint_id) - .ok_or(WorkcellToUrdfError::BrokenReference(*joint_id))?; - let parent_name = parent_frame.bundle.name.clone().ok_or( - WorkcellToUrdfError::MissingJointFrameName(joint_parent, *joint_id), - )?; - let child_name = child_frame.bundle.name.clone().ok_or( - WorkcellToUrdfError::MissingJointFrameName(*child_frame_id, *joint_id), - )?; - let Anchor::Pose3D(pose) = child_frame.bundle.anchor else { - return Err(WorkcellToUrdfError::InvalidAnchorType( - child_frame.bundle.anchor.clone(), - )); - }; - let (joint_type, axis, limit) = match &joint.properties { - JointProperties::Fixed => ( - urdf_rs::JointType::Fixed, - urdf_rs::Axis::default(), - urdf_rs::JointLimit::default(), - ), - JointProperties::Revolute(joint) => ( - urdf_rs::JointType::Revolute, - (&joint.axis).into(), - (&joint.limits).into(), - ), - JointProperties::Prismatic(joint) => ( - urdf_rs::JointType::Prismatic, - (&joint.axis).into(), - (&joint.limits).into(), - ), - JointProperties::Continuous(joint) => ( - urdf_rs::JointType::Continuous, - (&joint.axis).into(), - (&joint.limits).into(), - ), - }; - Ok(urdf_rs::Joint { - name: joint.name.0.clone(), - joint_type, - origin: pose.into(), - parent: urdf_rs::LinkName { - link: parent_name.0, - }, - child: urdf_rs::LinkName { link: child_name.0 }, - axis, - limit, - dynamics: None, - mimic: None, - safety_controller: None, - }) - }) - .collect::, WorkcellToUrdfError>>()?; - - // TODO(luca) implement materials - let robot = urdf_rs::Robot { - name: self.properties.name.0.clone(), - links, - joints, - materials: vec![], - }; - Ok(robot) - } - - pub fn to_urdf_string(&self) -> Result { - let urdf = self.to_urdf()?; - urdf_rs::write_to_string(&urdf).map_err(|e| WorkcellToUrdfError::WriteToStringError(e)) - } - - pub fn to_urdf_writer(&self, mut writer: impl io::Write) -> Result<(), std::io::Error> { - let urdf = self - .to_urdf_string() - .map_err(|e| std::io::Error::new(std::io::ErrorKind::Other, e))?; - writer.write_all(urdf.as_bytes()) - } - - 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, TypePath) -)] -#[cfg_attr(feature = "bevy", uuid = "fe707f9e-c6f3-11ed-afa1-0242ac120002")] -pub struct UrdfRoot(pub urdf_rs::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(PrimitiveShape::Box { - size: (**size).map(|f| f as f32), - }), - urdf_rs::Geometry::Cylinder { radius, length } => { - Geometry::Primitive(PrimitiveShape::Cylinder { - radius: *radius as f32, - length: *length as f32, - }) - } - urdf_rs::Geometry::Capsule { radius, length } => { - Geometry::Primitive(PrimitiveShape::Capsule { - radius: *radius as f32, - length: *length as f32, - }) - } - urdf_rs::Geometry::Sphere { radius } => Geometry::Primitive(PrimitiveShape::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)))); - // Most (all?) Urdf files use package references, we fallback to local if that is - // not the case - let source = if let Some(path) = filename.strip_prefix("package://") { - AssetSource::Package(path.to_owned()) - } else { - AssetSource::Local(filename.clone()) - }; - Geometry::Mesh { source, scale } - } - } - } -} - -impl From<&urdf_rs::Inertia> for Moment { - fn from(inertia: &urdf_rs::Inertia) -> Self { - Self { - ixx: inertia.ixx as f32, - ixy: inertia.ixy as f32, - ixz: inertia.ixz as f32, - iyy: inertia.iyy as f32, - iyz: inertia.iyz as f32, - izz: inertia.izz as f32, - } - } -} - -impl From<&urdf_rs::Inertial> for Inertia { - fn from(inertial: &urdf_rs::Inertial) -> Self { - Self { - center: (&inertial.origin).into(), - mass: Mass(inertial.mass.value as f32), - moment: (&inertial.inertia).into(), - } - } -} - -impl From<&Inertia> for urdf_rs::Inertial { - fn from(inertia: &Inertia) -> Self { - Self { - origin: inertia.center.into(), - mass: urdf_rs::Mass { - value: inertia.mass.0 as f64, - }, - inertia: urdf_rs::Inertia { - ixx: inertia.moment.ixx as f64, - ixy: inertia.moment.ixy as f64, - ixz: inertia.moment.ixz as f64, - iyy: inertia.moment.iyy as f64, - iyz: inertia.moment.iyz as f64, - izz: inertia.moment.izz as f64, - }, - } - } -} - -impl WorkcellModel { - fn from_urdf_data( - pose: &urdf_rs::Pose, - name: &Option, - geometry: &urdf_rs::Geometry, - ) -> Self { - WorkcellModel { - name: name.clone().unwrap_or_default(), - geometry: geometry.into(), - pose: pose.into(), - } - } -} - -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) - } -} - -#[cfg(test)] -mod tests { - use super::*; - use float_eq::{assert_float_eq, float_eq}; - - fn frame_by_name( - frames: &BTreeMap>, - name: &str, - ) -> Option<(u32, Parented)> { - frames - .iter() - .find(|(_, parented_frame)| { - parented_frame.bundle.name == Some(NameInWorkcell(name.to_string())) - }) - .map(|(id, f)| (*id, f.clone())) - } - - fn element_by_parent( - models: &BTreeMap>, - parent: u32, - ) -> Option<(u32, Parented)> { - models - .iter() - .find(|(_, parented_element)| parented_element.parent == parent) - .map(|(id, e)| (*id, e.clone())) - } - - fn is_pose_eq(p1: &Pose, p2: &Pose) -> bool { - if !p1 - .trans - .iter() - .zip(p2.trans.iter()) - .map(|(t1, t2)| float_eq!(t1, t2, abs <= 1e-6)) - .all(|eq| eq) - { - return false; - } - match ( - p1.rot.as_euler_extrinsic_xyz(), - p2.rot.as_euler_extrinsic_xyz(), - ) { - (Rotation::EulerExtrinsicXYZ(r1), Rotation::EulerExtrinsicXYZ(r2)) => r1 - .iter() - .zip(r2.iter()) - .map(|(a1, a2)| float_eq!(a1.radians(), a2.radians(), abs <= 1e-6)) - .all(|eq| eq), - _ => false, - } - } - - fn is_inertia_eq(i1: &Inertia, i2: &Inertia) -> bool { - is_pose_eq(&i1.center, &i2.center) - && float_eq!(i1.mass.0, i2.mass.0, abs <= 1e6) - && float_eq!(i1.moment.ixx, i2.moment.ixx, abs <= 1e6) - && float_eq!(i1.moment.ixy, i2.moment.ixy, abs <= 1e6) - && float_eq!(i1.moment.ixz, i2.moment.ixz, abs <= 1e6) - && float_eq!(i1.moment.iyy, i2.moment.iyy, abs <= 1e6) - && float_eq!(i1.moment.iyz, i2.moment.iyz, abs <= 1e6) - && float_eq!(i1.moment.izz, i2.moment.izz, abs <= 1e6) - } - - #[test] - fn urdf_roundtrip() { - let urdf = urdf_rs::read_file("test/07-physics.urdf").unwrap(); - let workcell = Workcell::from_urdf(&urdf).unwrap(); - assert_eq!(workcell.visuals.len(), 16); - assert_eq!(workcell.collisions.len(), 16); - assert_eq!(workcell.frames.len(), 16); - assert_eq!(workcell.joints.len(), 15); - assert_eq!(workcell.properties.name.0, "physics"); - // Test that we convert poses from joints to frames - let (right_leg_id, right_leg) = frame_by_name(&workcell.frames, "right_leg").unwrap(); - let target_right_leg_pose = Pose { - trans: [0.0, -0.22, 0.25], - rot: Default::default(), - }; - assert!(right_leg - .bundle - .anchor - .is_close(&Anchor::Pose3D(target_right_leg_pose), 1e-6)); - // Test that we can parse parenthood and properties of visuals and collisions correctly - let (_, right_leg_visual) = element_by_parent(&workcell.visuals, right_leg_id).unwrap(); - let target_right_leg_model_pose = Pose { - trans: [0.0, 0.0, -0.3], - rot: Rotation::EulerExtrinsicXYZ([ - Angle::Rad(0.0), - Angle::Rad(1.57075), - Angle::Rad(0.0), - ]), - }; - assert!(is_pose_eq( - &right_leg_visual.bundle.pose, - &target_right_leg_model_pose - )); - assert!(matches!( - right_leg_visual.bundle.geometry, - Geometry::Primitive(PrimitiveShape::Box { .. }) - )); - let (_, right_leg_collision) = - element_by_parent(&workcell.collisions, right_leg_id).unwrap(); - assert!(is_pose_eq( - &right_leg_collision.bundle.pose, - &target_right_leg_model_pose - )); - assert!(matches!( - right_leg_collision.bundle.geometry, - Geometry::Primitive(PrimitiveShape::Box { .. }) - )); - // Test inertia parenthood and parsing - let (_, right_leg_inertia) = element_by_parent(&workcell.inertias, right_leg_id).unwrap(); - assert_float_eq!(right_leg_inertia.bundle.mass.0, 10.0, abs <= 1e6); - let target_right_leg_inertia = Inertia { - center: Pose::default(), - mass: Mass(10.0), - moment: Moment { - ixx: 1.0, - ixy: 0.0, - ixz: 0.0, - iyy: 1.0, - iyz: 0.0, - izz: 1.0, - }, - }; - assert!(is_inertia_eq( - &right_leg_inertia.bundle, - &target_right_leg_inertia - )); - // Test joint parenthood and parsing - let (_, right_leg_joint) = element_by_parent(&workcell.joints, right_leg_id).unwrap(); - assert!(matches!( - right_leg_joint.bundle.properties, - JointProperties::Fixed - )); - assert_eq!( - right_leg_joint.bundle.name, - NameInWorkcell("right_base_joint".to_string()) - ); - // Test that the new urdf contains the same data - let new_urdf = workcell.to_urdf().unwrap(); - assert_eq!(new_urdf.name, "physics"); - assert_eq!(new_urdf.links.len(), 16); - assert_eq!(new_urdf.joints.len(), 15); - // Check that link information is preserved - let right_leg_link = new_urdf - .links - .iter() - .find(|l| l.name == "right_leg") - .unwrap(); - assert!(is_inertia_eq( - &(&right_leg_link.inertial).into(), - &target_right_leg_inertia - )); - assert_eq!(right_leg_link.visual.len(), 1); - assert_eq!(right_leg_link.collision.len(), 1); - let right_leg_visual = right_leg_link.visual.get(0).unwrap(); - let right_leg_collision = right_leg_link.collision.get(0).unwrap(); - assert!(is_pose_eq( - &(&right_leg_visual.origin).into(), - &target_right_leg_model_pose - )); - assert!(is_pose_eq( - &(&right_leg_collision.origin).into(), - &target_right_leg_model_pose - )); - assert!(matches!( - right_leg_visual.geometry, - urdf_rs::Geometry::Box { .. } - )); - assert!(matches!( - right_leg_collision.geometry, - urdf_rs::Geometry::Box { .. } - )); - // Check that joint origin is preserved - let right_leg_joint = new_urdf - .joints - .iter() - .find(|l| l.name == "base_to_right_leg") - .unwrap(); - assert!(is_pose_eq( - &(&right_leg_joint.origin).into(), - &target_right_leg_pose - )); - assert!(matches!( - right_leg_joint.joint_type, - urdf_rs::JointType::Fixed - )); - } -} From f67733f35a2094cf32e086639deef048e6f866e5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 12:27:30 +0800 Subject: [PATCH 04/25] Further cleanup of UI / minor migrations Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/lib.rs | 5 +-- rmf_site_editor/src/site/fuel_cache.rs | 19 ++++++++++ rmf_site_editor/src/site/georeference.rs | 15 +------- rmf_site_editor/src/site/mod.rs | 15 +++----- rmf_site_editor/src/{ => site}/view_menu.rs | 37 +------------------ rmf_site_editor/src/widgets/diagnostics.rs | 18 ++------- rmf_site_editor/src/widgets/menu_bar.rs | 18 --------- .../src/widgets/sdf_export_menu.rs | 13 +------ 8 files changed, 34 insertions(+), 106 deletions(-) rename rmf_site_editor/src/{ => site}/view_menu.rs (82%) diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index ea80b534..cdb43b99 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -45,9 +45,6 @@ pub mod sdf_loader; pub mod site_asset_io; use sdf_loader::*; -pub mod view_menu; -use view_menu::*; - pub mod wireframe; use wireframe::*; @@ -236,7 +233,7 @@ impl Plugin for SiteEditor { MainMenuPlugin, )) // Note order matters, plugins that edit the menus must be initialized after the UI - .add_plugins((ViewMenuPlugin, OSMViewPlugin, SiteWireframePlugin)); + .add_plugins((site::ViewMenuPlugin, OSMViewPlugin, SiteWireframePlugin)); } // Ref https://github.com/bevyengine/bevy/issues/10877. The default behavior causes issues diff --git a/rmf_site_editor/src/site/fuel_cache.rs b/rmf_site_editor/src/site/fuel_cache.rs index 58590229..7c74bb97 100644 --- a/rmf_site_editor/src/site/fuel_cache.rs +++ b/rmf_site_editor/src/site/fuel_cache.rs @@ -37,6 +37,25 @@ pub struct FuelCacheUpdated(Option>); #[derive(Deref, DerefMut, Event)] pub struct SetFuelApiKey(pub String); +#[derive(Default)] +pub struct FuelPlugin { } + +impl Plugin for FuelPlugin { + fn build(&self, app: &mut App) { + app + .add_event::() + .add_event::() + .init_resource::() + .init_resource::() + .init_resource::() + .add_systems(PostUpdate, + (handle_update_fuel_cache_requests, + read_update_fuel_cache_results, + reload_failed_models_with_new_api_key, + )); + } +} + /// 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)] diff --git a/rmf_site_editor/src/site/georeference.rs b/rmf_site_editor/src/site/georeference.rs index 3f741a94..0b30fd75 100644 --- a/rmf_site_editor/src/site/georeference.rs +++ b/rmf_site_editor/src/site/georeference.rs @@ -10,10 +10,10 @@ use crate::{ generate_map_tiles, interaction::camera_controls, widgets::menu_bar::{ - Menu, MenuDisabled, MenuEvent, MenuItem, MenuVisualizationStates, ToolMenu, ViewMenu, + Menu, MenuDisabled, MenuEvent, MenuItem, ToolMenu, ViewMenu, }, workspace::CurrentWorkspace, - AppState, OSMTile, + OSMTile, }; const MAX_ZOOM: i32 = 19; @@ -552,29 +552,19 @@ pub struct OSMMenu { impl FromWorld for OSMMenu { fn from_world(world: &mut World) -> Self { - // Only show the menu in site editor modes - let target_states = MenuVisualizationStates(HashSet::from([ - AppState::SiteEditor, - AppState::SiteDrawingEditor, - AppState::SiteVisualizer, - ])); // Tools menu let set_reference = world .spawn(MenuItem::Text("Set Reference".to_string())) - .insert(target_states.clone()) .id(); let view_reference = world .spawn(MenuItem::Text("View Reference".to_string())) - .insert(target_states.clone()) .id(); let settings_reference = world .spawn(MenuItem::Text("Settings".to_string())) - .insert(target_states.clone()) .id(); let sub_menu = world .spawn(Menu::from_title("Geographic Offset".to_string())) - .insert(target_states.clone()) .id(); world.entity_mut(sub_menu).push_children(&[ set_reference, @@ -589,7 +579,6 @@ impl FromWorld for OSMMenu { let view_header = world.resource::().get(); let satellite_map_check_button = world .spawn(MenuItem::CheckBox("Satellite Map".to_string(), false)) - .insert(target_states.clone()) .id(); world .entity_mut(view_header) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index aa67e34d..1c323add 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -114,13 +114,16 @@ pub use texture::*; pub mod util; pub use util::*; +pub mod view_menu; +pub use view_menu::*; + pub mod wall; pub use wall::*; use crate::recency::{RecencyRank, RecencyRankingPlugin}; use crate::{AppState, RegisterIssueType}; pub use rmf_site_format::{DirectionalLight, PointLight, SpotLight, Style, *}; -use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell, JointProperties}; +use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell}; use bevy::{prelude::*, render::view::visibility::VisibilitySystems, transform::TransformSystem}; @@ -186,12 +189,9 @@ impl Plugin for SitePlugin { apply_deferred.in_set(SiteUpdateSet::AssignOrphansFlush), ) .insert_resource(ClearColor(Color::rgb(0., 0., 0.))) - .init_resource::() .init_resource::() .init_resource::() .init_resource::() - .init_resource::() - .init_resource::() .register_type::() .register_type::() .register_type::() @@ -211,8 +211,6 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() - .add_event::() - .add_event::() .add_event::() .add_plugins(( ChangePlugin::>::default(), @@ -255,7 +253,6 @@ impl Plugin for SitePlugin { ChangePlugin::::default(), ChangePlugin::::default(), ChangePlugin::>::default(), - ChangePlugin::::default(), RecencyRankingPlugin::::default(), RecencyRankingPlugin::::default(), RecencyRankingPlugin::::default(), @@ -263,6 +260,7 @@ impl Plugin for SitePlugin { DrawingEditorPlugin, SiteVisualizerPlugin, ModelLoadingPlugin::default(), + FuelPlugin::default(), )) .add_issue_type(&DUPLICATED_DOOR_NAME_ISSUE_UUID, "Duplicate door name") .add_issue_type(&DUPLICATED_LIFT_NAME_ISSUE_UUID, "Duplicate lift name") @@ -328,9 +326,6 @@ impl Plugin for SitePlugin { PostUpdate, ( add_wall_visual, - handle_update_fuel_cache_requests, - read_update_fuel_cache_results, - reload_failed_models_with_new_api_key, update_walls_for_moved_anchors, update_walls, update_transforms_for_changed_poses, diff --git a/rmf_site_editor/src/view_menu.rs b/rmf_site_editor/src/site/view_menu.rs similarity index 82% rename from rmf_site_editor/src/view_menu.rs rename to rmf_site_editor/src/site/view_menu.rs index 12ec7534..9d0e20b6 100644 --- a/rmf_site_editor/src/view_menu.rs +++ b/rmf_site_editor/src/site/view_menu.rs @@ -20,12 +20,9 @@ use crate::site::{ CollisionMeshMarker, DoorMarker, FiducialMarker, FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, MeasurementMarker, VisualMeshMarker, WallMarker, }; -use crate::widgets::menu_bar::{MenuEvent, MenuItem, MenuVisualizationStates, ViewMenu}; -// use crate::workcell::WorkcellVisualizationMarker; -use crate::AppState; +use crate::widgets::menu_bar::{MenuEvent, MenuItem, ViewMenu}; use bevy::ecs::system::SystemParam; use bevy::prelude::*; -use std::collections::HashSet; #[derive(SystemParam)] struct VisibilityEvents<'w> { @@ -40,7 +37,6 @@ struct VisibilityEvents<'w> { walls: EventWriter<'w, SetCategoryVisibility>, visuals: EventWriter<'w, SetCategoryVisibility>, collisions: EventWriter<'w, SetCategoryVisibility>, - // origin_axis: EventWriter<'w, SetCategoryVisibility>, } #[derive(Default)] @@ -58,18 +54,10 @@ pub struct ViewMenuItems { collisions: Entity, visuals: Entity, walls: Entity, - // origin_axis: Entity, } impl FromWorld for ViewMenuItems { fn from_world(world: &mut World) -> Self { - let site_states = HashSet::from([ - AppState::SiteEditor, - AppState::SiteDrawingEditor, - AppState::SiteVisualizer, - ]); - let mut active_states = site_states.clone(); - active_states.insert(AppState::WorkcellEditor); let view_header = world.resource::().get(); let default_visibility = world.resource::>(); let doors = world @@ -77,7 +65,6 @@ impl FromWorld for ViewMenuItems { "Doors".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -86,7 +73,6 @@ impl FromWorld for ViewMenuItems { "Floors".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -95,7 +81,6 @@ impl FromWorld for ViewMenuItems { "Lanes".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>>(); @@ -104,7 +89,6 @@ impl FromWorld for ViewMenuItems { "Lifts".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -113,7 +97,6 @@ impl FromWorld for ViewMenuItems { "Locations".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -122,7 +105,6 @@ impl FromWorld for ViewMenuItems { "Fiducials".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -131,7 +113,6 @@ impl FromWorld for ViewMenuItems { "Measurements".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -140,7 +121,6 @@ impl FromWorld for ViewMenuItems { "Collision meshes".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(active_states.clone())) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -149,7 +129,6 @@ impl FromWorld for ViewMenuItems { "Visual meshes".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(active_states)) .set_parent(view_header) .id(); let default_visibility = world.resource::>(); @@ -158,21 +137,8 @@ impl FromWorld for ViewMenuItems { "Walls".to_string(), default_visibility.0, )) - .insert(MenuVisualizationStates(site_states)) .set_parent(view_header) .id(); - /* - let default_visibility = - world.resource::>(); - let origin_axis = world - .spawn(MenuItem::CheckBox( - "Reference axis".to_string(), - default_visibility.0, - )) - .insert(MenuVisualizationStates(workcell_states)) - .set_parent(view_header) - .id(); - */ ViewMenuItems { doors, @@ -185,7 +151,6 @@ impl FromWorld for ViewMenuItems { collisions, visuals, walls, - // origin_axis, } } } diff --git a/rmf_site_editor/src/widgets/diagnostics.rs b/rmf_site_editor/src/widgets/diagnostics.rs index 41a25028..365bb4d0 100644 --- a/rmf_site_editor/src/widgets/diagnostics.rs +++ b/rmf_site_editor/src/widgets/diagnostics.rs @@ -18,15 +18,14 @@ use crate::{ site::{Change, FilteredIssueKinds, FilteredIssues, IssueKey}, widgets::{ - menu_bar::{MenuEvent, MenuItem, MenuVisualizationStates, ToolMenu}, + menu_bar::{MenuEvent, MenuItem, ToolMenu}, prelude::*, SelectorWidget, }, - AppState, CurrentWorkspace, Icons, Issue, IssueDictionary, ValidateWorkspace, + CurrentWorkspace, Icons, Issue, IssueDictionary, ValidateWorkspace, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{self, Button, Checkbox, Grid, ImageButton, ScrollArea, Ui}; -use std::collections::HashSet; /// Add a [`Diagnostics`] widget to your application. #[derive(Default)] @@ -239,22 +238,13 @@ pub struct IssueMenu { impl FromWorld for IssueMenu { fn from_world(world: &mut World) -> Self { - let target_states = HashSet::from([ - AppState::SiteEditor, - AppState::SiteDrawingEditor, - AppState::SiteVisualizer, - ]); // Tools menu + let tool_header = world.resource::().get(); let diagnostic_tool = world .spawn(MenuItem::Text("Diagnostic Tool".to_string())) - .insert(MenuVisualizationStates(target_states)) + .set_parent(tool_header) .id(); - let tool_header = world.resource::().get(); - world - .entity_mut(tool_header) - .push_children(&[diagnostic_tool]); - IssueMenu { diagnostic_tool } } } diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index 1b077418..62ca4af9 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -21,8 +21,6 @@ use bevy::ecs::query::Has; use bevy::prelude::*; use bevy_egui::egui::{self, Button, Ui}; -use std::collections::HashSet; - /// Add the standard menu bar to the application. #[derive(Default)] pub struct MenuBarPlugin {} @@ -87,10 +85,6 @@ impl MenuItem { } } -/// Contains the states that the menu should be visualized in. -#[derive(Debug, Clone, Component, Deref, DerefMut)] -pub struct MenuVisualizationStates(pub HashSet); - /// This resource provides the root entity for the file menu #[derive(Resource)] pub struct FileMenu { @@ -189,15 +183,9 @@ fn render_sub_menu( children: &Query<&Children>, menus: &Query<(&Menu, Entity)>, menu_items: &Query<(&mut MenuItem, Has)>, - menu_states: &Query>, extension_events: &mut EventWriter, skip_top_label: bool, ) { - if let Some(states) = menu_states.get(*entity).ok().flatten() { - if !states.contains(state.get()) { - return; - } - } if let Ok((e, disabled)) = menu_items.get(*entity) { // Draw ui match e { @@ -236,7 +224,6 @@ fn render_sub_menu( children, menus, menu_items, - menu_states, extension_events, false, ); @@ -255,7 +242,6 @@ fn render_sub_menu( children, menus, menu_items, - menu_states, extension_events, false, ); @@ -268,7 +254,6 @@ struct MenuParams<'w, 's> { state: Res<'w, State>, menus: Query<'w, 's, (&'static Menu, Entity)>, menu_items: Query<'w, 's, (&'static mut MenuItem, Has)>, - menu_states: Query<'w, 's, Option<&'static MenuVisualizationStates>>, extension_events: EventWriter<'w, MenuEvent>, view_menu: Res<'w, ViewMenu>, } @@ -318,7 +303,6 @@ fn top_menu_bar( &children, &menu_params.menus, &menu_params.menu_items, - &menu_params.menu_states, &mut menu_params.extension_events, true, ); @@ -331,7 +315,6 @@ fn top_menu_bar( &children, &menu_params.menus, &menu_params.menu_items, - &menu_params.menu_states, &mut menu_params.extension_events, true, ); @@ -348,7 +331,6 @@ fn top_menu_bar( &children, &menu_params.menus, &menu_params.menu_items, - &menu_params.menu_states, &mut menu_params.extension_events, false, ); diff --git a/rmf_site_editor/src/widgets/sdf_export_menu.rs b/rmf_site_editor/src/widgets/sdf_export_menu.rs index 4b8ff591..7ee536ab 100644 --- a/rmf_site_editor/src/widgets/sdf_export_menu.rs +++ b/rmf_site_editor/src/widgets/sdf_export_menu.rs @@ -15,10 +15,9 @@ * */ -use crate::menu_bar::{FileMenu, MenuEvent, MenuItem, MenuVisualizationStates}; +use crate::menu_bar::{FileMenu, MenuEvent, MenuItem}; use crate::{AppState, ExportFormat, SaveWorkspace, SaveWorkspaceDestination}; use bevy::prelude::*; -use std::collections::HashSet; /// Keeps track of which entity is associated to the export sdf button. #[derive(Resource)] @@ -34,17 +33,9 @@ impl SdfExportMenu { impl FromWorld for SdfExportMenu { fn from_world(world: &mut World) -> Self { - let site_states = HashSet::from([ - AppState::SiteEditor, - AppState::SiteVisualizer, - AppState::SiteDrawingEditor, - ]); let file_header = world.resource::().get(); let export_sdf = world - .spawn(( - MenuItem::Text("Export Sdf".to_string()), - MenuVisualizationStates(site_states), - )) + .spawn(MenuItem::Text("Export Sdf".to_string())) .set_parent(file_header) .id(); From 721696e4ba6460ce86acd546f3569ce7f7917e3c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 12:33:29 +0800 Subject: [PATCH 05/25] Change state settings for UserCameraDisplay Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/deletion.rs | 5 +- .../src/widgets/user_camera_display.rs | 5 +- rmf_site_editor/src/workspace.rs | 74 ++++++++++++++++--- 3 files changed, 68 insertions(+), 16 deletions(-) diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index 6dbcd38f..1ef59184 100644 --- a/rmf_site_editor/src/site/deletion.rs +++ b/rmf_site_editor/src/site/deletion.rs @@ -22,7 +22,7 @@ use crate::{ Category, CurrentLevel, Dependents, LevelElevation, LevelProperties, NameInSite, SiteUpdateSet, }, - AppState, Issue, + Issue, }; use bevy::{ecs::system::SystemParam, prelude::*}; use rmf_site_format::{Edge, Path, Point}; @@ -106,8 +106,7 @@ impl Plugin for DeletionPlugin { .add_systems( First, handle_deletion_requests - .in_set(SiteUpdateSet::Deletion) - .run_if(AppState::in_displaying_mode()), + .in_set(SiteUpdateSet::Deletion), ); } } diff --git a/rmf_site_editor/src/widgets/user_camera_display.rs b/rmf_site_editor/src/widgets/user_camera_display.rs index 1918d335..9d2d6b8d 100644 --- a/rmf_site_editor/src/widgets/user_camera_display.rs +++ b/rmf_site_editor/src/widgets/user_camera_display.rs @@ -15,7 +15,8 @@ * */ -use crate::{widgets::RenderUiSet, AppState}; +use crate::interaction::InteractionState; +use crate::widgets::RenderUiSet; use bevy::prelude::*; use bevy_egui::EguiContexts; @@ -38,7 +39,7 @@ impl Plugin for UserCameraDisplayPlugin { update_user_camera_display .in_set(UserCameraDisplaySet) .after(RenderUiSet) - .run_if(AppState::in_site_mode()), + .run_if(in_state(InteractionState::Enable)), ); } } diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index 08fb72d8..d0a8a5e7 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -191,6 +191,7 @@ impl Plugin for WorkspacePlugin { .init_resource::() .init_resource::() .init_resource::() + .init_resource::() .init_resource::() .add_systems( Update, @@ -367,6 +368,63 @@ pub fn process_load_workspace_files( } } +/// Services that spawn async file dialogs for various purposes, i.e. loading files, saving files / +/// folders. +#[derive(Resource)] +pub struct FileDialogServices { + /// Open a dialog to pick a file, return its path and data + pub pick_file_for_loading: Service<(), (PathBuf, Vec)>, + /// Pick a file to save data into + // pub save_to_file: Service, ()>, + /// Pick a folder + pub pick_folder: Service<(), PathBuf>, +} + +impl FromWorld for FileDialogServices { + fn from_world(world: &mut World) -> Self { + let pick_file_for_loading = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_async(|_| async move { + if let Some(file) = AsyncFileDialog::new().pick_file().await { + let data = file.read().await; + #[cfg(not(target_arch = "wasm32"))] + let file = file.path().to_path_buf(); + #[cfg(target_arch = "wasm32")] + let file = PathBuf::from(file.file_name()); + return Some((file, data)); + } + None + }) + .cancel_on_none() + .connect(scope.terminate) + }); + + let pick_folder = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_async(|_| async move { + if cfg!(target_arch = "wasm32") { + AsyncFileDialog::new().pick_folder().await.map(|f| f.path().into()) + } else { + warn!("Folder dialogs are not implemented in wasm"); + None + } + }) + .cancel_on_none() + .connect(scope.terminate) + }); + + Self { + pick_file_for_loading, + // save_to_file, + pick_folder, + } + } +} + #[derive(Resource)] /// Services that deal with workspace loading pub struct WorkspaceLoadingServices { @@ -383,22 +441,16 @@ pub struct WorkspaceLoadingServices { impl FromWorld for WorkspaceLoadingServices { fn from_world(world: &mut World) -> Self { let process_load_files = world.spawn_service(process_load_workspace_files); + let pick_file = world.resource::().pick_file_for_loading.clone(); // Spawn all the services let load_workspace_from_dialog = world.spawn_workflow(|scope, builder| { scope .input .chain(builder) - .map_async(|_| async move { - if let Some(file) = AsyncFileDialog::new().pick_file().await { - let data = file.read().await; - #[cfg(not(target_arch = "wasm32"))] - let file = file.path().to_path_buf(); - #[cfg(target_arch = "wasm32")] - let file = PathBuf::from(file.file_name()); - let data = WorkspaceData::new(&file, data)?; - return Some(LoadWorkspaceFile(Some(file), data)); - } - None + .then(pick_file) + .map_async(|(path, data)| async move { + let data = WorkspaceData::new(&path, data)?; + Some(LoadWorkspaceFile(Some(path), data)) }) .cancel_on_none() .then(process_load_files) From 244df49e9dbbc005dc402429d70a242e9d93edfa Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 12:34:38 +0800 Subject: [PATCH 06/25] Cleanup Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/mod.rs | 6 +++--- rmf_site_editor/src/interaction/outline.rs | 4 ++-- .../src/interaction/select/place_object.rs | 4 +--- rmf_site_editor/src/lib.rs | 11 ++++------- rmf_site_editor/src/site/deletion.rs | 3 +-- rmf_site_editor/src/site/drawing_editor/mod.rs | 5 +---- rmf_site_editor/src/site/fuel_cache.rs | 18 ++++++++++-------- rmf_site_editor/src/site/georeference.rs | 8 ++------ rmf_site_editor/src/site/view_menu.rs | 4 ---- .../src/widgets/inspector/inspect_anchor.rs | 2 +- .../src/widgets/inspector/inspect_joint.rs | 2 +- rmf_site_editor/src/workspace.rs | 10 ++++++++-- 12 files changed, 34 insertions(+), 43 deletions(-) diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index f21b0e44..40890dc6 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -16,9 +16,9 @@ */ use crate::site::{ - update_anchor_transforms, CollisionMeshMarker, DoorMarker, FiducialMarker, - FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, MeasurementMarker, - SiteUpdateSet, VisualMeshMarker, WallMarker, + update_anchor_transforms, CollisionMeshMarker, DoorMarker, FiducialMarker, FloorMarker, + LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, MeasurementMarker, SiteUpdateSet, + VisualMeshMarker, WallMarker, }; // TODO(luca) restore // use crate::workcell::WorkcellVisualizationMarker; diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs index 3872ff25..03497d8d 100644 --- a/rmf_site_editor/src/interaction/outline.rs +++ b/rmf_site_editor/src/interaction/outline.rs @@ -19,8 +19,8 @@ use crate::{interaction::*, site::DrawingMarker}; use bevy::render::view::RenderLayers; use bevy_mod_outline::{OutlineBundle, OutlineMode, OutlineRenderLayers, OutlineVolume}; use rmf_site_format::{ - DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, - MeasurementMarker, ModelMarker, PhysicalCameraProperties, PrimitiveShape, WallMarker, + DoorType, FiducialMarker, FloorMarker, LiftCabin, LightKind, LocationTags, MeasurementMarker, + ModelMarker, PhysicalCameraProperties, PrimitiveShape, WallMarker, }; use smallvec::SmallVec; diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs index 16d5b788..c831590b 100644 --- a/rmf_site_editor/src/interaction/select/place_object.rs +++ b/rmf_site_editor/src/interaction/select/place_object.rs @@ -36,9 +36,7 @@ pub struct ObjectPlacementServices { impl ObjectPlacementServices { pub fn from_app(app: &mut App) -> Self { let place_object_2d = spawn_place_object_2d_workflow(app); - Self { - place_object_2d, - } + Self { place_object_2d } } } diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index cdb43b99..e6574246 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -33,9 +33,9 @@ use recency::*; mod shapes; use log::LogHistoryPlugin; +pub mod interaction; pub mod main_menu; pub mod site; -pub mod interaction; pub mod workspace; use workspace::*; @@ -228,12 +228,9 @@ impl Plugin for SiteEditor { )); if self.headless_export.is_none() { - app.add_plugins(( - StandardUiPlugin::default(), - MainMenuPlugin, - )) - // Note order matters, plugins that edit the menus must be initialized after the UI - .add_plugins((site::ViewMenuPlugin, OSMViewPlugin, SiteWireframePlugin)); + app.add_plugins((StandardUiPlugin::default(), MainMenuPlugin)) + // Note order matters, plugins that edit the menus must be initialized after the UI + .add_plugins((site::ViewMenuPlugin, OSMViewPlugin, SiteWireframePlugin)); } // Ref https://github.com/bevyengine/bevy/issues/10877. The default behavior causes issues diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs index 1ef59184..4dd60e27 100644 --- a/rmf_site_editor/src/site/deletion.rs +++ b/rmf_site_editor/src/site/deletion.rs @@ -105,8 +105,7 @@ impl Plugin for DeletionPlugin { .add_event::() .add_systems( First, - handle_deletion_requests - .in_set(SiteUpdateSet::Deletion), + handle_deletion_requests.in_set(SiteUpdateSet::Deletion), ); } } diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs index ae30d7fc..a10c400e 100644 --- a/rmf_site_editor/src/site/drawing_editor/mod.rs +++ b/rmf_site_editor/src/site/drawing_editor/mod.rs @@ -23,10 +23,7 @@ pub use alignment::*; use crate::AppState; use crate::{ interaction::{ChangeProjectionMode, Selection, SuppressHighlight}, - site::{ - DrawingMarker, Edge, MeasurementMarker, NameOfSite, Pending, - PreventDeletion, - }, + site::{DrawingMarker, Edge, MeasurementMarker, NameOfSite, Pending, PreventDeletion}, CurrentWorkspace, WorkspaceMarker, }; use rmf_workcell_format::NameOfWorkcell; diff --git a/rmf_site_editor/src/site/fuel_cache.rs b/rmf_site_editor/src/site/fuel_cache.rs index 7c74bb97..710103f3 100644 --- a/rmf_site_editor/src/site/fuel_cache.rs +++ b/rmf_site_editor/src/site/fuel_cache.rs @@ -38,21 +38,23 @@ pub struct FuelCacheUpdated(Option>); pub struct SetFuelApiKey(pub String); #[derive(Default)] -pub struct FuelPlugin { } +pub struct FuelPlugin {} impl Plugin for FuelPlugin { fn build(&self, app: &mut App) { - app - .add_event::() + app.add_event::() .add_event::() .init_resource::() .init_resource::() .init_resource::() - .add_systems(PostUpdate, - (handle_update_fuel_cache_requests, - read_update_fuel_cache_results, - reload_failed_models_with_new_api_key, - )); + .add_systems( + PostUpdate, + ( + handle_update_fuel_cache_requests, + read_update_fuel_cache_results, + reload_failed_models_with_new_api_key, + ), + ); } } diff --git a/rmf_site_editor/src/site/georeference.rs b/rmf_site_editor/src/site/georeference.rs index 0b30fd75..be1d3e3b 100644 --- a/rmf_site_editor/src/site/georeference.rs +++ b/rmf_site_editor/src/site/georeference.rs @@ -9,9 +9,7 @@ use utm::*; use crate::{ generate_map_tiles, interaction::camera_controls, - widgets::menu_bar::{ - Menu, MenuDisabled, MenuEvent, MenuItem, ToolMenu, ViewMenu, - }, + widgets::menu_bar::{Menu, MenuDisabled, MenuEvent, MenuItem, ToolMenu, ViewMenu}, workspace::CurrentWorkspace, OSMTile, }; @@ -559,9 +557,7 @@ impl FromWorld for OSMMenu { let view_reference = world .spawn(MenuItem::Text("View Reference".to_string())) .id(); - let settings_reference = world - .spawn(MenuItem::Text("Settings".to_string())) - .id(); + let settings_reference = world.spawn(MenuItem::Text("Settings".to_string())).id(); let sub_menu = world .spawn(Menu::from_title("Geographic Offset".to_string())) diff --git a/rmf_site_editor/src/site/view_menu.rs b/rmf_site_editor/src/site/view_menu.rs index 9d0e20b6..086fa430 100644 --- a/rmf_site_editor/src/site/view_menu.rs +++ b/rmf_site_editor/src/site/view_menu.rs @@ -190,10 +190,6 @@ fn handle_view_menu_events( events.visuals.send(toggle(event.source()).into()); } else if event.clicked() && event.source() == view_menu.walls { events.walls.send(toggle(event.source()).into()); - /* - } else if event.clicked() && event.source() == view_menu.origin_axis { - events.origin_axis.send(toggle(event.source()).into()); - */ } } } diff --git a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs index 01cb1ec0..7e2a39f2 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs @@ -27,9 +27,9 @@ use crate::{ // otherwise consider creating a new widget for anchor inspection // workcell::CreateJoint, }; -use rmf_workcell_format::JointProperties; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{DragValue, ImageButton, Ui}; +use rmf_workcell_format::JointProperties; use std::collections::{BTreeMap, BTreeSet}; #[derive(SystemParam)] diff --git a/rmf_site_editor/src/widgets/inspector/inspect_joint.rs b/rmf_site_editor/src/widgets/inspector/inspect_joint.rs index f1e7bb6d..c65e1e66 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_joint.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_joint.rs @@ -19,9 +19,9 @@ use crate::{ site::Dependents, widgets::{prelude::*, Inspect, SelectorWidget}, }; -use rmf_workcell_format::{FrameMarker, JointProperties}; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::Ui; +use rmf_workcell_format::{FrameMarker, JointProperties}; #[derive(SystemParam)] pub struct InspectJoint<'w, 's> { diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index d0a8a5e7..f140f72d 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -407,7 +407,10 @@ impl FromWorld for FileDialogServices { .chain(builder) .map_async(|_| async move { if cfg!(target_arch = "wasm32") { - AsyncFileDialog::new().pick_folder().await.map(|f| f.path().into()) + AsyncFileDialog::new() + .pick_folder() + .await + .map(|f| f.path().into()) } else { warn!("Folder dialogs are not implemented in wasm"); None @@ -441,7 +444,10 @@ pub struct WorkspaceLoadingServices { impl FromWorld for WorkspaceLoadingServices { fn from_world(world: &mut World) -> Self { let process_load_files = world.spawn_service(process_load_workspace_files); - let pick_file = world.resource::().pick_file_for_loading.clone(); + let pick_file = world + .resource::() + .pick_file_for_loading + .clone(); // Spawn all the services let load_workspace_from_dialog = world.spawn_workflow(|scope, builder| { scope From 2884774599e283e5a7b075f04d355fa39e657847 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 13:34:40 +0800 Subject: [PATCH 07/25] Make fields public to allow custom selectors Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/select.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rmf_site_editor/src/interaction/select.rs b/rmf_site_editor/src/interaction/select.rs index c9e810e3..64592f2b 100644 --- a/rmf_site_editor/src/interaction/select.rs +++ b/rmf_site_editor/src/interaction/select.rs @@ -233,15 +233,15 @@ fn process_new_selector( pub struct RunSelector { /// The select workflow will run this service until it terminates and then /// revert back to the inspector selector. - selector: Service, ()>, + pub selector: Service, ()>, /// If there is input for the selector, it will be stored in a [`SelectorInput`] /// component in this entity. The entity will be despawned as soon as the /// input is extracted. - input: Option, + pub input: Option, } #[derive(Component)] -pub struct SelectorInput(T); +pub struct SelectorInput(pub T); /// This component is put on entities with meshes to mark them as items that can /// be interacted with to From 67b07e65456e76a114c9f69281e438881d377ba4 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 16:23:02 +0800 Subject: [PATCH 08/25] WIP removing further appearances of AppState Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/change_plugin.rs | 2 -- rmf_site_editor/src/site/recall_plugin.rs | 2 -- rmf_site_editor/src/widgets/inspector/mod.rs | 3 +++ 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/rmf_site_editor/src/site/change_plugin.rs b/rmf_site_editor/src/site/change_plugin.rs index 46cf2f47..2fdf3e1c 100644 --- a/rmf_site_editor/src/site/change_plugin.rs +++ b/rmf_site_editor/src/site/change_plugin.rs @@ -16,7 +16,6 @@ */ use crate::site::SiteUpdateSet; -use crate::AppState; use bevy::prelude::*; use std::fmt::Debug; @@ -64,7 +63,6 @@ impl Plugin for ChangePlugin { app.add_event::>().add_systems( PreUpdate, update_changed_values:: - .run_if(AppState::in_displaying_mode()) .in_set(SiteUpdateSet::ProcessChanges), ); } diff --git a/rmf_site_editor/src/site/recall_plugin.rs b/rmf_site_editor/src/site/recall_plugin.rs index 36b24987..6c64a9ba 100644 --- a/rmf_site_editor/src/site/recall_plugin.rs +++ b/rmf_site_editor/src/site/recall_plugin.rs @@ -16,7 +16,6 @@ */ use crate::site::SiteUpdateSet; -use crate::AppState; use bevy::prelude::*; use rmf_site_format::Recall; @@ -39,7 +38,6 @@ where add_recaller::.after(SiteUpdateSet::ProcessChanges), update_recaller::.after(SiteUpdateSet::ProcessChanges), ) - .run_if(AppState::in_displaying_mode()), ); } } diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index a616f83f..a76b1703 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -290,10 +290,13 @@ impl<'w, 's> WidgetSystem for Inspector<'w, 's> { state: &mut SystemState, world: &mut World, ) { + // TODO(luca) make sure this doesn't show in building preview mode + /* match world.resource::>().get() { AppState::SiteEditor | AppState::SiteDrawingEditor | AppState::WorkcellEditor => {} _ => return, } + */ CollapsingHeader::new("Inspect") .default_open(true) From b6d2645341af5e6686668bb98954d004f57e7fec Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 17:13:28 +0800 Subject: [PATCH 09/25] Minor refactor and cleanup Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/sdf_loader.rs | 14 +++++++++++++- rmf_site_editor/src/site/change_plugin.rs | 3 +-- rmf_site_editor/src/site/mod.rs | 10 ---------- rmf_site_editor/src/site/recall_plugin.rs | 2 +- rmf_site_editor/src/widgets/menu_bar.rs | 11 ++--------- 5 files changed, 17 insertions(+), 23 deletions(-) diff --git a/rmf_site_editor/src/sdf_loader.rs b/rmf_site_editor/src/sdf_loader.rs index bb6ea7f2..78fb00b1 100644 --- a/rmf_site_editor/src/sdf_loader.rs +++ b/rmf_site_editor/src/sdf_loader.rs @@ -35,7 +35,19 @@ pub struct SdfPlugin; impl Plugin for SdfPlugin { fn build(&self, app: &mut App) { - app.init_asset_loader::(); + // Type registration is necessary to allow serializing the Scene that is loaded by this + // plugin + app.init_asset_loader::() + .register_type::() + .register_type::() + .register_type::() + .register_type::() + .register_type::() + .register_type::() + .register_type::() + .register_type::() + .register_type::() + .register_type::(); } } diff --git a/rmf_site_editor/src/site/change_plugin.rs b/rmf_site_editor/src/site/change_plugin.rs index 2fdf3e1c..cfc6c826 100644 --- a/rmf_site_editor/src/site/change_plugin.rs +++ b/rmf_site_editor/src/site/change_plugin.rs @@ -62,8 +62,7 @@ impl Plugin for ChangePlugin { fn build(&self, app: &mut App) { app.add_event::>().add_systems( PreUpdate, - update_changed_values:: - .in_set(SiteUpdateSet::ProcessChanges), + update_changed_values::.in_set(SiteUpdateSet::ProcessChanges), ); } } diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index 1c323add..ee8e3be8 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -192,16 +192,6 @@ impl Plugin for SitePlugin { .init_resource::() .init_resource::() .init_resource::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() - .register_type::() .add_event::() .add_event::() .add_event::() diff --git a/rmf_site_editor/src/site/recall_plugin.rs b/rmf_site_editor/src/site/recall_plugin.rs index 6c64a9ba..12f470c5 100644 --- a/rmf_site_editor/src/site/recall_plugin.rs +++ b/rmf_site_editor/src/site/recall_plugin.rs @@ -37,7 +37,7 @@ where ( add_recaller::.after(SiteUpdateSet::ProcessChanges), update_recaller::.after(SiteUpdateSet::ProcessChanges), - ) + ), ); } } diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index 62ca4af9..696eaf20 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -15,7 +15,7 @@ * */ -use crate::{widgets::prelude::*, AppState, CreateNewWorkspace, SaveWorkspace, WorkspaceLoader}; +use crate::{widgets::prelude::*, CreateNewWorkspace, SaveWorkspace, WorkspaceLoader}; use bevy::ecs::query::Has; use bevy::prelude::*; @@ -176,8 +176,7 @@ impl MenuEvent { } /// Helper function to render a submenu starting at the entity. -fn render_sub_menu( - state: &State, +pub fn render_sub_menu( ui: &mut Ui, entity: &Entity, children: &Query<&Children>, @@ -218,7 +217,6 @@ fn render_sub_menu( for child in child_items.iter() { render_sub_menu( - state, ui, child, children, @@ -236,7 +234,6 @@ fn render_sub_menu( for child in child_items.iter() { render_sub_menu( - state, ui, child, children, @@ -251,7 +248,6 @@ fn render_sub_menu( #[derive(SystemParam)] struct MenuParams<'w, 's> { - state: Res<'w, State>, menus: Query<'w, 's, (&'static Menu, Entity)>, menu_items: Query<'w, 's, (&'static mut MenuItem, Has)>, extension_events: EventWriter<'w, MenuEvent>, @@ -297,7 +293,6 @@ fn top_menu_bar( } render_sub_menu( - &menu_params.state, ui, &file_menu.get(), &children, @@ -309,7 +304,6 @@ fn top_menu_bar( }); ui.menu_button("View", |ui| { render_sub_menu( - &menu_params.state, ui, &menu_params.view_menu.get(), &children, @@ -325,7 +319,6 @@ fn top_menu_bar( && (*entity != file_menu.get() && *entity != menu_params.view_menu.get()) }) { render_sub_menu( - &menu_params.state, ui, &entity, &children, From 1bb998c85f40a2660e9865779c2a0caa63685cb7 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 9 Sep 2024 18:52:34 +0800 Subject: [PATCH 10/25] Try to make a configurable fuel asset browser Signed-off-by: Luca Della Vedova --- .../src/interaction/select/place_object.rs | 34 ++++++++++- rmf_site_editor/src/widgets/creation.rs | 13 +---- .../src/widgets/fuel_asset_browser.rs | 58 ++++++++----------- 3 files changed, 56 insertions(+), 49 deletions(-) diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs index c831590b..1791dec7 100644 --- a/rmf_site_editor/src/interaction/select/place_object.rs +++ b/rmf_site_editor/src/interaction/select/place_object.rs @@ -15,8 +15,8 @@ * */ -use crate::{interaction::select::*, site::Model}; -use bevy::ecs::system::SystemParam; +use crate::{interaction::select::*, site::{CurrentLevel, Model}}; +use bevy::ecs::system::{Command, SystemState, SystemParam}; #[derive(Default)] pub struct ObjectPlacementPlugin {} @@ -44,10 +44,15 @@ impl ObjectPlacementServices { pub struct ObjectPlacement<'w, 's> { pub services: Res<'w, ObjectPlacementServices>, pub commands: Commands<'w, 's>, + current_level: Res<'w, CurrentLevel>, } impl<'w, 's> ObjectPlacement<'w, 's> { - pub fn place_object_2d(&mut self, object: Model, level: Entity) { + pub fn place_object_2d(&mut self, object: Model) { + let Some(level) = self.current_level.0 else { + warn!("Unble to create [object:?] outside a level"); + return; + }; let state = self .commands .spawn(SelectorInput(PlaceObject2d { object, level })) @@ -64,3 +69,26 @@ impl<'w, 's> ObjectPlacement<'w, 's> { }); } } + +/// Trait to be implemented to allow placing models with commands +pub trait ObjectPlacementExt<'w, 's> { + fn place_object_2d(&mut self, object: Model); +} + +impl<'w, 's> ObjectPlacementExt<'w, 's> for Commands<'w, 's> { + fn place_object_2d(&mut self, object: Model) { + self.add(ObjectPlaceCommand(object)); + } +} + +#[derive(Deref, DerefMut)] +pub struct ObjectPlaceCommand(Model); + +impl Command for ObjectPlaceCommand { + fn apply(self, world: &mut World) { + let mut system_state: SystemState = SystemState::new(world); + let mut placement = system_state.get_mut(world); + placement.place_object_2d(self.0); + system_state.apply(world); + } +} diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index 286d2cec..e3ae5a14 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -19,7 +19,7 @@ use crate::{ inspector::{InspectAssetSourceComponent, InspectScaleComponent}, interaction::{AnchorSelection, ObjectPlacement}, site::{ - AssetSource, CurrentLevel, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale, + AssetSource, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale, }, widgets::{prelude::*, AssetGalleryStatus}, AppState, CurrentWorkspace, @@ -46,7 +46,6 @@ struct Creation<'w, 's> { default_file: Query<'w, 's, &'static DefaultFile>, app_state: Res<'w, State>, current_workspace: Res<'w, CurrentWorkspace>, - current_level: Res<'w, CurrentLevel>, pending_drawings: ResMut<'w, PendingDrawing>, pending_model: ResMut<'w, PendingModel>, asset_gallery: Option>, @@ -195,7 +194,7 @@ impl<'w, 's> Creation<'w, 's> { scale: self.pending_model.scale, ..default() }; - self.spawn_model_2d(model); + self.object_placement.place_object_2d(model); } } AppState::WorkcellEditor => { @@ -231,14 +230,6 @@ impl<'w, 's> Creation<'w, 's> { } }); } - - pub fn spawn_model_2d(&mut self, object: Model) { - if let Some(level) = self.current_level.0 { - self.object_placement.place_object_2d(object, level); - } else { - warn!("Unable to create [{object:?}] outside of a level"); - } - } } #[derive(Resource, Clone, Default)] diff --git a/rmf_site_editor/src/widgets/fuel_asset_browser.rs b/rmf_site_editor/src/widgets/fuel_asset_browser.rs index dad3f826..e615cdbc 100644 --- a/rmf_site_editor/src/widgets/fuel_asset_browser.rs +++ b/rmf_site_editor/src/widgets/fuel_asset_browser.rs @@ -16,25 +16,40 @@ */ use crate::{ - interaction::{ModelPreviewCamera, ObjectPlacement}, + interaction::{ModelPreviewCamera, ObjectPlacementExt}, site::{ - AssetSource, CurrentLevel, FuelClient, Model, ModelSpawningExt, SetFuelApiKey, + AssetSource, FuelClient, Model, ModelSpawningExt, SetFuelApiKey, UpdateFuelCache, }, widgets::prelude::*, - AppState, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{self, Button, ComboBox, ImageSource, RichText, ScrollArea, Ui, Window}; use gz_fuel::FuelModel; /// Add a [`FuelAssetBrowser`] widget to your application. -#[derive(Default)] -pub struct FuelAssetBrowserPlugin {} +pub struct FuelAssetBrowserPlugin { + pub model_spawner: fn(&mut Commands, Model), +} + +impl Default for FuelAssetBrowserPlugin { + fn default() -> Self { + Self { + model_spawner: |commands: &mut Commands, model: Model| { + commands.place_object_2d(model); + }, + } + } +} + +/// Stores the function that uses commands to place models for this application +#[derive(Resource, Deref, DerefMut)] +struct ModelPlacer(fn(&mut Commands, Model)); impl Plugin for FuelAssetBrowserPlugin { fn build(&self, app: &mut App) { - app.init_resource::(); + app.init_resource::() + .insert_resource(ModelPlacer(self.model_spawner)); let panel = PanelWidget::new(fuel_asset_browser_panel, &mut app.world); let widget = Widget::new::(&mut app.world); app.world.spawn((panel, widget)); @@ -81,9 +96,7 @@ pub struct FuelAssetBrowser<'w, 's> { update_cache: EventWriter<'w, UpdateFuelCache>, set_api_key: EventWriter<'w, SetFuelApiKey>, commands: Commands<'w, 's>, - place_object: ObjectPlacement<'w, 's>, - current_level: Res<'w, CurrentLevel>, - app_state: Res<'w, State>, + model_placer: Res<'w, ModelPlacer>, } fn fuel_asset_browser_panel(In(input): In, world: &mut World) { @@ -282,32 +295,7 @@ impl<'w, 's> FuelAssetBrowser<'w, 's> { ), ..default() }; - - match self.app_state.get() { - AppState::SiteEditor => { - if let Some(level) = self.current_level.0 { - self.place_object.place_object_2d(model, level); - } else { - warn!("Cannot spawn a model outside of a workspace"); - } - } - AppState::WorkcellEditor => { - /* - if let Some(workspace) = self.current_workspace.root { - self.place_object.place_object_3d( - PlaceableObject::Model(model), - self.current_selection.0, - workspace, - ); - } else { - warn!("Cannot spawn a model outside of a workspace"); - } - */ - } - _ => { - warn!("Invalid mode for spawning a model: {:?}", &self.app_state); - } - } + (self.model_placer)(&mut self.commands, model); } } } From 9251bb1db20cfba9a4fa29e39c5fa53fc0455983 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 10 Sep 2024 10:47:06 +0800 Subject: [PATCH 11/25] Remove workcell editor appstate Signed-off-by: Luca Della Vedova --- .../src/interaction/select/place_object.rs | 7 +- rmf_site_editor/src/lib.rs | 14 +- .../src/site/drawing_editor/mod.rs | 4 - rmf_site_editor/src/site/mod.rs | 14 +- rmf_site_editor/src/widgets/creation.rs | 251 +++++++----------- .../src/widgets/fuel_asset_browser.rs | 5 +- rmf_site_editor/src/widgets/mod.rs | 2 +- .../src/widgets/sdf_export_menu.rs | 2 +- rmf_site_editor/src/workspace.rs | 83 ------ 9 files changed, 117 insertions(+), 265 deletions(-) diff --git a/rmf_site_editor/src/interaction/select/place_object.rs b/rmf_site_editor/src/interaction/select/place_object.rs index 1791dec7..ce90f861 100644 --- a/rmf_site_editor/src/interaction/select/place_object.rs +++ b/rmf_site_editor/src/interaction/select/place_object.rs @@ -15,8 +15,11 @@ * */ -use crate::{interaction::select::*, site::{CurrentLevel, Model}}; -use bevy::ecs::system::{Command, SystemState, SystemParam}; +use crate::{ + interaction::select::*, + site::{CurrentLevel, Model}, +}; +use bevy::ecs::system::{Command, SystemParam, SystemState}; #[derive(Default)] pub struct ObjectPlacementPlugin {} diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index e6574246..2651eea8 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -81,26 +81,14 @@ pub enum AppState { MainMenu, SiteEditor, SiteVisualizer, - // TODO(luca) remove this - WorkcellEditor, SiteDrawingEditor, } impl AppState { - pub fn in_site_mode() -> impl Condition<()> { - IntoSystem::into_system(|state: Res>| match state.get() { - AppState::SiteEditor | AppState::SiteVisualizer | AppState::SiteDrawingEditor => true, - AppState::MainMenu | AppState::WorkcellEditor => false, - }) - } - pub fn in_displaying_mode() -> impl Condition<()> { IntoSystem::into_system(|state: Res>| match state.get() { AppState::MainMenu => false, - AppState::SiteEditor - | AppState::SiteVisualizer - | AppState::WorkcellEditor - | AppState::SiteDrawingEditor => true, + AppState::SiteEditor | AppState::SiteVisualizer | AppState::SiteDrawingEditor => true, }) } } diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs index a10c400e..454a204a 100644 --- a/rmf_site_editor/src/site/drawing_editor/mod.rs +++ b/rmf_site_editor/src/site/drawing_editor/mod.rs @@ -26,7 +26,6 @@ use crate::{ site::{DrawingMarker, Edge, MeasurementMarker, NameOfSite, Pending, PreventDeletion}, CurrentWorkspace, WorkspaceMarker, }; -use rmf_workcell_format::NameOfWorkcell; #[derive(Clone, Copy, Event)] pub struct BeginEditDrawing(pub Entity); @@ -84,7 +83,6 @@ fn switch_edit_drawing_mode( current_workspace: Res, parent: Query<&Parent, With>, is_site: Query<(), With>, - is_workcell: Query<(), With>, ) { // TODO(@mxgrey): We can make this implementation much cleaner after we // update to the latest version of bevy that distinguishes between inherited @@ -174,8 +172,6 @@ fn switch_edit_drawing_mode( if is_site.contains(w) { app_state.set(AppState::SiteEditor); - } else if is_workcell.contains(w) { - app_state.set(AppState::WorkcellEditor); } else { // This logic can probably be improved with an editor mode stack error!( diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index ee8e3be8..b3da662e 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -283,12 +283,12 @@ impl Plugin for SitePlugin { update_material_for_display_color, ) .after(SiteUpdateSet::ProcessChangesFlush) - .run_if(AppState::in_site_mode()), + .run_if(AppState::in_displaying_mode()), ) .add_systems( Update, (save_site, save_nav_graphs, change_site.before(load_site)) - .run_if(AppState::in_site_mode()), + .run_if(AppState::in_displaying_mode()), ) .add_systems( PostUpdate, @@ -309,7 +309,7 @@ impl Plugin for SitePlugin { add_material_for_display_colors, add_physical_lights, ) - .run_if(AppState::in_site_mode()) + .run_if(AppState::in_displaying_mode()) .in_set(SiteUpdateSet::AssignOrphans), ) .add_systems( @@ -323,7 +323,7 @@ impl Plugin for SitePlugin { export_lights, set_camera_transform_for_changed_site, ) - .run_if(AppState::in_site_mode()) + .run_if(AppState::in_displaying_mode()) .in_set(SiteUpdateSet::BetweenVisibilityAndTransform), ) .add_systems( @@ -346,7 +346,7 @@ impl Plugin for SitePlugin { update_changed_lane, update_lane_for_moved_anchor, ) - .run_if(AppState::in_site_mode()) + .run_if(AppState::in_displaying_mode()) .in_set(SiteUpdateSet::BetweenVisibilityAndTransform), ) .add_systems( @@ -369,7 +369,7 @@ impl Plugin for SitePlugin { update_physical_lights, toggle_physical_lights, ) - .run_if(AppState::in_site_mode()) + .run_if(AppState::in_displaying_mode()) .in_set(SiteUpdateSet::BetweenVisibilityAndTransform), ) .add_systems( @@ -387,7 +387,7 @@ impl Plugin for SitePlugin { update_drawing_rank, add_physical_camera_visuals, ) - .run_if(AppState::in_site_mode()) + .run_if(AppState::in_displaying_mode()) .in_set(SiteUpdateSet::BetweenVisibilityAndTransform), ); } diff --git a/rmf_site_editor/src/widgets/creation.rs b/rmf_site_editor/src/widgets/creation.rs index e3ae5a14..8c5b9b27 100644 --- a/rmf_site_editor/src/widgets/creation.rs +++ b/rmf_site_editor/src/widgets/creation.rs @@ -18,9 +18,7 @@ use crate::{ inspector::{InspectAssetSourceComponent, InspectScaleComponent}, interaction::{AnchorSelection, ObjectPlacement}, - site::{ - AssetSource, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale, - }, + site::{AssetSource, DefaultFile, DrawingBundle, Recall, RecallAssetSource, Scale}, widgets::{prelude::*, AssetGalleryStatus}, AppState, CurrentWorkspace, }; @@ -58,7 +56,7 @@ impl<'w, 's> WidgetSystem for Creation<'w, 's> { fn show(_: Tile, ui: &mut Ui, state: &mut SystemState, world: &mut World) -> () { let mut params = state.get_mut(world); match params.app_state.get() { - AppState::SiteEditor | AppState::SiteDrawingEditor | AppState::WorkcellEditor => {} + AppState::SiteEditor | AppState::SiteDrawingEditor => {} AppState::MainMenu | AppState::SiteVisualizer => return, } CollapsingHeader::new("Create") @@ -71,161 +69,114 @@ impl<'w, 's> WidgetSystem for Creation<'w, 's> { impl<'w, 's> Creation<'w, 's> { pub fn show_widget(&mut self, ui: &mut Ui) { - ui.vertical(|ui| { - match self.app_state.get() { - AppState::MainMenu | AppState::SiteVisualizer => { - return; + ui.vertical(|ui| match self.app_state.get() { + AppState::MainMenu | AppState::SiteVisualizer => { + return; + } + AppState::SiteEditor => { + if ui.button("Lane").clicked() { + self.anchor_selection.create_lanes(); } - AppState::SiteEditor => { - if ui.button("Lane").clicked() { - self.anchor_selection.create_lanes(); - } - - if ui.button("Location").clicked() { - self.anchor_selection.create_location(); - } - if ui.button("Wall").clicked() { - self.anchor_selection.create_walls(); - } - - if ui.button("Door").clicked() { - self.anchor_selection.create_door(); - } + if ui.button("Location").clicked() { + self.anchor_selection.create_location(); + } - if ui.button("Lift").clicked() { - self.anchor_selection.create_lift(); - } + if ui.button("Wall").clicked() { + self.anchor_selection.create_walls(); + } - if ui.button("Floor").clicked() { - self.anchor_selection.create_floor(); - } - if ui.button("Fiducial").clicked() { - self.anchor_selection.create_site_fiducial(); - } + if ui.button("Door").clicked() { + self.anchor_selection.create_door(); + } - ui.add_space(10.0); - CollapsingHeader::new("New drawing") - .default_open(false) - .show(ui, |ui| { - let default_file = self - .current_workspace - .root - .map(|e| self.default_file.get(e).ok()) - .flatten(); - if let Some(new_asset_source) = InspectAssetSourceComponent::new( - &self.pending_drawings.source, - &self.pending_drawings.recall_source, - default_file, - ) - .show(ui) - { - self.pending_drawings - .recall_source - .remember(&new_asset_source); - self.pending_drawings.source = new_asset_source; - } - ui.add_space(5.0); - if ui.button("Add Drawing").clicked() { - self.commands.spawn(DrawingBundle::new(DrawingProperties { - source: self.pending_drawings.source.clone(), - ..default() - })); - } - }); + if ui.button("Lift").clicked() { + self.anchor_selection.create_lift(); } - AppState::SiteDrawingEditor => { - if ui.button("Fiducial").clicked() { - self.anchor_selection.create_drawing_fiducial(); - } - if ui.button("Measurement").clicked() { - self.anchor_selection.create_measurements(); - } + + if ui.button("Floor").clicked() { + self.anchor_selection.create_floor(); } - AppState::WorkcellEditor => { - /* - if ui.button("Frame").clicked() { - self.place_object(PlaceableObject::Anchor); - } - */ + if ui.button("Fiducial").clicked() { + self.anchor_selection.create_site_fiducial(); } - } - match self.app_state.get() { - AppState::MainMenu | AppState::SiteDrawingEditor | AppState::SiteVisualizer => {} - AppState::SiteEditor | AppState::WorkcellEditor => { - ui.add_space(10.0); - CollapsingHeader::new("New model") - .default_open(false) - .show(ui, |ui| { - let default_file = self - .current_workspace - .root - .map(|e| self.default_file.get(e).ok()) - .flatten(); - if let Some(new_asset_source) = InspectAssetSourceComponent::new( - &self.pending_model.source, - &self.pending_model.recall_source, - default_file, - ) - .show(ui) - { - self.pending_model.recall_source.remember(&new_asset_source); - self.pending_model.source = new_asset_source; - } - ui.add_space(5.0); - if let Some(new_scale) = - InspectScaleComponent::new(&self.pending_model.scale).show(ui) - { - self.pending_model.scale = new_scale; - } - ui.add_space(5.0); - if let Some(asset_gallery) = &mut self.asset_gallery { - match self.app_state.get() { - AppState::MainMenu - | AppState::SiteDrawingEditor - | AppState::SiteVisualizer => {} - AppState::SiteEditor => { - if ui.button("Browse fuel").clicked() { - asset_gallery.show = true; - } - if ui.button("Spawn model").clicked() { - let model = Model { - source: self.pending_model.source.clone(), - scale: self.pending_model.scale, - ..default() - }; - self.object_placement.place_object_2d(model); - } - } - AppState::WorkcellEditor => { - /* - if ui.button("Browse fuel").clicked() { - asset_gallery.show = true; - } - if ui.button("Spawn visual").clicked() { - let model = Model { - source: self.pending_model.source.clone(), - scale: Scale(*self.pending_model.scale), - ..default() - }; - self.place_object(PlaceableObject::VisualMesh(model)); - } - if ui.button("Spawn collision").clicked() { - let model = Model { - source: self.pending_model.source.clone(), - scale: Scale(*self.pending_model.scale), - ..default() - }; - self.place_object(PlaceableObject::CollisionMesh( - model, - )); - } - ui.add_space(10.0); - */ - } - } + + ui.add_space(10.0); + CollapsingHeader::new("New drawing") + .default_open(false) + .show(ui, |ui| { + let default_file = self + .current_workspace + .root + .map(|e| self.default_file.get(e).ok()) + .flatten(); + if let Some(new_asset_source) = InspectAssetSourceComponent::new( + &self.pending_drawings.source, + &self.pending_drawings.recall_source, + default_file, + ) + .show(ui) + { + self.pending_drawings + .recall_source + .remember(&new_asset_source); + self.pending_drawings.source = new_asset_source; + } + ui.add_space(5.0); + if ui.button("Add Drawing").clicked() { + self.commands.spawn(DrawingBundle::new(DrawingProperties { + source: self.pending_drawings.source.clone(), + ..default() + })); + } + }); + ui.add_space(10.0); + CollapsingHeader::new("New model") + .default_open(false) + .show(ui, |ui| { + let default_file = self + .current_workspace + .root + .map(|e| self.default_file.get(e).ok()) + .flatten(); + if let Some(new_asset_source) = InspectAssetSourceComponent::new( + &self.pending_model.source, + &self.pending_model.recall_source, + default_file, + ) + .show(ui) + { + self.pending_model.recall_source.remember(&new_asset_source); + self.pending_model.source = new_asset_source; + } + ui.add_space(5.0); + if let Some(new_scale) = + InspectScaleComponent::new(&self.pending_model.scale).show(ui) + { + self.pending_model.scale = new_scale; + } + ui.add_space(5.0); + if let Some(asset_gallery) = &mut self.asset_gallery { + if ui.button("Browse fuel").clicked() { + asset_gallery.show = true; } - }); + } + if ui.button("Spawn model").clicked() { + let model = Model { + source: self.pending_model.source.clone(), + scale: self.pending_model.scale, + ..default() + }; + self.object_placement.place_object_2d(model); + } + }); + } + AppState::SiteDrawingEditor => { + if ui.button("Fiducial").clicked() { + self.anchor_selection.create_drawing_fiducial(); + } + if ui.button("Measurement").clicked() { + self.anchor_selection.create_measurements(); } } }); diff --git a/rmf_site_editor/src/widgets/fuel_asset_browser.rs b/rmf_site_editor/src/widgets/fuel_asset_browser.rs index e615cdbc..0eec9d7d 100644 --- a/rmf_site_editor/src/widgets/fuel_asset_browser.rs +++ b/rmf_site_editor/src/widgets/fuel_asset_browser.rs @@ -17,10 +17,7 @@ use crate::{ interaction::{ModelPreviewCamera, ObjectPlacementExt}, - site::{ - AssetSource, FuelClient, Model, ModelSpawningExt, SetFuelApiKey, - UpdateFuelCache, - }, + site::{AssetSource, FuelClient, Model, ModelSpawningExt, SetFuelApiKey, UpdateFuelCache}, widgets::prelude::*, }; use bevy::{ecs::system::SystemParam, prelude::*}; diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index ad8aef49..76e7d68f 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -174,7 +174,7 @@ impl Plugin for StandardUiPlugin { resolve_light_export_file, resolve_nav_graph_import_export_files, ) - .run_if(AppState::in_site_mode()), + .run_if(AppState::in_displaying_mode()), ); } } diff --git a/rmf_site_editor/src/widgets/sdf_export_menu.rs b/rmf_site_editor/src/widgets/sdf_export_menu.rs index 7ee536ab..bddcf674 100644 --- a/rmf_site_editor/src/widgets/sdf_export_menu.rs +++ b/rmf_site_editor/src/widgets/sdf_export_menu.rs @@ -65,7 +65,7 @@ impl Plugin for SdfExportMenuPlugin { fn build(&self, app: &mut App) { app.init_resource::().add_systems( Update, - handle_export_sdf_menu_events.run_if(AppState::in_site_mode()), + handle_export_sdf_menu_events.run_if(AppState::in_displaying_mode()), ); } } diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index f140f72d..fe4fc942 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -22,11 +22,9 @@ use std::path::PathBuf; use crate::interaction::InteractionState; use crate::site::{DefaultFile, LoadSite, SaveSite}; -// use crate::workcell::{LoadWorkcell, SaveWorkcell}; use crate::AppState; use rmf_site_format::legacy::building_map::BuildingMap; use rmf_site_format::{NameOfSite, Site}; -use rmf_workcell_format::Workcell; use crossbeam_channel::{Receiver, Sender}; @@ -51,8 +49,6 @@ pub enum WorkspaceData { LegacyBuilding(Vec), RonSite(Vec), JsonSite(Vec), - Workcell(Vec), - WorkcellUrdf(Vec), LoadSite(LoadSite), } @@ -65,10 +61,6 @@ impl WorkspaceData { Some(WorkspaceData::RonSite(data)) } else if filename.ends_with("site.json") { Some(WorkspaceData::JsonSite(data)) - } else if filename.ends_with("workcell.json") { - Some(WorkspaceData::Workcell(data)) - } else if filename.ends_with("urdf") { - Some(WorkspaceData::WorkcellUrdf(data)) } else { error!("Unrecognized file type {:?}", filename); None @@ -186,8 +178,6 @@ impl Plugin for WorkspacePlugin { app.add_event::() .add_event::() .add_event::() - //.add_event::() - //.add_event::() .init_resource::() .init_resource::() .init_resource::() @@ -210,7 +200,6 @@ 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.read().last() { match state.get() { @@ -224,15 +213,6 @@ pub fn dispatch_new_workspace_events( default_file: None, }); } - AppState::WorkcellEditor => { - /* - load_workcell.send(LoadWorkcell { - workcell: Workcell::default(), - focus: true, - default_file: None, - }); - */ - } } } } @@ -243,7 +223,6 @@ pub fn process_load_workspace_files( mut app_state: ResMut>, mut interaction_state: ResMut>, mut load_site: EventWriter, - // mut load_workcell: EventWriter, ) { let LoadWorkspaceFile(default_file, data) = request; match data { @@ -308,58 +287,6 @@ pub fn process_load_workspace_files( } } } - WorkspaceData::Workcell(data) => { - info!("Opening workcell file"); - match Workcell::from_bytes(&data) { - Ok(_workcell) => { - // Switch state - app_state.set(AppState::WorkcellEditor); - /* - load_workcell.send(LoadWorkcell { - workcell, - focus: true, - default_file, - }); - */ - interaction_state.set(InteractionState::Enable); - } - Err(err) => { - error!("Failed loading workcell {:?}", err); - } - } - } - WorkspaceData::WorkcellUrdf(data) => { - info!("Importing urdf workcell"); - let Ok(utf) = std::str::from_utf8(&data) else { - error!("Failed converting urdf bytes to string"); - return; - }; - match urdf_rs::read_from_string(utf) { - Ok(urdf) => { - // TODO(luca) make this function return a result and this a match statement - match Workcell::from_urdf(&urdf) { - Ok(_workcell) => { - // Switch state - app_state.set(AppState::WorkcellEditor); - /* - load_workcell.send(LoadWorkcell { - workcell, - focus: true, - default_file, - }); - */ - interaction_state.set(InteractionState::Enable); - } - Err(err) => { - error!("Failed converting urdf to workcell {:?}", err); - } - } - } - Err(err) => { - error!("Failed loading urdf workcell {:?}", err); - } - } - } WorkspaceData::LoadSite(site) => { app_state.set(AppState::SiteEditor); load_site.send(site); @@ -639,20 +566,10 @@ fn dispatch_save_workspace_events( fn workspace_file_save_complete( app_state: Res>, mut save_site: EventWriter, - // mut save_workcell: EventWriter, save_channels: Res, ) { if let Ok(result) = save_channels.receiver.try_recv() { match app_state.get() { - AppState::WorkcellEditor => { - /* - save_workcell.send(SaveWorkcell { - root: result.root, - to_file: result.path, - format: result.format, - }); - */ - } AppState::SiteEditor | AppState::SiteDrawingEditor | AppState::SiteVisualizer => { save_site.send(SaveSite { site: result.root, From 605fbe93aed2f463922f9dd36cd288e5ba37e86f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 10 Sep 2024 12:51:25 +0800 Subject: [PATCH 12/25] Cleanup workspace saving and transition to workflows Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/keyboard.rs | 8 +- rmf_site_editor/src/site/save.rs | 27 +- rmf_site_editor/src/site/sdf_exporter.rs | 6 +- rmf_site_editor/src/widgets/menu_bar.rs | 8 +- rmf_site_editor/src/widgets/mod.rs | 1 + .../src/widgets/sdf_export_menu.rs | 9 +- rmf_site_editor/src/workspace.rs | 345 +++++++++--------- 7 files changed, 206 insertions(+), 198 deletions(-) diff --git a/rmf_site_editor/src/keyboard.rs b/rmf_site_editor/src/keyboard.rs index b99838a5..2e13cf4f 100644 --- a/rmf_site_editor/src/keyboard.rs +++ b/rmf_site_editor/src/keyboard.rs @@ -18,7 +18,7 @@ use crate::{ interaction::{ChangeProjectionMode, Selection}, site::{AlignSiteDrawings, Delete}, - CreateNewWorkspace, CurrentWorkspace, SaveWorkspace, WorkspaceLoader, + CreateNewWorkspace, CurrentWorkspace, WorkspaceLoader, WorkspaceSaver, }; use bevy::{ prelude::{Input as UserInput, *}, @@ -57,7 +57,6 @@ fn handle_keyboard_input( selection: Res, mut egui_context: EguiContexts, mut delete: EventWriter, - mut save_workspace: EventWriter, mut new_workspace: EventWriter, mut change_camera_mode: EventWriter, mut debug_mode: ResMut, @@ -65,6 +64,7 @@ fn handle_keyboard_input( current_workspace: Res, primary_windows: Query>, mut workspace_loader: WorkspaceLoader, + mut workspace_saver: WorkspaceSaver, ) { let Some(egui_context) = primary_windows .get_single() @@ -106,9 +106,9 @@ fn handle_keyboard_input( if keyboard_input.any_pressed([KeyCode::ControlLeft, KeyCode::ControlRight]) { if keyboard_input.just_pressed(KeyCode::S) { if keyboard_input.any_pressed([KeyCode::ShiftLeft, KeyCode::ShiftRight]) { - save_workspace.send(SaveWorkspace::new().to_dialog()); + workspace_saver.save_to_dialog(); } else { - save_workspace.send(SaveWorkspace::new().to_default_file()); + workspace_saver.save_to_default_file(); } } diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index 9ef11cff..e3400a38 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -1307,25 +1307,24 @@ pub fn save_site(world: &mut World) { }; info!("Saving to {}", new_path.display()); - let Some(parent_folder) = new_path.parent() else { - error!("Unable to save SDF. Please select a save path that has a parent directory."); - continue; - }; - if !parent_folder.exists() { - if let Err(e) = std::fs::create_dir_all(parent_folder) { - error!("Unable to create folder {}: {e}", parent_folder.display()); + if !new_path.exists() { + if let Err(e) = std::fs::create_dir_all(&new_path) { + error!("Unable to create folder {}: {e}", new_path.display()); continue; } } - let f = match std::fs::File::create(&new_path) { + let mut sdf_path = new_path.clone(); + sdf_path.push(&site.properties.name.0); + sdf_path.set_extension("world"); + let f = match std::fs::File::create(&sdf_path) { Ok(f) => f, Err(err) => { - error!("Unable to save file {}: {err}", new_path.display()); + error!("Unable to save file {}: {err}", sdf_path.display()); continue; } }; - let mut meshes_dir = PathBuf::from(parent_folder); + let mut meshes_dir = new_path.clone(); meshes_dir.push("meshes"); if let Err(e) = std::fs::create_dir_all(&meshes_dir) { error!("Unable to create folder {}: {e}", meshes_dir.display()); @@ -1336,7 +1335,7 @@ pub fn save_site(world: &mut World) { continue; } - migrate_relative_paths(save_event.site, &new_path, world); + migrate_relative_paths(save_event.site, &sdf_path, world); let graphs = legacy::nav_graph::NavGraph::from_site(&site); let sdf = match site.to_sdf() { Ok(sdf) => sdf, @@ -1354,7 +1353,7 @@ pub fn save_site(world: &mut World) { error!("Failed serializing site to sdf: {e}"); continue; } - let mut navgraph_dir = PathBuf::from(parent_folder); + let mut navgraph_dir = new_path.clone(); navgraph_dir.push("nav_graphs"); if let Err(e) = std::fs::create_dir_all(&navgraph_dir) { error!("Unable to create folder {}: {e}", navgraph_dir.display()); @@ -1379,10 +1378,6 @@ pub fn save_site(world: &mut World) { } } } - ExportFormat::Urdf => { - warn!("Site exporting to Urdf is not supported."); - continue; - } } } } diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs index eae8ba69..bf867689 100644 --- a/rmf_site_editor/src/site/sdf_exporter.rs +++ b/rmf_site_editor/src/site/sdf_exporter.rs @@ -4,7 +4,7 @@ use bevy_gltf_export::{export_meshes, CompressGltfOptions, MeshData}; use std::path::Path; -use crate::SaveWorkspace; +use crate::WorkspaceSaver; use crate::{ site::{ @@ -44,7 +44,7 @@ impl HeadlessSdfExportState { pub fn headless_sdf_export( mut commands: Commands, - mut export: EventWriter, + mut workspace_saver: WorkspaceSaver, mut exit: EventWriter, missing_models: Query<(), With>, mut export_state: ResMut, @@ -85,7 +85,7 @@ pub fn headless_sdf_export( } else { if !export_state.save_requested && export_state.iterations > 5 { let path = std::path::PathBuf::from(export_state.target_path.clone()); - export.send(SaveWorkspace::new().to_sdf().to_path(&path)); + workspace_saver.export_sdf_to_path(path); export_state.save_requested = true; export_state.iterations = 0; } else if export_state.save_requested && export_state.iterations > 5 { diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index 696eaf20..101496f8 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -15,7 +15,7 @@ * */ -use crate::{widgets::prelude::*, CreateNewWorkspace, SaveWorkspace, WorkspaceLoader}; +use crate::{widgets::prelude::*, CreateNewWorkspace, WorkspaceLoader, WorkspaceSaver}; use bevy::ecs::query::Has; use bevy::prelude::*; @@ -257,8 +257,8 @@ struct MenuParams<'w, 's> { fn top_menu_bar( In(input): In, mut new_workspace: EventWriter, - mut save: EventWriter, mut workspace_loader: WorkspaceLoader, + mut workspace_saver: WorkspaceSaver, file_menu: Res, top_level_components: Query<(), Without>, children: Query<&Children>, @@ -276,13 +276,13 @@ fn top_menu_bar( .add(Button::new("Save").shortcut_text("Ctrl+S")) .clicked() { - save.send(SaveWorkspace::new().to_default_file()); + workspace_saver.save_to_default_file(); } if ui .add(Button::new("Save As").shortcut_text("Ctrl+Shift+S")) .clicked() { - save.send(SaveWorkspace::new().to_dialog()); + workspace_saver.save_to_dialog(); } } if ui diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 76e7d68f..c28bfd77 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -154,6 +154,7 @@ impl Plugin for StandardUiPlugin { .add_plugins(( IconsPlugin::default(), MenuBarPlugin::default(), + #[cfg(not(target_arch = "wasm32"))] SdfExportMenuPlugin::default(), StandardPropertiesPanelPlugin::default(), FuelAssetBrowserPlugin::default(), diff --git a/rmf_site_editor/src/widgets/sdf_export_menu.rs b/rmf_site_editor/src/widgets/sdf_export_menu.rs index bddcf674..14ad65cb 100644 --- a/rmf_site_editor/src/widgets/sdf_export_menu.rs +++ b/rmf_site_editor/src/widgets/sdf_export_menu.rs @@ -16,7 +16,7 @@ */ use crate::menu_bar::{FileMenu, MenuEvent, MenuItem}; -use crate::{AppState, ExportFormat, SaveWorkspace, SaveWorkspaceDestination}; +use crate::{AppState, WorkspaceSaver}; use bevy::prelude::*; /// Keeps track of which entity is associated to the export sdf button. @@ -46,14 +46,11 @@ impl FromWorld for SdfExportMenu { fn handle_export_sdf_menu_events( mut menu_events: EventReader, sdf_menu: Res, - mut save_events: EventWriter, + mut workspace_saver: WorkspaceSaver, ) { for event in menu_events.read() { if event.clicked() && event.source() == sdf_menu.get() { - save_events.send(SaveWorkspace { - destination: SaveWorkspaceDestination::Dialog, - format: ExportFormat::Sdf, - }); + workspace_saver.export_sdf_to_dialog(); } } } diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index fe4fc942..52be6758 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -15,7 +15,7 @@ * */ -use bevy::{ecs::system::SystemParam, prelude::*, tasks::AsyncComputeTaskPool}; +use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_impulse::*; use rfd::AsyncFileDialog; use std::path::PathBuf; @@ -26,8 +26,6 @@ use crate::AppState; use rmf_site_format::legacy::building_map::BuildingMap; use rmf_site_format::{NameOfSite, Site}; -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, Event)] pub struct ChangeCurrentWorkspace { @@ -79,87 +77,13 @@ pub struct CurrentWorkspace { pub struct LoadWorkspaceFile(pub Option, pub WorkspaceData); -#[derive(Event)] -pub struct SaveWorkspace { - /// If specified workspace will be saved to requested file, otherwise the default file - pub destination: SaveWorkspaceDestination, - /// If specified the workspace will be exported to a specific format - pub format: ExportFormat, -} - -impl SaveWorkspace { - pub fn new() -> Self { - Self { - destination: SaveWorkspaceDestination::default(), - format: ExportFormat::default(), - } - } - - pub fn to_default_file(mut self) -> Self { - self.destination = SaveWorkspaceDestination::DefaultFile; - self - } - - pub fn to_dialog(mut self) -> Self { - self.destination = SaveWorkspaceDestination::Dialog; - self - } - - pub fn to_path(mut self, path: &PathBuf) -> Self { - self.destination = SaveWorkspaceDestination::Path(path.clone()); - self - } - - pub fn to_urdf(mut self) -> Self { - self.format = ExportFormat::Urdf; - self - } - - pub fn to_sdf(mut self) -> Self { - self.format = ExportFormat::Sdf; - self - } -} - -#[derive(Default, Debug, Clone)] -pub enum SaveWorkspaceDestination { - #[default] - DefaultFile, - Dialog, - Path(PathBuf), -} - #[derive(Clone, Default, Debug)] pub enum ExportFormat { #[default] Default, - Urdf, Sdf, } -/// Event used in channels to communicate the file handle that was chosen by the user. -#[derive(Debug)] -pub struct SaveWorkspaceFile { - path: PathBuf, - format: ExportFormat, - root: Entity, -} - -/// Use a channel since file dialogs are async and channel senders can be cloned and passed into an -/// async block. -#[derive(Debug, Resource)] -pub struct SaveWorkspaceChannels { - pub sender: Sender, - pub receiver: Receiver, -} - -impl Default for SaveWorkspaceChannels { - 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); @@ -177,22 +101,15 @@ impl Plugin for WorkspacePlugin { fn build(&self, app: &mut App) { app.add_event::() .add_event::() - .add_event::() .init_resource::() .init_resource::() - .init_resource::() .init_resource::() .init_resource::() + .init_resource::() .add_systems( Update, - ( - dispatch_new_workspace_events, - sync_workspace_visibility, - workspace_file_save_complete, - ), + (dispatch_new_workspace_events, sync_workspace_visibility), ); - #[cfg(not(target_arch = "wasm32"))] - app.add_systems(Update, dispatch_save_workspace_events); } } @@ -300,16 +217,16 @@ pub fn process_load_workspace_files( #[derive(Resource)] pub struct FileDialogServices { /// Open a dialog to pick a file, return its path and data - pub pick_file_for_loading: Service<(), (PathBuf, Vec)>, + pub pick_file_and_load: Service<(), (PathBuf, Vec)>, /// Pick a file to save data into - // pub save_to_file: Service, ()>, + pub pick_file_for_saving: Service<(), PathBuf>, /// Pick a folder pub pick_folder: Service<(), PathBuf>, } impl FromWorld for FileDialogServices { fn from_world(world: &mut World) -> Self { - let pick_file_for_loading = world.spawn_workflow(|scope, builder| { + let pick_file_and_load = world.spawn_workflow(|scope, builder| { scope .input .chain(builder) @@ -328,17 +245,36 @@ impl FromWorld for FileDialogServices { .connect(scope.terminate) }); + let pick_file_for_saving = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_async(|_| async move { + let file = AsyncFileDialog::new().save_file().await?; + #[cfg(not(target_arch = "wasm32"))] + let file = file.path().to_path_buf(); + #[cfg(target_arch = "wasm32")] + let file = PathBuf::from(file.file_name()); + Some(file) + }) + .cancel_on_none() + .connect(scope.terminate) + }); + let pick_folder = world.spawn_workflow(|scope, builder| { scope .input .chain(builder) .map_async(|_| async move { - if cfg!(target_arch = "wasm32") { + #[cfg(not(target_arch = "wasm32"))] + { AsyncFileDialog::new() .pick_folder() .await .map(|f| f.path().into()) - } else { + } + #[cfg(target_arch = "wasm32")] + { warn!("Folder dialogs are not implemented in wasm"); None } @@ -348,8 +284,8 @@ impl FromWorld for FileDialogServices { }); Self { - pick_file_for_loading, - // save_to_file, + pick_file_and_load, + pick_file_for_saving, pick_folder, } } @@ -373,7 +309,7 @@ impl FromWorld for WorkspaceLoadingServices { let process_load_files = world.spawn_service(process_load_workspace_files); let pick_file = world .resource::() - .pick_file_for_loading + .pick_file_and_load .clone(); // Spawn all the services let load_workspace_from_dialog = world.spawn_workflow(|scope, builder| { @@ -501,87 +437,166 @@ pub struct WorkspaceLoader<'w, 's> { commands: Commands<'w, 's>, } -// TODO(luca) implement this in wasm, it's supported since rfd version 0.12, however it requires -// calling .write on the `FileHandle` object returned by the AsyncFileDialog. Such FileHandle is -// not Send in wasm so it can't be sent to another thread through an event. We would need to -// refactor saving to be fully done in the async task rather than send an event to have wasm saving. -#[cfg(not(target_arch = "wasm32"))] -fn dispatch_save_workspace_events( - mut save_events: EventReader, - save_channels: Res, - workspace: Res, - default_files: Query<&DefaultFile>, +/// Handles the file saving events +fn send_file_save( + In(BlockingService { request, .. }): BlockingServiceInput<(PathBuf, ExportFormat)>, + app_state: Res>, + mut save_site: EventWriter, + current_workspace: Res, ) { - let spawn_dialog = |format: &ExportFormat, root| { - let sender = save_channels.sender.clone(); - let format = format.clone(); - AsyncComputeTaskPool::get() - .spawn(async move { - if let Some(file) = AsyncFileDialog::new().save_file().await { - let path = file.path().to_path_buf(); - sender - .send(SaveWorkspaceFile { path, format, root }) - .expect("Failed sending save event"); - } - }) - .detach(); + let Some(ws_root) = current_workspace.root else { + warn!("Failed saving workspace, no current workspace found"); + return; }; - for event in save_events.read() { - if let Some(ws_root) = workspace.root { - match &event.destination { - SaveWorkspaceDestination::DefaultFile => { - if let Some(file) = default_files.get(ws_root).ok().map(|f| f.0.clone()) { - save_channels - .sender - .send(SaveWorkspaceFile { - path: file, - format: event.format.clone(), - root: ws_root, - }) - .expect("Failed sending save request"); - } else { - spawn_dialog(&event.format, ws_root); - } - } - SaveWorkspaceDestination::Dialog => spawn_dialog(&event.format, ws_root), - SaveWorkspaceDestination::Path(path) => { - save_channels - .sender - .send(SaveWorkspaceFile { - path: path.clone(), - format: event.format.clone(), - root: ws_root, - }) - .expect("Failed sending save request"); - } - } - } else { - warn!("Unable to save, no workspace loaded"); - return; + match app_state.get() { + AppState::SiteEditor | AppState::SiteDrawingEditor | AppState::SiteVisualizer => { + save_site.send(SaveSite { + site: ws_root, + to_file: request.0, + format: request.1, + }); } + AppState::MainMenu => { /* Noop */ } } } -/// Handles the file saving events -fn workspace_file_save_complete( - app_state: Res>, - mut save_site: EventWriter, - save_channels: Res, -) { - if let Ok(result) = save_channels.receiver.try_recv() { - match app_state.get() { - AppState::SiteEditor | AppState::SiteDrawingEditor | AppState::SiteVisualizer => { - save_site.send(SaveSite { - site: result.root, - to_file: result.path, - format: result.format, - }); - } - AppState::MainMenu => { /* Noop */ } +#[derive(Resource)] +/// Services that deal with workspace loading +pub struct WorkspaceSavingServices { + /// Service that spawns a save file dialog and saves the current site accordingly. + pub save_workspace_to_dialog: Service<(), ()>, + /// Saves the current workspace at the requested path. + pub save_workspace_to_path: Service, + /// Saves the current workspace in the current default file. + pub save_workspace_to_default_file: Service<(), ()>, + /// Opens a dialog to pick a folder and exports the requested workspace as an SDF. + pub export_sdf_to_dialog: Service<(), ()>, + /// Exports the requested workspace as an SDF in the requested path. + pub export_sdf_to_path: Service, +} + +impl FromWorld for WorkspaceSavingServices { + fn from_world(world: &mut World) -> Self { + let send_file_save = world.spawn_service(send_file_save); + let get_default_file = |In(()): In<_>, + current_workspace: Res, + default_files: Query<&DefaultFile>| + -> Option { + let ws_root = current_workspace.root?; + default_files.get(ws_root).ok().map(|f| f.0.clone()) + }; + let get_default_file = get_default_file.into_blocking_callback(); + let pick_file = world + .resource::() + .pick_file_for_saving + .clone(); + let pick_folder = world.resource::().pick_folder.clone(); + // Spawn all the services + let save_workspace_to_dialog = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .then(pick_file) + .map_block(|path| (path, ExportFormat::default())) + .then(send_file_save) + .connect(scope.terminate) + }); + let save_workspace_to_path = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_block(|path| (path, ExportFormat::default())) + .then(send_file_save) + .connect(scope.terminate) + }); + let save_workspace_to_default_file = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .then(get_default_file) + .branch_for_none(|chain: Chain<()>| { + chain + .then(save_workspace_to_dialog) + .connect(scope.terminate) + }) + .then(save_workspace_to_path) + .connect(scope.terminate) + }); + let export_sdf_to_dialog = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .then(pick_folder) + .map_block(|path| (path, ExportFormat::Sdf)) + .then(send_file_save) + .connect(scope.terminate) + }); + let export_sdf_to_path = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_block(|path| (path, ExportFormat::Sdf)) + .then(send_file_save) + .connect(scope.terminate) + }); + + Self { + save_workspace_to_dialog, + save_workspace_to_path, + save_workspace_to_default_file, + export_sdf_to_dialog, + export_sdf_to_path, } } } +// TODO(luca) implement saving in wasm, it's supported since rfd version 0.12, however it requires +// calling .write on the `FileHandle` object returned by the AsyncFileDialog. Such FileHandle is +// not Send in wasm so it can't be sent to another thread through an event. We would need to +// refactor saving to be fully done in the async task rather than send an event to have wasm saving. +impl<'w, 's> WorkspaceSaver<'w, 's> { + /// Request to spawn a dialog and load a workspace + pub fn save_to_dialog(&mut self) { + self.commands + .request((), self.workspace_saving.save_workspace_to_dialog) + .detach(); + } + + pub fn save_to_default_file(&mut self) { + self.commands + .request((), self.workspace_saving.save_workspace_to_default_file) + .detach(); + } + + /// Request to spawn a dialog to select a file and create a new site with a blank level + pub fn save_to_path(&mut self, path: PathBuf) { + self.commands + .request(path, self.workspace_saving.save_workspace_to_path) + .detach(); + } + + /// Request to load a workspace from a path + pub fn export_sdf_to_dialog(&mut self) { + self.commands + .request((), self.workspace_saving.export_sdf_to_dialog) + .detach(); + } + + /// Request to load a workspace from data + pub fn export_sdf_to_path(&mut self, path: PathBuf) { + self.commands + .request(path, self.workspace_saving.export_sdf_to_path) + .detach(); + } +} + +/// `SystemParam` used to request for workspace loading operations +#[derive(SystemParam)] +pub struct WorkspaceSaver<'w, 's> { + workspace_saving: Res<'w, WorkspaceSavingServices>, + commands: Commands<'w, 's>, +} + pub fn sync_workspace_visibility( current_workspace: Res, mut recall: ResMut, From 007f5248569138c213f30e5d666401a8aee5b487 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 10 Sep 2024 16:01:28 +0800 Subject: [PATCH 13/25] Make file buttons fully modular Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/keyboard.rs | 4 + rmf_site_editor/src/site/georeference.rs | 10 +- rmf_site_editor/src/widgets/diagnostics.rs | 2 +- rmf_site_editor/src/widgets/menu_bar.rs | 66 +++++++------ rmf_site_editor/src/widgets/mod.rs | 6 +- .../src/widgets/sdf_export_menu.rs | 6 +- rmf_site_editor/src/widgets/workspace.rs | 97 +++++++++++++++++++ 7 files changed, 148 insertions(+), 43 deletions(-) create mode 100644 rmf_site_editor/src/widgets/workspace.rs diff --git a/rmf_site_editor/src/keyboard.rs b/rmf_site_editor/src/keyboard.rs index 2e13cf4f..739c014d 100644 --- a/rmf_site_editor/src/keyboard.rs +++ b/rmf_site_editor/src/keyboard.rs @@ -118,6 +118,10 @@ fn handle_keyboard_input( } } + if keyboard_input.just_pressed(KeyCode::E) { + workspace_saver.export_sdf_to_dialog(); + } + // TODO(luca) pop up a confirmation prompt if the current file is not saved, or create a // gui to switch between open workspaces if keyboard_input.just_pressed(KeyCode::N) { diff --git a/rmf_site_editor/src/site/georeference.rs b/rmf_site_editor/src/site/georeference.rs index be1d3e3b..a55c3943 100644 --- a/rmf_site_editor/src/site/georeference.rs +++ b/rmf_site_editor/src/site/georeference.rs @@ -551,13 +551,9 @@ pub struct OSMMenu { impl FromWorld for OSMMenu { fn from_world(world: &mut World) -> Self { // Tools menu - let set_reference = world - .spawn(MenuItem::Text("Set Reference".to_string())) - .id(); - let view_reference = world - .spawn(MenuItem::Text("View Reference".to_string())) - .id(); - let settings_reference = world.spawn(MenuItem::Text("Settings".to_string())).id(); + let set_reference = world.spawn(MenuItem::Text("Set Reference".into())).id(); + let view_reference = world.spawn(MenuItem::Text("View Reference".into())).id(); + let settings_reference = world.spawn(MenuItem::Text("Settings".into())).id(); let sub_menu = world .spawn(Menu::from_title("Geographic Offset".to_string())) diff --git a/rmf_site_editor/src/widgets/diagnostics.rs b/rmf_site_editor/src/widgets/diagnostics.rs index 365bb4d0..82e9a1a2 100644 --- a/rmf_site_editor/src/widgets/diagnostics.rs +++ b/rmf_site_editor/src/widgets/diagnostics.rs @@ -241,7 +241,7 @@ impl FromWorld for IssueMenu { // Tools menu let tool_header = world.resource::().get(); let diagnostic_tool = world - .spawn(MenuItem::Text("Diagnostic Tool".to_string())) + .spawn(MenuItem::Text("Diagnostic Tool".into())) .set_parent(tool_header) .id(); diff --git a/rmf_site_editor/src/widgets/menu_bar.rs b/rmf_site_editor/src/widgets/menu_bar.rs index 101496f8..193d9475 100644 --- a/rmf_site_editor/src/widgets/menu_bar.rs +++ b/rmf_site_editor/src/widgets/menu_bar.rs @@ -15,7 +15,7 @@ * */ -use crate::{widgets::prelude::*, CreateNewWorkspace, WorkspaceLoader, WorkspaceSaver}; +use crate::widgets::prelude::*; use bevy::ecs::query::Has; use bevy::prelude::*; @@ -65,7 +65,8 @@ impl Menu { #[derive(Component)] #[non_exhaustive] pub enum MenuItem { - Text(String), + /// Text + Shortcut hint if available + Text(TextMenuItem), CheckBox(String, bool), } @@ -85,6 +86,31 @@ impl MenuItem { } } +pub struct TextMenuItem { + pub text: String, + pub shortcut: Option, +} + +impl From<&str> for TextMenuItem { + fn from(text: &str) -> Self { + Self { + text: text.into(), + shortcut: None, + } + } +} + +impl TextMenuItem { + pub fn new(text: &str) -> Self { + text.into() + } + + pub fn shortcut(mut self, shortcut: &str) -> Self { + self.shortcut = Some(shortcut.into()); + self + } +} + /// This resource provides the root entity for the file menu #[derive(Resource)] pub struct FileMenu { @@ -188,8 +214,12 @@ pub fn render_sub_menu( if let Ok((e, disabled)) = menu_items.get(*entity) { // Draw ui match e { - MenuItem::Text(title) => { - if ui.add_enabled(!disabled, Button::new(title)).clicked() { + MenuItem::Text(item) => { + let mut button = Button::new(&item.text); + if let Some(ref shortcut) = &item.shortcut { + button = button.shortcut_text(shortcut); + } + if ui.add_enabled(!disabled, button).clicked() { extension_events.send(MenuEvent::MenuClickEvent(*entity)); } } @@ -256,9 +286,6 @@ struct MenuParams<'w, 's> { fn top_menu_bar( In(input): In, - mut new_workspace: EventWriter, - mut workspace_loader: WorkspaceLoader, - mut workspace_saver: WorkspaceSaver, file_menu: Res, top_level_components: Query<(), Without>, children: Query<&Children>, @@ -267,31 +294,6 @@ fn top_menu_bar( egui::TopBottomPanel::top("top_panel").show(&input.context, |ui| { egui::menu::bar(ui, |ui| { ui.menu_button("File", |ui| { - if ui.add(Button::new("New").shortcut_text("Ctrl+N")).clicked() { - new_workspace.send(CreateNewWorkspace); - } - #[cfg(not(target_arch = "wasm32"))] - { - if ui - .add(Button::new("Save").shortcut_text("Ctrl+S")) - .clicked() - { - workspace_saver.save_to_default_file(); - } - if ui - .add(Button::new("Save As").shortcut_text("Ctrl+Shift+S")) - .clicked() - { - workspace_saver.save_to_dialog(); - } - } - if ui - .add(Button::new("Open").shortcut_text("Ctrl+O")) - .clicked() - { - workspace_loader.load_from_dialog(); - } - render_sub_menu( ui, &file_menu.get(), diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index c28bfd77..e2e355c7 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -125,6 +125,9 @@ use view_nav_graphs::*; pub mod view_occupancy; use view_occupancy::*; +pub mod workspace; +use workspace::*; + pub mod prelude { //! This module gives easy access to the traits, structs, and plugins that //! we expect downstream users are likely to want easy access to if they are @@ -155,12 +158,13 @@ impl Plugin for StandardUiPlugin { IconsPlugin::default(), MenuBarPlugin::default(), #[cfg(not(target_arch = "wasm32"))] - SdfExportMenuPlugin::default(), StandardPropertiesPanelPlugin::default(), FuelAssetBrowserPlugin::default(), DiagnosticsPlugin::default(), ConsoleWidgetPlugin::default(), UserCameraDisplayPlugin::default(), + WorkspaceMenuPlugin::default(), + SdfExportMenuPlugin::default(), )) .add_systems(Startup, init_ui_style) .add_systems( diff --git a/rmf_site_editor/src/widgets/sdf_export_menu.rs b/rmf_site_editor/src/widgets/sdf_export_menu.rs index 14ad65cb..f4a1efd4 100644 --- a/rmf_site_editor/src/widgets/sdf_export_menu.rs +++ b/rmf_site_editor/src/widgets/sdf_export_menu.rs @@ -15,7 +15,7 @@ * */ -use crate::menu_bar::{FileMenu, MenuEvent, MenuItem}; +use crate::menu_bar::{FileMenu, MenuEvent, MenuItem, TextMenuItem}; use crate::{AppState, WorkspaceSaver}; use bevy::prelude::*; @@ -35,7 +35,9 @@ impl FromWorld for SdfExportMenu { fn from_world(world: &mut World) -> Self { let file_header = world.resource::().get(); let export_sdf = world - .spawn(MenuItem::Text("Export Sdf".to_string())) + .spawn(MenuItem::Text( + TextMenuItem::new("Export Sdf").shortcut("Ctrl-E"), + )) .set_parent(file_header) .id(); diff --git a/rmf_site_editor/src/widgets/workspace.rs b/rmf_site_editor/src/widgets/workspace.rs new file mode 100644 index 00000000..bf838230 --- /dev/null +++ b/rmf_site_editor/src/widgets/workspace.rs @@ -0,0 +1,97 @@ +/* + * Copyright (C) 2024 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::widgets::{FileMenu, MenuDisabled, MenuEvent, MenuItem, TextMenuItem}; +use crate::{AppState, CreateNewWorkspace, WorkspaceLoader, WorkspaceSaver}; +use bevy::prelude::*; + +// TODO(luca) fill fields with callbacks used to create / load / save workspaces +#[derive(Default)] +pub struct WorkspaceMenuPlugin {} + +impl Plugin for WorkspaceMenuPlugin { + fn build(&self, app: &mut App) { + app.init_resource::().add_systems( + Update, + handle_workspace_menu_events.run_if(AppState::in_displaying_mode()), + ); + } +} + +#[derive(Resource)] +pub struct WorkspaceMenu { + new: Entity, + save: Entity, + save_as: Entity, + load: Entity, +} + +impl FromWorld for WorkspaceMenu { + fn from_world(world: &mut World) -> Self { + let file_menu = world.resource::().get(); + let new = world + .spawn(MenuItem::Text(TextMenuItem::new("New").shortcut("Ctrl-N"))) + .set_parent(file_menu) + .id(); + let save = world + .spawn(MenuItem::Text(TextMenuItem::new("Save").shortcut("Ctrl-S"))) + .set_parent(file_menu) + .id(); + let save_as = world + .spawn(MenuItem::Text( + TextMenuItem::new("Save As").shortcut("Ctrl-Shift-S"), + )) + .set_parent(file_menu) + .id(); + let load = world + .spawn(MenuItem::Text(TextMenuItem::new("Open").shortcut("Ctrl-O"))) + .set_parent(file_menu) + .id(); + + // Saving is not enabled in wasm + if cfg!(target_arch = "wasm32") { + world.entity_mut(save).insert(MenuDisabled); + world.entity_mut(save_as).insert(MenuDisabled); + } + Self { + new, + save, + save_as, + load, + } + } +} + +fn handle_workspace_menu_events( + mut menu_events: EventReader, + workspace_menu: Res, + mut workspace_saver: WorkspaceSaver, + mut workspace_loader: WorkspaceLoader, + mut new_workspace: EventWriter, +) { + for event in menu_events.read() { + if event.clicked() && event.source() == workspace_menu.new { + new_workspace.send(CreateNewWorkspace); + } else if event.clicked() && event.source() == workspace_menu.save { + workspace_saver.save_to_default_file(); + } else if event.clicked() && event.source() == workspace_menu.save_as { + workspace_saver.save_to_dialog(); + } else if event.clicked() && event.source() == workspace_menu.load { + workspace_loader.load_from_dialog(); + } + } +} From 12f937195c59a87160851178e5bbb73572695ce0 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 10 Sep 2024 17:07:21 +0800 Subject: [PATCH 14/25] Minor cleanup Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/save.rs | 5 +++++ rmf_site_editor/src/workspace.rs | 10 ++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs index e3400a38..3a6c42fb 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -1312,6 +1312,11 @@ pub fn save_site(world: &mut World) { error!("Unable to create folder {}: {e}", new_path.display()); continue; } + } else { + if !new_path.is_dir() { + error!("SDF can only be exported to a folder"); + continue; + } } let mut sdf_path = new_path.clone(); sdf_path.push(&site.properties.name.0); diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index 52be6758..0387047a 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -555,34 +555,36 @@ impl FromWorld for WorkspaceSavingServices { // not Send in wasm so it can't be sent to another thread through an event. We would need to // refactor saving to be fully done in the async task rather than send an event to have wasm saving. impl<'w, 's> WorkspaceSaver<'w, 's> { - /// Request to spawn a dialog and load a workspace + /// Request to spawn a dialog and save the workspace pub fn save_to_dialog(&mut self) { self.commands .request((), self.workspace_saving.save_workspace_to_dialog) .detach(); } + /// Request to save the workspace to the default file (or a dialog if no default file is + /// available). pub fn save_to_default_file(&mut self) { self.commands .request((), self.workspace_saving.save_workspace_to_default_file) .detach(); } - /// Request to spawn a dialog to select a file and create a new site with a blank level + /// Request to save the workspace to the requested path pub fn save_to_path(&mut self, path: PathBuf) { self.commands .request(path, self.workspace_saving.save_workspace_to_path) .detach(); } - /// Request to load a workspace from a path + /// Request to export the workspace as a sdf to a folder selected from a dialog pub fn export_sdf_to_dialog(&mut self) { self.commands .request((), self.workspace_saving.export_sdf_to_dialog) .detach(); } - /// Request to load a workspace from data + /// Request to export the workspace as a sdf to provided folder pub fn export_sdf_to_path(&mut self, path: PathBuf) { self.commands .request(path, self.workspace_saving.export_sdf_to_path) From 4d44e3ac423186f70ed1f568cb02d091e8e33b33 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 10 Sep 2024 17:12:33 +0800 Subject: [PATCH 15/25] Fix wasm condition Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/widgets/mod.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index e2e355c7..20665f96 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -157,13 +157,13 @@ impl Plugin for StandardUiPlugin { .add_plugins(( IconsPlugin::default(), MenuBarPlugin::default(), - #[cfg(not(target_arch = "wasm32"))] StandardPropertiesPanelPlugin::default(), FuelAssetBrowserPlugin::default(), DiagnosticsPlugin::default(), ConsoleWidgetPlugin::default(), UserCameraDisplayPlugin::default(), WorkspaceMenuPlugin::default(), + #[cfg(not(target_arch = "wasm32"))] SdfExportMenuPlugin::default(), )) .add_systems(Startup, init_ui_style) From 873ff9d7dec2b3618d661304c3e040502c3cb3c3 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 11 Sep 2024 11:44:07 +0800 Subject: [PATCH 16/25] Minor initialization changes to make interaction plugin standalone, readd create joint Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/interaction/mod.rs | 19 ++++++++++++------- .../src/site/drawing_editor/mod.rs | 1 - rmf_site_editor/src/site/mod.rs | 2 -- .../src/widgets/inspector/inspect_anchor.rs | 11 ++--------- rmf_site_editor/src/widgets/inspector/mod.rs | 1 - rmf_site_editor/src/widgets/mod.rs | 1 - 6 files changed, 14 insertions(+), 21 deletions(-) diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index 40890dc6..2cb8c19f 100644 --- a/rmf_site_editor/src/interaction/mod.rs +++ b/rmf_site_editor/src/interaction/mod.rs @@ -16,12 +16,11 @@ */ use crate::site::{ - update_anchor_transforms, CollisionMeshMarker, DoorMarker, FiducialMarker, FloorMarker, - LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, MeasurementMarker, SiteUpdateSet, - VisualMeshMarker, WallMarker, + update_anchor_transforms, CollisionMeshMarker, CurrentEditDrawing, CurrentLevel, DoorMarker, + FiducialMarker, FloorMarker, LaneMarker, LiftCabin, LiftCabinDoorMarker, LocationTags, + MeasurementMarker, SiteUpdateSet, ToggleLiftDoorAvailability, VisualMeshMarker, WallMarker, }; -// TODO(luca) restore -// use crate::workcell::WorkcellVisualizationMarker; +use crate::widgets::UserCameraDisplayPlugin; pub mod anchor; pub use anchor::*; @@ -152,11 +151,14 @@ impl Plugin for InteractionPlugin { .init_resource::() .init_resource::() .init_resource::() + .init_resource::() + .init_resource::() .insert_resource(HighlightAnchors(false)) .add_event::() .add_event::() .add_event::() .add_event::() + .add_event::() .add_plugins(( OutlinePlugin, CategoryVisibilityPlugin::::visible(true), @@ -170,9 +172,12 @@ impl Plugin for InteractionPlugin { CategoryVisibilityPlugin::::visible(false), CategoryVisibilityPlugin::::visible(true), CategoryVisibilityPlugin::::visible(true), - // CategoryVisibilityPlugin::::visible(true), )) - .add_plugins((CameraControlsPlugin, ModelPreviewPlugin)); + .add_plugins(( + CameraControlsPlugin, + ModelPreviewPlugin, + UserCameraDisplayPlugin::default(), + )); if !self.headless { app.add_plugins(SelectionPlugin::default()) diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs index 454a204a..3d0fe675 100644 --- a/rmf_site_editor/src/site/drawing_editor/mod.rs +++ b/rmf_site_editor/src/site/drawing_editor/mod.rs @@ -244,7 +244,6 @@ impl Plugin for DrawingEditorPlugin { app.add_event::() .add_event::() .add_event::() - .init_resource::() .add_systems(Update, switch_edit_drawing_mode) .add_systems( Update, diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index b3da662e..eef8c963 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -190,14 +190,12 @@ impl Plugin for SitePlugin { ) .insert_resource(ClearColor(Color::rgb(0., 0., 0.))) .init_resource::() - .init_resource::() .init_resource::() .add_event::() .add_event::() .add_event::() .add_event::() .add_event::() - .add_event::() .add_event::() .add_event::() .add_event::() diff --git a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs index 7e2a39f2..0683ab1f 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs @@ -23,13 +23,10 @@ use crate::{ prelude::*, Icons, SelectorWidget, }, - // TODO(luca) restore create joint feature by (temporarily) adding it to rmf_workcell_format? - // otherwise consider creating a new widget for anchor inspection - // workcell::CreateJoint, }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{DragValue, ImageButton, Ui}; -use rmf_workcell_format::JointProperties; +use rmf_workcell_format::{CreateJoint, JointProperties}; use std::collections::{BTreeMap, BTreeSet}; #[derive(SystemParam)] @@ -48,8 +45,7 @@ pub struct InspectAnchor<'w, 's> { joints: Query<'w, 's, Entity, With>, hover: EventWriter<'w, Hover>, move_to: EventWriter<'w, MoveTo>, - // TODO(luca) restore - // create_joint: EventWriter<'w, CreateJoint>, + create_joint: EventWriter<'w, CreateJoint>, } impl<'w, 's> ShareableWidget for InspectAnchor<'w, 's> {} @@ -181,13 +177,10 @@ fn impl_inspect_anchor( // If the parent is not a joint, add a joint creation widget if params.joints.get(parent.get()).is_err() { if ui.button("Create joint").on_hover_text("Create a fixed joint and place it between the parent frame and this frame").clicked() { - // TODO(luca) restore - /* params.create_joint.send(CreateJoint { parent: parent.get(), child: id, }); - */ } } }); diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index a76b1703..4ce15956 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -199,7 +199,6 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - //InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 20665f96..5ff23700 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -161,7 +161,6 @@ impl Plugin for StandardUiPlugin { FuelAssetBrowserPlugin::default(), DiagnosticsPlugin::default(), ConsoleWidgetPlugin::default(), - UserCameraDisplayPlugin::default(), WorkspaceMenuPlugin::default(), #[cfg(not(target_arch = "wasm32"))] SdfExportMenuPlugin::default(), From 11357ca5eba1d6bab2c9c2486b2d411dc1fa20ef Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 11 Sep 2024 14:58:53 +0800 Subject: [PATCH 17/25] Add filters for file saving / loading Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/workspace.rs | 45 +++++++++++++++++++++++++++----- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs index 0387047a..3a66cf98 100644 --- a/rmf_site_editor/src/workspace.rs +++ b/rmf_site_editor/src/workspace.rs @@ -86,7 +86,7 @@ pub enum ExportFormat { /// Used to keep track of visibility when switching workspace #[derive(Debug, Default, Resource)] -pub struct RecallWorkspace(Option); +pub struct RecallWorkspace(pub Option); impl CurrentWorkspace { pub fn to_site(self, open_sites: &Query>) -> Option { @@ -212,14 +212,21 @@ pub fn process_load_workspace_files( } } +/// Filter that can be added to a file dialog request to filter extensions +#[derive(Clone, Debug)] +pub struct FileDialogFilter { + pub name: String, + pub extensions: Vec, +} + /// Services that spawn async file dialogs for various purposes, i.e. loading files, saving files / /// folders. #[derive(Resource)] pub struct FileDialogServices { /// Open a dialog to pick a file, return its path and data - pub pick_file_and_load: Service<(), (PathBuf, Vec)>, + pub pick_file_and_load: Service, (PathBuf, Vec)>, /// Pick a file to save data into - pub pick_file_for_saving: Service<(), PathBuf>, + pub pick_file_for_saving: Service, PathBuf>, /// Pick a folder pub pick_folder: Service<(), PathBuf>, } @@ -230,8 +237,12 @@ impl FromWorld for FileDialogServices { scope .input .chain(builder) - .map_async(|_| async move { - if let Some(file) = AsyncFileDialog::new().pick_file().await { + .map_async(|filters: Vec| async move { + let mut dialog = AsyncFileDialog::new(); + for filter in filters { + dialog = dialog.add_filter(filter.name, &filter.extensions); + } + if let Some(file) = dialog.pick_file().await { let data = file.read().await; #[cfg(not(target_arch = "wasm32"))] let file = file.path().to_path_buf(); @@ -249,8 +260,12 @@ impl FromWorld for FileDialogServices { scope .input .chain(builder) - .map_async(|_| async move { - let file = AsyncFileDialog::new().save_file().await?; + .map_async(|filters: Vec| async move { + let mut dialog = AsyncFileDialog::new(); + for filter in filters { + dialog = dialog.add_filter(filter.name, &filter.extensions); + } + let file = dialog.save_file().await?; #[cfg(not(target_arch = "wasm32"))] let file = file.path().to_path_buf(); #[cfg(target_arch = "wasm32")] @@ -311,11 +326,22 @@ impl FromWorld for WorkspaceLoadingServices { .resource::() .pick_file_and_load .clone(); + let loading_filters = vec![ + FileDialogFilter { + name: "Legacy building".into(), + extensions: vec!["building.yaml".into()], + }, + FileDialogFilter { + name: "Site".into(), + extensions: vec!["site.ron".into(), "site.json".into()], + }, + ]; // Spawn all the services let load_workspace_from_dialog = world.spawn_workflow(|scope, builder| { scope .input .chain(builder) + .map_block(move |_| loading_filters.clone()) .then(pick_file) .map_async(|(path, data)| async move { let data = WorkspaceData::new(&path, data)?; @@ -491,11 +517,16 @@ impl FromWorld for WorkspaceSavingServices { .pick_file_for_saving .clone(); let pick_folder = world.resource::().pick_folder.clone(); + let saving_filters = vec![FileDialogFilter { + name: "Site".into(), + extensions: vec!["site.ron".into(), "site.json".into()], + }]; // Spawn all the services let save_workspace_to_dialog = world.spawn_workflow(|scope, builder| { scope .input .chain(builder) + .map_block(move |_| saving_filters.clone()) .then(pick_file) .map_block(|path| (path, ExportFormat::default())) .then(send_file_save) From 6039f3202810d07bc85d2a59239bddb06c0f8228 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 11 Sep 2024 16:42:48 +0800 Subject: [PATCH 18/25] Reexport bevy plugins that might be needed downstream Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/lib.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index 2651eea8..a54c18ec 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -16,6 +16,10 @@ pub use autoload::*; pub mod asset_loaders; use asset_loaders::*; +// Bevy plugins that are public dependencies, mixing versions won't work for downstream users +pub use bevy_egui; +pub use bevy_mod_raycast; + pub mod keyboard; use keyboard::*; From cbfeebf54cf9a34d6fc51348538cd107458efea6 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 11 Sep 2024 16:47:06 +0800 Subject: [PATCH 19/25] Fix compile without bevy feature Signed-off-by: Luca Della Vedova --- rmf_site_format/src/misc.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index c8f014b6..53de412f 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -18,7 +18,7 @@ use crate::RefTrait; #[cfg(feature = "bevy")] use bevy::prelude::*; -use glam::{Quat, Vec2, Vec3}; +use glam::{EulerRot, Quat, Vec2, Vec3}; use serde::{Deserialize, Serialize}; use std::collections::HashMap; From d9b0ffeb5f5105cdb9a0bd4c9d458ca1e430f45f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Sep 2024 10:14:09 +0800 Subject: [PATCH 20/25] Add missing event Signed-off-by: Luca Della Vedova --- rmf_site_editor/src/site/mod.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index eef8c963..b8546f67 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -123,7 +123,7 @@ pub use wall::*; use crate::recency::{RecencyRank, RecencyRankingPlugin}; use crate::{AppState, RegisterIssueType}; pub use rmf_site_format::{DirectionalLight, PointLight, SpotLight, Style, *}; -use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell}; +use rmf_workcell_format::{CreateJoint, NameInWorkcell, NameOfWorkcell}; use bevy::{prelude::*, render::view::visibility::VisibilitySystems, transform::TransformSystem}; @@ -200,6 +200,7 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() + .add_event::() .add_plugins(( ChangePlugin::>::default(), RecallPlugin::>::default(), From 1bc6bcfb34d6524181a4dff4541cb7422e5fdfc5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Sep 2024 13:48:30 +0800 Subject: [PATCH 21/25] Use git version of dependency Signed-off-by: Luca Della Vedova --- rmf_site_editor/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index 393911cb..c13de751 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -35,7 +35,7 @@ thread_local = "*" geo = "0.27" thiserror = "*" rmf_site_format = { path = "../rmf_site_format", features = ["bevy"] } -rmf_workcell_format = { path = "../../rmf_workcell/rmf_workcell_format", features = ["bevy"] } +rmf_workcell_format = { git = "https://github.com/luca-della-vedova/rmf_workcell", branch = "first_commit", features = ["bevy_support"]} itertools = "*" bitfield = "*" crossbeam-channel = "0.5" From 29f290a4c9a3a4e71d3aea601c39e1060acae75f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Sep 2024 14:33:26 +0800 Subject: [PATCH 22/25] Remove dependency on rmf_workcell_format Signed-off-by: Luca Della Vedova --- rmf_site_editor/Cargo.toml | 1 - rmf_site_editor/src/site/mod.rs | 4 - .../src/widgets/inspector/inspect_anchor.rs | 15 +--- .../src/widgets/inspector/inspect_joint.rs | 74 --------------- .../src/widgets/inspector/inspect_name.rs | 31 ------- .../inspector/inspect_workcell_parent.rs | 89 ------------------- rmf_site_editor/src/widgets/inspector/mod.rs | 4 - 7 files changed, 1 insertion(+), 217 deletions(-) delete mode 100644 rmf_site_editor/src/widgets/inspector/inspect_joint.rs delete mode 100644 rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index c13de751..f44ade03 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -35,7 +35,6 @@ thread_local = "*" geo = "0.27" thiserror = "*" rmf_site_format = { path = "../rmf_site_format", features = ["bevy"] } -rmf_workcell_format = { git = "https://github.com/luca-della-vedova/rmf_workcell", branch = "first_commit", features = ["bevy_support"]} itertools = "*" bitfield = "*" crossbeam-channel = "0.5" diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index b8546f67..ac29a0b9 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -123,7 +123,6 @@ pub use wall::*; use crate::recency::{RecencyRank, RecencyRankingPlugin}; use crate::{AppState, RegisterIssueType}; pub use rmf_site_format::{DirectionalLight, PointLight, SpotLight, Style, *}; -use rmf_workcell_format::{CreateJoint, NameInWorkcell, NameOfWorkcell}; use bevy::{prelude::*, render::view::visibility::VisibilitySystems, transform::TransformSystem}; @@ -200,7 +199,6 @@ impl Plugin for SitePlugin { .add_event::() .add_event::() .add_event::() - .add_event::() .add_plugins(( ChangePlugin::>::default(), RecallPlugin::>::default(), @@ -210,8 +208,6 @@ impl Plugin for SitePlugin { RecallPlugin::::default(), ChangePlugin::::default(), ChangePlugin::::default(), - ChangePlugin::::default(), - ChangePlugin::::default(), ChangePlugin::::default(), ChangePlugin::::default(), ChangePlugin::::default(), diff --git a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs index 0683ab1f..af51cdcf 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_anchor.rs @@ -26,7 +26,6 @@ use crate::{ }; use bevy::{ecs::system::SystemParam, prelude::*}; use bevy_egui::egui::{DragValue, ImageButton, Ui}; -use rmf_workcell_format::{CreateJoint, JointProperties}; use std::collections::{BTreeMap, BTreeSet}; #[derive(SystemParam)] @@ -38,14 +37,11 @@ pub struct InspectAnchor<'w, 's> { &'static Anchor, &'static Transform, Option<&'static Subordinate>, - &'static Parent, ), >, icons: Res<'w, Icons>, - joints: Query<'w, 's, Entity, With>, hover: EventWriter<'w, Hover>, move_to: EventWriter<'w, MoveTo>, - create_joint: EventWriter<'w, CreateJoint>, } impl<'w, 's> ShareableWidget for InspectAnchor<'w, 's> {} @@ -125,7 +121,7 @@ fn impl_inspect_anchor( let mut params = state.get_mut(world); - if let Ok((anchor, tf, subordinate, parent)) = params.anchors.get(id) { + if let Ok((anchor, tf, subordinate)) = params.anchors.get(id) { if let Some(subordinate) = subordinate.map(|s| s.0) { panel.orthogonal(ui, |ui| { if let Some(boss) = subordinate { @@ -174,15 +170,6 @@ fn impl_inspect_anchor( transform: new_pose.transform(), }); } - // If the parent is not a joint, add a joint creation widget - if params.joints.get(parent.get()).is_err() { - if ui.button("Create joint").on_hover_text("Create a fixed joint and place it between the parent frame and this frame").clicked() { - params.create_joint.send(CreateJoint { - parent: parent.get(), - child: id, - }); - } - } }); } } diff --git a/rmf_site_editor/src/widgets/inspector/inspect_joint.rs b/rmf_site_editor/src/widgets/inspector/inspect_joint.rs deleted file mode 100644 index c65e1e66..00000000 --- a/rmf_site_editor/src/widgets/inspector/inspect_joint.rs +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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::Dependents, - widgets::{prelude::*, Inspect, SelectorWidget}, -}; -use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::Ui; -use rmf_workcell_format::{FrameMarker, JointProperties}; - -#[derive(SystemParam)] -pub struct InspectJoint<'w, 's> { - joints: Query< - 'w, - 's, - ( - &'static Parent, - &'static Dependents, - &'static JointProperties, - ), - >, - frames: Query<'w, 's, (), With>, - selector: SelectorWidget<'w, 's>, -} - -impl<'w, 's> WidgetSystem for InspectJoint<'w, 's> { - fn show( - Inspect { selection, .. }: Inspect, - ui: &mut Ui, - state: &mut SystemState, - world: &mut World, - ) { - let mut params = state.get_mut(world); - params.show_widget(selection, ui); - } -} - -impl<'w, 's> InspectJoint<'w, 's> { - pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { - let Ok((parent, deps, joint_properties)) = self.joints.get(id) else { - return; - }; - - ui.label("Parent frame"); - self.selector.show_widget(**parent, ui); - - if let Some(frame_dep) = deps.iter().find(|d| self.frames.get(**d).is_ok()) { - ui.label("Child frame"); - self.selector.show_widget(*frame_dep, ui); - } - - ui.horizontal(|ui| { - ui.label("Joint Type"); - // TODO(luca) Make this a ComboBox to edit joint value data - ui.label(joint_properties.label()); - }); - // TODO(luca) add joint limit and joint axis inspectors - } -} diff --git a/rmf_site_editor/src/widgets/inspector/inspect_name.rs b/rmf_site_editor/src/widgets/inspector/inspect_name.rs index dca60646..3323cfd4 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_name.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_name.rs @@ -22,16 +22,11 @@ use crate::{ use bevy::prelude::*; use bevy_egui::egui::Ui; use rmf_site_format::NameInSite; -use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell}; #[derive(SystemParam)] pub struct InspectName<'w, 's> { names_in_site: Query<'w, 's, &'static NameInSite>, change_name_in_site: EventWriter<'w, Change>, - names_in_workcell: Query<'w, 's, &'static NameInWorkcell>, - change_name_in_workcell: EventWriter<'w, Change>, - names_of_workcells: Query<'w, 's, &'static NameOfWorkcell>, - change_name_of_workcell: EventWriter<'w, Change>, } impl<'w, 's> ShareableWidget for InspectName<'w, 's> {} @@ -56,31 +51,5 @@ impl<'w, 's> WidgetSystem for InspectName<'w, 's> { .send(Change::new(new_name, selection)); } } - - if let Ok(name) = params.names_in_workcell.get(selection) { - let mut new_name = name.clone(); - ui.horizontal(|ui| { - ui.label("Name"); - ui.text_edit_singleline(&mut new_name.0); - }); - if new_name != *name { - params - .change_name_in_workcell - .send(Change::new(new_name, selection)); - } - } - - if let Ok(name) = params.names_of_workcells.get(selection) { - let mut new_name = name.clone(); - ui.horizontal(|ui| { - ui.label("Name of workcell"); - ui.text_edit_singleline(&mut new_name.0); - }); - if new_name != *name { - params - .change_name_of_workcell - .send(Change::new(new_name, selection)); - } - } } } diff --git a/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs b/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs deleted file mode 100644 index 734e8144..00000000 --- a/rmf_site_editor/src/widgets/inspector/inspect_workcell_parent.rs +++ /dev/null @@ -1,89 +0,0 @@ -/* - * 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::{Hover, ObjectPlacement}, - widgets::{prelude::*, Icons, Inspect, SelectorWidget}, - CurrentWorkspace, -}; -use rmf_workcell_format::{FrameMarker, NameInWorkcell, NameOfWorkcell}; -use bevy::{ecs::system::SystemParam, prelude::*}; -use bevy_egui::egui::{ImageButton, Ui}; - -#[derive(SystemParam)] -pub struct InspectWorkcellParent<'w, 's> { - parents: Query<'w, 's, &'static Parent>, - workcell_elements: Query< - 'w, - 's, - Entity, - Or<( - With, - With, - With, - )>, - >, - icons: Res<'w, Icons>, - selector: SelectorWidget<'w, 's>, - object_placement: ObjectPlacement<'w, 's>, - current_workspace: Res<'w, CurrentWorkspace>, -} - -impl<'w, 's> WidgetSystem for InspectWorkcellParent<'w, 's> { - fn show( - Inspect { selection, .. }: Inspect, - ui: &mut Ui, - state: &mut SystemState, - world: &mut World, - ) { - let mut params = state.get_mut(world); - params.show_widget(selection, ui); - } -} - -impl<'w, 's> InspectWorkcellParent<'w, 's> { - pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { - // If the parent is a frame it should be reassignable - if let Ok(parent) = self - .parents - .get(id) - .and_then(|p| self.workcell_elements.get(**p)) - { - ui.vertical(|ui| { - ui.label("Parent Frame"); - self.selector.show_widget(parent, ui); - let assign_response = ui.add(ImageButton::new(self.icons.edit.egui())); - if assign_response.hovered() { - self.selector.hover.send(Hover(Some(id))); - } - - let parent_replace = assign_response.clicked(); - assign_response.on_hover_text("Reassign"); - - if parent_replace { - /* - if let Some(workspace) = self.current_workspace.root { - self.object_placement.replace_parent_3d(id, workspace) - } else { - warn!("Cannot replace a parent when no workspace is active"); - } - */ - } - }); - } - } -} diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 4ce15956..3a3a2586 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -45,9 +45,6 @@ pub use inspect_geography::*; pub mod inspect_group; pub use inspect_group::*; -pub mod inspect_joint; -pub use inspect_joint::*; - pub mod inspect_is_static; pub use inspect_is_static::*; @@ -199,7 +196,6 @@ impl Plugin for StandardInspectorPlugin { InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), - InspectionPlugin::::new(), InspectionPlugin::::new(), InspectionPlugin::::new(), InspectLiftPlugin::default(), From ef513f907f8c74440f1b7aab7777797d9c98bc72 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Sep 2024 15:09:14 +0800 Subject: [PATCH 23/25] Make urdf-rs dependency optional for rmf_site_format Signed-off-by: Luca Della Vedova --- rmf_site_format/Cargo.toml | 7 +++++-- rmf_site_format/src/misc.rs | 6 ++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml index 0de63d22..9f1dc908 100644 --- a/rmf_site_format/Cargo.toml +++ b/rmf_site_format/Cargo.toml @@ -16,14 +16,17 @@ thiserror = "*" glam = { version = "0.24", features = ["serde"] } uuid = { version = "1.1", features = ["v4", "serde"] } # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features -# We depend on a bugfix released specifically in 0.7.3 bevy = { version = "0.12", optional = true } sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "9fc35f2"} yaserde = "0.7" -urdf-rs = "0.7.3" +# We depend on a bugfix released specifically in 0.7.3 +urdf-rs = { version = "0.7.3", optional = true } # Used for lazy initialization of static variable when they are non const once_cell = "1" pathdiff = "*" [dev-dependencies] float_eq = "1.0" + +[features] +urdf = ["dep:urdf-rs"] diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs index 53de412f..cc6bfa1e 100644 --- a/rmf_site_format/src/misc.rs +++ b/rmf_site_format/src/misc.rs @@ -18,7 +18,7 @@ use crate::RefTrait; #[cfg(feature = "bevy")] use bevy::prelude::*; -use glam::{EulerRot, Quat, Vec2, Vec3}; +use glam::{Quat, Vec2, Vec3}; use serde::{Deserialize, Serialize}; use std::collections::HashMap; @@ -353,6 +353,7 @@ impl Default for Pose { } } +#[cfg(feature = "urdf")] impl From for urdf_rs::Pose { fn from(pose: Pose) -> Self { urdf_rs::Pose { @@ -360,7 +361,7 @@ impl From for urdf_rs::Pose { Rotation::EulerExtrinsicXYZ(arr) => urdf_rs::Vec3(arr.map(|v| v.radians().into())), Rotation::Yaw(v) => urdf_rs::Vec3([0.0, 0.0, v.radians().into()]), Rotation::Quat([x, y, z, w]) => { - let (z, y, x) = glam::quat(x, y, z, w).to_euler(EulerRot::ZYX); + let (z, y, x) = glam::quat(x, y, z, w).to_euler(glam::EulerRot::ZYX); urdf_rs::Vec3([x as f64, y as f64, z as f64]) } }, @@ -369,6 +370,7 @@ impl From for urdf_rs::Pose { } } +#[cfg(feature = "urdf")] impl From<&urdf_rs::Pose> for Pose { fn from(pose: &urdf_rs::Pose) -> Self { Pose { From 3496425691f9c43b46b6acb59bd257961e80a6f9 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Sep 2024 15:50:49 +0800 Subject: [PATCH 24/25] Minor cleanups Signed-off-by: Luca Della Vedova --- rmf_site_editor/Cargo.toml | 2 +- rmf_site_editor/src/lib.rs | 4 +--- rmf_site_editor/src/site/mod.rs | 6 +++--- rmf_site_editor/src/site/{sdf.rs => primitive_shape.rs} | 0 rmf_site_editor/src/widgets/inspector/mod.rs | 3 --- rmf_site_editor/src/widgets/workspace.rs | 1 - 6 files changed, 5 insertions(+), 11 deletions(-) rename rmf_site_editor/src/site/{sdf.rs => primitive_shape.rs} (100%) diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml index f44ade03..3302d6a9 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -29,7 +29,7 @@ serde_yaml = "0.8.23" serde_json = "1.0" wasm-bindgen = "0.2.87" futures-lite = "1.12.0" -bevy = { version = "0.12", features = ["pnm", "jpeg", "tga", "dynamic_linking"] } +bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } dirs = "5.0" thread_local = "*" geo = "0.27" diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs index a54c18ec..9614e481 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -1,10 +1,8 @@ -#[allow(warnings)] use bevy::{app::ScheduleRunnerPlugin, log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*}; use bevy_egui::EguiPlugin; -use main_menu::MainMenuPlugin; -// use warehouse_generator::WarehouseGeneratorPlugin; #[cfg(not(target_arch = "wasm32"))] use clap::Parser; +use main_menu::MainMenuPlugin; #[cfg(target_arch = "wasm32")] use wasm_bindgen::prelude::*; diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs index ac29a0b9..220fc6eb 100644 --- a/rmf_site_editor/src/site/mod.rs +++ b/rmf_site_editor/src/site/mod.rs @@ -90,12 +90,12 @@ pub use physical_camera::*; pub mod pose; pub use pose::*; +pub mod primitive_shape; +pub use primitive_shape::*; + pub mod recall_plugin; pub use recall_plugin::RecallPlugin; -pub mod sdf; -pub use sdf::*; - pub mod save; pub use save::*; diff --git a/rmf_site_editor/src/site/sdf.rs b/rmf_site_editor/src/site/primitive_shape.rs similarity index 100% rename from rmf_site_editor/src/site/sdf.rs rename to rmf_site_editor/src/site/primitive_shape.rs diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 3a3a2586..2f35ce9f 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -102,9 +102,6 @@ pub use inspect_texture::*; pub mod inspect_value; pub use inspect_value::*; -//pub mod inspect_workcell_parent; -//pub use inspect_workcell_parent::*; - use crate::{ interaction::Selection, site::{Category, SiteID}, diff --git a/rmf_site_editor/src/widgets/workspace.rs b/rmf_site_editor/src/widgets/workspace.rs index bf838230..05d55cd6 100644 --- a/rmf_site_editor/src/widgets/workspace.rs +++ b/rmf_site_editor/src/widgets/workspace.rs @@ -19,7 +19,6 @@ use crate::widgets::{FileMenu, MenuDisabled, MenuEvent, MenuItem, TextMenuItem}; use crate::{AppState, CreateNewWorkspace, WorkspaceLoader, WorkspaceSaver}; use bevy::prelude::*; -// TODO(luca) fill fields with callbacks used to create / load / save workspaces #[derive(Default)] pub struct WorkspaceMenuPlugin {} From d7be917543e687761ce9207acfcfed22505f5829 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Thu, 12 Sep 2024 16:35:25 +0800 Subject: [PATCH 25/25] Minor fixes Signed-off-by: Luca Della Vedova --- rmf_site_editor/examples/extending_menu.rs | 12 ++++-------- .../src/widgets/inspector/inspect_asset_source.rs | 1 + rmf_site_editor/src/widgets/inspector/mod.rs | 1 - rmf_site_editor/src/widgets/view_lights.rs | 1 + rmf_site_editor/src/widgets/view_nav_graphs.rs | 1 + 5 files changed, 7 insertions(+), 9 deletions(-) diff --git a/rmf_site_editor/examples/extending_menu.rs b/rmf_site_editor/examples/extending_menu.rs index 82510145..34ad7543 100644 --- a/rmf_site_editor/examples/extending_menu.rs +++ b/rmf_site_editor/examples/extending_menu.rs @@ -15,9 +15,7 @@ impl FromWorld for MyMenuHandler { // This is all it takes to register a new menu item // We need to keep track of the entity in order to make // sure that we can check the callback - let unique_export = world - .spawn(MenuItem::Text("My unique export".to_string())) - .id(); + let unique_export = world.spawn(MenuItem::Text("My unique export".into())).id(); // Make it a child of the "File Menu" let file_header = world.resource::().get(); @@ -37,9 +35,7 @@ impl FromWorld for MyMenuHandler { world.entity_mut(menu).push_children(&[sub_menu]); // Finally we can create a custom action - let custom_nested_menu = world - .spawn(MenuItem::Text("My Awesome Action".to_string())) - .id(); + let custom_nested_menu = world.spawn(MenuItem::Text("My Awesome Action".into())).id(); world .entity_mut(sub_menu) .push_children(&[custom_nested_menu]); @@ -56,7 +52,7 @@ impl FromWorld for MyMenuHandler { /// an event that is of the same type as the one we are supposed to /// handle. fn watch_unique_export_click(mut reader: EventReader, menu_handle: Res) { - for event in reader.iter() { + for event in reader.read() { if event.clicked() && event.source() == menu_handle.unique_export { println!("Doing our epic export"); } @@ -67,7 +63,7 @@ fn watch_unique_export_click(mut reader: EventReader, menu_handle: Re /// an event that is of the same type as the one we are supposed to /// handle. fn watch_submenu_click(mut reader: EventReader, menu_handle: Res) { - for event in reader.iter() { + for event in reader.read() { if event.clicked() && event.source() == menu_handle.custom_nested_menu { println!("Submenu clicked"); } diff --git a/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs b/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs index 8ab7abe0..cb5c8c75 100644 --- a/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs +++ b/rmf_site_editor/src/widgets/inspector/inspect_asset_source.rs @@ -144,6 +144,7 @@ impl<'a> InspectAssetSourceComponent<'a> { // Button to load from file, disabled for wasm since there are no local files #[cfg(not(target_arch = "wasm32"))] if ui.button("Browse").clicked() { + // TODO(luca) change this to use FileDialogServices and be async if let Some(file) = FileDialog::new().pick_file() { if let Some(src) = file.to_str() { if let (Some(default_file), true) = (self.default_file, is_relative) diff --git a/rmf_site_editor/src/widgets/inspector/mod.rs b/rmf_site_editor/src/widgets/inspector/mod.rs index 2f35ce9f..884d22d9 100644 --- a/rmf_site_editor/src/widgets/inspector/mod.rs +++ b/rmf_site_editor/src/widgets/inspector/mod.rs @@ -106,7 +106,6 @@ use crate::{ interaction::Selection, site::{Category, SiteID}, widgets::prelude::*, - AppState, }; use bevy::{ ecs::system::{SystemParam, SystemState}, diff --git a/rmf_site_editor/src/widgets/view_lights.rs b/rmf_site_editor/src/widgets/view_lights.rs index f5f9add2..ee3ccdbb 100644 --- a/rmf_site_editor/src/widgets/view_lights.rs +++ b/rmf_site_editor/src/widgets/view_lights.rs @@ -108,6 +108,7 @@ impl<'w, 's> ViewLights<'w, 's> { } None => { let future = AsyncComputeTaskPool::get().spawn(async move { + // TODO(luca) change this to use FileDialogServices let file = match AsyncFileDialog::new().save_file().await { Some(file) => file, None => return None, diff --git a/rmf_site_editor/src/widgets/view_nav_graphs.rs b/rmf_site_editor/src/widgets/view_nav_graphs.rs index 80f51bcf..f60e2337 100644 --- a/rmf_site_editor/src/widgets/view_nav_graphs.rs +++ b/rmf_site_editor/src/widgets/view_nav_graphs.rs @@ -215,6 +215,7 @@ impl<'w, 's> ViewNavGraphs<'w, 's> { } None => { let future = AsyncComputeTaskPool::get().spawn(async move { + // TODO(luca) change this to use FileDialogServices let file = match AsyncFileDialog::new().pick_file().await { Some(file) => file, None => return None,