diff --git a/assets/textures/add.png b/assets/textures/add.png
new file mode 100644
index 00000000..0a586af5
Binary files /dev/null and b/assets/textures/add.png differ
diff --git a/assets/textures/alignment.png b/assets/textures/alignment.png
new file mode 100644
index 00000000..28bf6e05
Binary files /dev/null and b/assets/textures/alignment.png differ
diff --git a/assets/textures/alignment.svg b/assets/textures/alignment.svg
new file mode 100644
index 00000000..7fe16006
--- /dev/null
+++ b/assets/textures/alignment.svg
@@ -0,0 +1,173 @@
+
+
+
+
diff --git a/assets/textures/attribution.md b/assets/textures/attribution.md
index 11eef4fd..3a6d66e2 100644
--- a/assets/textures/attribution.md
+++ b/assets/textures/attribution.md
@@ -1,4 +1,11 @@
* [`select.png`](https://thenounproject.com/icon/select-3324735/)
* [`edit.png`](https://thenounproject.com/icon/edit-2162449/)
+* [`exit.png`](https://thenounproject.com/icon/exit-1826632/)
+* [`confirm.png`](https://thenounproject.com/icon/confirm-2261637/)
+* [`reject.png`](https://thenounproject.com/icon/x-2289933/)
+* [`empty.png`](https://thenounproject.com/icon/empty-194055/)
+* [`add.png`](https://thenounproject.com/icon/plus-1809810/)
+* [`search.png`](https://thenounproject.com/icon/search-3743008/)
* `trash.png`: @mxgrey
-
+* `selected.png`: @mxgrey
+* `alignment.png`: @mxgrey
diff --git a/assets/textures/confirm.png b/assets/textures/confirm.png
new file mode 100644
index 00000000..5e7ae0e7
Binary files /dev/null and b/assets/textures/confirm.png differ
diff --git a/assets/textures/empty.png b/assets/textures/empty.png
new file mode 100644
index 00000000..5c20903e
Binary files /dev/null and b/assets/textures/empty.png differ
diff --git a/assets/textures/exit.png b/assets/textures/exit.png
new file mode 100644
index 00000000..bc6c6be1
Binary files /dev/null and b/assets/textures/exit.png differ
diff --git a/assets/textures/reject.png b/assets/textures/reject.png
new file mode 100644
index 00000000..e2f52070
Binary files /dev/null and b/assets/textures/reject.png differ
diff --git a/assets/textures/search.png b/assets/textures/search.png
new file mode 100644
index 00000000..70293f9c
Binary files /dev/null and b/assets/textures/search.png differ
diff --git a/assets/textures/selected.png b/assets/textures/selected.png
new file mode 100644
index 00000000..5aa9fee1
Binary files /dev/null and b/assets/textures/selected.png differ
diff --git a/assets/textures/selected.svg b/assets/textures/selected.svg
new file mode 100644
index 00000000..d551232c
--- /dev/null
+++ b/assets/textures/selected.svg
@@ -0,0 +1,91 @@
+
+
+
+
diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 159d2621..f1f2221b 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -21,7 +21,6 @@ bevy_polyline = "0.4"
bevy_stl = "0.7.0"
bevy_obj = { git = "https://github.com/luca-della-vedova/bevy_obj", branch = "luca/scene_0.9", features = ["scene"]}
bevy_rapier3d = "0.20.0"
-optimization_engine = "0.7.7"
smallvec = "*"
serde = { version = "1.0", features = ["derive"] }
serde_yaml = "0.8.23"
@@ -45,6 +44,7 @@ tracing-subscriber = "0.3.1"
rfd = "0.11"
urdf-rs = "0.7"
sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "a5daef0"}
+pathdiff = "*"
# only enable the 'dynamic' feature if we're not building for web or windows
[target.'cfg(all(not(target_arch = "wasm32"), not(target_os = "windows")))'.dependencies]
diff --git a/rmf_site_editor/src/interaction/camera_controls.rs b/rmf_site_editor/src/interaction/camera_controls.rs
index 0350c610..bf13cfc8 100644
--- a/rmf_site_editor/src/interaction/camera_controls.rs
+++ b/rmf_site_editor/src/interaction/camera_controls.rs
@@ -78,6 +78,18 @@ impl ProjectionMode {
}
}
+pub struct ChangeProjectionMode(pub ProjectionMode);
+
+impl ChangeProjectionMode {
+ pub fn to_perspective() -> ChangeProjectionMode {
+ ChangeProjectionMode(ProjectionMode::Perspective)
+ }
+
+ pub fn to_orthographic() -> ChangeProjectionMode {
+ ChangeProjectionMode(ProjectionMode::Orthographic)
+ }
+}
+
#[derive(Debug, Clone, Reflect, Resource)]
pub struct CameraControls {
mode: ProjectionMode,
@@ -152,6 +164,23 @@ impl CameraControls {
self.use_perspective(!choice, cameras, visibilities, headlights_on);
}
+ pub fn use_mode(
+ &mut self,
+ mode: ProjectionMode,
+ cameras: &mut Query<&mut Camera>,
+ visibilities: &mut Query<&mut Visibility>,
+ headlights_on: bool,
+ ) {
+ match mode {
+ ProjectionMode::Perspective => {
+ self.use_perspective(true, cameras, visibilities, headlights_on);
+ }
+ ProjectionMode::Orthographic => {
+ self.use_orthographic(true, cameras, visibilities, headlights_on);
+ }
+ }
+ }
+
pub fn mode(&self) -> ProjectionMode {
self.mode
}
@@ -329,10 +358,21 @@ fn camera_controls(
mut previous_mouse_location: ResMut,
mut controls: ResMut,
mut cameras: Query<(&mut Projection, &mut Transform)>,
+ mut bevy_cameras: Query<&mut Camera>,
mut visibility: Query<&mut Visibility>,
headlight_toggle: Res,
picking_blockers: Res,
+ mut change_mode: EventReader,
) {
+ if let Some(mode) = change_mode.iter().last() {
+ controls.use_mode(
+ mode.0,
+ &mut bevy_cameras,
+ &mut visibility,
+ headlight_toggle.0,
+ );
+ }
+
if headlight_toggle.is_changed() {
controls.toggle_lights(headlight_toggle.0, &mut visibility);
}
@@ -488,6 +528,7 @@ impl Plugin for CameraControlsPlugin {
app.insert_resource(MouseLocation::default())
.init_resource::()
.init_resource::()
+ .add_event::()
.add_system(camera_controls);
}
}
diff --git a/rmf_site_editor/src/interaction/highlight.rs b/rmf_site_editor/src/interaction/highlight.rs
new file mode 100644
index 00000000..e9c66596
--- /dev/null
+++ b/rmf_site_editor/src/interaction/highlight.rs
@@ -0,0 +1,86 @@
+/*
+ * 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::*, site::DrawingMarker};
+use bevy::prelude::*;
+
+#[derive(Component)]
+pub struct Highlight {
+ pub select: Color,
+ pub hover: Color,
+ pub hover_select: Color,
+}
+
+#[derive(Component)]
+pub struct SuppressHighlight;
+
+impl Highlight {
+ pub fn for_drawing() -> Self {
+ Self {
+ select: Color::rgb(1., 0.7, 1.),
+ hover: Color::rgb(0.7, 1., 1.),
+ hover_select: Color::rgb(1.0, 0.5, 0.7),
+ }
+ }
+}
+
+pub fn add_highlight_visualization(
+ mut commands: Commands,
+ new_drawings: Query>,
+) {
+ for e in &new_drawings {
+ commands.entity(e).insert(Highlight::for_drawing());
+ }
+}
+
+pub fn update_highlight_visualization(
+ highlightable: Query<
+ (
+ &Hovered,
+ &Selected,
+ &Handle,
+ &Highlight,
+ Option<&SuppressHighlight>,
+ ),
+ Or<(
+ Changed,
+ Changed,
+ Changed,
+ Changed>,
+ )>,
+ >,
+ mut materials: ResMut>,
+) {
+ for (hovered, selected, m, highlight, suppress) in &highlightable {
+ if let Some(material) = materials.get_mut(m) {
+ let mut color = if suppress.is_some() {
+ Color::WHITE
+ } else if hovered.cue() && selected.cue() {
+ highlight.hover_select
+ } else if hovered.cue() {
+ highlight.hover
+ } else if selected.cue() {
+ highlight.select
+ } else {
+ Color::WHITE
+ };
+ color.set_a(material.base_color.a());
+
+ material.base_color = color;
+ }
+ }
+}
diff --git a/rmf_site_editor/src/interaction/lane.rs b/rmf_site_editor/src/interaction/lane.rs
index 6f9e3816..4d9a7f0c 100644
--- a/rmf_site_editor/src/interaction/lane.rs
+++ b/rmf_site_editor/src/interaction/lane.rs
@@ -43,18 +43,18 @@ pub fn update_lane_visual_cues(
site_assets: Res,
cursor: Res,
) {
- for (hovering, selected, pieces, mut tf) in &mut lanes {
- if hovering.is_hovered {
+ for (hovered, selected, pieces, mut tf) in &mut lanes {
+ if hovered.is_hovered {
set_visibility(cursor.frame, &mut visibility, false);
}
- let (m, h, v) = if hovering.cue() && selected.cue() {
+ let (m, h, v) = if hovered.cue() && selected.cue() {
(
&site_assets.hover_select_material,
HOVERED_LANE_OFFSET,
true,
)
- } else if hovering.cue() {
+ } else if hovered.cue() {
(&site_assets.hover_material, HOVERED_LANE_OFFSET, true)
} else if selected.cue() {
(&site_assets.select_material, SELECTED_LANE_OFFSET, true)
diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs
index 95d88e68..5b064534 100644
--- a/rmf_site_editor/src/interaction/mod.rs
+++ b/rmf_site_editor/src/interaction/mod.rs
@@ -42,6 +42,9 @@ pub use edge::*;
pub mod gizmo;
pub use gizmo::*;
+pub mod highlight;
+pub use highlight::*;
+
pub mod lane;
pub use lane::*;
@@ -66,6 +69,9 @@ pub use picking::*;
pub mod point;
pub use point::*;
+pub mod popup;
+pub use popup::*;
+
pub mod preview;
pub use preview::*;
@@ -171,6 +177,7 @@ impl Plugin for InteractionPlugin {
.with_system(handle_select_anchor_mode.after(maintain_selected_entities))
.with_system(handle_select_anchor_3d_mode.after(maintain_selected_entities))
.with_system(update_anchor_visual_cues.after(maintain_selected_entities))
+ .with_system(update_popups.after(maintain_selected_entities))
.with_system(update_unassigned_anchor_cues)
.with_system(update_anchor_cues_for_mode)
.with_system(update_anchor_proximity_xray.after(update_cursor_transform))
@@ -181,6 +188,7 @@ impl Plugin for InteractionPlugin {
.with_system(update_point_visual_cues.after(maintain_selected_entities))
.with_system(update_path_visual_cues.after(maintain_selected_entities))
.with_system(update_outline_visualization.after(maintain_selected_entities))
+ .with_system(update_highlight_visualization.after(maintain_selected_entities))
.with_system(
update_cursor_hover_visualization.after(maintain_selected_entities),
)
@@ -207,8 +215,10 @@ impl Plugin for InteractionPlugin {
.with_system(add_point_visual_cues)
.with_system(add_path_visual_cues)
.with_system(add_outline_visualization)
+ .with_system(add_highlight_visualization)
.with_system(add_cursor_hover_visualization)
- .with_system(add_physical_light_visual_cues),
+ .with_system(add_physical_light_visual_cues)
+ .with_system(add_popups),
)
.add_system_set_to_stage(
InteractionUpdateStage::ProcessVisuals,
diff --git a/rmf_site_editor/src/interaction/outline.rs b/rmf_site_editor/src/interaction/outline.rs
index 4e087298..0f6df80a 100644
--- a/rmf_site_editor/src/interaction/outline.rs
+++ b/rmf_site_editor/src/interaction/outline.rs
@@ -15,7 +15,7 @@
*
*/
-use crate::interaction::*;
+use crate::{interaction::*, site::DrawingMarker};
use bevy::render::view::RenderLayers;
use bevy_mod_outline::{OutlineBundle, OutlineRenderLayers, OutlineVolume, SetOutlineDepth};
use rmf_site_format::{
@@ -108,6 +108,10 @@ impl OutlineVisualization {
}
}
+/// Use this to temporarily prevent objects from being highlighted.
+#[derive(Component)]
+pub struct SuppressOutline;
+
pub fn add_outline_visualization(
mut commands: Commands,
new_entities: Query<
@@ -120,6 +124,7 @@ pub fn add_outline_visualization(
Added,
Added,
Added,
+ Added,
Added,
Added,
Added,
@@ -138,13 +143,27 @@ pub fn add_outline_visualization(
pub fn update_outline_visualization(
mut commands: Commands,
outlinable: Query<
- (Entity, &Hovered, &Selected, &OutlineVisualization),
- Or<(Changed, Changed)>,
+ (
+ Entity,
+ &Hovered,
+ &Selected,
+ &OutlineVisualization,
+ Option<&SuppressOutline>,
+ ),
+ Or<(
+ Changed,
+ Changed,
+ Changed,
+ )>,
>,
descendants: Query<(Option<&Children>, Option<&ComputedVisualCue>)>,
) {
- for (e, hovered, selected, vis) in &outlinable {
- let color = vis.color(hovered, selected);
+ for (e, hovered, selected, vis, suppress) in &outlinable {
+ let color = if suppress.is_some() {
+ None
+ } else {
+ vis.color(hovered, selected)
+ };
let layers = vis.layers(hovered, selected);
let depth = vis.depth();
let root = vis.root().unwrap_or(e);
diff --git a/rmf_site_editor/src/interaction/popup.rs b/rmf_site_editor/src/interaction/popup.rs
new file mode 100644
index 00000000..914af825
--- /dev/null
+++ b/rmf_site_editor/src/interaction/popup.rs
@@ -0,0 +1,57 @@
+/*
+ * 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::*, site::*};
+
+#[derive(Component, Clone, Copy)]
+pub struct Popup {
+ regular: f32,
+ hovered: f32,
+ selected: f32,
+}
+
+pub fn add_popups(
+ mut commands: Commands,
+ new_poppers: Query, Added)>>,
+) {
+ for e in &new_poppers {
+ commands.entity(e).insert(Popup {
+ regular: 0.,
+ hovered: HOVERED_LANE_OFFSET,
+ selected: SELECTED_LANE_OFFSET,
+ });
+ }
+}
+
+// TODO(@mxgrey): Merge this implementation with the popup implementation for
+// lanes at some point.
+pub fn update_popups(
+ mut objects: Query<
+ (&Hovered, &Selected, &Popup, &mut Transform),
+ Or<(Changed, Changed, Changed)>,
+ >,
+) {
+ for (hovered, selected, popup, mut tf) in &mut objects {
+ if hovered.is_hovered {
+ tf.translation.z = popup.hovered;
+ } else if selected.cue() {
+ tf.translation.z = popup.selected;
+ } else {
+ tf.translation.z = popup.regular;
+ }
+ }
+}
diff --git a/rmf_site_editor/src/interaction/select_anchor.rs b/rmf_site_editor/src/interaction/select_anchor.rs
index 4f75c9be..9b50881b 100644
--- a/rmf_site_editor/src/interaction/select_anchor.rs
+++ b/rmf_site_editor/src/interaction/select_anchor.rs
@@ -18,16 +18,17 @@
use crate::{
interaction::*,
site::{
- Anchor, AnchorBundle, Category, Dependents, DrawingMarker, Original, PathBehavior, Pending,
+ drawing_editor::CurrentEditDrawing, Anchor, AnchorBundle, Category, Dependents,
+ DrawingMarker, Original, PathBehavior, Pending,
},
CurrentWorkspace,
};
use bevy::{ecs::system::SystemParam, prelude::*};
use rmf_site_format::{
Constraint, ConstraintDependents, Door, Edge, Fiducial, Floor, Lane, LiftProperties, Location,
- Measurement, MeshConstraint, MeshElement, Model, ModelMarker, NameInWorkcell, Path,
- PixelsPerMeter, Point, Pose, Side, SiteProperties, Wall, WorkcellCollisionMarker,
- WorkcellModel, WorkcellVisualMarker,
+ Measurement, MeshConstraint, MeshElement, Model, ModelMarker, NameInWorkcell, NameOfSite, Path,
+ PixelsPerMeter, Point, Pose, Side, Wall, WorkcellCollisionMarker, WorkcellModel,
+ WorkcellVisualMarker,
};
use std::collections::HashSet;
use std::sync::Arc;
@@ -588,9 +589,6 @@ impl Placement for EdgePlacement {
params: &mut SelectAnchorPlacementParams<'w, 's>,
) -> Result {
// Restore visibility to anchors that were hidden in this mode
- for e in params.hidden_entities.selected_drawing_anchors.drain() {
- set_visibility(e, &mut params.visibility, true);
- }
for e in params.hidden_entities.drawing_anchors.drain() {
set_visibility(e, &mut params.visibility, true);
}
@@ -1056,11 +1054,6 @@ impl Placement for PathPlacement {
#[derive(Resource, Default)]
pub struct HiddenSelectAnchorEntities {
- /// Level anchors but not assigned to a drawing, hidden when entering constraint creation mode
- pub level_anchors: HashSet,
- /// Anchors assigned to the the selected drawing, hidden when the user chose the first anchor
- /// of a constraint to make sure it is drawn between two different drawings
- pub selected_drawing_anchors: HashSet,
/// All drawing anchors, hidden when users draw level entities such as walls, lanes, floors to
/// make sure they don't connect to drawing anchors
pub drawing_anchors: HashSet,
@@ -1145,14 +1138,6 @@ impl<'w, 's> SelectAnchorPlacementParams<'w, 's> {
Ok(())
}
- fn get_visible_drawing(&self) -> Option<(Entity, &PixelsPerMeter)> {
- self.drawings.iter().find(|(e, _)| {
- self.visibility
- .get(*e)
- .is_ok_and(|vis| vis.is_visible == true)
- })
- }
-
/// Use this when exiting SelectAnchor mode
fn cleanup(&mut self) {
self.cursor
@@ -1169,12 +1154,6 @@ impl<'w, 's> SelectAnchorPlacementParams<'w, 's> {
);
set_visibility(self.cursor.frame_placement, &mut self.visibility, false);
self.cursor.set_model_preview(&mut self.commands, None);
- for e in self.hidden_entities.level_anchors.drain() {
- set_visibility(e, &mut self.visibility, true);
- }
- for e in self.hidden_entities.selected_drawing_anchors.drain() {
- set_visibility(e, &mut self.visibility, true);
- }
for e in self.hidden_entities.drawing_anchors.drain() {
set_visibility(e, &mut self.visibility, true);
}
@@ -1206,15 +1185,6 @@ impl SelectAnchorEdgeBuilder {
}
}
- pub fn for_constraint(self) -> SelectAnchor {
- SelectAnchor {
- target: self.for_element,
- placement: EdgePlacement::new::>(self.placement),
- continuity: self.continuity,
- scope: Scope::MultipleDrawings,
- }
- }
-
pub fn for_wall(self) -> SelectAnchor {
SelectAnchor {
target: self.for_element,
@@ -1269,7 +1239,16 @@ impl SelectAnchorPointBuilder {
}
}
- pub fn for_fiducial(self) -> SelectAnchor {
+ pub fn for_site_fiducial(self) -> SelectAnchor {
+ SelectAnchor {
+ target: self.for_element,
+ placement: PointPlacement::new::>(),
+ continuity: self.continuity,
+ scope: Scope::Site,
+ }
+ }
+
+ pub fn for_drawing_fiducial(self) -> SelectAnchor {
SelectAnchor {
target: self.for_element,
placement: PointPlacement::new::>(),
@@ -1337,7 +1316,6 @@ type PlacementArc = Arc;
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
pub enum Scope {
Drawing,
- MultipleDrawings,
General,
Site,
}
@@ -1904,7 +1882,8 @@ pub fn handle_select_anchor_mode(
mut hover: EventWriter,
blockers: Option>,
workspace: Res,
- open_sites: Query>,
+ open_sites: Query>,
+ current_drawing: Res,
) {
let mut request = match &*mode {
InteractionMode::SelectAnchor(request) => request.clone(),
@@ -1950,20 +1929,6 @@ pub fn handle_select_anchor_mode(
}
match request.scope {
- Scope::MultipleDrawings => {
- // If we are working with requests that span multiple drawings,
- // (constraints) hide all non fiducials
- for (e, _) in ¶ms.anchors {
- if !params.dependents.get(e).is_ok_and(|deps| {
- deps.0.iter().any(|dep| params.fiducials.get(*dep).is_ok())
- }) {
- if let Ok(mut visibility) = params.visibility.get_mut(e) {
- visibility.is_visible = false;
- params.hidden_entities.level_anchors.insert(e);
- }
- }
- }
- }
Scope::General | Scope::Site => {
// If we are working with normal level or site requests, hide all drawing anchors
for anchor in params.anchors.iter().filter(|(e, _)| {
@@ -2073,9 +2038,14 @@ pub fn handle_select_anchor_mode(
new_anchor
}
Scope::Drawing => {
+ let drawing_entity = current_drawing
+ .target()
+ .expect("No drawing while spawning drawing anchor")
+ .drawing;
let (parent, ppm) = params
- .get_visible_drawing()
- .expect("No drawing while spawning drawing anchor");
+ .drawings
+ .get(drawing_entity)
+ .expect("Entity being edited is not a drawing");
// We also need to have a transform such that the anchor will spawn in the
// right spot
let pose = compute_parent_inverse_pose(&tf, &transforms, parent);
@@ -2084,14 +2054,10 @@ pub fn handle_select_anchor_mode(
.commands
.spawn(AnchorBundle::new([pose.trans[0], pose.trans[1]].into()))
.insert(Transform::from_scale(Vec3::new(ppm, ppm, 1.0)))
+ .set_parent(parent)
.id();
- params.commands.entity(parent).add_child(new_anchor);
new_anchor
}
- Scope::MultipleDrawings => {
- warn!("Only existing fiducials can be connected through constraints");
- return;
- }
Scope::General => params.commands.spawn(AnchorBundle::at_transform(tf)).id(),
};
@@ -2143,25 +2109,7 @@ pub fn handle_select_anchor_mode(
.filter(|s| anchors.contains(*s))
{
request = match request.next(AnchorSelection::existing(new_selection), &mut params) {
- Some(next_mode) => {
- // We need to hide anchors connected to this drawing to force the user to
- // connect different drawing
- if matches!(request.scope, Scope::MultipleDrawings) {
- params
- .parents
- .get(new_selection)
- .and_then(|p| params.children.get(**p))
- .map(|children| {
- for c in children.iter().filter(|c| params.anchors.get(**c).is_ok())
- {
- set_visibility(*c, &mut params.visibility, false);
- params.hidden_entities.selected_drawing_anchors.insert(*c);
- }
- })
- .ok();
- }
- next_mode
- }
+ Some(next_mode) => next_mode,
None => {
params.cleanup();
*mode = InteractionMode::Inspect;
diff --git a/rmf_site_editor/src/keyboard.rs b/rmf_site_editor/src/keyboard.rs
index d9a948e1..f8773463 100644
--- a/rmf_site_editor/src/keyboard.rs
+++ b/rmf_site_editor/src/keyboard.rs
@@ -18,9 +18,9 @@
use crate::{
interaction::{
camera_controls::{CameraControls, HeadlightToggle},
- ChangeMode, InteractionMode, Selection,
+ ChangeMode, ChangeProjectionMode, InteractionMode, Selection,
},
- site::{AlignLevelDrawings, AlignSiteDrawings, CurrentLevel, Delete},
+ site::{AlignSiteDrawings, CurrentLevel, Delete},
CreateNewWorkspace, CurrentWorkspace, LoadWorkspace, SaveWorkspace,
};
use bevy::{ecs::system::SystemParam, prelude::*};
@@ -47,7 +47,6 @@ impl Plugin for KeyboardInputPlugin {
// TODO(luca) get rid of this once 16 parameters limit is lifted in bevy 0.10
#[derive(SystemParam)]
struct KeyboardParams<'w, 's> {
- align_drawings: EventWriter<'w, 's, AlignLevelDrawings>,
align_site: EventWriter<'w, 's, AlignSiteDrawings>,
current_workspace: Res<'w, CurrentWorkspace>,
}
@@ -56,17 +55,14 @@ fn handle_keyboard_input(
keyboard_input: Res>,
selection: Res,
current_mode: Res,
- mut camera_controls: ResMut,
- mut cameras: Query<&mut Camera>,
- mut visibilities: Query<&mut Visibility>,
mut egui_context: ResMut,
mut change_mode: EventWriter,
mut delete: EventWriter,
mut save_workspace: EventWriter,
mut new_workspace: EventWriter,
mut load_workspace: EventWriter,
+ mut change_camera_mode: EventWriter,
current_level: Res,
- headlight_toggle: Res,
mut debug_mode: ResMut,
mut params: KeyboardParams,
) {
@@ -80,11 +76,11 @@ fn handle_keyboard_input(
}
if keyboard_input.just_pressed(KeyCode::F2) {
- camera_controls.use_orthographic(true, &mut cameras, &mut visibilities, headlight_toggle.0);
+ change_camera_mode.send(ChangeProjectionMode::to_orthographic());
}
if keyboard_input.just_pressed(KeyCode::F3) {
- camera_controls.use_perspective(true, &mut cameras, &mut visibilities, headlight_toggle.0);
+ change_camera_mode.send(ChangeProjectionMode::to_perspective());
}
if keyboard_input.just_pressed(KeyCode::Escape) {
@@ -117,14 +113,8 @@ fn handle_keyboard_input(
}
if keyboard_input.just_pressed(KeyCode::T) {
- if keyboard_input.any_pressed([KeyCode::LShift, KeyCode::RShift]) {
- if let Some(site) = params.current_workspace.root {
- params.align_site.send(AlignSiteDrawings(site));
- }
- } else {
- if let Some(level) = **current_level {
- params.align_drawings.send(AlignLevelDrawings(level));
- }
+ if let Some(site) = params.current_workspace.root {
+ params.align_site.send(AlignSiteDrawings(site));
}
}
diff --git a/rmf_site_editor/src/occupancy.rs b/rmf_site_editor/src/occupancy.rs
index 1af75c53..877fe3c0 100644
--- a/rmf_site_editor/src/occupancy.rs
+++ b/rmf_site_editor/src/occupancy.rs
@@ -18,7 +18,7 @@
use crate::{
interaction::ComputedVisualCue,
shapes::*,
- site::{Category, LevelProperties, SiteAssets, SiteProperties, LANE_LAYER_START},
+ site::{Category, LevelElevation, NameOfSite, SiteAssets, LANE_LAYER_START},
};
use bevy::{
math::{swizzles::*, Affine3A, Mat3A, Vec2, Vec3A},
@@ -156,8 +156,8 @@ fn calculate_grid(
Option<&ComputedVisualCue>,
)>,
parents: Query<&Parent>,
- levels: Query>,
- sites: Query<(), With>,
+ levels: Query>,
+ sites: Query<(), With>,
mut meshes: ResMut>,
assets: Res,
grids: Query>,
@@ -296,7 +296,7 @@ fn calculate_grid(
}
fn get_levels_of_sites(
- levels: &Query>,
+ levels: &Query>,
parents: &Query<&Parent>,
) -> HashMap> {
let mut levels_of_sites: HashMap> = HashMap::new();
@@ -312,8 +312,8 @@ fn get_levels_of_sites(
fn get_group(
e: Entity,
parents: &Query<&Parent>,
- levels: &Query>,
- sites: &Query<(), With>,
+ levels: &Query>,
+ sites: &Query<(), With>,
) -> Group {
let mut e_meta = e;
loop {
diff --git a/rmf_site_editor/src/site/anchor.rs b/rmf_site_editor/src/site/anchor.rs
index 3ec59880..10d4ddd3 100644
--- a/rmf_site_editor/src/site/anchor.rs
+++ b/rmf_site_editor/src/site/anchor.rs
@@ -162,8 +162,9 @@ pub fn assign_orphan_anchors_to_parent(
// No level is currently assigned, so we should create one.
let new_level_id = commands
.spawn(LevelProperties {
- name: "".to_string(),
- elevation: 0.,
+ name: NameInSite("".to_owned()),
+ elevation: LevelElevation(0.),
+ ..default()
})
.insert(Category::Level)
.id();
diff --git a/rmf_site_editor/src/site/assets.rs b/rmf_site_editor/src/site/assets.rs
index 224d67ae..3ec8078d 100644
--- a/rmf_site_editor/src/site/assets.rs
+++ b/rmf_site_editor/src/site/assets.rs
@@ -193,16 +193,18 @@ impl FromWorld for SiteAssets {
);
let location_mesh = meshes.add(
Mesh::from(
- make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 6)
- .transform_by(Affine3A::from_translation(0.00125 * Vec3::Z)),
+ make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 6).transform_by(
+ Affine3A::from_translation((0.00125 + LOCATION_LAYER_HEIGHT) * Vec3::Z),
+ ),
)
.with_generated_outline_normals()
.unwrap(),
);
let fiducial_mesh = meshes.add(
Mesh::from(
- make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 4)
- .transform_by(Affine3A::from_translation(0.00125 * Vec3::Z)),
+ make_icon_halo(1.1 * LANE_WIDTH / 2.0, 0.01, 4).transform_by(
+ Affine3A::from_translation((0.00125 + LOCATION_LAYER_HEIGHT) * Vec3::Z),
+ ),
)
.with_generated_outline_normals()
.unwrap(),
diff --git a/rmf_site_editor/src/site/constraint.rs b/rmf_site_editor/src/site/constraint.rs
index 666bde80..0a2e0dbd 100644
--- a/rmf_site_editor/src/site/constraint.rs
+++ b/rmf_site_editor/src/site/constraint.rs
@@ -37,8 +37,8 @@ pub fn assign_orphan_constraints_to_parent(
constraints: Query<(Entity, &Edge), (Without, With)>,
current_workspace: Res,
parents: Query<&Parent>,
- levels: Query>,
- open_sites: Query>,
+ levels: Query>,
+ open_sites: Query>,
) {
if let Some(current_site) = current_workspace.to_site(&open_sites) {
for (e, edge) in &constraints {
@@ -164,7 +164,7 @@ pub fn update_constraint_for_changed_labels(
dependents: Query<&Dependents>,
fiducials: Query>,
constraints: Query<(Entity, &Edge, &Parent), With>,
- open_sites: Query>,
+ open_sites: Query>,
) {
let get_fiducial_label = |e: Entity| -> Option<&Label> {
let fiducial = dependents
diff --git a/rmf_site_editor/src/site/deletion.rs b/rmf_site_editor/src/site/deletion.rs
index a0305e33..35436066 100644
--- a/rmf_site_editor/src/site/deletion.rs
+++ b/rmf_site_editor/src/site/deletion.rs
@@ -18,7 +18,10 @@
use crate::{
interaction::{Select, Selection},
log::Log,
- site::{Category, CurrentLevel, Dependents, LevelProperties, SiteUpdateStage},
+ site::{
+ Category, CurrentLevel, Dependents, LevelElevation, LevelProperties, NameInSite,
+ SiteUpdateStage,
+ },
};
use bevy::{ecs::system::SystemParam, prelude::*};
use rmf_site_format::{ConstraintDependents, Edge, MeshConstraint, Path, Point};
@@ -85,7 +88,7 @@ struct DeletionParams<'w, 's> {
children: Query<'w, 's, &'static Children>,
selection: Res<'w, Selection>,
current_level: ResMut<'w, CurrentLevel>,
- levels: Query<'w, 's, Entity, With>,
+ levels: Query<'w, 's, Entity, With>,
select: EventWriter<'w, 's, Select>,
log: EventWriter<'w, 's, Log>,
}
@@ -345,8 +348,9 @@ fn perform_deletions(all_to_delete: HashSet, params: &mut DeletionParams
.commands
.spawn(SpatialBundle::default())
.insert(LevelProperties {
- elevation: 0.0,
- name: "".to_string(),
+ name: NameInSite("".to_owned()),
+ elevation: LevelElevation(0.0),
+ ..default()
})
.insert(Category::Level)
.id();
diff --git a/rmf_site_editor/src/site/drawing.rs b/rmf_site_editor/src/site/drawing.rs
index 44053297..2d1f0711 100644
--- a/rmf_site_editor/src/site/drawing.rs
+++ b/rmf_site_editor/src/site/drawing.rs
@@ -19,25 +19,20 @@ use crate::{
interaction::Selectable,
shapes::make_flat_rect_mesh,
site::{
- get_current_workspace_path, Anchor, DefaultFile, FiducialMarker, GlobalFloorVisibility,
+ get_current_workspace_path, Anchor, DefaultFile, FiducialMarker, GlobalDrawingVisibility,
LayerVisibility, MeasurementMarker, MeasurementSegment, RecencyRank,
DEFAULT_MEASUREMENT_OFFSET, FLOOR_LAYER_START,
},
CurrentWorkspace,
};
use bevy::{asset::LoadState, math::Affine3A, prelude::*};
-use rmf_site_format::{
- AssetSource, Category, Drawing, IsPrimary, NameInSite, PixelsPerMeter, Pose,
-};
+use rmf_site_format::{AssetSource, Category, DrawingProperties, NameInSite, PixelsPerMeter, Pose};
+use std::path::PathBuf;
#[derive(Bundle, Debug, Clone)]
pub struct DrawingBundle {
+ pub properties: DrawingProperties,
pub category: Category,
- pub name: NameInSite,
- pub source: AssetSource,
- pub pose: Pose,
- pub pixels_per_meter: PixelsPerMeter,
- pub is_primary: IsPrimary,
pub transform: Transform,
pub global_transform: GlobalTransform,
pub visibility: Visibility,
@@ -46,19 +41,15 @@ pub struct DrawingBundle {
}
impl DrawingBundle {
- pub fn new(drawing: &Drawing) -> Self {
+ pub fn new(properties: DrawingProperties) -> Self {
DrawingBundle {
+ properties,
category: Category::Drawing,
- name: drawing.name.clone(),
- source: drawing.source.clone(),
- pose: drawing.pose.clone(),
- pixels_per_meter: drawing.pixels_per_meter.clone(),
- is_primary: drawing.is_primary.clone(),
- transform: Transform::IDENTITY,
- global_transform: GlobalTransform::IDENTITY,
- visibility: Visibility::VISIBLE,
- computed: ComputedVisibility::default(),
- marker: DrawingMarker::default(),
+ transform: default(),
+ global_transform: default(),
+ visibility: default(),
+ computed: default(),
+ marker: default(),
}
}
}
@@ -66,24 +57,8 @@ impl DrawingBundle {
#[derive(Component, Clone, Copy, Debug, Default)]
pub struct DrawingMarker;
-#[derive(Debug, Clone, Copy, Default, Deref, DerefMut, Resource)]
-pub struct GlobalDrawingVisibility(pub LayerVisibility);
-
pub const DRAWING_LAYER_START: f32 = 0.0;
-// Semi transparency for drawings, more opaque than floors to make them visible
-const DEFAULT_DRAWING_SEMI_TRANSPARENCY: f32 = 0.5;
-
-/// Resource used to set what the alpha value for partially transparent drawings should be
-#[derive(Clone, Resource, Deref, DerefMut)]
-pub struct DrawingSemiTransparency(f32);
-
-impl Default for DrawingSemiTransparency {
- fn default() -> Self {
- DrawingSemiTransparency(DEFAULT_DRAWING_SEMI_TRANSPARENCY)
- }
-}
-
#[derive(Debug, Clone, Copy, Component)]
pub struct DrawingSegments {
leaf: Entity,
@@ -101,24 +76,24 @@ fn drawing_layer_height(rank: Option<&RecencyRank>) -> f32 {
pub fn add_drawing_visuals(
mut commands: Commands,
- changed_drawings: Query<
- (Entity, &AssetSource, &IsPrimary),
- (With, Changed),
- >,
+ changed_drawings: Query<(Entity, &AssetSource), (With, Changed)>,
asset_server: Res,
current_workspace: Res,
site_files: Query<&DefaultFile>,
- mut default_floor_vis: ResMut,
- drawing_transparency: Res,
+ default_drawing_vis: Query<&GlobalDrawingVisibility>,
) {
+ if changed_drawings.is_empty() {
+ return;
+ }
+
// TODO(luca) depending on when this system is executed, this function might be called between
// the creation of the drawing and the change of the workspace, making this silently fail
// Look into reordering systems, or adding a marker component, to make sure this doesn't happen
let file_path = match get_current_workspace_path(current_workspace, site_files) {
Some(file_path) => file_path,
- None => return,
+ None => PathBuf::new(),
};
- for (e, source, is_primary) in &changed_drawings {
+ for (e, source) in &changed_drawings {
// Append file name to path if it's a local file
// TODO(luca) cleanup
let asset_source = match source {
@@ -128,19 +103,7 @@ pub fn add_drawing_visuals(
_ => source.clone(),
};
let texture_handle: Handle = asset_server.load(&String::from(&asset_source));
- let visibility = if is_primary.0 == true {
- LayerVisibility::Opaque
- } else {
- LayerVisibility::Alpha(**drawing_transparency)
- };
- commands
- .entity(e)
- .insert(LoadingDrawing(texture_handle))
- .insert(visibility);
- }
-
- if !changed_drawings.is_empty() {
- **default_floor_vis = LayerVisibility::new_semi_transparent();
+ commands.entity(e).insert(LoadingDrawing(texture_handle));
}
}
@@ -155,15 +118,18 @@ pub fn handle_loaded_drawing(
&PixelsPerMeter,
&LoadingDrawing,
Option<&LayerVisibility>,
+ Option<&Parent>,
+ Option<&RecencyRank>,
)>,
mut mesh_assets: ResMut>,
asset_server: Res,
mut materials: ResMut>,
- rank: Query<&RecencyRank>,
segments: Query<&DrawingSegments>,
- default_drawing_vis: Res,
+ default_drawing_vis: Query<&GlobalDrawingVisibility>,
) {
- for (entity, source, pose, pixels_per_meter, handle, vis) in loading_drawings.iter() {
+ for (entity, source, pose, pixels_per_meter, handle, vis, parent, rank) in
+ loading_drawings.iter()
+ {
match asset_server.get_load_state(&handle.0) {
LoadState::Loaded => {
let img = assets.get(&handle.0).unwrap();
@@ -175,6 +141,16 @@ pub fn handle_loaded_drawing(
Affine3A::from_translation(Vec3::new(width / 2.0, -height / 2.0, 0.0)),
);
let mesh = mesh_assets.add(mesh.into());
+ let default = parent
+ .map(|p| default_drawing_vis.get(p.get()).ok())
+ .flatten();
+ let (alpha, alpha_mode) = drawing_alpha(vis, rank, default);
+ let material = materials.add(StandardMaterial {
+ base_color_texture: Some(handle.0.clone()),
+ base_color: *Color::default().set_a(alpha),
+ alpha_mode,
+ ..Default::default()
+ });
let leaf = if let Ok(segment) = segments.get(entity) {
segment.leaf
@@ -191,20 +167,22 @@ pub fn handle_loaded_drawing(
.insert(Selectable::new(entity));
leaf
};
- let z = drawing_layer_height(rank.get(entity).ok());
- let (alpha, alpha_mode) = drawing_alpha(vis, &default_drawing_vis);
- commands.entity(leaf).insert(PbrBundle {
- mesh,
- material: materials.add(StandardMaterial {
- base_color_texture: Some(handle.0.clone()),
- base_color: *Color::default().set_a(alpha),
- alpha_mode,
- ..default()
- }),
- transform: Transform::from_xyz(0.0, 0.0, z),
- ..default()
- });
- commands.entity(entity).remove::();
+ let z = drawing_layer_height(rank);
+ commands
+ .entity(leaf)
+ .insert(PbrBundle {
+ mesh,
+ material: material.clone(),
+ transform: Transform::from_xyz(0.0, 0.0, z),
+ ..Default::default()
+ })
+ .insert(Selectable::new(entity));
+ commands
+ .entity(entity)
+ // Put a handle for the material into the main entity
+ // so that we can modify it during interactions.
+ .insert(material)
+ .remove::();
}
LoadState::Failed => {
error!("Failed loading drawing {:?}", String::from(source));
@@ -257,7 +235,10 @@ pub fn update_drawing_pixels_per_meter(
pub fn update_drawing_children_to_pixel_coordinates(
mut commands: Commands,
- changed_drawings: Query<(&PixelsPerMeter, &Children), Changed>,
+ changed_drawings: Query<
+ (&PixelsPerMeter, &Children),
+ Or<(Changed, Changed)>,
+ >,
meshes: Query, With, With)>>,
mut transforms: Query<&mut Transform>,
) {
@@ -278,11 +259,28 @@ pub fn update_drawing_children_to_pixel_coordinates(
}
}
+#[inline]
fn drawing_alpha(
specific: Option<&LayerVisibility>,
- general: &LayerVisibility,
+ rank: Option<&RecencyRank>,
+ general: Option<&GlobalDrawingVisibility>,
) -> (f32, AlphaMode) {
- let alpha = specific.map(|s| s.alpha()).unwrap_or(general.alpha());
+ let alpha = specific
+ .copied()
+ .unwrap_or_else(|| {
+ general
+ .map(|v| {
+ if let Some(r) = rank {
+ if r.rank() < v.bottom_count {
+ return v.bottom;
+ }
+ }
+ v.general
+ })
+ .unwrap_or(LayerVisibility::Opaque)
+ })
+ .alpha();
+
let alpha_mode = if alpha < 1.0 {
AlphaMode::Blend
} else {
@@ -291,16 +289,27 @@ fn drawing_alpha(
(alpha, alpha_mode)
}
+#[inline]
fn iter_update_drawing_visibility<'a>(
- iter: impl Iterator- , &'a DrawingSegments)>,
+ iter: impl Iterator<
+ Item = (
+ Option<&'a LayerVisibility>,
+ Option<&'a Parent>,
+ Option<&'a RecencyRank>,
+ &'a DrawingSegments,
+ ),
+ >,
material_handles: &Query<&Handle>,
material_assets: &mut ResMut>,
- default_drawing_vis: &LayerVisibility,
+ default_drawing_vis: &Query<&GlobalDrawingVisibility>,
) {
- for (vis, segments) in iter {
+ for (vis, parent, rank, segments) in iter {
if let Ok(handle) = material_handles.get(segments.leaf) {
if let Some(mat) = material_assets.get_mut(handle) {
- let (alpha, alpha_mode) = drawing_alpha(vis, &default_drawing_vis);
+ let default = parent
+ .map(|p| default_drawing_vis.get(p.get()).ok())
+ .flatten();
+ let (alpha, alpha_mode) = drawing_alpha(vis, rank, default);
mat.base_color = *mat.base_color.set_a(alpha);
mat.alpha_mode = alpha_mode;
}
@@ -309,35 +318,49 @@ fn iter_update_drawing_visibility<'a>(
}
// TODO(luca) RemovedComponents is brittle, maybe wrap component in an option?
-// TODO(luca) This is copy-pasted from floor.rs, consider having a generic plugin
pub fn update_drawing_visibility(
- changed_floors: Query<(Option<&LayerVisibility>, &DrawingSegments), Changed>,
+ changed_drawings: Query<
+ Entity,
+ Or<(
+ Changed,
+ Changed,
+ Changed>,
+ )>,
+ >,
removed_vis: RemovedComponents,
- all_floors: Query<(Option<&LayerVisibility>, &DrawingSegments)>,
+ all_drawings: Query<(
+ Option<&LayerVisibility>,
+ Option<&Parent>,
+ Option<&RecencyRank>,
+ &DrawingSegments,
+ )>,
material_handles: Query<&Handle>,
mut material_assets: ResMut>,
- default_drawing_vis: Res,
+ default_drawing_vis: Query<&GlobalDrawingVisibility>,
+ changed_default_drawing_vis: Query<&Children, Changed>,
) {
- if default_drawing_vis.is_changed() {
- iter_update_drawing_visibility(
- all_floors.iter(),
- &material_handles,
- &mut material_assets,
- &default_drawing_vis,
- );
- } else {
- iter_update_drawing_visibility(
- changed_floors.iter(),
- &material_handles,
- &mut material_assets,
- &default_drawing_vis,
- );
+ iter_update_drawing_visibility(
+ changed_drawings
+ .iter()
+ .filter_map(|e| all_drawings.get(e).ok()),
+ &material_handles,
+ &mut material_assets,
+ &default_drawing_vis,
+ );
+
+ iter_update_drawing_visibility(
+ removed_vis.iter().filter_map(|e| all_drawings.get(e).ok()),
+ &material_handles,
+ &mut material_assets,
+ &default_drawing_vis,
+ );
+ for children in &changed_default_drawing_vis {
iter_update_drawing_visibility(
- removed_vis.iter().filter_map(|e| all_floors.get(e).ok()),
+ children.iter().filter_map(|e| all_drawings.get(*e).ok()),
&material_handles,
&mut material_assets,
&default_drawing_vis,
);
- };
+ }
}
diff --git a/rmf_site_editor/src/site/drawing_editor/alignment.rs b/rmf_site_editor/src/site/drawing_editor/alignment.rs
new file mode 100644
index 00000000..a69d8cae
--- /dev/null
+++ b/rmf_site_editor/src/site/drawing_editor/alignment.rs
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2023 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+use bevy::ecs::system::SystemParam;
+use bevy::{
+ math::{DVec2, Vec2},
+ prelude::*,
+};
+
+use crate::site::{
+ Affiliation, AlignSiteDrawings, Anchor, Angle, Category, Change, ConstraintMarker, Distance,
+ DrawingMarker, Edge, FiducialMarker, LevelElevation, MeasurementMarker, NameOfSite,
+ PixelsPerMeter, Point, Pose, Rotation, SiteProperties,
+};
+use itertools::{Either, Itertools};
+use rmf_site_format::alignment::{
+ align_site, DrawingVariables, FiducialVariables, MeasurementVariables, SiteVariables,
+};
+use std::collections::HashSet;
+
+#[derive(SystemParam)]
+pub struct OptimizationParams<'w, 's> {
+ drawings: Query<
+ 'w,
+ 's,
+ (
+ &'static Children,
+ &'static mut Pose,
+ &'static mut PixelsPerMeter,
+ ),
+ With,
+ >,
+ anchors: Query<'w, 's, &'static Anchor>,
+ fiducials:
+ Query<'w, 's, (&'static Affiliation, &'static Point), With>,
+ measurements:
+ Query<'w, 's, (&'static Edge, &'static Distance), With>,
+}
+
+pub fn align_site_drawings(
+ levels: Query<&Children, With>,
+ sites: Query<&Children, With>,
+ mut events: EventReader,
+ mut params: OptimizationParams,
+) {
+ for AlignSiteDrawings(site) in events.iter() {
+ let mut site_variables = SiteVariables::::default();
+ let Ok(children) = sites.get(*site) else { continue };
+ for child in children {
+ let Ok((group, point)) = params.fiducials.get(*child) else { continue };
+ let Ok(anchor) = params.anchors.get(point.0) else { continue };
+ let Some(group) = group.0 else { continue };
+ let p = anchor.translation_for_category(Category::Fiducial);
+ site_variables.fiducials.push(FiducialVariables {
+ group,
+ position: DVec2::new(p[0] as f64, p[1] as f64),
+ });
+ }
+
+ for child in children {
+ let Ok(level_children) = levels.get(*child) else { continue };
+ for level_child in level_children {
+ let Ok((drawing_children, pose, ppm)) = params.drawings.get(*level_child) else { continue };
+ let mut drawing_variables = DrawingVariables::::new(
+ Vec2::from_slice(&pose.trans).as_dvec2(),
+ pose.rot.yaw().radians() as f64,
+ (1.0 / ppm.0) as f64,
+ );
+ for child in drawing_children {
+ if let Ok((group, point)) = params.fiducials.get(*child) {
+ let Ok(anchor) = params.anchors.get(point.0) else { continue };
+ let Some(group) = group.0 else { continue };
+ let p = anchor.translation_for_category(Category::Fiducial);
+ drawing_variables.fiducials.push(FiducialVariables {
+ group,
+ position: DVec2::new(p[0] as f64, p[1] as f64),
+ });
+ }
+
+ if let Ok((edge, distance)) = params.measurements.get(*child) {
+ let Ok([anchor0, anchor1]) = params.anchors.get_many(edge.array()) else { continue };
+ let Some(in_meters) = distance.0 else { continue };
+ let in_meters = in_meters as f64;
+ let p0 =
+ Vec2::from_slice(anchor0.translation_for_category(Category::Fiducial));
+ let p1 =
+ Vec2::from_slice(anchor1.translation_for_category(Category::Fiducial));
+ let in_pixels = (p1 - p0).length() as f64;
+ drawing_variables.measurements.push(MeasurementVariables {
+ in_pixels,
+ in_meters,
+ });
+ }
+ }
+
+ site_variables
+ .drawings
+ .insert(*level_child, drawing_variables);
+ }
+ }
+
+ // TODO(@mxgrey): When we implement an undo buffer, remember to make an
+ // undo operation for this set of changes.
+ let alignments = align_site(&site_variables);
+ for (e, alignment) in alignments {
+ let Ok((_, mut pose, mut ppm)) = params.drawings.get_mut(e) else { continue };
+ pose.trans[0] = alignment.translation.x as f32;
+ pose.trans[1] = alignment.translation.y as f32;
+ pose.rot =
+ Rotation::Yaw(Angle::Rad(alignment.rotation as f32).match_variant(pose.rot.yaw()));
+ ppm.0 = 1.0 / alignment.scale as f32;
+ }
+ }
+}
diff --git a/rmf_site_editor/src/site/drawing_editor/mod.rs b/rmf_site_editor/src/site/drawing_editor/mod.rs
index e19b1e29..0be419f2 100644
--- a/rmf_site_editor/src/site/drawing_editor/mod.rs
+++ b/rmf_site_editor/src/site/drawing_editor/mod.rs
@@ -15,174 +15,251 @@
*
*/
-use bevy::prelude::*;
+use bevy::{prelude::*, render::view::visibility::RenderLayers};
-pub mod optimizer;
-pub use optimizer::*;
+pub mod alignment;
+pub use alignment::*;
-use crate::interaction::{CameraControls, HeadlightToggle, Selection};
-use crate::site::{
- Anchor, DrawingMarker, Edge, FiducialMarker, MeasurementMarker, Pending, PixelsPerMeter, Point,
+use crate::AppState;
+use crate::{
+ interaction::{ChangeProjectionMode, Selection, SuppressHighlight, SuppressOutline},
+ site::{
+ Anchor, DrawingMarker, Edge, FiducialMarker, MeasurementMarker, NameOfSite, Pending,
+ PixelsPerMeter, Point, PreventDeletion, SiteProperties, WorkcellProperties,
+ },
+ CurrentWorkspace, WorkspaceMarker,
};
-use crate::{AppState, VisibilityEvents};
use std::collections::HashSet;
-#[derive(Default)]
-pub struct DrawingEditorPlugin;
+#[derive(Clone, Copy)]
+pub struct BeginEditDrawing(pub Entity);
-#[derive(Resource, Default, Deref, DerefMut)]
-pub struct DrawingEditorHiddenEntities(HashSet);
+/// Command to finish editing a drawing. Use None to command any drawing to finish.
+#[derive(Clone, Copy)]
+pub struct FinishEditDrawing(pub Option);
-// TODO(luca) should these events be defined somewhere else?
-#[derive(Deref, DerefMut)]
-pub struct ScaleDrawing(pub Entity);
+#[derive(Clone, Copy)]
+pub struct EditDrawing {
+ /// What drawing is being edited
+ pub drawing: Entity,
+ /// What is the original parent level for the drawing
+ pub level: Entity,
+}
-#[derive(Deref, DerefMut)]
-pub struct AlignLevelDrawings(pub Entity);
+#[derive(Clone, Copy, Resource)]
+pub struct CurrentEditDrawing {
+ editor: Entity,
+ target: Option,
+}
+
+impl FromWorld for CurrentEditDrawing {
+ fn from_world(world: &mut World) -> Self {
+ let editor = world.spawn(SpatialBundle::default()).id();
+ Self {
+ editor,
+ target: None,
+ }
+ }
+}
+
+impl CurrentEditDrawing {
+ pub fn target(&self) -> &Option {
+ &self.target
+ }
+}
+
+#[derive(Default)]
+pub struct DrawingEditorPlugin;
#[derive(Deref, DerefMut)]
pub struct AlignSiteDrawings(pub Entity);
-fn hide_level_entities(
- mut visibilities: Query<&mut Visibility>,
- mut camera_controls: ResMut,
- mut cameras: Query<&mut Camera>,
- headlight_toggle: Res,
- mut visibility_events: VisibilityEvents,
+fn switch_edit_drawing_mode(
+ mut commands: Commands,
+ mut begin: EventReader,
+ mut finish: EventReader,
+ mut current: ResMut,
+ mut workspace_visibility: Query<&mut Visibility, With>,
+ mut app_state: ResMut>,
+ mut local_tf: Query<&mut Transform>,
+ mut change_camera_mode: EventWriter,
+ global_tf: Query<&GlobalTransform>,
+ current_workspace: Res,
+ parent: Query<&Parent, With>,
+ is_site: Query<(), With>,
+ is_workcell: Query<(), With>,
) {
- camera_controls.use_orthographic(true, &mut cameras, &mut visibilities, headlight_toggle.0);
- visibility_events.constraints.send(false.into());
- visibility_events.doors.send(false.into());
- visibility_events.lanes.send(false.into());
- visibility_events.lift_cabins.send(false.into());
- visibility_events.lift_cabin_doors.send(false.into());
- visibility_events.locations.send(false.into());
- visibility_events.floors.send(false.into());
- visibility_events.models.send(false.into());
- visibility_events.walls.send(false.into());
-}
+ // TODO(@mxgrey): We can make this implementation much cleaner after we
+ // update to the latest version of bevy that distinguishes between inherited
+ // vs independent visibility.
+ //
+ // We should also consider using an edit mode stack instead of simply
+ // CurrentWorkspace and AppState.
+ 'handle_begin: {
+ if let Some(BeginEditDrawing(e)) = begin.iter().last() {
+ if current.target().is_some_and(|c| c.drawing == *e) {
+ break 'handle_begin;
+ }
-fn hide_non_drawing_entities(
- mut anchors: Query<(Entity, &mut Visibility), (With, Without)>,
- parents: Query<&Parent>,
- mut drawings: Query<(Entity, &mut Visibility), (Without, With)>,
- mut anchor_set: ResMut,
- selection: Res,
-) {
- for (e, mut vis) in &mut anchors {
- if let Ok(parent) = parents.get(e) {
- if drawings.get(**parent).is_err() {
- if vis.is_visible {
- vis.is_visible = false;
- anchor_set.insert(e);
+ if let Some(c) = current.target() {
+ // A drawing was being edited and now we're switching to a
+ // different drawing, so we need to reset the previous drawing.
+ restore_edited_drawing(c, &mut commands);
+ }
+
+ let level = if let Ok(p) = parent.get(*e) {
+ p.get()
+ } else {
+ error!("Cannot edit {e:?} as a drawing");
+ current.target = None;
+ break 'handle_begin;
+ };
+
+ current.target = Some(EditDrawing { drawing: *e, level });
+ commands
+ .entity(*e)
+ .set_parent(current.editor)
+ .insert(Visibility { is_visible: true })
+ .insert(ComputedVisibility::default())
+ .insert(PreventDeletion::because(
+ "Cannot delete a drawing that is currently being edited".to_owned(),
+ ))
+ // Highlighting the drawing looks bad when the user will be
+ // constantly hovering over it anyway.
+ .insert(SuppressHighlight);
+
+ change_camera_mode.send(ChangeProjectionMode::to_orthographic());
+
+ if let Ok(mut editor_tf) = local_tf.get_mut(current.editor) {
+ if let Ok(mut level_tf) = global_tf.get(level) {
+ *editor_tf = level_tf.compute_transform();
+ } else {
+ error!("Cannot get transform of current level");
}
+ } else {
+ error!("Cannot change transform of drawing editor view");
}
- }
- }
- for (e, mut vis) in &mut drawings {
- if **selection != Some(e) {
- if vis.is_visible {
- vis.is_visible = false;
- anchor_set.insert(e);
+
+ if let Some(err) = app_state.set(AppState::SiteDrawingEditor).err() {
+ error!("Unable to switch to drawing editor mode: {err:?}");
+ }
+
+ for mut v in &mut workspace_visibility {
+ v.is_visible = false;
}
}
}
-}
-fn restore_non_drawing_entities(
- mut visibilities: Query<&mut Visibility>,
- mut anchor_set: ResMut,
-) {
- for e in anchor_set.drain() {
- visibilities
- .get_mut(e)
- .map(|mut vis| vis.is_visible = true)
- .ok();
+ for FinishEditDrawing(finish) in finish.iter() {
+ let c = if let Some(c) = current.target() {
+ if finish.is_some_and(|e| e != c.drawing) {
+ continue;
+ }
+ c
+ } else {
+ continue;
+ };
+
+ restore_edited_drawing(c, &mut commands);
+ current.target = None;
+
+ // This camera change would not be needed if we have an edit mode stack
+ change_camera_mode.send(ChangeProjectionMode::to_perspective());
+
+ if let Some(w) = current_workspace.root {
+ if let Ok(mut v) = workspace_visibility.get_mut(w) {
+ v.is_visible = current_workspace.display;
+ }
+
+ if is_site.contains(w) {
+ if let Some(err) = app_state.set(AppState::SiteEditor).err() {
+ error!("Failed to switch back to site editing mode: {err:?}");
+ }
+ } else if is_workcell.contains(w) {
+ if let Some(err) = app_state.set(AppState::WorkcellEditor).err() {
+ error!("Failed to switch back to workcell editing mode: {err:?}");
+ }
+ } else {
+ // This logic can probably be improved with an editor mode stack
+ error!(
+ "Unable to identify the type for the current workspace \
+ {w:?}, so we will default to site editing mode",
+ );
+ if let Some(err) = app_state.set(AppState::SiteEditor).err() {
+ error!("Failed to switch back to site editing mode: {err:?}");
+ }
+ }
+ }
}
}
-fn restore_level_entities(
- mut visibilities: Query<&mut Visibility>,
- mut camera_controls: ResMut,
- mut cameras: Query<&mut Camera>,
- headlight_toggle: Res,
- mut visibility_events: VisibilityEvents,
-) {
- camera_controls.use_perspective(true, &mut cameras, &mut visibilities, headlight_toggle.0);
- visibility_events.constraints.send(true.into());
- visibility_events.doors.send(true.into());
- visibility_events.lanes.send(true.into());
- visibility_events.lift_cabins.send(true.into());
- visibility_events.lift_cabin_doors.send(true.into());
- visibility_events.locations.send(true.into());
- visibility_events.floors.send(true.into());
- visibility_events.models.send(true.into());
- visibility_events.walls.send(true.into());
+/// Restore a drawing that was being edited back to its normal place and behavior
+fn restore_edited_drawing(edit: &EditDrawing, commands: &mut Commands) {
+ commands
+ .entity(edit.drawing)
+ .set_parent(edit.level)
+ .remove::()
+ .remove::();
}
-fn assign_drawing_parent_to_new_measurements_and_fiducials(
+fn assign_drawing_parent_to_new_measurements(
mut commands: Commands,
- mut new_elements: Query<
- (Entity, Option<&Parent>, &mut Transform),
+ mut changed_measurement: Query<
+ (Entity, &Edge, Option<&Parent>),
(
Without,
- Or<(
- (With, Changed>),
- (Changed>, With),
- )>,
+ (With, Changed>),
),
>,
- drawings: Query<(Entity, &Visibility, &PixelsPerMeter), With>,
+ parents: Query<&Parent>,
) {
- if new_elements.is_empty() {
- return;
- }
- let (parent, ppm) = match drawings.iter().find(|(_, vis, _)| vis.is_visible == true) {
- Some(parent) => (parent.0, parent.2),
- None => return,
- };
- for (e, old_parent, mut tf) in &mut new_elements {
- if old_parent.map(|p| drawings.get(**p).ok()).is_none() {
- commands.entity(parent).add_child(e);
- // Set its scale to the parent's pixels per meter to make it in pixel coordinates
- tf.scale = Vec3::new(ppm.0, ppm.0, 1.0);
+ for (e, edge, mut tf) in &mut changed_measurement {
+ if let (Ok(p0), Ok(p1)) = (parents.get(edge.left()), parents.get(edge.right())) {
+ if p0.get() != p1.get() {
+ commands.entity(e).set_parent(p0.get());
+ } else {
+ warn!(
+ "Mismatch in parents of anchors for measurement {e:?}: {:?}, {:?}",
+ p0, p1
+ );
+ }
+ } else {
+ warn!(
+ "Missing parents of anchors for measurement {e:?}: {:?}, {:?}",
+ parents.get(edge.left()),
+ parents.get(edge.right()),
+ );
}
}
}
fn make_drawing_default_selected(
- drawings: Query<(Entity, &Visibility), With>,
mut selection: ResMut,
+ current: Res,
) {
if selection.is_changed() {
if selection.0.is_none() {
- if let Some(drawing) = drawings.iter().find(|(_, vis)| vis.is_visible == true) {
- selection.0 = Some(drawing.0);
- }
+ let drawing_entity = current
+ .target()
+ .expect("No drawing while spawning drawing anchor")
+ .drawing;
+ selection.0 = Some(drawing_entity);
}
}
}
impl Plugin for DrawingEditorPlugin {
fn build(&self, app: &mut App) {
- app.add_event::()
- .add_system_set(
- SystemSet::on_enter(AppState::SiteDrawingEditor)
- .with_system(hide_level_entities)
- .with_system(hide_non_drawing_entities),
- )
- .add_system_set(
- SystemSet::on_exit(AppState::SiteDrawingEditor)
- .with_system(restore_level_entities)
- .with_system(restore_non_drawing_entities),
- )
+ app.add_event::()
+ .add_event::()
+ .add_event::()
+ .init_resource::()
+ .add_system(switch_edit_drawing_mode)
.add_system_set(
SystemSet::on_update(AppState::SiteDrawingEditor)
- .with_system(assign_drawing_parent_to_new_measurements_and_fiducials)
- .with_system(scale_drawings)
+ .with_system(assign_drawing_parent_to_new_measurements)
.with_system(make_drawing_default_selected),
- )
- .init_resource::();
+ );
}
}
diff --git a/rmf_site_editor/src/site/drawing_editor/optimizer.rs b/rmf_site_editor/src/site/drawing_editor/optimizer.rs
deleted file mode 100644
index 362ab791..00000000
--- a/rmf_site_editor/src/site/drawing_editor/optimizer.rs
+++ /dev/null
@@ -1,375 +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::SystemParam;
-use bevy::prelude::*;
-
-use crate::site::{
- AlignLevelDrawings, AlignSiteDrawings, Anchor, Angle, Category, Change, ConstraintMarker,
- Distance, DrawingMarker, Edge, IsPrimary, LevelProperties, MeasurementMarker, PixelsPerMeter,
- Pose, Rotation, ScaleDrawing, SiteProperties,
-};
-use itertools::{Either, Itertools};
-use optimization_engine::{panoc::*, *};
-use std::collections::HashSet;
-
-// Simple optimization purely based on measurement scale, used to transform between pixel and
-// cartesian coordinates
-pub fn scale_drawings(
- mut drawings: Query<(&Children, &mut PixelsPerMeter), With>,
- measurements: Query<(&Edge, &Distance), With>,
- anchors: Query<&Anchor>,
- mut events: EventReader,
-) {
- for e in events.iter() {
- if let Ok((children, mut ppm)) = drawings.get_mut(**e) {
- let mut scale_numerator = 0.0;
- let mut scale_denominator = 0;
- for child in children {
- if let Ok((edge, distance)) = measurements.get(*child) {
- if let Some(in_meters) = distance.0 {
- let a0 = anchors
- .get(edge.start())
- .expect("Broken measurement anchor reference");
- let d0 = a0.translation_for_category(Category::Drawing);
- let a1 = anchors
- .get(edge.end())
- .expect("Broken measurement anchor reference");
- let d1 = a1.translation_for_category(Category::Drawing);
- let in_pixels = ((d0[0] - d1[0]) * (d0[0] - d1[0])
- + (d0[1] - d1[1]) * (d0[1] - d1[1]))
- .sqrt();
- scale_numerator += in_pixels / in_meters;
- scale_denominator += 1;
- }
- }
- }
- if scale_denominator > 0 {
- ppm.0 = scale_numerator / (scale_denominator as f32);
- } else {
- warn!("No measurements found on current drawing, skipping scaling");
- }
- }
- }
-}
-
-// The cost will be the sum of the square distances between pairs of points in constraints.
-// Reference point pose is just world pose in meters, while the pose of the point to be optimized
-// is expressed as a function of drawing translation, rotation and scale
-// In matching points, first is reference second is to be optimized
-// Order in u is x, y, theta, scale
-fn align_level_cost(
- matching_points: &Vec<([f64; 2], [f64; 2])>,
- u: &[f64],
- cost: &mut f64,
-) -> Result<(), SolverError> {
- *cost = 0.0;
- let (x, y, theta, s) = (u[0], u[1], u[2], u[3]);
- for (p0, p1) in matching_points {
- *cost += (x + (theta.cos() * p1[0] - theta.sin() * p1[1]) / s - p0[0]).powi(2)
- + (y + (theta.sin() * p1[0] + theta.cos() * p1[1]) / s - p0[1]).powi(2);
- }
- Ok(())
-}
-
-// Calculates the partial derivatives for the cost function for each variable
-fn align_level_gradient(
- matching_points: &Vec<([f64; 2], [f64; 2])>,
- u: &[f64],
- grad: &mut [f64],
-) -> Result<(), SolverError> {
- let (x, y, theta, s) = (u[0], u[1], u[2], u[3]);
- grad[0] = 0.0;
- grad[1] = 0.0;
- grad[2] = 0.0;
- grad[3] = 0.0;
- for (p0, p1) in matching_points {
- grad[0] += 2.0 * (x + (theta.cos() * p1[0] - theta.sin() * p1[1]) / s - p0[0]);
- grad[1] += 2.0 * (y + (theta.sin() * p1[0] + theta.cos() * p1[1]) / s - p0[1]);
- // ref https://www.wolframalpha.com/input?i=d%2Fdtheta+%28x+%2B+%28cos%28theta%29+*+p1%5B0%5D+-+sin%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B0%5D%29%5E2+%2B+%28y+%2B+%28sin%28theta%29+*+p1%5B0%5D+%2B+cos%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B1%5D%29%5E2
- grad[2] += 2.0 / s
- * (theta.sin() * (p0[1] * p1[1] + p0[0] * p1[0] - p1[0] * x - p1[1] * y)
- + theta.cos() * (p0[0] * p1[1] - p0[1] * p1[0] - p1[1] * x + p1[0] * y));
- // ref https://www.wolframalpha.com/input?i=d%2Fds+%28x+%2B+%28cos%28theta%29+*+p1%5B0%5D+-+sin%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B0%5D%29%5E2+%2B+%28y+%2B+%28sin%28theta%29+*+p1%5B0%5D+%2B+cos%28theta%29+*+p1%5B1%5D%29+%2F+s+-+p0%5B1%5D%29%5E2
- grad[3] += -2.0
- * (p1[0] * theta.cos() - p1[1] * theta.sin())
- * (-p0[0] + (p1[0] * theta.cos() - p1[1] * theta.sin()) / s + x)
- / (s * s)
- - 2.0
- * (p1[0] * theta.sin() + p1[1] * theta.cos())
- * (-p0[1] + (p1[0] * theta.sin() + p1[1] * theta.cos()) / s + y)
- / (s * s);
- }
- Ok(())
-}
-
-fn align_drawing_pair(
- references: &HashSet,
- target_drawing: Entity,
- constraints: &Vec<&Edge>,
- params: &OptimizationParams,
- change: &mut OptimizationChangeParams,
-) -> Option<(f64, f64, f64, f64)> {
- // Function that creates a pair of reference point and target point poses, their distance to be
- // minimized as part of the optimization
- let make_point_pair = |reference: Entity, target: Entity| {
- let reference_point = params
- .global_tfs
- .get(reference)
- .expect("Transform for anchor not found")
- .translation()
- .truncate()
- .to_array()
- .map(|t| t as f64);
- let target_point = params
- .anchors
- .get(target)
- .expect("Broken constraint anchor reference")
- .translation_for_category(Category::Drawing)
- .map(|t| t as f64);
- (reference_point, target_point)
- };
- // Guaranteed safe since caller passes a drawing entity
- let (_, _, target_pose, target_ppm, _) = params.drawings.get(target_drawing).unwrap();
- let mut matching_points = Vec::new();
- for edge in constraints.iter() {
- let start_parent = params
- .parents
- .get(edge.start())
- .expect("Anchor in constraint without drawing parent");
- let end_parent = params
- .parents
- .get(edge.end())
- .expect("Anchor in constraint without drawing parent");
- if (references.contains(&*start_parent)) & (target_drawing == **end_parent) {
- matching_points.push(make_point_pair(edge.start(), edge.end()));
- } else if (references.contains(&*end_parent)) & (target_drawing == **start_parent) {
- matching_points.push(make_point_pair(edge.end(), edge.start()));
- } else {
- continue;
- }
- }
- if matching_points.is_empty() {
- warn!(
- "No constraints found for drawing {:?}, skipping optimization",
- target_drawing
- );
- return None;
- }
- // Optimize the transform
- let min_vals = vec![
- -std::f64::INFINITY,
- -std::f64::INFINITY,
- -180_f64.to_radians(),
- 1e-3,
- ];
- let max_vals = vec![
- std::f64::INFINITY,
- std::f64::INFINITY,
- 180_f64.to_radians(),
- 1e6,
- ];
- let x = target_pose.trans[0];
- let y = target_pose.trans[1];
- let theta = match target_pose.rot.as_yaw() {
- Rotation::Yaw(yaw) => yaw.radians(),
- _ => unreachable!(),
- };
- let s = target_ppm.0;
- let mut u = vec![x as f64, y as f64, theta as f64, s as f64];
- // Now optimize it
- let opt_constraints = constraints::Rectangle::new(Some(&min_vals), Some(&max_vals));
- let mut panoc_cache = PANOCCache::new(u.len(), 1e-6, 10);
- let f = |u: &[f64], c: &mut f64| -> Result<(), SolverError> {
- align_level_cost(&matching_points, u, c)
- };
-
- let df = |u: &[f64], gradient: &mut [f64]| -> Result<(), SolverError> {
- align_level_gradient(&matching_points, u, gradient)
- };
- let problem = Problem::new(&opt_constraints, df, f);
- let mut panoc = PANOCOptimizer::new(problem, &mut panoc_cache).with_max_iter(1000);
- panoc.solve(&mut u).ok();
-
- // Update transform parameters with results of the optimization
- let mut new_pose = target_pose.clone();
- new_pose.trans[0] = u[0] as f32;
- new_pose.trans[1] = u[1] as f32;
- new_pose.rot = Rotation::Yaw(Angle::Rad(u[2] as f32));
- change.pose.send(Change::new(new_pose, target_drawing));
- change
- .ppm
- .send(Change::new(PixelsPerMeter(u[3] as f32), target_drawing));
- Some((u[0], u[1], u[2], u[3]))
-}
-
-#[derive(SystemParam)]
-pub struct OptimizationChangeParams<'w, 's> {
- pose: EventWriter<'w, 's, Change>,
- ppm: EventWriter<'w, 's, Change>,
-}
-
-#[derive(SystemParam)]
-pub struct OptimizationParams<'w, 's> {
- drawings: Query<
- 'w,
- 's,
- (
- Entity,
- &'static Children,
- &'static Pose,
- &'static PixelsPerMeter,
- &'static IsPrimary,
- ),
- With,
- >,
- global_tfs: Query<'w, 's, &'static GlobalTransform>,
- parents: Query<'w, 's, &'static Parent>,
- anchors: Query<'w, 's, &'static Anchor>,
- constraints: Query<'w, 's, &'static Edge, With>,
-}
-
-pub fn align_level_drawings(
- levels: Query<&Children, With>,
- mut events: EventReader,
- params: OptimizationParams,
- mut change: OptimizationChangeParams,
-) {
- for e in events.iter() {
- // Get the matching points for this entity
- let level_children = levels
- .get(**e)
- .expect("Align level event sent to non level entity");
- let constraints = level_children
- .iter()
- .filter_map(|child| params.constraints.get(*child).ok())
- .collect::>();
- if constraints.is_empty() {
- warn!("No constraints found for level, skipping optimization");
- continue;
- }
- let (references, layers): (HashSet<_>, Vec<_>) = level_children
- .iter()
- .filter_map(|child| params.drawings.get(*child).ok())
- .partition_map(|(e, _, _, _, primary)| {
- if primary.0 == true {
- Either::Left(e)
- } else {
- Either::Right(e)
- }
- });
- if layers.is_empty() {
- warn!(
- "No non-primary drawings found for level, at least one drawing must be set to \
- non-primary to be optimized against primary drawings.Skipping optimization"
- );
- continue;
- }
- if references.is_empty() {
- warn!(
- "No primary drawings found for level. At least one drawing must be set to \
- primary to use as a reference for other drawings. Skipping optimization"
- );
- continue;
- }
- for layer_entity in layers {
- align_drawing_pair(
- &references,
- layer_entity,
- &constraints,
- ¶ms,
- &mut change,
- );
- }
- }
-}
-
-pub fn align_site_drawings(
- levels: Query<(Entity, &Children, &Parent, &LevelProperties)>,
- sites: Query<&Children, With>,
- mut events: EventReader,
- params: OptimizationParams,
- mut change: OptimizationChangeParams,
-) {
- for e in events.iter() {
- // Get the levels that are children of the requested site
- let levels = levels
- .iter()
- .filter(|(_, _, p, _)| ***p == **e)
- .collect::>();
- let reference_level = levels
- .iter()
- .min_by(|l_a, l_b| l_a.3.elevation.partial_cmp(&l_b.3.elevation).unwrap())
- .expect("Site has no levels");
- // Reference level will be the one with minimum elevation
- let references = reference_level
- .1
- .iter()
- .filter_map(|c| {
- params
- .drawings
- .get(*c)
- .ok()
- .filter(|(_, _, _, _, primary)| primary.0 == true)
- })
- .map(|(e, _, _, _, _)| e)
- .collect::>();
- // Layers to be optimized are primary drawings in the non reference level
- let layers = levels
- .iter()
- .filter_map(|(e, c, _, _)| (*e != reference_level.0).then(|| c.iter()))
- .flatten()
- .filter_map(|child| params.drawings.get(*child).ok())
- .filter_map(|(e, _, _, _, primary)| (primary.0 == true).then(|| e))
- .collect::>();
- // Inter level constraints are children of the site
- let constraints = sites
- .get(**e)
- .expect("Align site sent to non site entity")
- .iter()
- .filter_map(|child| params.constraints.get(*child).ok())
- .collect::>();
- if constraints.is_empty() {
- warn!("No constraints found for site, skipping optimization");
- continue;
- }
- if layers.is_empty() {
- warn!(
- "No other levels drawings found for site, at least one other level must have a \
- primary drawing to be optimized against reference level. Skipping optimization"
- );
- continue;
- }
- if references.is_empty() {
- warn!(
- "No reference level drawing found for site. At least one primary drawing must be \
- present in the lowest level to use as a reference for other levels. \
- Skipping optimization"
- );
- continue;
- }
- for layer_entity in layers {
- align_drawing_pair(
- &references,
- layer_entity,
- &constraints,
- ¶ms,
- &mut change,
- );
- }
- }
-}
diff --git a/rmf_site_editor/src/site/fiducial.rs b/rmf_site_editor/src/site/fiducial.rs
index 414fd026..8e143b10 100644
--- a/rmf_site_editor/src/site/fiducial.rs
+++ b/rmf_site_editor/src/site/fiducial.rs
@@ -18,6 +18,176 @@
use crate::interaction::VisualCue;
use crate::site::*;
use bevy::prelude::*;
+use std::collections::HashMap;
+
+#[derive(Component)]
+pub struct FiducialUsage {
+ site: Entity,
+ used: HashMap,
+ unused: HashMap,
+}
+
+impl FiducialUsage {
+ pub fn used(&self) -> &HashMap {
+ &self.used
+ }
+
+ pub fn unused(&self) -> &HashMap {
+ &self.unused
+ }
+
+ pub fn site(&self) -> Entity {
+ self.site
+ }
+}
+
+pub fn add_unused_fiducial_tracker(
+ mut commands: Commands,
+ new_fiducial_scope: Query, Added)>>,
+ sites: Query<(), With>,
+ parent: Query<&Parent>,
+ fiducials: Query<&Affiliation, With>,
+ fiducial_groups: Query<(Entity, &NameInSite, &Parent), (With, With)>,
+ children: Query<&Children>,
+) {
+ for e in &new_fiducial_scope {
+ if let Some(site) = find_parent_site(e, &sites, &parent) {
+ let mut tracker = FiducialUsage {
+ site,
+ used: Default::default(),
+ unused: Default::default(),
+ };
+ reset_fiducial_usage(e, &mut tracker, &fiducials, &fiducial_groups, &children);
+ commands.entity(e).insert(tracker);
+ }
+ }
+}
+
+pub fn update_fiducial_usage_tracker(
+ mut unused_fiducial_trackers: Query<(Entity, &mut FiducialUsage)>,
+ changed_parent: Query, Changed)>,
+ changed_fiducial: Query<&Parent, (Changed>, With)>,
+ sites: Query<(), With>,
+ parent: Query<&Parent>,
+ children: Query<&Children>,
+ fiducials: Query<&Affiliation, With>,
+ changed_fiducials: Query<
+ (Entity, &Parent),
+ (Changed>, With),
+ >,
+ fiducial_groups: Query<(Entity, &NameInSite, &Parent), (With, With)>,
+ changed_fiducial_groups: Query<
+ Entity,
+ Or<(
+ (Added, With),
+ (With, Added),
+ (
+ Or<(Changed, Changed)>,
+ With,
+ With,
+ ),
+ )>,
+ >,
+ removed_fiducial_groups: RemovedComponents,
+) {
+ for e in &changed_parent {
+ if let Some(site) = find_parent_site(e, &sites, &parent) {
+ if let Ok((_, mut unused)) = unused_fiducial_trackers.get_mut(e) {
+ unused.site = site;
+ }
+ }
+ }
+
+ for e in changed_parent
+ .iter()
+ .chain(changed_fiducial.iter().map(|p| p.get()))
+ {
+ let Ok((_, mut tracker)) = unused_fiducial_trackers.get_mut(e) else { continue };
+ reset_fiducial_usage(e, &mut tracker, &fiducials, &fiducial_groups, &children);
+ }
+
+ for changed_group in &changed_fiducial_groups {
+ let Ok((_, name, site)) = fiducial_groups.get(changed_group) else { continue };
+ for (e, mut tracker) in &mut unused_fiducial_trackers {
+ if tracker.site == site.get() {
+ tracker.unused.insert(changed_group, name.0.clone());
+ let Ok(scope_children) = children.get(e) else { continue };
+ for child in scope_children {
+ let Ok(affiliation) = fiducials.get(*child) else { continue };
+ if let Some(group) = affiliation.0 {
+ if changed_group == group {
+ tracker.unused.remove(&changed_group);
+ tracker.used.insert(changed_group, name.0.clone());
+ }
+ }
+ }
+ } else {
+ // If we ever want to support moving a fiducial group between
+ // sites, this will take care of that. Otherwise this line will
+ // never be executed.
+ tracker.used.remove(&changed_group);
+ tracker.unused.remove(&changed_group);
+ }
+ }
+ }
+
+ for (changed_fiducial, parent) in &changed_fiducials {
+ let Ok((e, mut tracker)) = unused_fiducial_trackers.get_mut(parent.get()) else { continue };
+ reset_fiducial_usage(e, &mut tracker, &fiducials, &fiducial_groups, &children);
+ }
+
+ for removed_group in &removed_fiducial_groups {
+ for (_, mut tracker) in &mut unused_fiducial_trackers {
+ tracker.used.remove(&removed_group);
+ tracker.unused.remove(&removed_group);
+ }
+ }
+}
+
+fn find_parent_site(
+ mut entity: Entity,
+ sites: &Query<(), With>,
+ parents: &Query<&Parent>,
+) -> Option {
+ loop {
+ if sites.contains(entity) {
+ return Some(entity);
+ }
+
+ if let Ok(parent) = parents.get(entity) {
+ entity = parent.get();
+ } else {
+ return None;
+ }
+ }
+}
+
+fn reset_fiducial_usage(
+ entity: Entity,
+ tracker: &mut FiducialUsage,
+ fiducials: &Query<&Affiliation, With>,
+ fiducial_groups: &Query<(Entity, &NameInSite, &Parent), (With, With)>,
+ children: &Query<&Children>,
+) {
+ tracker.unused.clear();
+ for (group, name, site) in fiducial_groups {
+ if site.get() == tracker.site {
+ tracker.unused.insert(group, name.0.clone());
+ tracker.used.remove(&group);
+ }
+ }
+
+ let Ok(scope_children) = children.get(entity) else { return };
+ for child in scope_children {
+ let Ok(affiliation) = fiducials.get(*child) else { continue };
+ if let Some(group) = affiliation.0 {
+ tracker.unused.remove(&group);
+ if let Ok((_, name, _)) = fiducial_groups.get(group) {
+ tracker.used.insert(group, name.0.clone());
+ }
+ }
+ }
+}
pub fn add_fiducial_visuals(
mut commands: Commands,
@@ -38,15 +208,36 @@ pub fn add_fiducial_visuals(
.entity(e)
.insert(assets.fiducial_mesh.clone())
.insert(assets.fiducial_material.clone())
+ .insert(Visibility::default())
+ .insert(ComputedVisibility::default())
.insert(Category::Fiducial)
.insert(VisualCue::outline());
}
}
+pub fn assign_orphan_fiducials_to_parent(
+ mut commands: Commands,
+ orphans: Query<
+ (Entity, &Point),
+ (With, Without, Without),
+ >,
+ anchors: Query<&Parent, With>,
+ site_id: Query<&SiteID>,
+) {
+ for (e, point) in &orphans {
+ if let Ok(parent) = anchors.get(point.0) {
+ commands.entity(e).set_parent(parent.get());
+ }
+ }
+}
+
pub fn update_changed_fiducial(
mut fiducials: Query<
(Entity, &Point, &mut Transform),
- (Changed