From fe3f6bf511ec4c1d10e2c6d91666eda175f5510c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 16:36:33 +0800 Subject: [PATCH] First implementation of Open-RMF workcell editor Signed-off-by: Luca Della Vedova Co-authored-by: Grey --- .github/workflows/ci_linux.yaml | 30 + .github/workflows/ci_web.yaml | 40 ++ .github/workflows/ci_windows.yaml | 42 ++ .github/workflows/style.yaml | 29 + .gitignore | 7 + Cargo.toml | 22 + assets/demo_workcells/demo.workcell.json | 86 +++ rmf_workcell_editor/Cargo.toml | 35 + rmf_workcell_editor/package.xml | 16 + rmf_workcell_editor/src/demo_world.rs | 5 + rmf_workcell_editor/src/interaction/mod.rs | 48 ++ rmf_workcell_editor/src/interaction/select.rs | 24 + .../src/interaction/select/place_object.rs | 126 ++++ .../src/interaction/select/place_object_3d.rs | 532 +++++++++++++++ .../interaction/select/replace_parent_3d.rs | 305 +++++++++ rmf_workcell_editor/src/keyboard.rs | 102 +++ rmf_workcell_editor/src/lib.rs | 197 ++++++ rmf_workcell_editor/src/main.rs | 3 + rmf_workcell_editor/src/main_menu.rs | 96 +++ rmf_workcell_editor/src/shapes.rs | 46 ++ rmf_workcell_editor/src/view_menu.rs | 109 +++ rmf_workcell_editor/src/widgets/creation.rs | 126 ++++ .../src/widgets/inspector/inspect_joint.rs | 114 ++++ .../src/widgets/inspector/inspect_name.rs | 70 ++ .../inspector/inspect_workcell_parent.rs | 88 +++ .../src/widgets/inspector/mod.rs | 56 ++ rmf_workcell_editor/src/widgets/mod.rs | 147 +++++ rmf_workcell_editor/src/workcell/frame.rs | 33 + rmf_workcell_editor/src/workcell/joint.rs | 83 +++ rmf_workcell_editor/src/workcell/load.rs | 175 +++++ rmf_workcell_editor/src/workcell/menu.rs | 91 +++ rmf_workcell_editor/src/workcell/mod.rs | 102 +++ rmf_workcell_editor/src/workcell/model.rs | 109 +++ rmf_workcell_editor/src/workcell/save.rs | 330 ++++++++++ .../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_workcell_editor/src/workcell/workcell.rs | 75 +++ rmf_workcell_editor/src/workspace.rs | 442 +++++++++++++ rmf_workcell_format/.gitignore | 1 + rmf_workcell_format/Cargo.toml | 25 + rmf_workcell_format/package.xml | 11 + rmf_workcell_format/src/geometry.rs | 143 ++++ rmf_workcell_format/src/inertial.rs | 87 +++ rmf_workcell_format/src/is_default.rs | 20 + rmf_workcell_format/src/joint.rs | 186 ++++++ rmf_workcell_format/src/lib.rs | 40 ++ rmf_workcell_format/src/workcell.rs | 620 ++++++++++++++++++ rmf_workcell_format/test/07-physics.urdf | 418 ++++++++++++ scripts/build-web.sh | 9 + scripts/deploy-web.sh | 35 + scripts/serve-web.sh | 5 + web/index.html | 29 + web/root_index.html | 8 + 58 files changed, 5857 insertions(+) create mode 100644 .github/workflows/ci_linux.yaml create mode 100644 .github/workflows/ci_web.yaml create mode 100644 .github/workflows/ci_windows.yaml create mode 100644 .github/workflows/style.yaml create mode 100644 .gitignore create mode 100644 Cargo.toml create mode 100644 assets/demo_workcells/demo.workcell.json create mode 100644 rmf_workcell_editor/Cargo.toml create mode 100644 rmf_workcell_editor/package.xml create mode 100644 rmf_workcell_editor/src/demo_world.rs create mode 100644 rmf_workcell_editor/src/interaction/mod.rs create mode 100644 rmf_workcell_editor/src/interaction/select.rs create mode 100644 rmf_workcell_editor/src/interaction/select/place_object.rs create mode 100644 rmf_workcell_editor/src/interaction/select/place_object_3d.rs create mode 100644 rmf_workcell_editor/src/interaction/select/replace_parent_3d.rs create mode 100644 rmf_workcell_editor/src/keyboard.rs create mode 100644 rmf_workcell_editor/src/lib.rs create mode 100644 rmf_workcell_editor/src/main.rs create mode 100644 rmf_workcell_editor/src/main_menu.rs create mode 100644 rmf_workcell_editor/src/shapes.rs create mode 100644 rmf_workcell_editor/src/view_menu.rs create mode 100644 rmf_workcell_editor/src/widgets/creation.rs create mode 100644 rmf_workcell_editor/src/widgets/inspector/inspect_joint.rs create mode 100644 rmf_workcell_editor/src/widgets/inspector/inspect_name.rs create mode 100644 rmf_workcell_editor/src/widgets/inspector/inspect_workcell_parent.rs create mode 100644 rmf_workcell_editor/src/widgets/inspector/mod.rs create mode 100644 rmf_workcell_editor/src/widgets/mod.rs create mode 100644 rmf_workcell_editor/src/workcell/frame.rs create mode 100644 rmf_workcell_editor/src/workcell/joint.rs create mode 100644 rmf_workcell_editor/src/workcell/load.rs create mode 100644 rmf_workcell_editor/src/workcell/menu.rs create mode 100644 rmf_workcell_editor/src/workcell/mod.rs create mode 100644 rmf_workcell_editor/src/workcell/model.rs create mode 100644 rmf_workcell_editor/src/workcell/save.rs create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/generate_package.rs create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/mod.rs create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/template.rs create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 create mode 100644 rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 create mode 100644 rmf_workcell_editor/src/workcell/workcell.rs create mode 100644 rmf_workcell_editor/src/workspace.rs create mode 100644 rmf_workcell_format/.gitignore create mode 100644 rmf_workcell_format/Cargo.toml create mode 100644 rmf_workcell_format/package.xml create mode 100644 rmf_workcell_format/src/geometry.rs create mode 100644 rmf_workcell_format/src/inertial.rs create mode 100644 rmf_workcell_format/src/is_default.rs create mode 100644 rmf_workcell_format/src/joint.rs create mode 100644 rmf_workcell_format/src/lib.rs create mode 100644 rmf_workcell_format/src/workcell.rs create mode 100644 rmf_workcell_format/test/07-physics.urdf create mode 100755 scripts/build-web.sh create mode 100755 scripts/deploy-web.sh create mode 100755 scripts/serve-web.sh create mode 100644 web/index.html create mode 100644 web/root_index.html diff --git a/.github/workflows/ci_linux.yaml b/.github/workflows/ci_linux.yaml new file mode 100644 index 0000000..b1af6c0 --- /dev/null +++ b/.github/workflows/ci_linux.yaml @@ -0,0 +1,30 @@ +name: ci_linux + +on: + pull_request: + workflow_dispatch: + push: + branches: [main] + +env: + CARGO_TERM_COLOR: always + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - name: deps + run: | + sudo apt-get update + sudo apt-get install libasound2-dev libudev-dev libgtk-3-dev + - uses: actions/checkout@v3 + - name: Build rmf_workcell_format + run: | + cd rmf_workcell_format + cargo build + - name: Build rmf_workcell_editor + run: cargo build + - name: Run tests + run: cargo test diff --git a/.github/workflows/ci_web.yaml b/.github/workflows/ci_web.yaml new file mode 100644 index 0000000..754b1fc --- /dev/null +++ b/.github/workflows/ci_web.yaml @@ -0,0 +1,40 @@ +name: ci_web + +on: + pull_request: + workflow_dispatch: + push: + branches: [main] + +env: + CARGO_TERM_COLOR: always + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + + - name: rust-wasm-target + run: | + rustup target add wasm32-unknown-unknown + + - name: apt-deps + run: | + sudo apt-get update + sudo apt-get install libasound2-dev libudev-dev binaryen + + - name: cargo-deps + run: | + cargo install -f wasm-bindgen-cli --version 0.2.93 + + - uses: actions/checkout@v3 + + - name: build rmf_workcell_format + run: | + cd rmf_workcell_format + cargo build + + - name: build rmf_workcell_editor + run: scripts/build-web.sh diff --git a/.github/workflows/ci_windows.yaml b/.github/workflows/ci_windows.yaml new file mode 100644 index 0000000..f85cefe --- /dev/null +++ b/.github/workflows/ci_windows.yaml @@ -0,0 +1,42 @@ +name: ci_windows +on: + pull_request: + workflow_dispatch: + push: + branches: [main] + +jobs: + build: + runs-on: windows-latest + + steps: + - name: checkout + uses: actions/checkout@v3 + + # Run build + - name: Install Rustup using win.rustup.rs + run: | + # Disable the download progress bar which can cause perf issues + $ProgressPreference = "SilentlyContinue" + Invoke-WebRequest https://win.rustup.rs/ -OutFile rustup-init.exe + .\rustup-init.exe -y --default-host=x86_64-pc-windows-msvc --default-toolchain=none + del rustup-init.exe + rustup target add x86_64-pc-windows-msvc + shell: powershell + + - name: build rmf_workcell_format + run: | + cd rmf_workcell_format + cargo build + + - name: build rmf_workcell_editor + run: | + rustc -Vv + cargo -V + cargo build + shell: cmd + + - name: test + run: | + cargo test + shell: cmd diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml new file mode 100644 index 0000000..48a2b6e --- /dev/null +++ b/.github/workflows/style.yaml @@ -0,0 +1,29 @@ +name: style + +on: + pull_request: + workflow_dispatch: + push: + branches: [main] + +env: + CARGO_TERM_COLOR: always + +jobs: + style: + + runs-on: ubuntu-latest + + steps: + + - uses: actions/checkout@v3 + + - name: rustfmt + run: | + rustup component add rustfmt + + - name: style + run: rustfmt --check --edition 2021 rmf_workcell_format/src/lib.rs rmf_workcell_editor/src/lib.rs rmf_workcell_editor/src/main.rs + + - name: minimal feature build + run: cd rmf_workcell_format && cargo check --no-default-features diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..eec455b --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +target +web/librmf_workcell_editor.d.ts +web/librmf_workcell_editor.js +web/librmf_workcell_editor_bg.wasm +web/librmf_workcell_editor_bg_optimized.wasm +web/librmf_workcell_editor_bg.wasm.d.ts +/test_output/ diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..bac00fc --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,22 @@ +[workspace] +members = [ + "rmf_workcell_editor", + "rmf_workcell_format", +] + +# This is needed for packages that are part of a workspace to use the 2nd version +# of the cargo dependency resolving algorithm. In ordinary crates this would be +# enabled with edition="2021", but it seems that when a crate is in a workspace, +# the workspace needs to explicitly specify this. +# For more see: https://doc.rust-lang.org/edition-guide/rust-2021/default-cargo-resolver.html +resolver = "2" + +# Compile time optimizations as suggested in +# https://bevyengine.org/learn/quick-start/getting-started/setup/#compile-with-performance-optimizations +# Enable a small amount of optimization in debug mode +[profile.dev] +opt-level = 1 + +# Enable high optimizations for dependencies (incl. Bevy), but not for our code: +[profile.dev.package."*"] +opt-level = 3 diff --git a/assets/demo_workcells/demo.workcell.json b/assets/demo_workcells/demo.workcell.json new file mode 100644 index 0000000..afe87d1 --- /dev/null +++ b/assets/demo_workcells/demo.workcell.json @@ -0,0 +1,86 @@ +{ + "name": "test_workcell", + "id": 0, + "frames": { + "1": { + "parent": 0, + "name": "frame_1", + "Pose3D": { + "trans": [ + 5.0, + 0.0, + 0.0 + ], + "rot": { + "euler_xyz": [ + { + "deg": 45.0 + }, + { + "deg": 30.0 + }, + { + "deg": 90.0 + } + ] + } + } + }, + "2": { + "parent": 0, + "name": "frame_2", + "Pose3D": { + "trans": [ + 0.0, + 5.0, + 0.0 + ], + "rot": { + "euler_xyz": [ + { + "deg": 45.0 + }, + { + "deg": 30.0 + }, + { + "deg": 90.0 + } + ] + } + } + }, + "3": { + "parent": 1, + "name": "frame_3", + "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_workcell_editor/Cargo.toml b/rmf_workcell_editor/Cargo.toml new file mode 100644 index 0000000..006e0ef --- /dev/null +++ b/rmf_workcell_editor/Cargo.toml @@ -0,0 +1,35 @@ +[package] +name = "rmf_workcell_editor" +version = "0.0.1" +edition = "2021" + +[lib] +crate-type = ["cdylib", "rlib"] +name = "librmf_workcell_editor" + +[[bin]] +path = "src/main.rs" +name = "rmf_workcell_editor" + +[dependencies] +# 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" } +serde = { version = "1.0", features = ["derive"] } +serde_json = "1.0" +wasm-bindgen = "=0.2.93" +bevy = { version = "0.12", features = ["pnm", "jpeg", "tga"] } +thiserror = "*" +rmf_workcell_format = { path = "../rmf_workcell_format", features = ["bevy_support"] } +rmf_site_editor = { git = "https://github.com/open-rmf/rmf_site", tag = "v0.0.1"} +urdf-rs = "0.7" +yaserde = "0.7" +tera = "1.19.1" +anyhow = "*" + +[target.'cfg(not(target_arch = "wasm32"))'.dependencies] +clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] } +bevy_impulse = { git = "https://github.com/open-rmf/bevy_impulse", branch = "main" } + +[target.'cfg(target_arch = "wasm32")'.dependencies] +console_error_panic_hook = "0.1.7" +bevy_impulse = { git = "https://github.com/open-rmf/bevy_impulse", branch = "main", features = ["single_threaded_async"]} diff --git a/rmf_workcell_editor/package.xml b/rmf_workcell_editor/package.xml new file mode 100644 index 0000000..ac91abf --- /dev/null +++ b/rmf_workcell_editor/package.xml @@ -0,0 +1,16 @@ + + rmf_workcell_editor + 0.0.1 + An editor and visualizer for workcells + Luca Della Vedova + Apache License 2.0 + + rmf_workcell_format + gtk3 + libudev-dev + libasound2-dev + + + ament_cargo + + diff --git a/rmf_workcell_editor/src/demo_world.rs b/rmf_workcell_editor/src/demo_world.rs new file mode 100644 index 0000000..eeb11ba --- /dev/null +++ b/rmf_workcell_editor/src/demo_world.rs @@ -0,0 +1,5 @@ +pub fn demo_workcell() -> Vec { + return include_str!("../../assets/demo_workcells/demo.workcell.json") + .as_bytes() + .to_vec(); +} diff --git a/rmf_workcell_editor/src/interaction/mod.rs b/rmf_workcell_editor/src/interaction/mod.rs new file mode 100644 index 0000000..ea8c9bd --- /dev/null +++ b/rmf_workcell_editor/src/interaction/mod.rs @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2022 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 use librmf_site_editor::interaction::InteractionPlugin as SiteInteractionPlugin; +// Reexported types used for widgets / plugins +pub use librmf_site_editor::interaction::{ + aligned_z_axis, extract_selector_input, hover_service, print_if_err, set_visibility, + AnchorVisualization, CategoryVisibility, CategoryVisibilityPlugin, CommonNodeErrors, Cursor, + DragPlaneBundle, GizmoBlockers, HighlightAnchors, Hover, Hovering, InspectorFilter, + InspectorService, InteractionAssets, InteractionState, IntersectGroundPlaneParams, + PickingBlockers, Preview, RunSelector, Select, Selectable, Selection, SelectionFilter, + SelectionNodeResult, SelectionServiceStages, SelectorInput, SetCategoryVisibility, + SiteRaycastSet, VisualCue, +}; + +use crate::WorkcellVisualizationMarker; + +pub mod select; +pub use select::*; + +use bevy::prelude::*; + +#[derive(Default)] +pub struct InteractionPlugin; + +impl Plugin for InteractionPlugin { + fn build(&self, app: &mut App) { + app.add_plugins(( + SiteInteractionPlugin::default(), + CategoryVisibilityPlugin::::visible(true), + place_object::ObjectPlacementPlugin::default(), + )); + } +} diff --git a/rmf_workcell_editor/src/interaction/select.rs b/rmf_workcell_editor/src/interaction/select.rs new file mode 100644 index 0000000..e7038a8 --- /dev/null +++ b/rmf_workcell_editor/src/interaction/select.rs @@ -0,0 +1,24 @@ +/* + * Copyright (C) 2022 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 place_object; +pub use place_object::*; + +pub mod place_object_3d; +pub use place_object_3d::*; + +pub mod replace_parent_3d; diff --git a/rmf_workcell_editor/src/interaction/select/place_object.rs b/rmf_workcell_editor/src/interaction/select/place_object.rs new file mode 100644 index 0000000..0281a51 --- /dev/null +++ b/rmf_workcell_editor/src/interaction/select/place_object.rs @@ -0,0 +1,126 @@ +/* + * 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::{place_object_3d::*, replace_parent_3d::*}; +use crate::{interaction::*, CurrentWorkspace}; +use bevy::ecs::system::{Command, SystemParam, SystemState}; +use bevy::prelude::*; +use bevy_impulse::*; +use rmf_workcell_format::Model; + +#[derive(Default)] +pub struct ObjectPlacementPlugin {} + +impl Plugin for ObjectPlacementPlugin { + fn build(&self, app: &mut App) { + let services = ObjectPlacementServices::from_app(app); + app.insert_resource(services); + } +} + +#[derive(Resource, Clone, Copy)] +pub struct ObjectPlacementServices { + 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_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_3d, + replace_parent_3d, + hover_service_object_3d, + } + } +} + +#[derive(SystemParam)] +pub struct ObjectPlacement<'w, 's> { + pub services: Res<'w, ObjectPlacementServices>, + pub commands: Commands<'w, 's>, + current_workspace: Res<'w, CurrentWorkspace>, + current_selection: Res<'w, Selection>, +} + +impl<'w, 's> ObjectPlacement<'w, 's> { + pub fn place_object_3d(&mut self, object: PlaceableObject) { + let Some(workspace) = self.current_workspace.root else { + warn!("Cannot spawn a model outside of a workspace"); + return; + }; + let state = self + .commands + .spawn(SelectorInput(PlaceObject3d { + object, + parent: self.current_selection.0, + 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); + }); + } +} + +/// Trait to be implemented to allow placing models with commands +pub trait ObjectPlacementExt<'w, 's> { + fn place_model_3d(&mut self, object: Model); +} + +impl<'w, 's> ObjectPlacementExt<'w, 's> for Commands<'w, 's> { + fn place_model_3d(&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_3d(PlaceableObject::Model(self.0)); + system_state.apply(world); + } +} diff --git a/rmf_workcell_editor/src/interaction/select/place_object_3d.rs b/rmf_workcell_editor/src/interaction/select/place_object_3d.rs new file mode 100644 index 0000000..4f245e4 --- /dev/null +++ b/rmf_workcell_editor/src/interaction/select/place_object_3d.rs @@ -0,0 +1,532 @@ +/* + * 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::*; +use crate::workcell::flatten_loaded_model_hierarchy; +use crate::{ + bevy_mod_raycast::deferred::RaycastSource, keyboard::KeyboardServices, widgets::CanvasTooltips, + AnchorBundle, CollisionMeshMarker, Dependents, ModelLoader, VisualMeshMarker, +}; +use bevy::{ + ecs::system::{EntityCommands, SystemParam}, + prelude::{Input as UserInput, *}, +}; +use bevy_impulse::*; +use rmf_workcell_format::{ + Anchor, Angle, Category, FrameMarker, Model, NameInWorkcell, Pending, Pose, Rotation, SiteID, +}; +use std::borrow::Cow; + +pub const PLACE_OBJECT_3D_MODE_LABEL: &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, + mut model_loader: ModelLoader, +) -> 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, &mut model_loader, 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_deref().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).any(|e| e == object) { + 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_workcell_editor/src/keyboard.rs b/rmf_workcell_editor/src/keyboard.rs new file mode 100644 index 0000000..6562325 --- /dev/null +++ b/rmf_workcell_editor/src/keyboard.rs @@ -0,0 +1,102 @@ +/* + * Copyright (C) 2022 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::Selection, CreateNewWorkspace, Delete}; + +pub use librmf_site_editor::keyboard::{keyboard_just_pressed_stream, KeyboardServices}; + +use crate::bevy_egui::EguiContexts; +use crate::workspace::{WorkspaceLoader, WorkspaceSaver}; +use bevy::{ + prelude::{Input as UserInput, *}, + window::PrimaryWindow, +}; +use bevy_impulse::*; + +pub struct KeyboardInputPlugin; + +impl Plugin for KeyboardInputPlugin { + fn build(&self, app: &mut App) { + app.add_systems(Last, handle_keyboard_input); + + let keyboard_just_pressed = + app.spawn_continuous_service(Last, keyboard_just_pressed_stream); + + app.insert_resource(KeyboardServices { + keyboard_just_pressed, + }); + } +} + +fn handle_keyboard_input( + keyboard_input: Res>, + selection: Res, + mut egui_context: EguiContexts, + mut delete: EventWriter, + mut new_workspace: EventWriter, + primary_windows: Query>, + mut workspace_loader: WorkspaceLoader, + mut workspace_saver: WorkspaceSaver, +) { + 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.just_pressed(KeyCode::Delete) || keyboard_input.just_pressed(KeyCode::Back) { + if let Some(selection) = selection.0 { + delete.send(Delete::new(selection)); + } else { + warn!("No selected entity to delete"); + } + } + + // Ctrl keybindings + 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]) { + workspace_saver.save_to_dialog(); + } else { + workspace_saver.save_to_default_file(); + } + } + + if keyboard_input.just_pressed(KeyCode::E) { + workspace_saver.export_urdf_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) { + new_workspace.send(CreateNewWorkspace); + } + + if keyboard_input.just_pressed(KeyCode::O) { + workspace_loader.load_from_dialog(); + } + } +} diff --git a/rmf_workcell_editor/src/lib.rs b/rmf_workcell_editor/src/lib.rs new file mode 100644 index 0000000..1892168 --- /dev/null +++ b/rmf_workcell_editor/src/lib.rs @@ -0,0 +1,197 @@ +use bevy::{log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*}; +#[cfg(not(target_arch = "wasm32"))] +use clap::Parser; +use main_menu::MainMenuPlugin; +#[cfg(target_arch = "wasm32")] +use wasm_bindgen::prelude::*; + +pub mod keyboard; +use keyboard::*; + +pub mod widgets; +use widgets::*; + +pub mod demo_world; + +pub mod main_menu; +pub mod workcell; +use workcell::*; +pub mod interaction; + +pub mod workspace; +use workspace::*; + +mod shapes; + +pub mod view_menu; +use view_menu::*; + +// Parts of the site editor that we reexport since they are needed to build workcell editor apps +pub use librmf_site_editor::{ + aabb::AabbUpdatePlugin, + animate::AnimationPlugin, + asset_loaders::AssetLoadersPlugin, + bevy_egui, + bevy_mod_raycast, + log::LogHistoryPlugin, + // Misc components + site::{ + AnchorBundle, CollisionMeshMarker, DefaultFile, Delete, Dependents, ModelLoader, + ModelLoadingResult, PreventDeletion, VisualMeshMarker, + }, + site::{ + Change, ChangePlugin, Recall, RecallAssetSource, RecallPlugin, RecallPrimitiveShape, + SiteAssets, + }, + site::{DeletionPlugin, FuelPlugin, ModelLoadingPlugin}, + site_asset_io, + wireframe::SiteWireframePlugin, + // Workspace related objects that are shared with site editor, the rest are reimplemented since + // the functionality differs significantly + workspace::{ + ChangeCurrentWorkspace, CreateNewWorkspace, CurrentWorkspace, FileDialogFilter, + FileDialogServices, RecallWorkspace, + }, + Autoload, +}; + +pub use bevy_impulse; + +use crate::interaction::InteractionPlugin; + +use bevy::render::{ + render_resource::{AddressMode, SamplerDescriptor}, + settings::{WgpuFeatures, WgpuSettings}, + RenderPlugin, +}; + +use rmf_workcell_format::{ + AssetSource, NameInWorkcell, NameOfWorkcell, Pose, PrimitiveShape, Scale, +}; + +#[cfg_attr(not(target_arch = "wasm32"), derive(Parser))] +pub struct CommandLineArgs { + /// Filename of a Site (.site.ron) or Building (.building.yaml) file to load. + /// Exclude this argument to get the main menu. + pub filename: Option, + /// Name of a Site (.site.ron) file to import on top of the base FILENAME. + #[cfg_attr(not(target_arch = "wasm32"), arg(short, long))] + pub import: Option, +} + +#[derive(Clone, Default, Eq, PartialEq, Debug, Hash, States)] +pub enum AppState { + #[default] + MainMenu, + WorkcellEditor, +} + +#[cfg(target_arch = "wasm32")] +#[wasm_bindgen] +pub fn run_js() { + extern crate console_error_panic_hook; + std::panic::set_hook(Box::new(console_error_panic_hook::hook)); + run(vec!["web".to_owned()]); +} + +pub fn run(command_line_args: Vec) { + let mut app = App::new(); + + #[cfg(not(target_arch = "wasm32"))] + { + let command_line_args = CommandLineArgs::parse_from(command_line_args); + if let Some(path) = command_line_args.filename { + app.insert_resource(Autoload::file( + path.into(), + command_line_args.import.map(Into::into), + )); + } + } + + app.add_plugins(WorkcellEditor::default()); + app.run(); +} + +#[derive(Default)] +pub struct WorkcellEditor {} + +impl Plugin for WorkcellEditor { + fn build(&self, app: &mut App) { + app.add_plugins(( + site_asset_io::SiteAssetIoPlugin, + DefaultPlugins + .build() + .disable::() + .set(WindowPlugin { + primary_window: Some(Window { + title: "RMF Workcell Editor".to_owned(), + #[cfg(not(target_arch = "wasm32"))] + resolution: (1600., 900.).into(), + #[cfg(target_arch = "wasm32")] + canvas: Some(String::from("#rmf_workcell_editor_canvas")), + #[cfg(target_arch = "wasm32")] + fit_canvas_to_parent: true, + ..default() + }), + ..default() + }) + .set(ImagePlugin { + default_sampler: SamplerDescriptor { + address_mode_u: AddressMode::Repeat, + address_mode_v: AddressMode::Repeat, + address_mode_w: AddressMode::Repeat, + ..Default::default() + } + .into(), + }) + .set(RenderPlugin { + render_creation: WgpuSettings { + #[cfg(not(target_arch = "wasm32"))] + features: WgpuFeatures::POLYGON_MODE_LINE, + ..default() + } + .into(), + }), + )); + + app.insert_resource(DirectionalLightShadowMap { size: 2048 }) + .init_resource::() + .add_plugins(( + ChangePlugin::::default(), + ChangePlugin::::default(), + ChangePlugin::::default(), + ChangePlugin::::default(), + ChangePlugin::::default(), + RecallPlugin::::default(), + ChangePlugin::::default(), + RecallPlugin::::default(), + )) + .add_state::() + .add_plugins(( + ModelLoadingPlugin::default(), + FuelPlugin::default(), + DeletionPlugin, + )) + .add_plugins(( + AssetLoadersPlugin, + LogHistoryPlugin, + AabbUpdatePlugin, + bevy_egui::EguiPlugin, + KeyboardInputPlugin, + InteractionPlugin, + AnimationPlugin, + WorkspacePlugin, + bevy_impulse::ImpulsePlugin::default(), + StandardUiPlugin::default(), + MainMenuPlugin, + WorkcellEditorPlugin, + )) + // Note order matters, plugins that edit the menus must be initialized after the UI + .add_plugins((ViewMenuPlugin, SiteWireframePlugin)); + + // Ref https://github.com/bevyengine/bevy/issues/10877. The default behavior causes issues + // with events being accumulated when not read (i.e. scrolling mouse wheel on a UI widget). + app.world + .remove_resource::(); + } +} diff --git a/rmf_workcell_editor/src/main.rs b/rmf_workcell_editor/src/main.rs new file mode 100644 index 0000000..8e4ce36 --- /dev/null +++ b/rmf_workcell_editor/src/main.rs @@ -0,0 +1,3 @@ +fn main() { + librmf_workcell_editor::run(std::env::args().collect()); +} diff --git a/rmf_workcell_editor/src/main_menu.rs b/rmf_workcell_editor/src/main_menu.rs new file mode 100644 index 0000000..4ec20d3 --- /dev/null +++ b/rmf_workcell_editor/src/main_menu.rs @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2022 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 super::demo_world::*; +use crate::bevy_egui::{egui, EguiContexts}; +use crate::{AppState, Autoload, WorkspaceData, WorkspaceLoader}; +use bevy::{app::AppExit, prelude::*, window::PrimaryWindow}; + +fn egui_ui( + mut egui_context: EguiContexts, + mut _exit: EventWriter, + mut workspace_loader: WorkspaceLoader, + autoload: Option>, + primary_windows: Query>, +) { + if let Some(mut autoload) = autoload { + #[cfg(not(target_arch = "wasm32"))] + { + if let Some(filename) = autoload.filename.take() { + workspace_loader.load_from_path(filename); + } + } + return; + } + + let Some(ctx) = primary_windows + .get_single() + .ok() + .and_then(|w| egui_context.try_ctx_for_window_mut(w)) + else { + return; + }; + + egui::Window::new("Welcome!") + .collapsible(false) + .resizable(false) + .title_bar(false) + .anchor(egui::Align2::CENTER_CENTER, egui::vec2(0., 0.)) + .show(ctx, |ui| { + ui.heading("Welcome to The RMF Workcell Editor!"); + ui.add_space(10.); + + ui.horizontal(|ui| { + if ui.button("Load workcell").clicked() { + workspace_loader.load_from_dialog(); + } + + if ui.button("New workcell").clicked() { + workspace_loader.load_from_data(WorkspaceData::Workcell( + rmf_workcell_format::Workcell::default() + .to_string() + .unwrap() + .into(), + )); + } + + if ui.button("Demo workcell").clicked() { + workspace_loader.load_from_data(WorkspaceData::Workcell(demo_workcell())); + } + }); + + #[cfg(not(target_arch = "wasm32"))] + { + ui.add_space(20.); + ui.horizontal(|ui| { + ui.with_layout(egui::Layout::right_to_left(egui::Align::Center), |ui| { + if ui.button("Exit").clicked() { + _exit.send(AppExit); + } + }); + }); + } + }); +} + +pub struct MainMenuPlugin; + +impl Plugin for MainMenuPlugin { + fn build(&self, app: &mut App) { + app.add_systems(Update, egui_ui.run_if(in_state(AppState::MainMenu))); + } +} diff --git a/rmf_workcell_editor/src/shapes.rs b/rmf_workcell_editor/src/shapes.rs new file mode 100644 index 0000000..a3882fa --- /dev/null +++ b/rmf_workcell_editor/src/shapes.rs @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use bevy::prelude::*; +use bevy_infinite_grid::{InfiniteGridBundle, InfiniteGridSettings}; + +const X_AXIS_COLOR: Color = Color::rgb(1.0, 0.2, 0.2); +const Y_AXIS_COLOR: Color = Color::rgb(0.2, 1.0, 0.2); + +pub(crate) fn make_infinite_grid( + scale: f32, + fadeout_distance: f32, + shadow_color: Option, +) -> InfiniteGridBundle { + // 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. + let settings = InfiniteGridSettings { + x_axis_color: X_AXIS_COLOR, + z_axis_color: Y_AXIS_COLOR, + fadeout_distance, + shadow_color, + ..default() + }; + let transform = Transform::from_rotation(Quat::from_rotation_x(90_f32.to_radians())) + .with_scale(Vec3::splat(scale)); + + InfiniteGridBundle { + settings, + transform, + ..default() + } +} diff --git a/rmf_workcell_editor/src/view_menu.rs b/rmf_workcell_editor/src/view_menu.rs new file mode 100644 index 0000000..b5f04a2 --- /dev/null +++ b/rmf_workcell_editor/src/view_menu.rs @@ -0,0 +1,109 @@ +/* + * 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::menu_bar::{MenuEvent, MenuItem, ViewMenu}; +use crate::workcell::WorkcellVisualizationMarker; +use crate::{ + interaction::{CategoryVisibility, SetCategoryVisibility}, + CollisionMeshMarker, VisualMeshMarker, +}; +use bevy::ecs::system::SystemParam; +use bevy::prelude::*; + +#[derive(SystemParam)] +struct VisibilityEvents<'w> { + visuals: EventWriter<'w, SetCategoryVisibility>, + collisions: EventWriter<'w, SetCategoryVisibility>, + origin_axis: EventWriter<'w, SetCategoryVisibility>, +} + +#[derive(Default)] +pub struct ViewMenuPlugin; + +#[derive(Resource)] +pub struct ViewMenuItems { + visuals: Entity, + collisions: Entity, + origin_axis: Entity, +} + +impl FromWorld for ViewMenuItems { + fn from_world(world: &mut World) -> Self { + let view_header = world.resource::().get(); + let default_visibility = world.resource::>(); + let collisions = world + .spawn(MenuItem::CheckBox( + "Collision meshes".to_string(), + default_visibility.0, + )) + .set_parent(view_header) + .id(); + let default_visibility = world.resource::>(); + let visuals = world + .spawn(MenuItem::CheckBox( + "Visual meshes".to_string(), + default_visibility.0, + )) + .set_parent(view_header) + .id(); + let default_visibility = + world.resource::>(); + let origin_axis = world + .spawn(MenuItem::CheckBox( + "Reference axis".to_string(), + default_visibility.0, + )) + .set_parent(view_header) + .id(); + + ViewMenuItems { + collisions, + visuals, + origin_axis, + } + } +} + +fn handle_view_menu_events( + mut menu_events: EventReader, + view_menu: Res, + mut menu_items: Query<&mut MenuItem>, + mut events: VisibilityEvents, +) { + let mut toggle = |entity| { + let mut menu = menu_items.get_mut(entity).unwrap(); + let value = menu.checkbox_value_mut().unwrap(); + *value = !*value; + *value + }; + for event in menu_events.read() { + if event.clicked() && event.source() == view_menu.collisions { + events.collisions.send(toggle(event.source()).into()); + } else if event.clicked() && event.source() == view_menu.visuals { + events.visuals.send(toggle(event.source()).into()); + } else if event.clicked() && event.source() == view_menu.origin_axis { + events.origin_axis.send(toggle(event.source()).into()); + } + } +} + +impl Plugin for ViewMenuPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_systems(Update, handle_view_menu_events); + } +} diff --git a/rmf_workcell_editor/src/widgets/creation.rs b/rmf_workcell_editor/src/widgets/creation.rs new file mode 100644 index 0000000..7149dd8 --- /dev/null +++ b/rmf_workcell_editor/src/widgets/creation.rs @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2022 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::{ObjectPlacement, PlaceableObject}; +use crate::{ + bevy_egui::egui::{CollapsingHeader, Ui}, + widgets::inspector::{InspectAssetSourceComponent, InspectScaleComponent}, + widgets::prelude::*, + widgets::AssetGalleryStatus, + CurrentWorkspace, DefaultFile, Recall, RecallAssetSource, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; + +use rmf_workcell_format::{AssetSource, Model, Scale}; + +/// This widget provides a widget with buttons for creating new site elements. +#[derive(Default)] +pub struct CreationPlugin {} + +impl Plugin for CreationPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_plugins(PropertiesTilePlugin::::new()); + } +} + +#[derive(SystemParam)] +struct Creation<'w, 's> { + default_file: Query<'w, 's, &'static DefaultFile>, + current_workspace: Res<'w, CurrentWorkspace>, + pending_model: ResMut<'w, PendingModel>, + asset_gallery: ResMut<'w, AssetGalleryStatus>, + object_placement: ObjectPlacement<'w, 's>, +} + +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); + CollapsingHeader::new("Create") + .default_open(true) + .show(ui, |ui| { + params.show_widget(ui); + }); + } +} + +impl<'w, 's> Creation<'w, 's> { + pub fn show_widget(&mut self, ui: &mut Ui) { + ui.vertical(|ui| { + if ui.button("Frame").clicked() { + self.place_object(PlaceableObject::Anchor); + } + ui.add_space(10.0); + CollapsingHeader::new("New model") + .default_open(false) + .show(ui, |ui| { + let default_file = self + .current_workspace + .root + .and_then(|e| self.default_file.get(e).ok()); + 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 ui.button("Browse fuel").clicked() { + self.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); + }); + }); + } + + pub fn place_object(&mut self, object: PlaceableObject) { + self.object_placement.place_object_3d(object); + } +} + +#[derive(Resource, Clone, Default)] +struct PendingModel { + pub source: AssetSource, + pub recall_source: RecallAssetSource, + pub scale: Scale, +} diff --git a/rmf_workcell_editor/src/widgets/inspector/inspect_joint.rs b/rmf_workcell_editor/src/widgets/inspector/inspect_joint.rs new file mode 100644 index 0000000..27d0197 --- /dev/null +++ b/rmf_workcell_editor/src/widgets/inspector/inspect_joint.rs @@ -0,0 +1,114 @@ +/* + * 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::{ + bevy_egui::egui::Ui, + widgets::{prelude::*, Inspect, SelectorWidget}, + CreateJoint, Dependents, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +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 + } +} + +#[derive(SystemParam)] +pub struct InspectJointCreator<'w, 's> { + frame_parents: Query<'w, 's, &'static Parent, With>, + create_joint: EventWriter<'w, CreateJoint>, +} + +impl<'w, 's> WidgetSystem for InspectJointCreator<'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> InspectJointCreator<'w, 's> { + pub fn show_widget(&mut self, id: Entity, ui: &mut Ui) { + let Ok(parent) = self.frame_parents.get(id) else { + return; + }; + // Allow creating a joint only if this frame has another frame as a parent + if self.frame_parents.get(parent.get()).is_ok() + && ui + .button("Create joint") + .on_hover_text( + "Create a fixed joint and place it between the parent frame and this frame", + ) + .clicked() + { + self.create_joint.send(CreateJoint { + parent: parent.get(), + child: id, + }); + } + } +} diff --git a/rmf_workcell_editor/src/widgets/inspector/inspect_name.rs b/rmf_workcell_editor/src/widgets/inspector/inspect_name.rs new file mode 100644 index 0000000..3fc3d02 --- /dev/null +++ b/rmf_workcell_editor/src/widgets/inspector/inspect_name.rs @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2022 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::{ + bevy_egui::egui::Ui, + widgets::{prelude::*, Inspect}, + Change, +}; +use bevy::prelude::*; +use rmf_workcell_format::{NameInWorkcell, NameOfWorkcell}; + +#[derive(SystemParam)] +pub struct InspectName<'w, 's> { + 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> {} + +impl<'w, 's> WidgetSystem for InspectName<'w, 's> { + fn show( + Inspect { selection, .. }: Inspect, + ui: &mut Ui, + state: &mut SystemState, + world: &mut World, + ) { + let mut params = state.get_mut(world); + 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_workcell_editor/src/widgets/inspector/inspect_workcell_parent.rs b/rmf_workcell_editor/src/widgets/inspector/inspect_workcell_parent.rs new file mode 100644 index 0000000..dcfca50 --- /dev/null +++ b/rmf_workcell_editor/src/widgets/inspector/inspect_workcell_parent.rs @@ -0,0 +1,88 @@ +/* + * 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::ObjectPlacement; +use crate::{ + bevy_egui::egui::{ImageButton, Ui}, + interaction::Hover, + widgets::{prelude::*, Icons, Inspect, SelectorWidget}, + CurrentWorkspace, +}; +use bevy::{ecs::system::SystemParam, prelude::*}; +use rmf_workcell_format::{FrameMarker, NameInWorkcell, NameOfWorkcell}; + +#[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_workcell_editor/src/widgets/inspector/mod.rs b/rmf_workcell_editor/src/widgets/inspector/mod.rs new file mode 100644 index 0000000..99251aa --- /dev/null +++ b/rmf_workcell_editor/src/widgets/inspector/mod.rs @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2022 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 inspect_joint; +pub use inspect_joint::*; + +pub mod inspect_name; +pub use inspect_name::*; + +pub mod inspect_workcell_parent; +pub use inspect_workcell_parent::*; + +use crate::widgets::prelude::*; +use bevy::prelude::*; +pub use librmf_site_editor::widgets::{ + InspectAnchor, InspectAnchorDependents, InspectAssetSource, InspectAssetSourceComponent, + InspectPose, InspectPrimitiveShape, InspectScale, InspectScaleComponent, + MinimalInspectorPlugin, +}; + +/// Use this to create a standard inspector plugin that covers the common use +/// cases of the site editor. +#[derive(Default)] +pub struct StandardInspectorPlugin {} + +impl Plugin for StandardInspectorPlugin { + fn build(&self, app: &mut App) { + app.add_plugins(MinimalInspectorPlugin::default()) + .add_plugins(( + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + InspectionPlugin::::new(), + )); + } +} diff --git a/rmf_workcell_editor/src/widgets/mod.rs b/rmf_workcell_editor/src/widgets/mod.rs new file mode 100644 index 0000000..04910f0 --- /dev/null +++ b/rmf_workcell_editor/src/widgets/mod.rs @@ -0,0 +1,147 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +//! The site editor allows you to insert your own egui widgets into the UI. +//! Simple examples of custom widgets can be found in the docs for +//! [`PropertiesTilePlugin`] and [`InspectionPlugin`]. +//! +//! There are three categories of widgets that the site editor provides +//! out-of-the-box support for inserting, but the widget system itself is +//! highly extensible, allowing you to define your own categories of widgets. +//! +//! The three categories provided out of the box include: +//! - [Panel widget][1]: Add a new panel to the UI. +//! - Tile widget: Add a tile into a [panel of tiles][2] such as the [`PropertiesPanel`]. Use [`PropertiesTilePlugin`] to make a new tile widget that goes inside of the standard `PropertiesPanel`. +//! - [`InspectionPlugin`]: Add a widget to the [`MainInspector`] to display more information about the currently selected entity. +//! +//! In our terminology, there are two kinds of panels: +//! - Side panels: A vertical column widget on the left or right side of the screen. +//! - [`PropertiesPanel`] is usually a side panel placed on the right side of the screen. +//! - [`FuelAssetBrowser`] is a side panel typically placed on the left side of the screen. +//! - [`Diagnostics`] is a side panel that interactively flags issues that have been found in the site. +//! - Top / Bottom Panels: +//! - The [`MenuBarPlugin`] provides a menu bar at the top of the screen. +//! - Create an entity with a [`Menu`] component to create a new menu inside the menu bar. +//! - Add an entity with a [`MenuItem`] component as a child to a menu entity to add a new item into a menu. +//! - The [`FileMenu`], [`ToolMenu`], and [`ViewMenu`] are resources that provide access to various standard menus. +//! - The [`ConsoleWidgetPlugin`] provides a console at the bottom of the screen to display information, warning, and error messages. +//! +//! [1]: crate::widgets::PanelWidget +//! [2]: crate::widgets::show_panel_of_tiles + +use crate::bevy_egui::{ + egui::{self}, + EguiContexts, +}; +use crate::interaction::{Hover, PickingBlockers}; +use crate::{interaction::ObjectPlacementExt, AppState}; +use bevy::prelude::*; +pub use librmf_site_editor::widgets::{ + canvas_tooltips::CanvasTooltips, console::ConsoleWidgetPlugin, menu_bar, prelude, prelude::*, + render_panels, AssetGalleryStatus, FuelAssetBrowserPlugin, Icons, IconsPlugin, Inspect, + MenuBarPlugin, RenderUiSet, SelectorWidget, +}; + +pub mod creation; +use creation::*; + +pub mod inspector; +pub use inspector::*; + +use rmf_workcell_format::Model; + +/// This plugin provides the standard UI layout that was designed for the common +/// use cases of the site editor. +#[derive(Default)] +pub struct StandardUiPlugin {} + +impl Plugin for StandardUiPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_plugins(( + IconsPlugin::default(), + MenuBarPlugin::default(), + WorkcellPropertiesPanelPlugin::default(), + FuelAssetBrowserPlugin { + model_spawner: |commands: &mut Commands, model: Model| { + commands.place_model_3d(model); + }, + }, + ConsoleWidgetPlugin::default(), + )) + .add_systems(Startup, init_ui_style) + .add_systems( + Update, + workcell_ui_layout + .in_set(RenderUiSet) + .run_if(in_state(AppState::WorkcellEditor)), + ); + } +} + +#[derive(Default)] +pub struct WorkcellPropertiesPanelPlugin {} + +impl Plugin for WorkcellPropertiesPanelPlugin { + fn build(&self, app: &mut App) { + app.add_plugins(( + PropertiesPanelPlugin::new(PanelSide::Right), + StandardInspectorPlugin::default(), + CreationPlugin::default(), + )); + } +} + +/// This system renders all UI panels in the application and makes sure that the +/// UI rendering works correctly with the picking system, and any other systems +/// as needed. +pub fn workcell_ui_layout( + world: &mut World, + panel_widgets: &mut QueryState<(Entity, &mut PanelWidget)>, + egui_context_state: &mut SystemState, +) { + render_panels(world, panel_widgets, egui_context_state); + + let mut egui_context = egui_context_state.get_mut(world); + let mut ctx = egui_context.ctx_mut().clone(); + let ui_has_focus = + ctx.wants_pointer_input() || ctx.wants_keyboard_input() || ctx.is_pointer_over_area(); + + if let Some(mut picking_blocker) = world.get_resource_mut::() { + picking_blocker.ui = ui_has_focus; + } + + if ui_has_focus { + // If the UI has focus and there were no hover events emitted by the UI, + // then we should emit a None hover event + let mut hover = world.resource_mut::>(); + if hover.is_empty() { + hover.send(Hover(None)); + } + } else { + // If the UI does not have focus then render the CanvasTooltips. + world.resource_mut::().render(&mut ctx); + } +} + +fn init_ui_style(mut egui_context: EguiContexts) { + // I think the default egui dark mode text color is too dim, so this changes + // it to a brighter white. + let mut visuals = egui::Visuals::dark(); + visuals.override_text_color = Some(egui::Color32::from_rgb(250, 250, 250)); + egui_context.ctx_mut().set_visuals(visuals); +} diff --git a/rmf_workcell_editor/src/workcell/frame.rs b/rmf_workcell_editor/src/workcell/frame.rs new file mode 100644 index 0000000..bf36221 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/frame.rs @@ -0,0 +1,33 @@ +/* + * 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_workcell_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_workcell_editor/src/workcell/joint.rs b/rmf_workcell_editor/src/workcell/joint.rs new file mode 100644 index 0000000..7053d53 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/joint.rs @@ -0,0 +1,83 @@ +/* + * 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::{Delete, Dependents}; +use bevy::prelude::*; +use rmf_workcell_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_workcell_editor/src/workcell/load.rs b/rmf_workcell_editor/src/workcell/load.rs new file mode 100644 index 0000000..bff0a0b --- /dev/null +++ b/rmf_workcell_editor/src/workcell/load.rs @@ -0,0 +1,175 @@ +/* + * 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::workcell::ChangeCurrentWorkcell; +use crate::{ + AnchorBundle, CollisionMeshMarker, DefaultFile, Dependents, ModelLoader, PreventDeletion, + VisualMeshMarker, +}; +use bevy::prelude::*; +use std::collections::HashSet; + +use rmf_workcell_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, + model_loader: &mut ModelLoader, +) -> 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(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, + NameInWorkcell(parented.bundle.name.clone()), + )); + } + Geometry::Mesh { source, scale } => { + commands.entity(e).insert(( + NameInWorkcell(parented.bundle.name.clone()), + parented.bundle.pose, + Scale(scale.unwrap_or(Vec3::ONE)), + ModelMarker, + )); + model_loader.update_asset_source(e, source.clone()); + } + }; + 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) + .insert(parented_anchor.bundle.name.clone()) + .id(); + 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, + mut model_loader: ModelLoader, +) { + for cmd in load_workcells.read() { + info!("Loading workcell"); + let root = generate_workcell_entities(&mut commands, &cmd.workcell, &mut model_loader); + 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_workcell_editor/src/workcell/menu.rs b/rmf_workcell_editor/src/workcell/menu.rs new file mode 100644 index 0000000..c1b0c38 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/menu.rs @@ -0,0 +1,91 @@ +/* + * 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::menu_bar::{FileMenu, MenuEvent, MenuItem, TextMenuItem}; +use crate::CreateNewWorkspace; +use crate::{WorkspaceLoader, WorkspaceSaver}; +use bevy::prelude::*; + +/// Keeps track of which entity is associated to the export urdf button. +#[derive(Resource)] +pub struct WorkcellFileMenu { + new: Entity, + save: Entity, + save_as: Entity, + load: Entity, + export_urdf: Entity, +} + +impl FromWorld for WorkcellFileMenu { + fn from_world(world: &mut World) -> Self { + let file_header = world.resource::().get(); + let new = world + .spawn(MenuItem::Text(TextMenuItem::new("New").shortcut("Ctrl-N"))) + .set_parent(file_header) + .id(); + let save = world + .spawn(MenuItem::Text(TextMenuItem::new("Save").shortcut("Ctrl-S"))) + .set_parent(file_header) + .id(); + let save_as = world + .spawn(MenuItem::Text( + TextMenuItem::new("Save As").shortcut("Ctrl-Shift-S"), + )) + .set_parent(file_header) + .id(); + let load = world + .spawn(MenuItem::Text(TextMenuItem::new("Open").shortcut("Ctrl-O"))) + .set_parent(file_header) + .id(); + let export_urdf = world + .spawn(MenuItem::Text( + TextMenuItem::new("Export Urdf").shortcut("Ctrl-E"), + )) + .set_parent(file_header) + .id(); + + WorkcellFileMenu { + new, + save, + save_as, + load, + export_urdf, + } + } +} + +pub fn handle_export_urdf_menu_events( + mut menu_events: EventReader, + file_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() == file_menu.new { + new_workspace.send(CreateNewWorkspace); + } else if event.clicked() && event.source() == file_menu.save { + workspace_saver.save_to_default_file(); + } else if event.clicked() && event.source() == file_menu.save_as { + workspace_saver.save_to_dialog(); + } else if event.clicked() && event.source() == file_menu.load { + workspace_loader.load_from_dialog(); + } else if event.clicked() && event.source() == file_menu.export_urdf { + workspace_saver.export_urdf_to_dialog(); + } + } +} diff --git a/rmf_workcell_editor/src/workcell/mod.rs b/rmf_workcell_editor/src/workcell/mod.rs new file mode 100644 index 0000000..91f8fba --- /dev/null +++ b/rmf_workcell_editor/src/workcell/mod.rs @@ -0,0 +1,102 @@ +/* + * 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 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::{shapes::make_infinite_grid, AppState}; +pub use librmf_site_editor::site::{ + handle_new_primitive_shapes, 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, + 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_workcell_editor/src/workcell/model.rs b/rmf_workcell_editor/src/workcell/model.rs new file mode 100644 index 0000000..4b5eb36 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/model.rs @@ -0,0 +1,109 @@ +/* + * 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::{DragPlaneBundle, Preview, VisualCue}, + Dependents, ModelLoadingResult, +}; +use bevy::prelude::*; +use rmf_workcell_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(result): 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(res) = result else { + return; + }; + if res.unchanged { + return; + } + let old_parent = res.request.parent; + 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(DragPlaneBundle::new(p, Vec3::Z)); + parent_found = true; + break; + } + } + if !parent_found { + commands.entity(c).insert(DragPlaneBundle::new(c, Vec3::Z)); + } + } + // 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); + } + if let Ok(preview) = previews.get(old_parent) { + c_mut.insert(*preview); + } + } + 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_workcell_editor/src/workcell/save.rs b/rmf_workcell_editor/src/workcell/save.rs new file mode 100644 index 0000000..2d5c30c --- /dev/null +++ b/rmf_workcell_editor/src/workcell/save.rs @@ -0,0 +1,330 @@ +/* + * 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::collections::BTreeMap; +use std::path::PathBuf; + +use crate::workcell::urdf_package_exporter::{generate_package, PackageContext, Person}; +use crate::ExportFormat; +use crate::{CollisionMeshMarker, VisualMeshMarker}; + +use thiserror::Error as ThisError; + +use rmf_workcell_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).any(|p| p == root) +} + +// 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 q_used_entities.get(e).is_ok() { + 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, &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; + }; + let push_model = |container: &mut BTreeMap>| { + container.insert( + id.0, + Parented { + parent, + bundle: WorkcellModel { + name: name.0.clone(), + geometry: geom, + pose: *pose, + }, + }, + ); + }; + if q_visuals.get(e).is_ok() { + push_model(&mut workcell.visuals); + } else if q_collisions.get(e).is_ok() { + push_model(&mut workcell.collisions); + } + } + + // 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.clone(), + 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, + 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 => { + match export_package(&path, workcell) { + Ok(()) => { + info!("Successfully exported package"); + } + Err(err) => { + error!("Failed to export package: {err}"); + } + }; + } + } + } +} + +fn export_package( + output_directory: &PathBuf, + workcell: Workcell, +) -> Result<(), Box> { + 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_workcell_editor/src/workcell/urdf_package_exporter/generate_package.rs b/rmf_workcell_editor/src/workcell/urdf_package_exporter/generate_package.rs new file mode 100644 index 0000000..80b4bbe --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/generate_package.rs @@ -0,0 +1,136 @@ +use crate::site_asset_io::cache_path; +use crate::workcell::urdf_package_exporter::template::PackageContext; +use rmf_workcell_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_workcell_editor/src/workcell/urdf_package_exporter/mod.rs b/rmf_workcell_editor/src/workcell/urdf_package_exporter/mod.rs new file mode 100644 index 0000000..5d3e09b --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/mod.rs @@ -0,0 +1,5 @@ +pub mod generate_package; +pub use generate_package::generate_package; + +pub use template::{PackageContext, Person}; +pub mod template; diff --git a/rmf_workcell_editor/src/workcell/urdf_package_exporter/template.rs b/rmf_workcell_editor/src/workcell/urdf_package_exporter/template.rs new file mode 100644 index 0000000..f84eca0 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/template.rs @@ -0,0 +1,27 @@ +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_workcell_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 new file mode 100644 index 0000000..063522f --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/CMakeLists.txt.j2 @@ -0,0 +1,20 @@ +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_workcell_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 new file mode 100644 index 0000000..8e8c9ef --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/display.launch.py.j2 @@ -0,0 +1,31 @@ +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_workcell_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 new file mode 100644 index 0000000..737b279 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/package.xml.j2 @@ -0,0 +1,25 @@ + + + + {{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_workcell_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 new file mode 100644 index 0000000..bb8cc09 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/urdf_package_exporter/templates/urdf.rviz.j2 @@ -0,0 +1,35 @@ +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_workcell_editor/src/workcell/workcell.rs b/rmf_workcell_editor/src/workcell/workcell.rs new file mode 100644 index 0000000..91ca855 --- /dev/null +++ b/rmf_workcell_editor/src/workcell/workcell.rs @@ -0,0 +1,75 @@ +/* + * 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::CurrentWorkspace; +use crate::SiteAssets; +use bevy::prelude::*; +use rmf_workcell_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_workcell_editor/src/workspace.rs b/rmf_workcell_editor/src/workspace.rs new file mode 100644 index 0000000..bb110a8 --- /dev/null +++ b/rmf_workcell_editor/src/workspace.rs @@ -0,0 +1,442 @@ +/* + * 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, prelude::*}; +use bevy_impulse::*; +use std::path::PathBuf; + +use crate::workcell::{LoadWorkcell, SaveWorkcell}; +use crate::AppState; +use rmf_workcell_format::Workcell; + +use crate::{ + interaction::InteractionState, ChangeCurrentWorkspace, CreateNewWorkspace, CurrentWorkspace, + DefaultFile, FileDialogFilter, FileDialogServices, RecallWorkspace, +}; + +#[derive(Clone)] +pub enum WorkspaceData { + Workcell(Vec), + WorkcellUrdf(Vec), +} + +impl WorkspaceData { + pub fn new(path: &PathBuf, data: Vec) -> Option { + let filename = path.file_name().and_then(|f| f.to_str())?; + if filename.ends_with("workcell.json") { + Some(WorkspaceData::Workcell(data)) + } else if filename.ends_with("urdf") { + Some(WorkspaceData::WorkcellUrdf(data)) + } else { + error!("Unrecognized file type {:?}", filename); + None + } + } +} + +pub struct LoadWorkspaceFile(pub Option, pub WorkspaceData); + +#[derive(Clone, Default, Debug)] +pub enum ExportFormat { + #[default] + Default, + Urdf, +} + +pub struct WorkspacePlugin; + +impl Plugin for WorkspacePlugin { + fn build(&self, app: &mut App) { + app.add_event::() + .add_event::() + .add_event::() + .add_event::() + .init_resource::() + .init_resource::() + .init_resource::() + .init_resource::() + .init_resource::() + .add_systems( + Update, + (dispatch_new_workspace_events, sync_workspace_visibility), + ); + } +} + +pub fn dispatch_new_workspace_events( + state: Res>, + mut new_workspace: EventReader, + mut load_workcell: EventWriter, +) { + if let Some(_cmd) = new_workspace.read().last() { + match state.get() { + AppState::MainMenu => { + error!("Sent generic new workspace while in main menu"); + } + AppState::WorkcellEditor => { + println!("Creating new workspace"); + load_workcell.send(LoadWorkcell { + workcell: Workcell::default(), + focus: true, + default_file: None, + }); + } + } + } +} + +/// Service that takes workspace data and loads a site / workcell, as well as transition state. +pub fn process_load_workspace_files( + In(BlockingService { request, .. }): BlockingServiceInput, + mut app_state: ResMut>, + mut interaction_state: ResMut>, + mut load_workcell: EventWriter, +) { + let LoadWorkspaceFile(default_file, data) = request; + match data { + 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) => { + 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); + } + } + } + } +} + +#[derive(Resource)] +/// Services that deal with workspace loading +pub struct WorkspaceLoadingServices { + /// Service that spawns an open file dialog and loads a site accordingly. + pub load_workspace_from_dialog: Service<(), ()>, + /// Loads the workspace at the requested path + pub load_workspace_from_path: Service, + /// Loads the workspace from the requested data + pub load_workspace_from_data: Service, +} + +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_and_load; + // Spawn all the services + let loading_filters = vec![ + FileDialogFilter { + name: "Workcell".into(), + extensions: vec!["workcell.json".into()], + }, + FileDialogFilter { + name: "Urdf".into(), + extensions: vec!["urdf".into()], + }, + ]; + 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)?; + Some(LoadWorkspaceFile(Some(path), data)) + }) + .cancel_on_none() + .then(process_load_files) + .connect(scope.terminate) + }); + + let load_workspace_from_path = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_async(|path| async move { + let Some(data) = std::fs::read(&path) + .ok() + .and_then(|data| WorkspaceData::new(&path, data)) + else { + warn!("Unable to read file [{path:?}] so it cannot be loaded"); + return None; + }; + Some(LoadWorkspaceFile(Some(path.clone()), data)) + }) + .cancel_on_none() + .then(process_load_files) + .connect(scope.terminate) + }); + + let load_workspace_from_data = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .map_block(|data| LoadWorkspaceFile(None, data)) + .then(process_load_files) + .connect(scope.terminate) + }); + + Self { + load_workspace_from_dialog, + load_workspace_from_path, + load_workspace_from_data, + } + } +} + +impl<'w, 's> WorkspaceLoader<'w, 's> { + /// Request to spawn a dialog and load a workspace + pub fn load_from_dialog(&mut self) { + self.commands + .request((), self.workspace_loading.load_workspace_from_dialog) + .detach(); + } + + /// Request to load a workspace from a path + pub fn load_from_path(&mut self, path: PathBuf) { + self.commands + .request(path, self.workspace_loading.load_workspace_from_path) + .detach(); + } + + /// Request to load a workspace from data + pub fn load_from_data(&mut self, data: WorkspaceData) { + self.commands + .request(data, self.workspace_loading.load_workspace_from_data) + .detach(); + } +} + +/// `SystemParam` used to request for workspace loading operations +#[derive(SystemParam)] +pub struct WorkspaceLoader<'w, 's> { + workspace_loading: Res<'w, WorkspaceLoadingServices>, + commands: Commands<'w, 's>, +} + +/// Handles the file saving events +fn send_file_save( + In(BlockingService { request, .. }): BlockingServiceInput<(PathBuf, ExportFormat)>, + app_state: Res>, + mut save_workcell: EventWriter, + current_workspace: Res, +) { + let Some(ws_root) = current_workspace.root else { + warn!("Failed saving workspace, no current workspace found"); + return; + }; + match app_state.get() { + AppState::WorkcellEditor => { + save_workcell.send(SaveWorkcell { + root: ws_root, + to_file: request.0, + format: request.1, + }); + } + 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 a URDF package. + pub export_urdf_to_dialog: 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; + let pick_folder = world.resource::().pick_folder; + let saving_filters = vec![FileDialogFilter { + name: "Workcell".into(), + extensions: vec!["workcell.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) + .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_urdf_to_dialog = world.spawn_workflow(|scope, builder| { + scope + .input + .chain(builder) + .then(pick_folder) + .map_block(|path| (path, ExportFormat::Urdf)) + .then(send_file_save) + .connect(scope.terminate) + }); + + Self { + save_workspace_to_dialog, + save_workspace_to_path, + save_workspace_to_default_file, + export_urdf_to_dialog, + } + } +} + +// 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 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 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 export the workspace as a urdf to a folder selected from a dialog + pub fn export_urdf_to_dialog(&mut self) { + self.commands + .request((), self.workspace_saving.export_urdf_to_dialog) + .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, + mut visibility: Query<&mut Visibility>, +) { + if !current_workspace.is_changed() { + return; + } + + if recall.0 != current_workspace.root { + // Set visibility of current to target + if let Some(current_workspace_entity) = current_workspace.root { + if let Ok(mut v) = visibility.get_mut(current_workspace_entity) { + *v = if current_workspace.display { + Visibility::Inherited + } else { + Visibility::Hidden + }; + } + } + // Disable visibility in recall + if let Some(recall) = recall.0 { + if let Ok(mut v) = visibility.get_mut(recall) { + *v = Visibility::Hidden; + } + } + recall.0 = current_workspace.root; + } +} diff --git a/rmf_workcell_format/.gitignore b/rmf_workcell_format/.gitignore new file mode 100644 index 0000000..94b656f --- /dev/null +++ b/rmf_workcell_format/.gitignore @@ -0,0 +1 @@ +test_output/* diff --git a/rmf_workcell_format/Cargo.toml b/rmf_workcell_format/Cargo.toml new file mode 100644 index 0000000..430e876 --- /dev/null +++ b/rmf_workcell_format/Cargo.toml @@ -0,0 +1,25 @@ +[package] +name = "rmf_workcell_format" +version = "0.0.1" +edition = "2021" +authors = ["Luca Della Vedova "] + +[lib] +crate-type = ["rlib"] + +[dependencies] +serde = { version = "1.0", features = ["derive"] } +serde_json = "*" +thiserror = "*" +glam = { version = "0.24", features = ["serde"] } +bevy = { version = "0.12", optional = true } +# rmf_site_format = { git = "https://github.com/open-rmf/rmf_site", rev = "fe86373"} +rmf_site_format = { git = "https://github.com/open-rmf/rmf_site", tag = "v0.0.1"} +yaserde = "0.7" +urdf-rs = "0.7.3" + +[dev-dependencies] +float_eq = "1.0" + +[features] +bevy_support = ["rmf_site_format/bevy", "bevy"] diff --git a/rmf_workcell_format/package.xml b/rmf_workcell_format/package.xml new file mode 100644 index 0000000..f86bfe9 --- /dev/null +++ b/rmf_workcell_format/package.xml @@ -0,0 +1,11 @@ + + rmf_workcell_format + 0.0.1 + A library with reusable elements for the RMF Workcell Editor + Luca Della Vedova + Apache License 2.0 + + + ament_cargo + + diff --git a/rmf_workcell_format/src/geometry.rs b/rmf_workcell_format/src/geometry.rs new file mode 100644 index 0000000..35ae246 --- /dev/null +++ b/rmf_workcell_format/src/geometry.rs @@ -0,0 +1,143 @@ +/* + * 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::is_default; + +use rmf_site_format::{AssetSource, Pose, PrimitiveShape}; + +use glam::Vec3; +use serde::{Deserialize, Serialize}; + +#[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] }) + } +} + +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, + }, + } + } +} + +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).map(|s| 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 } + } + } + } +} + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +pub struct WorkcellModel { + pub name: String, + pub geometry: Geometry, + pub pose: Pose, +} + +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) + } +} diff --git a/rmf_workcell_format/src/inertial.rs b/rmf_workcell_format/src/inertial.rs new file mode 100644 index 0000000..e2ba0e7 --- /dev/null +++ b/rmf_workcell_format/src/inertial.rs @@ -0,0 +1,87 @@ +/* + * 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 rmf_site_format::Pose; + +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component, Deref, DerefMut}; +use serde::{Deserialize, Serialize}; + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))] +pub struct Mass(pub 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, +} + +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, + } + } +} + +#[derive(Serialize, Deserialize, Debug, Default, Clone)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct Inertia { + pub center: Pose, + pub mass: Mass, + pub moment: Moment, +} + +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, + }, + } + } +} diff --git a/rmf_workcell_format/src/is_default.rs b/rmf_workcell_format/src/is_default.rs new file mode 100644 index 0000000..34dc7b7 --- /dev/null +++ b/rmf_workcell_format/src/is_default.rs @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2022 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 fn is_default(v: &T) -> bool { + *v == T::default() +} diff --git a/rmf_workcell_format/src/joint.rs b/rmf_workcell_format/src/joint.rs new file mode 100644 index 0000000..730cd27 --- /dev/null +++ b/rmf_workcell_format/src/joint.rs @@ -0,0 +1,186 @@ +/* + * 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. + * +*/ + +#[cfg(feature = "bevy")] +use bevy::ecs::system::EntityCommands; +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component, SpatialBundle}; + +use crate::{Category, NameInWorkcell}; + +use serde::{Deserialize, Serialize}; + +#[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_workcell_editor instead of rmf_workcell_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(), + )); + } +} diff --git a/rmf_workcell_format/src/lib.rs b/rmf_workcell_format/src/lib.rs new file mode 100644 index 0000000..9662643 --- /dev/null +++ b/rmf_workcell_format/src/lib.rs @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2022 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 geometry; +pub use geometry::*; + +pub mod inertial; +pub use inertial::*; + +pub mod joint; +pub use joint::*; + +pub mod workcell; +pub use workcell::*; + +// TODO(luca) move away from SiteID? +pub use rmf_site_format::{ + Anchor, Angle, AssetSource, Category, Model, ModelMarker, NameInSite, Pending, Pose, + PrimitiveShape, Rotation, Scale, SiteID, +}; + +mod is_default; +pub(crate) use is_default::*; + +pub const CURRENT_MAJOR_VERSION: u32 = 0; +pub const CURRENT_MINOR_VERSION: u32 = 1; diff --git a/rmf_workcell_format/src/workcell.rs b/rmf_workcell_format/src/workcell.rs new file mode 100644 index 0000000..c10dad1 --- /dev/null +++ b/rmf_workcell_format/src/workcell.rs @@ -0,0 +1,620 @@ +/* + * 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::*; +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component, Deref, DerefMut}; +#[cfg(feature = "bevy")] +use bevy::reflect::{TypePath, TypeUuid}; +use rmf_site_format::{misc::Rotation, Anchor, Pose, RefTrait}; +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)] +#[cfg_attr(feature = "bevy", derive(Bundle))] +pub struct Frame { + #[serde(flatten)] + pub anchor: Anchor, + pub name: NameInWorkcell, + #[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); + +// 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, +} + +#[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), +} + +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: 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: NameInWorkcell(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 = parented_frame.bundle.name.0.clone(); + 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 = self + .frames + .values() + .find(|frame| frame.parent == *joint_id) + .ok_or(WorkcellToUrdfError::BrokenReference(*joint_id))?; + let parent_name = parent_frame.bundle.name.clone(); + let child_name = child_frame.bundle.name.clone(); + 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(WorkcellToUrdfError::WriteToStringError) + } + + 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(s: &str) -> serde_json::Result { + serde_json::de::from_str(s) + } + + pub fn from_bytes(s: &[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); + +#[cfg(test)] +mod tests { + use super::*; + use float_eq::{assert_float_eq, float_eq}; + use rmf_site_format::{Angle, PrimitiveShape}; + + fn frame_by_name( + frames: &BTreeMap>, + name: &str, + ) -> Option<(u32, Parented)> { + frames + .iter() + .find(|(_, parented_frame)| { + parented_frame.bundle.name == 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 + )); + } +} diff --git a/rmf_workcell_format/test/07-physics.urdf b/rmf_workcell_format/test/07-physics.urdf new file mode 100644 index 0000000..95c4419 --- /dev/null +++ b/rmf_workcell_format/test/07-physics.urdf @@ -0,0 +1,418 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scripts/build-web.sh b/scripts/build-web.sh new file mode 100755 index 0000000..be8bc89 --- /dev/null +++ b/scripts/build-web.sh @@ -0,0 +1,9 @@ +#!/bin/bash +set -o verbose +set -o errexit +export CARGO_PROFILE_RELEASE_LTO=true +export CARGO_PROFILE_RELEASE_OPT_LEVEL=z +cargo build --target wasm32-unknown-unknown --release +RUST_BACKTRACE=full wasm-bindgen --target web --out-dir web target/wasm32-unknown-unknown/release/librmf_workcell_editor.wasm +cd web +wasm-opt -Oz -o librmf_workcell_editor_bg_optimized.wasm librmf_workcell_editor_bg.wasm diff --git a/scripts/deploy-web.sh b/scripts/deploy-web.sh new file mode 100755 index 0000000..1077f86 --- /dev/null +++ b/scripts/deploy-web.sh @@ -0,0 +1,35 @@ +#!/bin/bash +set -o errexit + +read -p "Do you really want to deploy to GitHub Pages? (y/N) " +echo # send a carriage return +if [[ ! $REPLY =~ ^[Yy]$ ]]; then + echo "Exiting..." + exit 1 +fi + +read -p "Which branch? [main] ? " +echo # send a carriage return +BRANCH=$REPLY +if [[ -z $BRANCH ]]; then + BRANCH="main" +fi + +echo "Starting deployment process using branch $BRANCH" + +set -o verbose + +git clone ssh://git@github.com/open-rmf/rmf_site temp-deploy-checkout --branch $BRANCH --single-branch --depth 1 + +cd temp-deploy-checkout +git checkout --orphan gh-pages +git reset +scripts/build-web.sh + +git add -f web +cp web/root_index.html index.html +git add index.html + +git commit -a -m "publish to github pages" + +git push origin gh-pages --force diff --git a/scripts/serve-web.sh b/scripts/serve-web.sh new file mode 100755 index 0000000..933fe4e --- /dev/null +++ b/scripts/serve-web.sh @@ -0,0 +1,5 @@ +#!/bin/bash +set -o verbose +set -o errexit +cd web +basic-http-server . -a 127.0.0.1:1234 diff --git a/web/index.html b/web/index.html new file mode 100644 index 0000000..067f183 --- /dev/null +++ b/web/index.html @@ -0,0 +1,29 @@ + + + + The RMF Workcell Editor + + + + + + + + diff --git a/web/root_index.html b/web/root_index.html new file mode 100644 index 0000000..392b599 --- /dev/null +++ b/web/root_index.html @@ -0,0 +1,8 @@ + + + + + + Redirecting to the 'web' subdirectory... + +