From a0f1a9d83c69f4fdd074f956b67f510ae9d029f9 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 5 Dec 2023 17:59:57 +0800
Subject: [PATCH 01/71] WIP, proof of concept of SDF export

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 Cargo.lock                               | 1147 ++++++++++++++--------
 rmf_site_editor/Cargo.toml               |    4 +-
 rmf_site_editor/src/lib.rs               |    3 +-
 rmf_site_editor/src/site/file_menu.rs    |   75 ++
 rmf_site_editor/src/site/floor.rs        |    2 +-
 rmf_site_editor/src/site/mod.rs          |    6 +
 rmf_site_editor/src/site/save.rs         |  120 ++-
 rmf_site_editor/src/site/sdf_exporter.rs |   77 ++
 rmf_site_editor/src/workcell/save.rs     |    3 +
 rmf_site_editor/src/workspace.rs         |    2 +
 rmf_site_format/Cargo.toml               |    2 +
 rmf_site_format/src/lib.rs               |    3 +
 rmf_site_format/src/sdf.rs               |  239 +++++
 13 files changed, 1216 insertions(+), 467 deletions(-)
 create mode 100644 rmf_site_editor/src/site/file_menu.rs
 create mode 100644 rmf_site_editor/src/site/sdf_exporter.rs
 create mode 100644 rmf_site_format/src/sdf.rs

diff --git a/Cargo.lock b/Cargo.lock
index d61a1d96..449e0e6e 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -4,9 +4,9 @@ version = 3
 
 [[package]]
 name = "ab_glyph"
-version = "0.2.22"
+version = "0.2.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1061f3ff92c2f65800df1f12fc7b4ff44ee14783104187dd04dfee6f11b0fd2"
+checksum = "80179d7dd5d7e8c285d67c4a1e652972a92de7475beddfb92028c76463b13225"
 dependencies = [
  "ab_glyph_rasterizer",
  "owned_ttf_parser",
@@ -142,21 +142,22 @@ dependencies = [
 
 [[package]]
 name = "ahash"
-version = "0.8.3"
+version = "0.8.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2c99f64d1e06488f620f932677e24bc6e2897582980441ae90a671415bd7ec2f"
+checksum = "91429305e9f0a25f6205c5b8e0d2db09e0708a7a6df0f42212bb56c32c8ac97a"
 dependencies = [
  "cfg-if",
- "getrandom 0.2.10",
+ "getrandom 0.2.11",
  "once_cell",
  "version_check",
+ "zerocopy",
 ]
 
 [[package]]
 name = "aho-corasick"
-version = "1.1.1"
+version = "1.1.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ea5d730647d4fadd988536d06fecce94b7b4f2a7efdae548f1cf4b63205518ab"
+checksum = "b2969dcb958b36655471fc61f7e416fa76033bdd4bfed0678d8fee1e2d07a1f0"
 dependencies = [
  "memchr",
 ]
@@ -293,9 +294,9 @@ dependencies = [
 
 [[package]]
 name = "arboard"
-version = "3.2.1"
+version = "3.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ac57f2b058a76363e357c056e4f74f1945bf734d37b8b3ef49066c4787dde0fc"
+checksum = "aafb29b107435aa276664c1db8954ac27a6e105cdad3c88287a199eb0e313c08"
 dependencies = [
  "clipboard-win",
  "core-graphics",
@@ -332,36 +333,49 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "81953c529336010edd6d8e358f886d9581267795c61b19475b71314bffa46d35"
 dependencies = [
  "concurrent-queue",
- "event-listener",
+ "event-listener 2.5.3",
  "futures-core",
 ]
 
+[[package]]
+name = "async-channel"
+version = "2.1.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1ca33f4bc4ed1babef42cad36cc1f51fa88be00420404e5b1e80ab1b18f7678c"
+dependencies = [
+ "concurrent-queue",
+ "event-listener 4.0.0",
+ "event-listener-strategy",
+ "futures-core",
+ "pin-project-lite",
+]
+
 [[package]]
 name = "async-executor"
-version = "1.5.1"
+version = "1.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fa3dc5f2a8564f07759c008b9109dc0d39de92a88d5588b8a5036d286383afb"
+checksum = "17ae5ebefcc48e7452b4987947920dac9450be1110cadf34d1b8c116bdbaf97c"
 dependencies = [
- "async-lock",
+ "async-lock 3.1.2",
  "async-task",
  "concurrent-queue",
- "fastrand 1.9.0",
- "futures-lite",
+ "fastrand 2.0.1",
+ "futures-lite 2.0.1",
  "slab",
 ]
 
 [[package]]
 name = "async-global-executor"
-version = "2.3.1"
+version = "2.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f1b6f5d7df27bd294849f8eec66ecfc63d11814df7a4f5d74168a2394467b776"
+checksum = "9b4353121d5644cdf2beb5726ab752e79a8db1ebb52031770ec47db31d245526"
 dependencies = [
- "async-channel",
+ "async-channel 2.1.1",
  "async-executor",
- "async-io",
- "async-lock",
+ "async-io 2.2.1",
+ "async-lock 3.1.2",
  "blocking",
- "futures-lite",
+ "futures-lite 2.0.1",
  "once_cell",
 ]
 
@@ -371,27 +385,57 @@ version = "1.13.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0fc5b45d93ef0529756f812ca52e44c221b35341892d3dcc34132ac02f3dd2af"
 dependencies = [
- "async-lock",
+ "async-lock 2.8.0",
  "autocfg",
  "cfg-if",
  "concurrent-queue",
- "futures-lite",
+ "futures-lite 1.13.0",
  "log",
  "parking",
- "polling",
- "rustix",
+ "polling 2.8.0",
+ "rustix 0.37.27",
  "slab",
  "socket2",
  "waker-fn",
 ]
 
+[[package]]
+name = "async-io"
+version = "2.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d6d3b15875ba253d1110c740755e246537483f152fa334f91abd7fe84c88b3ff"
+dependencies = [
+ "async-lock 3.1.2",
+ "cfg-if",
+ "concurrent-queue",
+ "futures-io",
+ "futures-lite 2.0.1",
+ "parking",
+ "polling 3.3.1",
+ "rustix 0.38.25",
+ "slab",
+ "tracing",
+ "windows-sys 0.52.0",
+]
+
 [[package]]
 name = "async-lock"
 version = "2.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "287272293e9d8c41773cec55e365490fe034813a2f172f502d6ddcf75b2f582b"
 dependencies = [
- "event-listener",
+ "event-listener 2.5.3",
+]
+
+[[package]]
+name = "async-lock"
+version = "3.1.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dea8b3453dd7cc96711834b75400d671b73e3656975fa68d9f277163b7f7e316"
+dependencies = [
+ "event-listener 4.0.0",
+ "event-listener-strategy",
+ "pin-project-lite",
 ]
 
 [[package]]
@@ -400,15 +444,15 @@ version = "1.12.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "62565bb4402e926b29953c785397c6dc0391b7b446e45008b0049eb43cec6f5d"
 dependencies = [
- "async-channel",
+ "async-channel 1.9.0",
  "async-global-executor",
- "async-io",
- "async-lock",
+ "async-io 1.13.0",
+ "async-lock 2.8.0",
  "crossbeam-utils",
  "futures-channel",
  "futures-core",
  "futures-io",
- "futures-lite",
+ "futures-lite 1.13.0",
  "gloo-timers",
  "kv-log-macro",
  "log",
@@ -422,19 +466,19 @@ dependencies = [
 
 [[package]]
 name = "async-task"
-version = "4.4.1"
+version = "4.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b9441c6b2fe128a7c2bf680a44c34d0df31ce09e5b7e401fcca3faa483dbc921"
+checksum = "b4eb2cdb97421e01129ccb49169d8279ed21e829929144f4a22a6e54ac549ca1"
 
 [[package]]
 name = "async-trait"
-version = "0.1.73"
+version = "0.1.74"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bc00ceb34980c03614e35a3a4e218276a0a824e911d07651cd0d858a51e8c0f0"
+checksum = "a66537f1bb974b254c98ed142ff995236e81b9d0fe4db0575f46612cb15eb0f9"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -457,9 +501,9 @@ checksum = "1505bd5d3d116872e7271a6d4e16d81d0c8570876c8de68093a09ac269d8aac0"
 
 [[package]]
 name = "atomic_refcell"
-version = "0.1.12"
+version = "0.1.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "76f2bfe491d41d45507b8431da8274f7feeca64a49e86d980eed2937ec2ff020"
+checksum = "41e67cd8309bbd06cd603a9e693a784ac2e5d1e955f11286e355089fcab3047c"
 
 [[package]]
 name = "autocfg"
@@ -496,9 +540,9 @@ checksum = "9e1b586273c5702936fe7b7d6896644d8be71e6314cfe09d3167c95f712589e8"
 
 [[package]]
 name = "base64"
-version = "0.21.4"
+version = "0.21.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ba43ea6f343b788c8764558649e08df62f86c6ef251fdaeb1ffd010a9ae50a2"
+checksum = "35636a1494ede3b646cc98f74f8e62c773a38a659ebc777a2cf26b9b74171df9"
 
 [[package]]
 name = "bevy"
@@ -564,7 +608,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "9714af523da4cdf58c42a317e5ed40349708ad954a18533991fd64c8ae0a6f68"
 dependencies = [
  "anyhow",
- "async-channel",
+ "async-channel 1.9.0",
  "bevy_app",
  "bevy_diagnostic",
  "bevy_ecs",
@@ -637,7 +681,7 @@ dependencies = [
  "bevy_render",
  "bevy_transform",
  "bevy_utils",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "radsort",
  "serde",
 ]
@@ -650,7 +694,7 @@ checksum = "a44e4e2784a81430199e4157e02903a987a32127c773985506f020e7d501b62e"
 dependencies = [
  "bevy_macro_utils",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -683,14 +727,14 @@ version = "0.11.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "266144b36df7e834d5198049e037ecdf2a2310a76ce39ed937d1b0a6a2c4e8c6"
 dependencies = [
- "async-channel",
+ "async-channel 1.9.0",
  "bevy_ecs_macros",
  "bevy_ptr",
  "bevy_reflect",
  "bevy_tasks",
  "bevy_utils",
  "downcast-rs",
- "event-listener",
+ "event-listener 2.5.3",
  "fixedbitset",
  "rustc-hash",
  "serde",
@@ -707,7 +751,7 @@ dependencies = [
  "bevy_macro_utils",
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -800,6 +844,21 @@ dependencies = [
  "thiserror",
 ]
 
+[[package]]
+name = "bevy_gltf_export"
+version = "0.1.0"
+dependencies = [
+ "bevy_asset",
+ "bevy_pbr",
+ "bevy_render",
+ "gltf",
+ "gltf-json",
+ "image",
+ "serde",
+ "serde_json",
+ "thiserror",
+]
+
 [[package]]
 name = "bevy_hierarchy"
 version = "0.11.3"
@@ -888,7 +947,7 @@ dependencies = [
  "bevy_ecs",
  "bevy_utils",
  "console_error_panic_hook",
- "tracing-log",
+ "tracing-log 0.1.4",
  "tracing-subscriber",
  "tracing-wasm",
 ]
@@ -901,7 +960,7 @@ checksum = "23ddc18d489b4e57832d4958cde7cd2f349f0ad91e5892ac9e2f2ee16546b981"
 dependencies = [
  "quote",
  "rustc-hash",
- "syn 2.0.37",
+ "syn 2.0.39",
  "toml_edit 0.19.15",
 ]
 
@@ -926,9 +985,9 @@ dependencies = [
 
 [[package]]
 name = "bevy_mod_outline"
-version = "0.5.0"
+version = "0.5.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "479f08715afd391e7012c7d8c28f771fd1acdbf8a2bccea0ebacec0f81648cfe"
+checksum = "6bc7f9d694a9b767def28e2a649bdc9b290bb9cd3c8f6d8ae90c3ad2234e3580"
 dependencies = [
  "bevy",
  "bitfield",
@@ -982,7 +1041,7 @@ dependencies = [
  "bevy_transform",
  "bevy_utils",
  "bevy_window",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "bytemuck",
  "naga_oil",
  "radsort",
@@ -995,7 +1054,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b82c222b756d1d41ca808595934fc02edc5b05604151930bc7d00bb4fd8d1bd"
 dependencies = [
  "bevy",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "naga",
 ]
 
@@ -1014,7 +1073,7 @@ dependencies = [
  "bevy",
  "bitflags 1.3.2",
  "log",
- "nalgebra 0.32.3",
+ "nalgebra",
  "rapier3d",
 ]
 
@@ -1049,7 +1108,7 @@ dependencies = [
  "bit-set",
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
  "uuid",
 ]
 
@@ -1060,7 +1119,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "39df4824b760928c27afc7b00fb649c7a63c9d76661ab014ff5c86537ee906cb"
 dependencies = [
  "anyhow",
- "async-channel",
+ "async-channel 1.9.0",
  "bevy_app",
  "bevy_asset",
  "bevy_core",
@@ -1078,12 +1137,12 @@ dependencies = [
  "bevy_transform",
  "bevy_utils",
  "bevy_window",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "bytemuck",
  "codespan-reporting",
  "downcast-rs",
  "encase",
- "futures-lite",
+ "futures-lite 1.13.0",
  "hexasphere",
  "image",
  "js-sys",
@@ -1112,7 +1171,7 @@ dependencies = [
  "bevy_macro_utils",
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -1154,7 +1213,7 @@ dependencies = [
  "bevy_render",
  "bevy_transform",
  "bevy_utils",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "bytemuck",
  "fixedbitset",
  "guillotiere",
@@ -1180,11 +1239,11 @@ version = "0.11.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "c73bbb847c83990d3927005090df52f8ac49332e1643d2ad9aac3cd2974e66bf"
 dependencies = [
- "async-channel",
+ "async-channel 1.9.0",
  "async-executor",
  "async-task",
  "concurrent-queue",
- "futures-lite",
+ "futures-lite 1.13.0",
  "wasm-bindgen-futures",
 ]
 
@@ -1276,8 +1335,8 @@ checksum = "08d9484e32434ea84dc548cff246ce0c6f756c1336f5ea03f24ac120a48595c7"
 dependencies = [
  "ahash",
  "bevy_utils_proc_macros",
- "getrandom 0.2.10",
- "hashbrown 0.14.1",
+ "getrandom 0.2.11",
+ "hashbrown 0.14.3",
  "instant",
  "petgraph",
  "thiserror",
@@ -1293,7 +1352,7 @@ checksum = "5391b242c36f556db01d5891444730c83aa9dd648b6a8fd2b755d22cb3bddb57"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -1338,11 +1397,11 @@ dependencies = [
 
 [[package]]
 name = "bindgen"
-version = "0.68.1"
+version = "0.69.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "726e4313eb6ec35d2730258ad4e15b547ee75d6afaa1361a922e78e59b7d8078"
+checksum = "9ffcebc3849946a7170a05992aac39da343a90676ab392c51a4280981d6379c2"
 dependencies = [
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "cexpr",
  "clang-sys",
  "lazy_static",
@@ -1353,7 +1412,7 @@ dependencies = [
  "regex",
  "rustc-hash",
  "shlex",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -1371,6 +1430,12 @@ version = "0.6.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "349f9b6a179ed607305526ca489b34ad0a41aed5f7980fa90eb03160b69598fb"
 
+[[package]]
+name = "bit_field"
+version = "0.10.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61"
+
 [[package]]
 name = "bitfield"
 version = "0.14.0"
@@ -1385,9 +1450,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
 
 [[package]]
 name = "bitflags"
-version = "2.4.0"
+version = "2.4.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b4682ae6287fcf752ecaabbfcc7b6f9b72aa33933dc23a554d853aea8eea8635"
+checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07"
 dependencies = [
  "serde",
 ]
@@ -1428,16 +1493,16 @@ dependencies = [
 
 [[package]]
 name = "blocking"
-version = "1.4.0"
+version = "1.5.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "94c4ef1f913d78636d78d538eec1f18de81e481f44b1be0a81060090530846e1"
+checksum = "6a37913e8dc4ddcc604f0c6d3bf2887c995153af3611de9e23c352b44c1b9118"
 dependencies = [
- "async-channel",
- "async-lock",
+ "async-channel 2.1.1",
+ "async-lock 3.1.2",
  "async-task",
  "fastrand 2.0.1",
  "futures-io",
- "futures-lite",
+ "futures-lite 2.0.1",
  "piper",
  "tracing",
 ]
@@ -1465,14 +1530,14 @@ checksum = "965ab7eb5f8f97d2a083c799f3a1b994fc397b2fe2da5d1da1626ce15a39f2b1"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
 name = "byteorder"
-version = "1.4.3"
+version = "1.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610"
+checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
 
 [[package]]
 name = "bytes"
@@ -1565,9 +1630,9 @@ dependencies = [
 
 [[package]]
 name = "clap"
-version = "4.4.6"
+version = "4.4.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d04704f56c2cde07f43e8e2c154b43f216dc5c92fc98ada720177362f953b956"
+checksum = "41fffed7514f420abec6d183b1d3acfd9099c79c3a10a06ade4f8203f1411272"
 dependencies = [
  "clap_builder",
  "clap_derive",
@@ -1575,9 +1640,9 @@ dependencies = [
 
 [[package]]
 name = "clap_builder"
-version = "4.4.6"
+version = "4.4.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0e231faeaca65ebd1ea3c737966bf858971cd38c3849107aa3ea7de90a804e45"
+checksum = "63361bae7eef3771745f02d8d892bec2fee5f6e34af316ba556e7f97a7069ff1"
 dependencies = [
  "anstream",
  "anstyle",
@@ -1587,21 +1652,21 @@ dependencies = [
 
 [[package]]
 name = "clap_derive"
-version = "4.4.2"
+version = "4.4.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0862016ff20d69b84ef8247369fabf5c008a7417002411897d40ee1f4532b873"
+checksum = "cf9804afaaf59a91e75b022a30fb7229a7901f60c755489cc61c9b423b836442"
 dependencies = [
  "heck 0.4.1",
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
 name = "clap_lex"
-version = "0.5.1"
+version = "0.6.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cd7cc57abe963c6d3b9d8be5b06ba7c8957a930305ca90304f24ef040aa6f961"
+checksum = "702fc72eb24e5a1e48ce58027a675bc24edd52096d5397d4aea7c6dd9eca0bd1"
 
 [[package]]
 name = "clipboard-win"
@@ -1730,16 +1795,10 @@ version = "0.9.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "194a7a9e6de53fa55116934067c844d9d749312f75c6f6d0980e8c252f8c2146"
 dependencies = [
- "core-foundation-sys 0.8.4",
+ "core-foundation-sys",
  "libc",
 ]
 
-[[package]]
-name = "core-foundation-sys"
-version = "0.6.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e7ca8a5221364ef15ce201e8ed2f609fc312682a8f4e0e3d4aa5879764e0fa3b"
-
 [[package]]
 name = "core-foundation-sys"
 version = "0.8.4"
@@ -1772,20 +1831,20 @@ dependencies = [
 
 [[package]]
 name = "coreaudio-rs"
-version = "0.11.2"
+version = "0.11.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cb17e2d1795b1996419648915df94bc7103c28f7b48062d7acf4652fc371b2ff"
+checksum = "321077172d79c662f64f5071a03120748d5bb652f5231570141be24cfcd2bace"
 dependencies = [
  "bitflags 1.3.2",
- "core-foundation-sys 0.6.2",
+ "core-foundation-sys",
  "coreaudio-sys",
 ]
 
 [[package]]
 name = "coreaudio-sys"
-version = "0.2.13"
+version = "0.2.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d8478e5bdad14dce236b9898ea002eabfa87cbe14f0aa538dbe3b6a4bec4332d"
+checksum = "f3120ebb80a9de008e638ad833d4127d50ea3d3a960ea23ea69bc66d9358a028"
 dependencies = [
  "bindgen",
 ]
@@ -1797,7 +1856,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6d959d90e938c5493000514b446987c07aed46c668faaa7d34d6c7a67b1a578c"
 dependencies = [
  "alsa",
- "core-foundation-sys 0.8.4",
+ "core-foundation-sys",
  "coreaudio-rs",
  "dasp_sample",
  "jni 0.19.0",
@@ -1817,9 +1876,9 @@ dependencies = [
 
 [[package]]
 name = "cpufeatures"
-version = "0.2.9"
+version = "0.2.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a17b76ff3a4162b0b27f354a0c87015ddad39d35f9c0c36607a3bdd175dde1f1"
+checksum = "ce420fe07aecd3e67c5f910618fe65e94158f6dcc0adf44e00d69ce2bdfe0fd0"
 dependencies = [
  "libc",
 ]
@@ -1906,6 +1965,12 @@ dependencies = [
  "cfg-if",
 ]
 
+[[package]]
+name = "crunchy"
+version = "0.2.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7"
+
 [[package]]
 name = "crypto-mac"
 version = "0.10.1"
@@ -1942,9 +2007,9 @@ dependencies = [
 
 [[package]]
 name = "curl-sys"
-version = "0.4.66+curl-8.3.0"
+version = "0.4.68+curl-8.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "70c44a72e830f0e40ad90dda8a6ab6ed6314d39776599a58a2e5e37fbc6db5b9"
+checksum = "b4a0d18d88360e374b16b2273c832b5e57258ffc1d4aa4f96b108e0738d5752f"
 dependencies = [
  "cc",
  "libc",
@@ -1975,9 +2040,9 @@ checksum = "0c87e182de0887fd5361989c677c4e8f5000cd9491d6d563161a8f3a5519fc7f"
 
 [[package]]
 name = "data-encoding"
-version = "2.4.0"
+version = "2.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c2e66c9d817f1720209181c316d28635c050fa304f9c79e47a520882661b7308"
+checksum = "7e962a19be5cfc3f3bf6dd8f61eb50107f356ad6270fbb3ed41476571db78be5"
 
 [[package]]
 name = "digest"
@@ -2111,7 +2176,7 @@ checksum = "3fe2568f851fd6144a45fa91cfed8fe5ca8fc0b56ba6797bfc1ed2771b90e37c"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -2156,23 +2221,12 @@ dependencies = [
 
 [[package]]
 name = "errno"
-version = "0.3.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "add4f07d43996f76ef320709726a556a9d4f965d9410d8d0271132d2f8293480"
-dependencies = [
- "errno-dragonfly",
- "libc",
- "windows-sys 0.48.0",
-]
-
-[[package]]
-name = "errno-dragonfly"
-version = "0.1.2"
+version = "0.3.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "aa68f1b12764fab894d2755d2518754e71b4fd80ecfb822714a1206c2aab39bf"
+checksum = "a258e46cdc063eb8519c00b9fc845fc47bcfca4130e2f08e88665ceda8474245"
 dependencies = [
- "cc",
  "libc",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
@@ -2200,6 +2254,43 @@ version = "2.5.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0206175f82b8d6bf6652ff7d71a1e27fd2e4efde587fd368662814d6ec1d9ce0"
 
+[[package]]
+name = "event-listener"
+version = "4.0.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "770d968249b5d99410d61f5bf89057f3199a077a04d087092f58e7d10692baae"
+dependencies = [
+ "concurrent-queue",
+ "parking",
+ "pin-project-lite",
+]
+
+[[package]]
+name = "event-listener-strategy"
+version = "0.4.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "958e4d70b6d5e81971bebec42271ec641e7ff4e170a6fa605f2b8a8b65cb97d3"
+dependencies = [
+ "event-listener 4.0.0",
+ "pin-project-lite",
+]
+
+[[package]]
+name = "exr"
+version = "1.71.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "832a761f35ab3e6664babfbdc6cef35a4860e816ec3916dcfd0882954e98a8a8"
+dependencies = [
+ "bit_field",
+ "flume 0.11.0",
+ "half",
+ "lebe",
+ "miniz_oxide",
+ "rayon-core",
+ "smallvec",
+ "zune-inflate",
+]
+
 [[package]]
 name = "fastrand"
 version = "1.9.0"
@@ -2217,9 +2308,9 @@ checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5"
 
 [[package]]
 name = "fdeflate"
-version = "0.3.0"
+version = "0.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d329bdeac514ee06249dabc27877490f17f5d371ec693360768b838e19f3ae10"
+checksum = "64d6dafc854908ff5da46ff3f8f473c6984119a2876a383a860246dd7841a868"
 dependencies = [
  "simd-adler32",
 ]
@@ -2244,9 +2335,9 @@ checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80"
 
 [[package]]
 name = "flate2"
-version = "1.0.27"
+version = "1.0.28"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c6c98ee8095e9d1dcbf2fcc6d95acccb90d1c81db1e44725c6a984b1dbdfb010"
+checksum = "46303f565772937ffe1d394a4fac6f411c6013172fadde9dcdb1e147a086940e"
 dependencies = [
  "crc32fast",
  "miniz_oxide",
@@ -2263,12 +2354,9 @@ dependencies = [
 
 [[package]]
 name = "float_next_after"
-version = "0.1.5"
+version = "1.0.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4fc612c5837986b7104a87a0df74a5460931f1c5274be12f8d0f40aa2f30d632"
-dependencies = [
- "num-traits",
-]
+checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8"
 
 [[package]]
 name = "flume"
@@ -2281,6 +2369,15 @@ dependencies = [
  "spinning_top",
 ]
 
+[[package]]
+name = "flume"
+version = "0.11.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "55ac459de2512911e4b674ce33cf20befaba382d05b62b008afc1c8b57cbf181"
+dependencies = [
+ "spin",
+]
+
 [[package]]
 name = "fnv"
 version = "1.0.7"
@@ -2304,9 +2401,9 @@ checksum = "00b0228411908ca8685dba7fc2cdd70ec9990a6e753e89b6ac91a84c40fbaf4b"
 
 [[package]]
 name = "form_urlencoded"
-version = "1.2.0"
+version = "1.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a62bc1cf6f830c2ec14a513a9fb124d0a213a629668a4186f329db21fe045652"
+checksum = "e13624c2627564efccf4934284bdd98cbaa14e79b0b5a141218e507b3a823456"
 dependencies = [
  "percent-encoding",
 ]
@@ -2322,9 +2419,9 @@ dependencies = [
 
 [[package]]
 name = "futures"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "23342abe12aba583913b2e62f22225ff9c950774065e4bfb61a19cd9770fec40"
+checksum = "da0290714b38af9b4a7b094b8a37086d1b4e61f2df9122c3cad2577669145335"
 dependencies = [
  "futures-channel",
  "futures-core",
@@ -2337,9 +2434,9 @@ dependencies = [
 
 [[package]]
 name = "futures-channel"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "955518d47e09b25bbebc7a18df10b81f0c766eaf4c4f1cccef2fca5f2a4fb5f2"
+checksum = "ff4dd66668b557604244583e3e1e1eada8c5c2e96a6d0d6653ede395b78bbacb"
 dependencies = [
  "futures-core",
  "futures-sink",
@@ -2347,15 +2444,15 @@ dependencies = [
 
 [[package]]
 name = "futures-core"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4bca583b7e26f571124fe5b7561d49cb2868d79116cfa0eefce955557c6fee8c"
+checksum = "eb1d22c66e66d9d72e1758f0bd7d4fd0bee04cad842ee34587d68c07e45d088c"
 
 [[package]]
 name = "futures-executor"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ccecee823288125bd88b4d7f565c9e58e41858e47ab72e8ea2d64e93624386e0"
+checksum = "0f4fb8693db0cf099eadcca0efe2a5a22e4550f98ed16aba6c48700da29597bc"
 dependencies = [
  "futures-core",
  "futures-task",
@@ -2364,9 +2461,9 @@ dependencies = [
 
 [[package]]
 name = "futures-io"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4fff74096e71ed47f8e023204cfd0aa1289cd54ae5430a9523be060cdb849964"
+checksum = "8bf34a163b5c4c52d0478a4d757da8fb65cabef42ba90515efee0f6f9fa45aaa"
 
 [[package]]
 name = "futures-lite"
@@ -2383,34 +2480,48 @@ dependencies = [
  "waker-fn",
 ]
 
+[[package]]
+name = "futures-lite"
+version = "2.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d3831c2651acb5177cbd83943f3d9c8912c5ad03c76afcc0e9511ba568ec5ebb"
+dependencies = [
+ "fastrand 2.0.1",
+ "futures-core",
+ "futures-io",
+ "memchr",
+ "parking",
+ "pin-project-lite",
+]
+
 [[package]]
 name = "futures-macro"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72"
+checksum = "53b153fd91e4b0147f4aced87be237c98248656bb01050b96bf3ee89220a8ddb"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
 name = "futures-sink"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f43be4fe21a13b9781a69afa4985b0f6ee0e1afab2c6f454a8cf30e2b2237b6e"
+checksum = "e36d3378ee38c2a36ad710c5d30c2911d752cb941c00c72dbabfb786a7970817"
 
 [[package]]
 name = "futures-task"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "76d3d132be6c0e6aa1534069c705a74a5997a356c0dc2f86a47765e5617c5b65"
+checksum = "efd193069b0ddadc69c46389b740bbccdd97203899b48d09c5f7969591d6bae2"
 
 [[package]]
 name = "futures-util"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "26b01e40b772d54cf6c6d721c1d1abd0647a0106a12ecaa1c186273392a69533"
+checksum = "a19526d624e703a3179b3d322efec918b6246ea0fa51d41124525f00f1cc8104"
 dependencies = [
  "futures-channel",
  "futures-core",
@@ -2466,9 +2577,9 @@ dependencies = [
 
 [[package]]
 name = "gethostname"
-version = "0.2.3"
+version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1ebd34e35c46e00bb73e81363248d627782724609fe1b6396f553f68fe3862e"
+checksum = "bb65d4ba3173c56a500b555b532f72c42e8d1fe64962b518897f8959fae2c177"
 dependencies = [
  "libc",
  "winapi",
@@ -2487,9 +2598,9 @@ dependencies = [
 
 [[package]]
 name = "getrandom"
-version = "0.2.10"
+version = "0.2.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "be4136b2a15dd319360be1c07d9933517ccf0be8f16bf62a3bee4f0d618df427"
+checksum = "fe9006bed769170c11f845cf00c7c1e9092aeb3f268e007c3e760ac68008070f"
 dependencies = [
  "cfg-if",
  "js-sys",
@@ -2508,11 +2619,21 @@ dependencies = [
  "polyval",
 ]
 
+[[package]]
+name = "gif"
+version = "0.12.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "80792593675e051cf94a4b111980da2ba60d4a83e43e0048c5693baab3977045"
+dependencies = [
+ "color_quant",
+ "weezl",
+]
+
 [[package]]
 name = "gilrs"
-version = "0.10.2"
+version = "0.10.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "62fd19844d0eb919aca41d3e4ea0e0b6bf60e1e827558b101c269015b8f5f27a"
+checksum = "9e9eec02069fcbd7abe00a28adf216547774889129a777cb5e53fdfb75d59f09"
 dependencies = [
  "fnv",
  "gilrs-core",
@@ -2523,17 +2644,18 @@ dependencies = [
 
 [[package]]
 name = "gilrs-core"
-version = "0.5.7"
+version = "0.5.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5ccc99e9b8d63ffcaa334c4babfa31f46e156618a11f63efb6e8e6bcb37b830d"
+checksum = "178769da179a47b187837d1ab2b5b9b684a21180166a77a4ca37e7e58ee3833d"
 dependencies = [
  "core-foundation",
+ "inotify 0.10.2",
  "io-kit-sys",
  "js-sys",
  "libc",
  "libudev-sys",
  "log",
- "nix 0.26.4",
+ "nix 0.27.1",
  "uuid",
  "vec_map",
  "wasm-bindgen",
@@ -2543,9 +2665,9 @@ dependencies = [
 
 [[package]]
 name = "gimli"
-version = "0.28.0"
+version = "0.28.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6fb8d784f27acf97159b40fc4db5ecd8aa23b9ad5ef69cdd136d3bc80665f0c0"
+checksum = "4271d37baee1b8c7e4b708028c57d816cf9d2434acb33a549475f78c181f6253"
 
 [[package]]
 name = "gio-sys"
@@ -2630,7 +2752,7 @@ dependencies = [
  "inflections",
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -2705,9 +2827,9 @@ version = "0.2.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "cc11df1ace8e7e564511f53af41f3e42ddc95b56fd07b3f4445d2a6048bc682c"
 dependencies = [
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "gpu-descriptor-types",
- "hashbrown 0.14.1",
+ "hashbrown 0.14.3",
 ]
 
 [[package]]
@@ -2716,7 +2838,7 @@ version = "0.1.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6bf0b36e6f090b7e1d8a4b49c0cb81c1f8376f72198c65dd3ad9ff3556b8b78c"
 dependencies = [
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
 ]
 
 [[package]]
@@ -2759,13 +2881,22 @@ version = "0.1.0"
 source = "git+https://github.com/open-rmf/gz-fuel-rs?branch=first_implementation#44c2e1e074df4499d3c40eedaa386d9d612a9314"
 dependencies = [
  "dirs 5.0.1",
- "futures-lite",
+ "futures-lite 2.0.1",
  "itertools",
  "serde",
  "serde_json",
  "surf",
 ]
 
+[[package]]
+name = "half"
+version = "2.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "02b4af3693f1b705df946e9fe5631932443781d0aabb423b62fcd4d73f6d2fd0"
+dependencies = [
+ "crunchy",
+]
+
 [[package]]
 name = "hashbrown"
 version = "0.12.3"
@@ -2774,9 +2905,9 @@ checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888"
 
 [[package]]
 name = "hashbrown"
-version = "0.14.1"
+version = "0.14.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7dfda62a12f55daeae5015f81b0baea145391cb4520f86c248fc615d72640d12"
+checksum = "290f1a1d9242c78d09ce40a5e87e7554ee637af1351968159f4952f028f75604"
 dependencies = [
  "ahash",
  "allocator-api2",
@@ -2866,9 +2997,9 @@ dependencies = [
 
 [[package]]
 name = "http"
-version = "0.2.9"
+version = "0.2.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bd6effc99afb63425aff9b05836f029929e345a6148a14b7ecd5ab67af944482"
+checksum = "8947b1a6fad4393052c7ba1f4cd97bed3e953a95c79c92ad9b051a04611d9fbb"
 dependencies = [
  "bytes 1.5.0",
  "fnv",
@@ -2901,11 +3032,11 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6e9b187a72d63adbfba487f48095306ac823049cb504ee195541e91c7775f5ad"
 dependencies = [
  "anyhow",
- "async-channel",
+ "async-channel 1.9.0",
  "async-std",
  "base64 0.13.1",
  "cookie",
- "futures-lite",
+ "futures-lite 1.13.0",
  "infer",
  "pin-project-lite",
  "rand 0.7.3",
@@ -2918,9 +3049,9 @@ dependencies = [
 
 [[package]]
 name = "idna"
-version = "0.4.0"
+version = "0.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7d20d6b07bfbc108882d88ed8e37d39636dcc260e15e30c45e6ba089610b917c"
+checksum = "634d9b1461af396cad843f47fdba5597a4f9e6ddd4bfb6ff5d85028c25cb12f6"
 dependencies = [
  "unicode-bidi",
  "unicode-normalization",
@@ -2935,10 +3066,13 @@ dependencies = [
  "bytemuck",
  "byteorder",
  "color_quant",
+ "exr",
+ "gif",
  "jpeg-decoder",
  "num-rational",
  "num-traits",
  "png",
+ "qoi",
  "tiff",
 ]
 
@@ -2954,12 +3088,12 @@ dependencies = [
 
 [[package]]
 name = "indexmap"
-version = "2.0.2"
+version = "2.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8adf3ddd720272c6ea8bf59463c04e0f93d0bbf7c5439b691bca2987e0270897"
+checksum = "d530e1a18b1cb4c484e6e34556a0d948706958449fca0cab753d649f2bce3d1f"
 dependencies = [
  "equivalent",
- "hashbrown 0.14.1",
+ "hashbrown 0.14.3",
 ]
 
 [[package]]
@@ -2985,6 +3119,17 @@ dependencies = [
  "libc",
 ]
 
+[[package]]
+name = "inotify"
+version = "0.10.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "fdd168d97690d0b8c412d6b6c10360277f4d7ee495c5d0d5d5fe0854923255cc"
+dependencies = [
+ "bitflags 1.3.2",
+ "inotify-sys",
+ "libc",
+]
+
 [[package]]
 name = "inotify-sys"
 version = "0.1.5"
@@ -3014,11 +3159,11 @@ checksum = "d3b7357d2bbc5ee92f8e899ab645233e43d21407573cceb37fed8bc3dede2c02"
 
 [[package]]
 name = "io-kit-sys"
-version = "0.3.0"
+version = "0.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b2d4429acc1deff0fbdece0325b4997bdb02b2c245ab7023fd5deca0f6348de"
+checksum = "4769cb30e5dcf1710fc6730d3e94f78c47723a014a567de385e113c737394640"
 dependencies = [
- "core-foundation-sys 0.8.4",
+ "core-foundation-sys",
  "mach2",
 ]
 
@@ -3043,8 +3188,8 @@ dependencies = [
  "crossbeam-utils",
  "curl",
  "curl-sys",
- "flume",
- "futures-lite",
+ "flume 0.9.2",
+ "futures-lite 1.13.0",
  "http",
  "log",
  "once_cell",
@@ -3058,9 +3203,9 @@ dependencies = [
 
 [[package]]
 name = "itertools"
-version = "0.11.0"
+version = "0.12.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57"
+checksum = "25db6b064527c5d482d0423354fcd07a89a2dfe07b67892e62411946db7f07b0"
 dependencies = [
  "either",
 ]
@@ -3123,9 +3268,9 @@ checksum = "8eaf4bc02d17cbdd7ff4c7438cafcdf7fb9a4613313ad11b4f8fefe7d3fa0130"
 
 [[package]]
 name = "jobserver"
-version = "0.1.26"
+version = "0.1.27"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "936cfd212a0155903bcbc060e316fb6cc7cbf2e1907329391ebadc1fe0ce77c2"
+checksum = "8c37f63953c4c63420ed5fd3d6d398c719489b9f872b9fa683262f8edd363c7d"
 dependencies = [
  "libc",
 ]
@@ -3135,6 +3280,9 @@ name = "jpeg-decoder"
 version = "0.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "bc0000e42512c92e31c2252315bda326620a4e034105e900c98ec492fa077b3e"
+dependencies = [
+ "rayon",
+]
 
 [[package]]
 name = "js-sys"
@@ -3206,6 +3354,12 @@ version = "1.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55"
 
+[[package]]
+name = "lebe"
+version = "0.5.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "03087c2bad5e1034e8cace5926dec053fb3790248370865f5117a7d0213354c8"
+
 [[package]]
 name = "lewton"
 version = "0.10.2"
@@ -3219,9 +3373,9 @@ dependencies = [
 
 [[package]]
 name = "libc"
-version = "0.2.148"
+version = "0.2.150"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9cdc71e17332e86d2e1d38c1f99edcb6288ee11b815fb1a4b049eaa2114d369b"
+checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c"
 
 [[package]]
 name = "libloading"
@@ -3245,9 +3399,9 @@ dependencies = [
 
 [[package]]
 name = "libm"
-version = "0.2.7"
+version = "0.2.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f7012b1bbb0719e1097c47611d3898568c546d597c2e74d66f6087edd5233ff4"
+checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058"
 
 [[package]]
 name = "libnghttp2-sys"
@@ -3259,6 +3413,28 @@ dependencies = [
  "libc",
 ]
 
+[[package]]
+name = "libredox"
+version = "0.0.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "85c833ca1e66078851dba29046874e38f08b2c883700aa29a03ddd3b23814ee8"
+dependencies = [
+ "bitflags 2.4.1",
+ "libc",
+ "redox_syscall 0.4.1",
+]
+
+[[package]]
+name = "libredox"
+version = "0.0.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3af92c55d7d839293953fcd0fda5ecfe93297cfde6ffbdec13b41d99c0ba6607"
+dependencies = [
+ "bitflags 2.4.1",
+ "libc",
+ "redox_syscall 0.4.1",
+]
+
 [[package]]
 name = "libudev-sys"
 version = "0.1.4"
@@ -3293,11 +3469,17 @@ version = "0.3.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ef53942eb7bf7ff43a617b3e2c1c4a5ecf5944a7c1bc12d7ee39bbb15e5c1519"
 
+[[package]]
+name = "linux-raw-sys"
+version = "0.4.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "969488b55f8ac402214f3f5fd243ebb7206cf82de60d3172994707a4bcc2b829"
+
 [[package]]
 name = "lock_api"
-version = "0.4.10"
+version = "0.4.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1cc9717a20b1bb222f333e6a92fd32f7d8a18ddc5a3191a11af45dcbf4dcd16"
+checksum = "3c168f8615b12bc01f9c17e2eb0cc07dcae1940121185446edc3744920e8ef45"
 dependencies = [
  "autocfg",
  "scopeguard",
@@ -3324,9 +3506,9 @@ dependencies = [
 
 [[package]]
 name = "lyon_algorithms"
-version = "1.0.3"
+version = "1.0.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "00a0349cd8f0270781bb93a824b63df6178e3b4a27794e7be3ce3763f5a44d6e"
+checksum = "a3bca95f9a4955b3e4a821fbbcd5edfbd9be2a9a50bb5758173e5358bfb4c623"
 dependencies = [
  "lyon_path",
  "num-traits",
@@ -3355,9 +3537,9 @@ dependencies = [
 
 [[package]]
 name = "lyon_tessellation"
-version = "1.0.10"
+version = "1.0.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7d2124218d5428149f9e09520b9acc024334a607e671f032d06567b61008977c"
+checksum = "23bcac20d47825850fabf1e869bf7c2bbe2daefa0776c3cd2eb7cb74635f6e4a"
 dependencies = [
  "float_next_after",
  "lyon_path",
@@ -3409,9 +3591,9 @@ checksum = "f665ee40bc4a3c5590afb1e9677db74a508659dfd71e126420da8274909a0167"
 
 [[package]]
 name = "memoffset"
-version = "0.6.5"
+version = "0.7.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5aa361d4faea93603064a027415f07bd8e1d5c88c9fbf68bf56a285428fd79ce"
+checksum = "5de893c32cde5f383baa4c04c5d6dbdd735cfd4a794b0debdb2bb1b421da5ff4"
 dependencies = [
  "autocfg",
 ]
@@ -3455,15 +3637,6 @@ dependencies = [
  "unicase",
 ]
 
-[[package]]
-name = "minidom"
-version = "0.15.2"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f45614075738ce1b77a1768912a60c0227525971b03e09122a05b8a34a2a6278"
-dependencies = [
- "rxml",
-]
-
 [[package]]
 name = "minimal-lexical"
 version = "0.2.1"
@@ -3482,9 +3655,9 @@ dependencies = [
 
 [[package]]
 name = "mio"
-version = "0.8.8"
+version = "0.8.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "927a765cd3fc26206e66b296465fa9d3e5ab003e651c1b3c060e7956d96b19d2"
+checksum = "3dce281c5e46beae905d4de1870d8b1509a9142b62eedf18b443b011ca8343d0"
 dependencies = [
  "libc",
  "log",
@@ -3533,22 +3706,6 @@ dependencies = [
  "unicode-ident",
 ]
 
-[[package]]
-name = "nalgebra"
-version = "0.31.4"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "20bd243ab3dbb395b39ee730402d2e5405e448c75133ec49cc977762c4cba3d1"
-dependencies = [
- "approx",
- "matrixmultiply",
- "nalgebra-macros 0.1.0",
- "num-complex",
- "num-rational",
- "num-traits",
- "simba 0.7.3",
- "typenum",
-]
-
 [[package]]
 name = "nalgebra"
 version = "0.32.3"
@@ -3558,25 +3715,14 @@ dependencies = [
  "approx",
  "glam",
  "matrixmultiply",
- "nalgebra-macros 0.2.1",
+ "nalgebra-macros",
  "num-complex",
  "num-rational",
  "num-traits",
- "simba 0.8.1",
+ "simba",
  "typenum",
 ]
 
-[[package]]
-name = "nalgebra-macros"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "01fcc0b8149b4632adc89ac3b7b31a12fb6099a0317a4eb2ebff574ef7de7218"
-dependencies = [
- "proc-macro2",
- "quote",
- "syn 1.0.109",
-]
-
 [[package]]
 name = "nalgebra-macros"
 version = "0.2.1"
@@ -3626,7 +3772,6 @@ dependencies = [
  "bitflags 1.3.2",
  "cfg-if",
  "libc",
- "memoffset 0.6.5",
 ]
 
 [[package]]
@@ -3638,6 +3783,18 @@ dependencies = [
  "bitflags 1.3.2",
  "cfg-if",
  "libc",
+ "memoffset 0.7.1",
+]
+
+[[package]]
+name = "nix"
+version = "0.27.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "2eb04e9c688eff1c89d72b407f168cf79bb9e867a9d3323ed6c01519eb9cc053"
+dependencies = [
+ "bitflags 2.4.1",
+ "cfg-if",
+ "libc",
 ]
 
 [[package]]
@@ -3662,11 +3819,11 @@ version = "6.1.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6205bd8bb1e454ad2e27422015fb5e4f2bcc7e08fa8f27058670d208324a4d2d"
 dependencies = [
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "crossbeam-channel",
  "filetime",
  "fsevent-sys",
- "inotify",
+ "inotify 0.9.6",
  "kqueue",
  "libc",
  "log",
@@ -3737,9 +3894,9 @@ dependencies = [
 
 [[package]]
 name = "num-traits"
-version = "0.2.16"
+version = "0.2.17"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f30b0abd723be7e2ffca1272140fac1a2f084c77ec3e123c192b66af1ee9e6c2"
+checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c"
 dependencies = [
  "autocfg",
  "libm",
@@ -3784,7 +3941,7 @@ dependencies = [
  "proc-macro-crate",
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -3913,9 +4070,9 @@ checksum = "ff011a302c396a5197692431fc1948019154afc178baf7d8e37367442a4601cf"
 
 [[package]]
 name = "openssl-sys"
-version = "0.9.93"
+version = "0.9.96"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "db4d56a4c0478783083cfafcc42493dd4a981d41669da64b4572a2a089b51b1d"
+checksum = "3812c071ba60da8b5677cc12bcb1d42989a65553772897a7e0355545a819838f"
 dependencies = [
  "cc",
  "libc",
@@ -3929,19 +4086,13 @@ version = "0.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d"
 
-[[package]]
-name = "optional"
-version = "0.5.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "978aa494585d3ca4ad74929863093e87cac9790d81fe7aba2b3dc2890643a0fc"
-
 [[package]]
 name = "orbclient"
-version = "0.3.46"
+version = "0.3.47"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8378ac0dfbd4e7895f2d2c1f1345cab3836910baf3a300b000d04250f0c8428f"
+checksum = "52f0d54bde9774d3a51dcf281a5def240c71996bc6ca05d2c847ec8b2b216166"
 dependencies = [
- "redox_syscall 0.3.5",
+ "libredox 0.0.2",
 ]
 
 [[package]]
@@ -3952,9 +4103,9 @@ checksum = "b15813163c1d831bf4a13c3610c05c0d03b39feb07f7e09fa234dac9b15aaf39"
 
 [[package]]
 name = "owned_ttf_parser"
-version = "0.19.0"
+version = "0.20.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "706de7e2214113d63a8238d1910463cfce781129a6f263d13fdb09ff64355ba4"
+checksum = "d4586edfe4c648c71797a74c84bacb32b52b212eff5dfe2bb9f2c599844023e7"
 dependencies = [
  "ttf-parser",
 ]
@@ -3973,9 +4124,9 @@ dependencies = [
 
 [[package]]
 name = "parking"
-version = "2.1.1"
+version = "2.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e52c774a4c39359c1d1c52e43f73dd91a75a614652c825408eec30c95a9b2067"
+checksum = "bb813b8af86854136c6922af0598d719255ecb2179515e6e7730d468f05c9cae"
 
 [[package]]
 name = "parking_lot"
@@ -3989,13 +4140,13 @@ dependencies = [
 
 [[package]]
 name = "parking_lot_core"
-version = "0.9.8"
+version = "0.9.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "93f00c865fe7cabf650081affecd3871070f26767e7b2070a3ffae14c654b447"
+checksum = "4c42a9226546d68acdd9c0a280d17ce19bfe27a46bf68784e4066115788d008e"
 dependencies = [
  "cfg-if",
  "libc",
- "redox_syscall 0.3.5",
+ "redox_syscall 0.4.1",
  "smallvec",
  "windows-targets 0.48.5",
 ]
@@ -4011,11 +4162,11 @@ dependencies = [
  "bitflags 1.3.2",
  "downcast-rs",
  "either",
- "nalgebra 0.32.3",
+ "nalgebra",
  "num-derive",
  "num-traits",
  "rustc-hash",
- "simba 0.8.1",
+ "simba",
  "slab",
  "smallvec",
  "spade",
@@ -4041,9 +4192,9 @@ checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099"
 
 [[package]]
 name = "percent-encoding"
-version = "2.3.0"
+version = "2.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b2a4787296e9989611394c33f193f676704af1686e70b8f8033ab5ba9a35a94"
+checksum = "e3148f5046208a5d56bcfc03053e3ca6334e51da8dfb19b6cdc8b306fae3283e"
 
 [[package]]
 name = "petgraph"
@@ -4052,7 +4203,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "e1d3afd2628e69da2be385eb6f2fd57c8ac7977ceeff6dc166ff1657b0e386a9"
 dependencies = [
  "fixedbitset",
- "indexmap 2.0.2",
+ "indexmap 2.1.0",
 ]
 
 [[package]]
@@ -4072,7 +4223,7 @@ checksum = "4359fd9c9171ec6e8c62926d6faaf553a8dc3f64e1507e76da7911b4f6a04405"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -4133,6 +4284,20 @@ dependencies = [
  "windows-sys 0.48.0",
 ]
 
+[[package]]
+name = "polling"
+version = "3.3.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cf63fa624ab313c11656b4cda960bfc46c410187ad493c41f6ba2d8c1e991c9e"
+dependencies = [
+ "cfg-if",
+ "concurrent-queue",
+ "pin-project-lite",
+ "rustix 0.38.25",
+ "tracing",
+ "windows-sys 0.52.0",
+]
+
 [[package]]
 name = "polyval"
 version = "0.4.5"
@@ -4177,9 +4342,9 @@ checksum = "dc375e1527247fe1a97d8b7156678dfe7c1af2fc075c9a4db3690ecd2a148068"
 
 [[package]]
 name = "proc-macro2"
-version = "1.0.67"
+version = "1.0.70"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3d433d9f1a3e8c1263d9456598b16fec66f4acc9a74dacffd35c7bb09b3a1328"
+checksum = "39278fbbf5fb4f646ce651690877f89d1c5811a3d4acb27700c1cb3cdb78fd3b"
 dependencies = [
  "unicode-ident",
 ]
@@ -4190,6 +4355,15 @@ version = "1.0.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "f89dff0959d98c9758c88826cc002e2c3d0b9dfac4139711d1f30de442f1139b"
 
+[[package]]
+name = "qoi"
+version = "0.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "7f6d64c71eb498fe9eae14ce4ec935c555749aef511cca85b5568910d6e48001"
+dependencies = [
+ "bytemuck",
+]
+
 [[package]]
 name = "quote"
 version = "1.0.33"
@@ -4264,7 +4438,7 @@ version = "0.6.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
 dependencies = [
- "getrandom 0.2.10",
+ "getrandom 0.2.11",
 ]
 
 [[package]]
@@ -4294,12 +4468,12 @@ dependencies = [
  "bitflags 1.3.2",
  "crossbeam",
  "downcast-rs",
- "nalgebra 0.32.3",
+ "nalgebra",
  "num-derive",
  "num-traits",
  "parry3d",
  "rustc-hash",
- "simba 0.8.1",
+ "simba",
 ]
 
 [[package]]
@@ -4314,6 +4488,26 @@ version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
 
+[[package]]
+name = "rayon"
+version = "1.8.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9c27db03db7734835b3f53954b534c91069375ce6ccaa2e065441e07d9b6cdb1"
+dependencies = [
+ "either",
+ "rayon-core",
+]
+
+[[package]]
+name = "rayon-core"
+version = "1.12.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "5ce3fb6ad83f861aac485e76e1985cd109d9a3713802152be56c3b1f0e0658ed"
+dependencies = [
+ "crossbeam-deque",
+ "crossbeam-utils",
+]
+
 [[package]]
 name = "rectangle-pack"
 version = "0.4.2"
@@ -4322,43 +4516,43 @@ checksum = "a0d463f2884048e7153449a55166f91028d5b0ea53c79377099ce4e8cf0cf9bb"
 
 [[package]]
 name = "redox_syscall"
-version = "0.2.16"
+version = "0.3.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fb5a58c1855b4b6819d59012155603f0b22ad30cad752600aadfcb695265519a"
+checksum = "567664f262709473930a4bf9e51bf2ebf3348f2e748ccc50dea20646858f8f29"
 dependencies = [
  "bitflags 1.3.2",
 ]
 
 [[package]]
 name = "redox_syscall"
-version = "0.3.5"
+version = "0.4.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "567664f262709473930a4bf9e51bf2ebf3348f2e748ccc50dea20646858f8f29"
+checksum = "4722d768eff46b75989dd134e5c353f0d6296e5aaa3132e776cbdb56be7731aa"
 dependencies = [
  "bitflags 1.3.2",
 ]
 
 [[package]]
 name = "redox_users"
-version = "0.4.3"
+version = "0.4.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b033d837a7cf162d7993aded9304e30a83213c648b6e389db233191f891e5c2b"
+checksum = "a18479200779601e498ada4e8c1e1f50e3ee19deb0259c25825a98b5603b2cb4"
 dependencies = [
- "getrandom 0.2.10",
- "redox_syscall 0.2.16",
+ "getrandom 0.2.11",
+ "libredox 0.0.1",
  "thiserror",
 ]
 
 [[package]]
 name = "regex"
-version = "1.9.6"
+version = "1.10.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ebee201405406dbf528b8b672104ae6d6d63e6d118cb10e4d51abbc7b58044ff"
+checksum = "380b951a9c5e80ddfd6136919eef32310721aa4aacd4889a8d39124b026ab343"
 dependencies = [
  "aho-corasick",
  "memchr",
- "regex-automata 0.3.9",
- "regex-syntax 0.7.5",
+ "regex-automata 0.4.3",
+ "regex-syntax 0.8.2",
 ]
 
 [[package]]
@@ -4372,13 +4566,13 @@ dependencies = [
 
 [[package]]
 name = "regex-automata"
-version = "0.3.9"
+version = "0.4.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "59b23e92ee4318893fa3fe3e6fb365258efbfe6ac6ab30f090cdcbb7aa37efa9"
+checksum = "5f804c7828047e88b2d32e2d7fe5a105da8ee3264f01902f796c8e067dc2483f"
 dependencies = [
  "aho-corasick",
  "memchr",
- "regex-syntax 0.7.5",
+ "regex-syntax 0.8.2",
 ]
 
 [[package]]
@@ -4389,9 +4583,9 @@ checksum = "f162c6dd7b008981e4d40210aca20b4bd0f9b60ca9271061b07f78537722f2e1"
 
 [[package]]
 name = "regex-syntax"
-version = "0.7.5"
+version = "0.8.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dbb5fb1acd8a1a18b3dd5be62d25485eb770e05afb408a9627d14d451bae12da"
+checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f"
 
 [[package]]
 name = "renderdoc-sys"
@@ -4428,6 +4622,7 @@ version = "0.0.1"
 dependencies = [
  "bevy",
  "bevy_egui",
+ "bevy_gltf_export",
  "bevy_infinite_grid",
  "bevy_mod_outline",
  "bevy_mod_raycast",
@@ -4441,7 +4636,7 @@ dependencies = [
  "console_error_panic_hook",
  "crossbeam-channel",
  "dirs 4.0.0",
- "futures-lite",
+ "futures-lite 1.13.0",
  "gz-fuel",
  "itertools",
  "lyon",
@@ -4461,6 +4656,7 @@ dependencies = [
  "urdf-rs",
  "utm",
  "wasm-bindgen",
+ "yaserde",
 ]
 
 [[package]]
@@ -4471,25 +4667,27 @@ dependencies = [
  "glam",
  "pathdiff",
  "ron",
+ "sdformat_rs",
  "serde",
  "serde_json",
  "serde_yaml",
  "thiserror",
  "urdf-rs",
  "uuid",
+ "yaserde",
 ]
 
 [[package]]
 name = "robust"
-version = "0.2.3"
+version = "1.1.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e5864e7ef1a6b7bcf1d6ca3f655e65e724ed3b52546a0d0a663c991522f552ea"
+checksum = "cbf4a6aa5f6d6888f39e980649f3ad6b666acdce1d78e95b8a2cb076e687ae30"
 
 [[package]]
 name = "rodio"
-version = "0.17.1"
+version = "0.17.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bdf1d4dea18dff2e9eb6dca123724f8b60ef44ad74a9ad283cdfe025df7e73fa"
+checksum = "3b1bb7b48ee48471f55da122c0044fcc7600cfcc85db88240b89cb832935e611"
 dependencies = [
  "cpal",
  "lewton",
@@ -4501,8 +4699,8 @@ version = "0.8.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b91f7eff05f748767f183df4320a63d6936e9c6107d97c9e6bdd9784f4289c94"
 dependencies = [
- "base64 0.21.4",
- "bitflags 2.4.0",
+ "base64 0.21.5",
+ "bitflags 2.4.1",
  "serde",
  "serde_derive",
 ]
@@ -4530,15 +4728,28 @@ dependencies = [
 
 [[package]]
 name = "rustix"
-version = "0.37.24"
+version = "0.37.27"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4279d76516df406a8bd37e7dff53fd37d1a093f997a3c34a5c21658c126db06d"
+checksum = "fea8ca367a3a01fe35e6943c400addf443c0f57670e6ec51196f71a4b8762dd2"
 dependencies = [
  "bitflags 1.3.2",
  "errno",
  "io-lifetimes",
  "libc",
- "linux-raw-sys",
+ "linux-raw-sys 0.3.8",
+ "windows-sys 0.48.0",
+]
+
+[[package]]
+name = "rustix"
+version = "0.38.25"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dc99bc2d4f1fed22595588a013687477aedf3cdcfb26558c559edb67b4d9b22e"
+dependencies = [
+ "bitflags 2.4.1",
+ "errno",
+ "libc",
+ "linux-raw-sys 0.4.11",
  "windows-sys 0.48.0",
 ]
 
@@ -4553,23 +4764,6 @@ dependencies = [
  "twox-hash",
 ]
 
-[[package]]
-name = "rxml"
-version = "0.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a98f186c7a2f3abbffb802984b7f1dfd65dac8be1aafdaabbca4137f53f0dff7"
-dependencies = [
- "bytes 1.5.0",
- "rxml_validation",
- "smartstring",
-]
-
-[[package]]
-name = "rxml_validation"
-version = "0.9.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "22a197350ece202f19a166d1ad6d9d6de145e1d2a8ef47db299abe164dbd7530"
-
 [[package]]
 name = "ryu"
 version = "1.0.15"
@@ -4612,11 +4806,10 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
 [[package]]
 name = "sdformat_rs"
 version = "0.1.0"
-source = "git+https://github.com/open-rmf/sdf_rust_experimental?rev=f86344f#f86344f95adedf9c330aa92931015041135e6594"
+source = "git+https://github.com/open-rmf/sdf_rust_experimental?branch=luca/serde_plugin#9ab9e0da9dfd84114dba7c2e6ca9db4b3f0ad08c"
 dependencies = [
  "convert_case",
- "minidom",
- "nalgebra 0.31.4",
+ "nalgebra",
  "xmltree",
  "yaserde",
  "yaserde_derive",
@@ -4639,29 +4832,29 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
 
 [[package]]
 name = "serde"
-version = "1.0.188"
+version = "1.0.193"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cf9e0fcba69a370eed61bcf2b728575f726b50b55cba78064753d708ddc7549e"
+checksum = "25dd9975e68d0cb5aa1120c288333fc98731bd1dd12f561e468ea4728c042b89"
 dependencies = [
  "serde_derive",
 ]
 
 [[package]]
 name = "serde_derive"
-version = "1.0.188"
+version = "1.0.193"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4eca7ac642d82aa35b60049a6eccb4be6be75e599bd2e9adb5f875a737654af2"
+checksum = "43576ca501357b9b071ac53cdc7da8ef0cbd9493d8df094cd821777ea6e894d3"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
 name = "serde_json"
-version = "1.0.107"
+version = "1.0.108"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6b420ce6e3d8bd882e9b243c6eed35dbc9a6110c9769e74b584e0d68d1f20c65"
+checksum = "3d1c7e3eac408d115102c4c24ad393e0821bb3a5df4d506a80f85f7a742a526b"
 dependencies = [
  "itoa",
  "ryu",
@@ -4681,9 +4874,9 @@ dependencies = [
 
 [[package]]
 name = "serde_spanned"
-version = "0.6.3"
+version = "0.6.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "96426c9936fd7a0124915f9185ea1d20aa9445cc9821142f0a73bc9207a2e186"
+checksum = "12022b835073e5b11e90a14f86838ceb1c8fb0325b72416845c487ac0fa95e80"
 dependencies = [
  "serde",
 ]
@@ -4755,19 +4948,6 @@ version = "1.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a7cee0529a6d40f580e7a5e6c495c8fbfe21b7b52795ed4bb5e62cdf92bc6380"
 
-[[package]]
-name = "simba"
-version = "0.7.3"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2f3fd720c48c53cace224ae62bef1bbff363a70c68c4802a78b5cc6159618176"
-dependencies = [
- "approx",
- "num-complex",
- "num-traits",
- "paste",
- "wide",
-]
-
 [[package]]
 name = "simba"
 version = "0.8.1"
@@ -4811,31 +4991,20 @@ version = "0.5.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6d7400c0eff44aa2fcb5e31a5f24ba9716ed90138769e4977a2ba6014ae63eb5"
 dependencies = [
- "async-channel",
+ "async-channel 1.9.0",
  "futures-core",
  "futures-io",
 ]
 
 [[package]]
 name = "smallvec"
-version = "1.11.1"
+version = "1.11.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "942b4a808e05215192e39f4ab80813e599068285906cc91aa64f923db842bd5a"
+checksum = "4dccd0940a2dcdf68d092b8cbab7dc0ad8fa938bf95787e1b916b0e3d0e8e970"
 dependencies = [
  "serde",
 ]
 
-[[package]]
-name = "smartstring"
-version = "1.0.1"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3fb72c633efbaa2dd666986505016c32c3044395ceaf881518399d2f4127ee29"
-dependencies = [
- "autocfg",
- "static_assertions",
- "version_check",
-]
-
 [[package]]
 name = "smol_str"
 version = "0.2.0"
@@ -4847,9 +5016,9 @@ dependencies = [
 
 [[package]]
 name = "socket2"
-version = "0.4.9"
+version = "0.4.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "64a4a911eed85daf18834cfaa86a79b7d266ff93ff5ba14005426219480ed662"
+checksum = "9f7916fc008ca5542385b89a3d3ce689953c143e9304a9bf8beec1de48994c0d"
 dependencies = [
  "libc",
  "winapi",
@@ -4857,16 +5026,25 @@ dependencies = [
 
 [[package]]
 name = "spade"
-version = "2.2.0"
+version = "2.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "88e65803986868d2372c582007c39ba89936a36ea5f236bf7a7728dc258f04f9"
+checksum = "c3d3bf265ec2d5dd1ddf87863252123447c550491adba2c70c574173a95cd8ba"
 dependencies = [
+ "hashbrown 0.14.3",
  "num-traits",
- "optional",
  "robust",
  "smallvec",
 ]
 
+[[package]]
+name = "spin"
+version = "0.9.8"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67"
+dependencies = [
+ "lock_api",
+]
+
 [[package]]
 name = "spinning_top"
 version = "0.2.5"
@@ -4989,7 +5167,7 @@ dependencies = [
  "cfg-if",
  "encoding_rs",
  "futures-util",
- "getrandom 0.2.10",
+ "getrandom 0.2.11",
  "http-client",
  "http-types",
  "log",
@@ -5020,9 +5198,9 @@ dependencies = [
 
 [[package]]
 name = "syn"
-version = "2.0.37"
+version = "2.0.39"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7303ef2c05cd654186cb250d29049a24840ca25d2747c25c0381c8d9e2f582e8"
+checksum = "23e78b90f2fcf45d3e842032ce32e3f2d1545ba6636271dcbf24fa306d87be7a"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -5031,12 +5209,12 @@ dependencies = [
 
 [[package]]
 name = "sysinfo"
-version = "0.29.10"
+version = "0.29.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0a18d114d420ada3a891e6bc8e96a2023402203296a47cdd65083377dad18ba5"
+checksum = "cd727fc423c2060f6c92d9534cef765c65a6ed3f428a03d7def74a8c4348e666"
 dependencies = [
  "cfg-if",
- "core-foundation-sys 0.8.4",
+ "core-foundation-sys",
  "libc",
  "ntapi",
  "once_cell",
@@ -5045,9 +5223,9 @@ dependencies = [
 
 [[package]]
 name = "system-deps"
-version = "6.1.2"
+version = "6.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "94af52f9402f94aac4948a2518b43359be8d9ce6cd9efc1c4de3b2f7b7e897d6"
+checksum = "2a2d580ff6a20c55dfb86be5f9c238f67835d0e81cbdea8bf5680e0897320331"
 dependencies = [
  "cfg-expr",
  "heck 0.4.1",
@@ -5058,9 +5236,9 @@ dependencies = [
 
 [[package]]
 name = "taffy"
-version = "0.3.15"
+version = "0.3.18"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "798053135b826949571726ab5e30bf9509a65b3bae6425d2e2b2dd391c50a101"
+checksum = "3c2287b6d7f721ada4cddf61ade5e760b2c6207df041cac9bfaa192897362fd3"
 dependencies = [
  "arrayvec",
  "grid",
@@ -5070,57 +5248,57 @@ dependencies = [
 
 [[package]]
 name = "target-lexicon"
-version = "0.12.11"
+version = "0.12.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9d0e916b1148c8e263850e1ebcbd046f333e0683c724876bb0da63ea4373dc8a"
+checksum = "14c39fd04924ca3a864207c66fc2cd7d22d7c016007f9ce846cbb9326331930a"
 
 [[package]]
 name = "termcolor"
-version = "1.3.0"
+version = "1.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6093bad37da69aab9d123a8091e4be0aa4a03e4d601ec641c327398315f62b64"
+checksum = "ff1bc3d3f05aff0403e8ac0d92ced918ec05b666a43f83297ccef5bea8a3d449"
 dependencies = [
  "winapi-util",
 ]
 
 [[package]]
 name = "thiserror"
-version = "1.0.49"
+version = "1.0.50"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1177e8c6d7ede7afde3585fd2513e611227efd6481bd78d2e82ba1ce16557ed4"
+checksum = "f9a7210f5c9a7156bb50aa36aed4c95afb51df0df00713949448cf9e97d382d2"
 dependencies = [
  "thiserror-impl",
 ]
 
 [[package]]
 name = "thiserror-core"
-version = "1.0.38"
+version = "1.0.50"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0d97345f6437bb2004cd58819d8a9ef8e36cdd7661c2abc4bbde0a7c40d9f497"
+checksum = "c001ee18b7e5e3f62cbf58c7fe220119e68d902bb7443179c0c8aef30090e999"
 dependencies = [
  "thiserror-core-impl",
 ]
 
 [[package]]
 name = "thiserror-core-impl"
-version = "1.0.38"
+version = "1.0.50"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "10ac1c5050e43014d16b2f94d0d2ce79e65ffdd8b38d8048f9c8f6a8a6da62ac"
+checksum = "e4c60d69f36615a077cc7663b9cb8e42275722d23e58a7fa3d2c7f2915d09d04"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 1.0.109",
+ "syn 2.0.39",
 ]
 
 [[package]]
 name = "thiserror-impl"
-version = "1.0.49"
+version = "1.0.50"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "10712f02019e9288794769fba95cd6847df9874d49d871d062172f9dd41bc4cc"
+checksum = "266b2e40bc00e5a6c09c3584011e08b06f123c00362c92b975ba9843aaaa14b8"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -5209,21 +5387,21 @@ dependencies = [
 
 [[package]]
 name = "toml"
-version = "0.8.2"
+version = "0.8.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "185d8ab0dfbb35cf1399a6344d8484209c088f75f8f68230da55d48d95d43e3d"
+checksum = "a1a195ec8c9da26928f773888e0742ca3ca1040c6cd859c919c9f59c1954ab35"
 dependencies = [
  "serde",
  "serde_spanned",
  "toml_datetime",
- "toml_edit 0.20.2",
+ "toml_edit 0.21.0",
 ]
 
 [[package]]
 name = "toml_datetime"
-version = "0.6.3"
+version = "0.6.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7cda73e2f1397b1262d6dfdcef8aafae14d1de7748d66822d3bfeeb6d03e5e4b"
+checksum = "3550f4e9685620ac18a50ed434eb3aec30db8ba93b0287467bca5826ea25baf1"
 dependencies = [
  "serde",
 ]
@@ -5234,18 +5412,18 @@ version = "0.19.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "1b5bb770da30e5cbfde35a2d7b9b8a2c4b8ef89548a7a6aeab5c9a576e3e7421"
 dependencies = [
- "indexmap 2.0.2",
+ "indexmap 2.1.0",
  "toml_datetime",
  "winnow",
 ]
 
 [[package]]
 name = "toml_edit"
-version = "0.20.2"
+version = "0.21.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "396e4d48bbb2b7554c944bde63101b5ae446cff6ec4a24227428f15eb72ef338"
+checksum = "d34d383cd00a163b4a5b85053df514d45bc330f6de7737edfe0a93311d1eaa03"
 dependencies = [
- "indexmap 2.0.2",
+ "indexmap 2.1.0",
  "serde",
  "serde_spanned",
  "toml_datetime",
@@ -5254,11 +5432,10 @@ dependencies = [
 
 [[package]]
 name = "tracing"
-version = "0.1.37"
+version = "0.1.40"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8ce8c33a8d48bd45d624a6e523445fd21ec13d3653cd51f681abf67418f54eb8"
+checksum = "c3523ab5a71916ccf420eebdf5521fcef02141234bbc0b8a49f2fdc4544364ef"
 dependencies = [
- "cfg-if",
  "log",
  "pin-project-lite",
  "tracing-attributes",
@@ -5267,20 +5444,20 @@ dependencies = [
 
 [[package]]
 name = "tracing-attributes"
-version = "0.1.26"
+version = "0.1.27"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5f4f31f56159e98206da9efd823404b79b6ef3143b4a7ab76e67b1751b25a4ab"
+checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.37",
+ "syn 2.0.39",
 ]
 
 [[package]]
 name = "tracing-core"
-version = "0.1.31"
+version = "0.1.32"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0955b8137a1df6f1a2e9a37d8a6656291ff0297c1a97c24e0d8425fe2312f79a"
+checksum = "c06d3da6113f116aaee68e4d601191614c9053067f9ab7f6edbcb161237daa54"
 dependencies = [
  "once_cell",
  "valuable",
@@ -5298,20 +5475,31 @@ dependencies = [
 
 [[package]]
 name = "tracing-log"
-version = "0.1.3"
+version = "0.1.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "78ddad33d2d10b1ed7eb9d1f518a5674713876e97e5bb9b7345a7984fbb4f922"
+checksum = "f751112709b4e791d8ce53e32c4ed2d353565a795ce84da2285393f41557bdf2"
+dependencies = [
+ "log",
+ "once_cell",
+ "tracing-core",
+]
+
+[[package]]
+name = "tracing-log"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3"
 dependencies = [
- "lazy_static",
  "log",
+ "once_cell",
  "tracing-core",
 ]
 
 [[package]]
 name = "tracing-subscriber"
-version = "0.3.17"
+version = "0.3.18"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "30a651bc37f915e81f087d86e62a18eec5f79550c7faff886f7090b4ea757c77"
+checksum = "ad0f048c97dbd9faa9b7df56362b8ebcaa52adb06b498c050d2f4e32f90a7a8b"
 dependencies = [
  "matchers",
  "nu-ansi-term",
@@ -5322,7 +5510,7 @@ dependencies = [
  "thread_local",
  "tracing",
  "tracing-core",
- "tracing-log",
+ "tracing-log 0.2.0",
 ]
 
 [[package]]
@@ -5338,9 +5526,9 @@ dependencies = [
 
 [[package]]
 name = "ttf-parser"
-version = "0.19.2"
+version = "0.20.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "49d64318d8311fc2668e48b63969f4343e0a85c4a109aa8460d6672e364b8bd1"
+checksum = "17f77d76d837a7830fe1d4f12b7b4ba4192c1888001c7164257e4bc6d21d96b4"
 
 [[package]]
 name = "twox-hash"
@@ -5431,9 +5619,9 @@ dependencies = [
 
 [[package]]
 name = "url"
-version = "2.4.1"
+version = "2.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "143b538f18257fac9cad154828a57c6bf5157e1aa604d4816b5995bf6de87ae5"
+checksum = "31e6302e3bb753d46e83516cae55ae196fc0c309407cf11ab35cc51a4c2a4633"
 dependencies = [
  "form_urlencoded",
  "idna",
@@ -5455,11 +5643,11 @@ checksum = "95b09e3b3a0abd1ccb77673a6b7b8875d9d1c80626154add451cf18392dc4c3c"
 
 [[package]]
 name = "uuid"
-version = "1.4.1"
+version = "1.6.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "79daa5ed5740825c40b389c5e50312b9c86df53fccd33f281df655642b43869d"
+checksum = "5e395fcf16a7a3d8127ec99782007af141946b4795001f876d54fb0d55978560"
 dependencies = [
- "getrandom 0.2.10",
+ "getrandom 0.2.11",
  "serde",
 ]
 
@@ -5471,9 +5659,9 @@ checksum = "830b7e5d4d90034032940e4ace0d9a9a057e7a45cd94e6c007832e39edb82f6d"
 
 [[package]]
 name = "value-bag"
-version = "1.4.1"
+version = "1.4.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d92ccd67fb88503048c01b59152a04effd0782d035a83a6d256ce6085f08f4a3"
+checksum = "4a72e1902dde2bd6441347de2b70b7f5d59bf157c6c62f0c44572607a1d55bbe"
 
 [[package]]
 name = "vcpkg"
@@ -5616,9 +5804,9 @@ dependencies = [
 
 [[package]]
 name = "webbrowser"
-version = "0.8.11"
+version = "0.8.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b2c79b77f525a2d670cb40619d7d9c673d09e0666f72c591ebd7861f84a87e57"
+checksum = "82b2391658b02c27719fc5a0a73d6e696285138e8b12fba9d4baa70451023c71"
 dependencies = [
  "core-foundation",
  "home",
@@ -5669,7 +5857,7 @@ checksum = "8f478237b4bf0d5b70a39898a66fa67ca3a007d79f2520485b8b0c3dfc46f8c2"
 dependencies = [
  "arrayvec",
  "bit-vec",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "codespan-reporting",
  "log",
  "naga",
@@ -5694,7 +5882,7 @@ dependencies = [
  "arrayvec",
  "ash",
  "bit-set",
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "block",
  "core-graphics-types",
  "d3d12",
@@ -5732,16 +5920,16 @@ version = "0.16.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d0c153280bb108c2979eb5c7391cb18c56642dd3c072e55f52065e13e2a1252a"
 dependencies = [
- "bitflags 2.4.0",
+ "bitflags 2.4.1",
  "js-sys",
  "web-sys",
 ]
 
 [[package]]
 name = "wide"
-version = "0.7.12"
+version = "0.7.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ebecebefc38ff1860b4bc47550bbfa63af5746061cf0d29fcd7fa63171602598"
+checksum = "c68938b57b33da363195412cfc5fc37c9ed49aa9cfe2156fde64b8d2c9498242"
 dependencies = [
  "bytemuck",
  "safe_arch",
@@ -5881,6 +6069,15 @@ dependencies = [
  "windows-targets 0.48.5",
 ]
 
+[[package]]
+name = "windows-sys"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d"
+dependencies = [
+ "windows-targets 0.52.0",
+]
+
 [[package]]
 name = "windows-targets"
 version = "0.42.2"
@@ -5911,6 +6108,21 @@ dependencies = [
  "windows_x86_64_msvc 0.48.5",
 ]
 
+[[package]]
+name = "windows-targets"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "8a18201040b24831fbb9e4eb208f8892e1f50a37feb53cc7ff887feb8f50e7cd"
+dependencies = [
+ "windows_aarch64_gnullvm 0.52.0",
+ "windows_aarch64_msvc 0.52.0",
+ "windows_i686_gnu 0.52.0",
+ "windows_i686_msvc 0.52.0",
+ "windows_x86_64_gnu 0.52.0",
+ "windows_x86_64_gnullvm 0.52.0",
+ "windows_x86_64_msvc 0.52.0",
+]
+
 [[package]]
 name = "windows_aarch64_gnullvm"
 version = "0.42.2"
@@ -5923,6 +6135,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8"
 
+[[package]]
+name = "windows_aarch64_gnullvm"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cb7764e35d4db8a7921e09562a0304bf2f93e0a51bfccee0bd0bb0b666b015ea"
+
 [[package]]
 name = "windows_aarch64_msvc"
 version = "0.42.2"
@@ -5935,6 +6153,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc"
 
+[[package]]
+name = "windows_aarch64_msvc"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "bbaa0368d4f1d2aaefc55b6fcfee13f41544ddf36801e793edbbfd7d7df075ef"
+
 [[package]]
 name = "windows_i686_gnu"
 version = "0.42.2"
@@ -5947,6 +6171,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e"
 
+[[package]]
+name = "windows_i686_gnu"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "a28637cb1fa3560a16915793afb20081aba2c92ee8af57b4d5f28e4b3e7df313"
+
 [[package]]
 name = "windows_i686_msvc"
 version = "0.42.2"
@@ -5959,6 +6189,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406"
 
+[[package]]
+name = "windows_i686_msvc"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ffe5e8e31046ce6230cc7215707b816e339ff4d4d67c65dffa206fd0f7aa7b9a"
+
 [[package]]
 name = "windows_x86_64_gnu"
 version = "0.42.2"
@@ -5971,6 +6207,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e"
 
+[[package]]
+name = "windows_x86_64_gnu"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "3d6fa32db2bc4a2f5abeacf2b69f7992cd09dca97498da74a151a3132c26befd"
+
 [[package]]
 name = "windows_x86_64_gnullvm"
 version = "0.42.2"
@@ -5983,6 +6225,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc"
 
+[[package]]
+name = "windows_x86_64_gnullvm"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1a657e1e9d3f514745a572a6846d3c7aa7dbe1658c056ed9c3344c4109a6949e"
+
 [[package]]
 name = "windows_x86_64_msvc"
 version = "0.42.2"
@@ -5995,6 +6243,12 @@ version = "0.48.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538"
 
+[[package]]
+name = "windows_x86_64_msvc"
+version = "0.52.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dff9641d1cd4be8d1a070daf9e3773c5f67e78b4d9d42263020c057706765c04"
+
 [[package]]
 name = "winit"
 version = "0.28.7"
@@ -6027,9 +6281,9 @@ dependencies = [
 
 [[package]]
 name = "winnow"
-version = "0.5.15"
+version = "0.5.19"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7c2e3184b9c4e92ad5167ca73039d0c42476302ab603e2fec4487511f38ccefc"
+checksum = "829846f3e3db426d4cee4510841b71a8e58aa2a76b1132579487ae430ccd9c7b"
 dependencies = [
  "memchr",
 ]
@@ -6047,12 +6301,12 @@ dependencies = [
 
 [[package]]
 name = "x11rb"
-version = "0.10.1"
+version = "0.12.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "592b4883219f345e712b3209c62654ebda0bb50887f330cbd018d0f654bfd507"
+checksum = "b1641b26d4dec61337c35a1b1aaf9e3cba8f46f0b43636c609ab0291a648040a"
 dependencies = [
  "gethostname",
- "nix 0.24.3",
+ "nix 0.26.4",
  "winapi",
  "winapi-wsapoll",
  "x11rb-protocol",
@@ -6060,11 +6314,11 @@ dependencies = [
 
 [[package]]
 name = "x11rb-protocol"
-version = "0.10.0"
+version = "0.12.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "56b245751c0ac9db0e006dc812031482784e434630205a93c73cfefcaabeac67"
+checksum = "82d6c3f9a0fb6701fab8f6cea9b0c0bd5d6876f1f89f7fada07e558077c344bc"
 dependencies = [
- "nix 0.24.3",
+ "nix 0.26.4",
 ]
 
 [[package]]
@@ -6120,3 +6374,32 @@ dependencies = [
  "syn 1.0.109",
  "xml-rs",
 ]
+
+[[package]]
+name = "zerocopy"
+version = "0.7.26"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e97e415490559a91254a2979b4829267a57d2fcd741a98eee8b722fb57289aa0"
+dependencies = [
+ "zerocopy-derive",
+]
+
+[[package]]
+name = "zerocopy-derive"
+version = "0.7.26"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dd7e48ccf166952882ca8bd778a43502c64f33bf94c12ebe2a7f08e5a0f6689f"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn 2.0.39",
+]
+
+[[package]]
+name = "zune-inflate"
+version = "0.2.54"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "73ab332fe2f6680068f3582b16a24f90ad7096d5d39b974d1c0aff0125116f02"
+dependencies = [
+ "simd-adler32",
+]
diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 2be40461..bee9740b 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -21,6 +21,7 @@ bevy_mod_raycast = "0.13"
 bevy_mod_outline = "0.5"
 bevy_infinite_grid = "0.8"
 bevy_polyline = "0.7"
+bevy_gltf_export = { path = "../../bevy_gltf_export" }
 bevy_stl = "0.11"
 bevy_obj = { version = "0.11", features = ["scene"] }
 bevy_rapier3d = "0.22.0"
@@ -45,8 +46,9 @@ tracing = "0.1.37"
 tracing-subscriber = "0.3.1"
 rfd = "0.12"
 urdf-rs = "0.7"
+yaserde = "0.7"
 utm = "0.1.6"
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "f86344f"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/serde_plugin"}
 gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "first_implementation" }
 pathdiff = "*"
 
diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 8e29696f..1488e8b9 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -56,7 +56,7 @@ use wireframe::*;
 use aabb::AabbUpdatePlugin;
 use animate::AnimationPlugin;
 use interaction::InteractionPlugin;
-use site::{OSMViewPlugin, SitePlugin};
+use site::{OSMViewPlugin, SiteFileMenuPlugin, SitePlugin};
 use site_asset_io::SiteAssetIoPlugin;
 
 pub mod osm_slippy_map;
@@ -219,6 +219,7 @@ impl Plugin for SiteEditor {
                 IssuePlugin,
                 OSMViewPlugin,
                 SiteWireframePlugin,
+                SiteFileMenuPlugin,
             ));
     }
 }
diff --git a/rmf_site_editor/src/site/file_menu.rs b/rmf_site_editor/src/site/file_menu.rs
new file mode 100644
index 00000000..1e7e7800
--- /dev/null
+++ b/rmf_site_editor/src/site/file_menu.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::menu_bar::{FileMenu, MenuEvent, MenuItem, MenuVisualizationStates};
+use crate::{AppState, ExportFormat, SaveWorkspace, SaveWorkspaceDestination};
+use bevy::prelude::*;
+use std::collections::HashSet;
+
+/// Keeps track of which entity is associated to the export sdf button.
+#[derive(Resource)]
+pub struct ExportSdfMenu {
+    export_sdf: Entity,
+}
+
+impl FromWorld for ExportSdfMenu {
+    fn from_world(world: &mut World) -> Self {
+        let site_states = HashSet::from([
+            AppState::SiteEditor,
+            AppState::SiteVisualizer,
+            AppState::SiteDrawingEditor,
+        ]);
+        let file_header = world.resource::<FileMenu>().get();
+        let export_sdf = world
+            .spawn((
+                MenuItem::Text("Export Sdf".to_string()),
+                MenuVisualizationStates(site_states),
+            ))
+            .set_parent(file_header)
+            .id();
+
+        ExportSdfMenu { export_sdf }
+    }
+}
+
+pub fn handle_export_sdf_menu_events(
+    mut commands: Commands,
+    mut menu_events: EventReader<MenuEvent>,
+    sdf_menu: Res<ExportSdfMenu>,
+    mut save_events: EventWriter<SaveWorkspace>,
+) {
+    for event in menu_events.iter() {
+        if event.clicked() && event.source() == sdf_menu.export_sdf {
+            save_events.send(SaveWorkspace {
+                destination: SaveWorkspaceDestination::Dialog,
+                format: ExportFormat::Sdf,
+            });
+        }
+    }
+}
+
+#[derive(Default)]
+pub struct SiteFileMenuPlugin;
+
+impl Plugin for SiteFileMenuPlugin {
+    fn build(&self, app: &mut App) {
+        app.init_resource::<ExportSdfMenu>().add_systems(
+            Update,
+            handle_export_sdf_menu_events.run_if(AppState::in_site_mode()),
+        );
+    }
+}
diff --git a/rmf_site_editor/src/site/floor.rs b/rmf_site_editor/src/site/floor.rs
index 8db208d4..fa038c5d 100644
--- a/rmf_site_editor/src/site/floor.rs
+++ b/rmf_site_editor/src/site/floor.rs
@@ -33,7 +33,7 @@ pub const FLOOR_LAYER_START: f32 = DRAWING_LAYER_START + 0.001;
 
 #[derive(Debug, Clone, Copy, Component)]
 pub struct FloorSegments {
-    mesh: Entity,
+    pub mesh: Entity,
 }
 
 fn make_fallback_floor_mesh(p: Vec3) -> Mesh {
diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs
index c82e308e..bb1779f9 100644
--- a/rmf_site_editor/src/site/mod.rs
+++ b/rmf_site_editor/src/site/mod.rs
@@ -42,6 +42,9 @@ pub use drawing::*;
 pub mod fiducial;
 pub use fiducial::*;
 
+pub mod file_menu;
+pub use file_menu::*;
+
 pub mod floor;
 pub use floor::*;
 
@@ -99,6 +102,9 @@ pub use sdf::*;
 pub mod save;
 pub use save::*;
 
+pub mod sdf_exporter;
+pub use sdf_exporter::*;
+
 pub mod site;
 pub use site::*;
 
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 1006a28c..b05edd52 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -21,17 +21,19 @@ use bevy::{
 };
 use std::{
     collections::{BTreeMap, BTreeSet},
+    io::Write,
     path::PathBuf,
 };
 use thiserror::Error as ThisError;
 
-use crate::{recency::RecencyRanking, site::*};
+use crate::{recency::RecencyRanking, site::*, ExportFormat};
 use rmf_site_format::*;
 
 #[derive(Event)]
 pub struct SaveSite {
     pub site: Entity,
     pub to_file: PathBuf,
+    pub format: ExportFormat,
 }
 
 #[derive(Event)]
@@ -1206,42 +1208,96 @@ pub fn save_site(world: &mut World) {
                 continue;
             }
         };
-        if path_str.ends_with(".building.yaml") {
-            warn!("Detected old file format, converting to new format");
-            new_path = path_str.replace(".building.yaml", ".site.ron").into();
-        } else if !path_str.ends_with("site.ron") {
-            info!("Appending .site.ron to {}", new_path.display());
-            new_path = new_path.with_extension("site.ron");
-        }
-        info!("Saving to {}", new_path.display());
-        let f = match std::fs::File::create(new_path.clone()) {
-            Ok(f) => f,
-            Err(err) => {
-                error!("Unable to save file: {err}");
-                continue;
-            }
-        };
+        match save_event.format {
+            ExportFormat::Default => {
+                if path_str.ends_with(".building.yaml") {
+                    warn!("Detected old file format, converting to new format");
+                    new_path = path_str.replace(".building.yaml", ".site.ron").into();
+                } else if !path_str.ends_with("site.ron") {
+                    info!("Appending .site.ron to {}", new_path.display());
+                    new_path = new_path.with_extension("site.ron");
+                }
+                info!("Saving to {}", new_path.display());
+                let f = match std::fs::File::create(new_path.clone()) {
+                    Ok(f) => f,
+                    Err(err) => {
+                        error!("Unable to save file: {err}");
+                        continue;
+                    }
+                };
 
-        let old_default_path = world.get::<DefaultFile>(save_event.site).cloned();
-        migrate_relative_paths(save_event.site, &new_path, world);
+                let old_default_path = world.get::<DefaultFile>(save_event.site).cloned();
+                migrate_relative_paths(save_event.site, &new_path, world);
 
-        let site = match generate_site(world, save_event.site) {
-            Ok(site) => site,
-            Err(err) => {
-                error!("Unable to compile site: {err}");
-                continue;
-            }
-        };
+                let site = match generate_site(world, save_event.site) {
+                    Ok(site) => site,
+                    Err(err) => {
+                        error!("Unable to compile site: {err}");
+                        continue;
+                    }
+                };
 
-        match site.to_writer(f) {
-            Ok(()) => {
-                info!("Save successful");
+                match site.to_writer(f) {
+                    Ok(()) => {
+                        info!("Save successful");
+                    }
+                    Err(err) => {
+                        if let Some(old_default_path) = old_default_path {
+                            world.entity_mut(save_event.site).insert(old_default_path);
+                        }
+                        error!("Save failed: {err}");
+                    }
+                }
             }
-            Err(err) => {
-                if let Some(old_default_path) = old_default_path {
-                    world.entity_mut(save_event.site).insert(old_default_path);
+            ExportFormat::Sdf => {
+                // TODO(luca) reduce code duplication with default exporting
+                if path_str.ends_with(".building.yaml") {
+                    warn!("Detected old file format, converting to new format");
+                    new_path = path_str.replace(".building.yaml", ".world").into();
+                } else if !path_str.ends_with("site.ron") {
+                    info!("Appending .world to {}", new_path.display());
+                    new_path = new_path.with_extension("world");
                 }
-                error!("Save failed: {err}");
+                info!("Saving to {}", new_path.display());
+                let f = match std::fs::File::create(new_path.clone()) {
+                    Ok(f) => f,
+                    Err(err) => {
+                        error!("Unable to save file: {err}");
+                        continue;
+                    }
+                };
+
+                std::fs::create_dir("meshes").ok();
+                collect_site_meshes(world, save_event.site, "meshes");
+
+                let old_default_path = world.get::<DefaultFile>(save_event.site).cloned();
+                migrate_relative_paths(save_event.site, &new_path, world);
+
+                let site = match generate_site(world, save_event.site) {
+                    Ok(site) => site,
+                    Err(err) => {
+                        error!("Unable to compile site: {err}");
+                        continue;
+                    }
+                };
+                let sdf = match site.to_sdf() {
+                    Ok(sdf) => sdf,
+                    Err(err) => {
+                        //error!("Unable to convert site to sdf: {err}");
+                        error!("Unable to convert site to sdf");
+                        continue;
+                    }
+                };
+                let config = yaserde::ser::Config {
+                    perform_indent: true,
+                    write_document_declaration: true,
+                    ..Default::default()
+                };
+                let s = yaserde::ser::serialize_with_writer(&sdf, f, &config).unwrap();
+            }
+            ExportFormat::Urdf => {
+                warn!("Site exporting to Urdf is not supported.");
+                continue;
             }
         }
     }
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
new file mode 100644
index 00000000..0c00cd21
--- /dev/null
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -0,0 +1,77 @@
+use bevy::ecs::system::SystemState;
+use bevy::prelude::*;
+use bevy_gltf_export::{export_mesh, gltf_to_bytes, GltfPose};
+
+use std::collections::BTreeMap;
+
+use crate::site::FloorSegments;
+use rmf_site_format::{FloorMarker, LevelElevation, Pose, WallMarker};
+
+// Returns a map from level site id to
+//fn collect_wall_meshes(world: &World, site: Entity) -> HashMap<u32,
+
+pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
+    let mut state: SystemState<(
+        Query<&Children>,
+        Query<(Entity, &LevelElevation)>,
+        Query<Entity, With<WallMarker>>,
+        Query<&FloorSegments, With<FloorMarker>>,
+        Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
+    )> = SystemState::new(world);
+    // TODO(luca) level elevation into meshes transform
+    let (q_children, q_levels, q_walls, q_floors, q_pbr) = state.get(world);
+
+    let image_assets = world.resource::<Assets<Image>>();
+    let mesh_assets = world.resource::<Assets<Mesh>>();
+    let material_assets = world.resource::<Assets<StandardMaterial>>();
+    let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
+
+    info!("Looking for children");
+    for site_child in q_children.get(site).unwrap().iter() {
+        if let Ok((level, elevation)) = q_levels.get(*site_child) {
+            info!("Found level {:?}", level);
+            for (i, child) in q_children.get(level).unwrap().iter().enumerate() {
+                let entity = if let Ok(res) = q_walls.get(*child) {
+                    res
+                } else if let Ok(res) = q_floors.get(*child) {
+                    res.mesh
+                } else {
+                    continue;
+                };
+                info!("Found a mesh");
+                // Get mesh and material
+                let Ok((mesh, material)) = q_pbr.get(entity) else {
+                    // This shouldn't happen
+                    warn!("Wall or floor {:?} without a mesh", entity);
+                    continue;
+                };
+                let Some(mesh) = mesh_assets.get(mesh) else {
+                    warn!("Mesh asset not found");
+                    continue;
+                };
+                let Some(material) = material_assets.get(material) else {
+                    warn!("Material asset not found");
+                    continue;
+                };
+
+                let pose = GltfPose {
+                    translation: [0.0, 0.0, **elevation],
+                    ..Default::default()
+                };
+                let (root, bytes) = bevy_gltf_export::export_mesh(
+                    mesh.clone(),
+                    material.clone(),
+                    Some(pose),
+                    image_getter,
+                )
+                .unwrap();
+                // TODO(luca) merge the roots
+                let bytes = gltf_to_bytes(&root, bytes).unwrap();
+                let filename = format!("{}/level_{}_mesh_{}.glb", folder, level.index(), i);
+                info!("Writing to {}", filename);
+                std::fs::write(filename, bytes).unwrap();
+                info!("Wrote mesh");
+            }
+        }
+    }
+}
diff --git a/rmf_site_editor/src/workcell/save.rs b/rmf_site_editor/src/workcell/save.rs
index 30489d64..1beded60 100644
--- a/rmf_site_editor/src/workcell/save.rs
+++ b/rmf_site_editor/src/workcell/save.rs
@@ -267,6 +267,9 @@ pub fn save_workcell(world: &mut World) {
             ExportFormat::Urdf => {
                 info!("Saving to urdf");
             }
+            ExportFormat::Sdf => {
+                warn!("Exporting workcells to sdf is not supported.");
+            }
         }
     }
 }
diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs
index 449046fc..ebb08a9f 100644
--- a/rmf_site_editor/src/workspace.rs
+++ b/rmf_site_editor/src/workspace.rs
@@ -157,6 +157,7 @@ pub enum ExportFormat {
     #[default]
     Default,
     Urdf,
+    Sdf,
 }
 
 /// Event used in channels to communicate the file handle that was chosen by the user.
@@ -456,6 +457,7 @@ fn workspace_file_save_complete(
                 save_site.send(SaveSite {
                     site: result.root,
                     to_file: result.path,
+                    format: result.format,
                 });
             }
             AppState::MainMenu => { /* Noop */ }
diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index b6709986..93621283 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -18,4 +18,6 @@ uuid = { version = "1.1", features = ["v4", "serde"] }
 # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features
 bevy = { version = "0.11", optional = true }
 urdf-rs = "0.7"
+yaserde = "0.7"
 pathdiff = "*"
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/serde_plugin"}
diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs
index 336b96b0..bf0f6fda 100644
--- a/rmf_site_format/src/lib.rs
+++ b/rmf_site_format/src/lib.rs
@@ -98,6 +98,9 @@ pub use point::*;
 pub mod recall;
 pub use recall::*;
 
+pub mod sdf;
+pub use sdf::*;
+
 pub mod semver;
 pub use semver::*;
 
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
new file mode 100644
index 00000000..b4fc3509
--- /dev/null
+++ b/rmf_site_format/src/sdf.rs
@@ -0,0 +1,239 @@
+/*
+ * 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::{Angle, Anchor, Level, AssetSource, Category, NameInSite, Pose, Rotation, Site};
+use sdformat_rs::*;
+use std::collections::{hash_map::Entry, HashMap};
+
+#[derive(Debug)]
+pub enum SdfConversionError {
+    /// An asset that can't be converted to an sdf world was found.
+    UnsupportedAssetType,
+    /// Entity referenced a non existing anchor.
+    BrokenAnchorReference,
+}
+
+impl AssetSource {
+    fn to_sdf(&self) -> Result<String, SdfConversionError> {
+        // TODO(luca) check this function
+        match self {
+            AssetSource::Local(path) => Ok(path.clone()),
+            AssetSource::Remote(name) => Ok(name.clone()),
+            AssetSource::Search(name) => {
+                let name = name
+                    .rsplit("/")
+                    .next()
+                    .ok_or(SdfConversionError::UnsupportedAssetType)?;
+                Ok("model://".to_owned() + name)
+            }
+            AssetSource::Bundled(_) | AssetSource::Package(_) | AssetSource::OSMTile { .. } => {
+                Err(SdfConversionError::UnsupportedAssetType)
+            }
+        }
+    }
+}
+
+impl Pose {
+    fn to_sdf(&self, elevation: f32) -> SdfPose {
+        let p = &self.trans;
+        let r = match self.rot {
+            Rotation::Yaw(angle) => format!("0 0 {}", angle.radians()),
+            Rotation::EulerExtrinsicXYZ(rpy) => format!(
+                "{} {} {}",
+                rpy[0].radians(),
+                rpy[1].radians(),
+                rpy[2].radians()
+            ),
+            Rotation::Quat(quat) => format!("{} {} {} {}", quat[0], quat[1], quat[2], quat[3]),
+        };
+        SdfPose {
+            data: format!("{} {} {} {}", p[0], p[1], p[2] + elevation, r),
+            ..Default::default()
+        }
+    }
+}
+
+impl NameInSite {
+    fn to_sdf(&self, model_counts: &mut HashMap<String, i64>) -> String {
+        match model_counts.entry(self.0.to_string()) {
+            Entry::Occupied(mut entry) => {
+                let name = format!("{}_{}", self.0, entry.get());
+                *entry.get_mut() += 1;
+                name
+            }
+            Entry::Vacant(entry) => {
+                entry.insert(1);
+                self.0.clone()
+            }
+        }
+    }
+}
+
+impl Site {
+    pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
+        let get_anchor = |id: u32, level: &Level, site: &Site| -> Option<Anchor> {
+            level
+                .anchors
+                .get(&id)
+                .or_else(|| self.anchors.get(&id))
+                .cloned()
+        };
+        //let mut levels = Vec::new();
+        let mut includes = Vec::new();
+        let mut models = Vec::new();
+        // Models must have a unique name, use this to add a counter
+        let mut model_counts = HashMap::new();
+        let mut floor_count = 0;
+        let mut wall_count = 0;
+        let floor_thickness = 0.01_f32;
+        let wall_thickness = 0.01;
+        let wall_height = 2.5;
+        for level in self.levels.values() {
+            let z = level.properties.elevation.0;
+            // TODO(luca) meshes for floor, walls,
+            for model in level.models.values() {
+                let source = model.source.to_sdf()?;
+                includes.push(SdfWorldInclude {
+                    name: Some(model.name.to_sdf(&mut model_counts)),
+                    uri: source,
+                    pose: Some(model.pose.to_sdf(z)),
+                    r#static: Some(model.is_static.0),
+                    ..Default::default()
+                })
+            }
+            for floor in level.floors.values() {
+                // TODO(luca) materials for floors
+                floor_count += 1;
+                let anchors = floor
+                    .anchors
+                    .0
+                    .iter()
+                    .map(|id| {
+                        let anchor = get_anchor(*id, level, self).ok_or(SdfConversionError::BrokenAnchorReference)?;
+                        let pose = anchor.translation_for_category(Category::General);
+                        Ok(format!("{} {}", pose[0], pose[1]))
+                    })
+                    .collect::<Result<Vec<_>, _>>()?;
+                let geometry = SdfGeometry::Polyline(SdfPolylineShape {
+                    point: anchors,
+                    height: floor_thickness as f64,
+                });
+                models.push(SdfModel {
+                    name: format!("Floor_{}", floor_count),
+                    r#static: Some(true),
+                    pose: Some(Pose::default().to_sdf(z - floor_thickness / 2.0)),
+                    link: vec![SdfLink {
+                        name: format!("Floor_{}", floor_count),
+                        collision: vec![SdfCollision {
+                            name: "collision".into(),
+                            geometry: geometry.clone(),
+                            ..Default::default()
+                        }],
+                        visual: vec![SdfVisual {
+                            name: "visual".into(),
+                            geometry,
+                            ..Default::default()
+                        }],
+                        ..Default::default()
+                    }],
+                    ..Default::default()
+                })
+            }
+            for wall in level.walls.values() {
+                wall_count += 1;
+                // TODO(luca) materials for walls
+                let start = get_anchor(wall.anchors.start(), level, self).ok_or(SdfConversionError::BrokenAnchorReference)?;
+                let end = get_anchor(wall.anchors.end(), level, self).ok_or(SdfConversionError::BrokenAnchorReference)?;
+                let start = start.translation_for_category(Category::General);
+                let end = end.translation_for_category(Category::General);
+                let length = ((start[0] - end[0]).powi(2) + (start[1] - end[1]).powi(2)).sqrt();
+                let geometry = SdfGeometry::r#Box(SdfBoxShape {
+                    size: Vector3d::new(length.into(), wall_thickness, wall_height),
+                });
+                let pose = Pose {
+                    trans: Default::default(),
+                    rot: Rotation::Yaw(Angle::Rad((start[0] - end[0]).atan2(start[1] - end[1]))),
+                };
+                models.push(SdfModel {
+                    name: format!("Wall_{}", wall_count),
+                    r#static: Some(true),
+                    pose: Some(pose.to_sdf(0.0)),
+                    link: vec![SdfLink {
+                        name: format!("Wall_{}", wall_count),
+                        collision: vec![SdfCollision {
+                            name: "collision".into(),
+                            geometry: geometry.clone(),
+                            ..Default::default()
+                        }],
+                        visual: vec![SdfVisual {
+                            name: "visual".into(),
+                            geometry,
+                            ..Default::default()
+                        }],
+                        ..Default::default()
+                    }],
+                    ..Default::default()
+                })
+            }
+        }
+
+        Ok(SdfRoot {
+            version: "1.7".to_string(),
+            world: vec![SdfWorld {
+                name: self.properties.name.0.clone(),
+                include: includes,
+                model: models,
+                atmosphere: SdfAtmosphere {
+                    r#type: "adiabatic".to_string(),
+                    ..Default::default()
+                },
+                scene: SdfScene {
+                    ambient: "1 1 1".to_string(),
+                    background: "0.8 0.8 0.8".to_string(),
+                    ..Default::default()
+                },
+                ..Default::default()
+            }],
+            ..Default::default()
+        })
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::legacy::building_map::BuildingMap;
+
+    #[test]
+    fn serde_roundtrip() {
+        let data = std::fs::read("../assets/demo_maps/hotel.building.yaml").unwrap();
+        let map = BuildingMap::from_bytes(&data).unwrap();
+        let site = map.to_site().unwrap();
+        // Convert to an sdf
+        let sdf = site.to_sdf().unwrap();
+        dbg!(&sdf);
+        let config = yaserde::ser::Config {
+            perform_indent: true,
+            write_document_declaration: true,
+            ..Default::default()
+        };
+        let s = yaserde::ser::to_string_with_config(&sdf, &config).unwrap();
+        println!("{}", s);
+        std::fs::write("test.sdf", s);
+        panic!();
+    }
+}

From 1ce7c4e55e1dcb00bb9ea9e22f1abd0870d60deb Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 6 Dec 2023 18:30:51 +0800
Subject: [PATCH 02/71] Continue iterating, walls and floors are now exported

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 51 ++++++++++++++----------
 rmf_site_format/src/sdf.rs               | 11 +++--
 2 files changed, 36 insertions(+), 26 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 0c00cd21..7efc8048 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -1,24 +1,20 @@
 use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
-use bevy_gltf_export::{export_mesh, gltf_to_bytes, GltfPose};
+use bevy_gltf_export::{export_mesh, CompressGltfOptions, GltfPose};
 
 use std::collections::BTreeMap;
 
 use crate::site::FloorSegments;
-use rmf_site_format::{FloorMarker, LevelElevation, Pose, WallMarker};
-
-// Returns a map from level site id to
-//fn collect_wall_meshes(world: &World, site: Entity) -> HashMap<u32,
+use rmf_site_format::{FloorMarker, LevelElevation, NameInSite, Pose, WallMarker};
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
     let mut state: SystemState<(
         Query<&Children>,
-        Query<(Entity, &LevelElevation)>,
+        Query<(&NameInSite, &LevelElevation, &Children)>,
         Query<Entity, With<WallMarker>>,
         Query<&FloorSegments, With<FloorMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
     )> = SystemState::new(world);
-    // TODO(luca) level elevation into meshes transform
     let (q_children, q_levels, q_walls, q_floors, q_pbr) = state.get(world);
 
     let image_assets = world.resource::<Assets<Image>>();
@@ -26,11 +22,13 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
     let material_assets = world.resource::<Assets<StandardMaterial>>();
     let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
 
-    info!("Looking for children");
-    for site_child in q_children.get(site).unwrap().iter() {
-        if let Ok((level, elevation)) = q_levels.get(*site_child) {
-            info!("Found level {:?}", level);
-            for (i, child) in q_children.get(level).unwrap().iter().enumerate() {
+    let Ok(site_children) = q_children.get(site) else {
+        return;
+    };
+    for site_child in site_children.iter() {
+        let mut mesh_data = Vec::new();
+        if let Ok((level_name, elevation, children)) = q_levels.get(*site_child) {
+            for child in children.iter() {
                 let entity = if let Ok(res) = q_walls.get(*child) {
                     res
                 } else if let Ok(res) = q_floors.get(*child) {
@@ -38,7 +36,6 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
                 } else {
                     continue;
                 };
-                info!("Found a mesh");
                 // Get mesh and material
                 let Ok((mesh, material)) = q_pbr.get(entity) else {
                     // This shouldn't happen
@@ -58,20 +55,30 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
                     translation: [0.0, 0.0, **elevation],
                     ..Default::default()
                 };
-                let (root, bytes) = bevy_gltf_export::export_mesh(
+                match bevy_gltf_export::export_mesh(
                     mesh.clone(),
                     material.clone(),
                     Some(pose),
                     image_getter,
-                )
-                .unwrap();
-                // TODO(luca) merge the roots
-                let bytes = gltf_to_bytes(&root, bytes).unwrap();
-                let filename = format!("{}/level_{}_mesh_{}.glb", folder, level.index(), i);
-                info!("Writing to {}", filename);
-                std::fs::write(filename, bytes).unwrap();
-                info!("Wrote mesh");
+                ) {
+                    Ok(exported) => mesh_data.push(exported),
+                    Err(e) => error!("Error exporting mesh to glb"),
+                }
             }
+            let mut meshes = mesh_data.into_iter();
+            let Some(mut combined) = meshes.next() else {
+                continue;
+            };
+            let combined = combined.combine_with(meshes, CompressGltfOptions::maximum());
+            let Ok(bytes) = combined.to_bytes() else {
+                error!("Error converting glb to bytes");
+                continue;
+            };
+            let filename = format!("{}/level_{}.glb", folder, **level_name);
+            let Ok(e) = std::fs::write(filename, bytes) else {
+                error!("Error writing mesh to file");
+                continue;
+            };
         }
     }
 }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index b4fc3509..e8ef2539 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -15,7 +15,7 @@
  *
 */
 
-use crate::{Angle, Anchor, Level, AssetSource, Category, NameInSite, Pose, Rotation, Site};
+use crate::{Anchor, Angle, AssetSource, Category, Level, NameInSite, Pose, Rotation, Site};
 use sdformat_rs::*;
 use std::collections::{hash_map::Entry, HashMap};
 
@@ -123,7 +123,8 @@ impl Site {
                     .0
                     .iter()
                     .map(|id| {
-                        let anchor = get_anchor(*id, level, self).ok_or(SdfConversionError::BrokenAnchorReference)?;
+                        let anchor = get_anchor(*id, level, self)
+                            .ok_or(SdfConversionError::BrokenAnchorReference)?;
                         let pose = anchor.translation_for_category(Category::General);
                         Ok(format!("{} {}", pose[0], pose[1]))
                     })
@@ -156,8 +157,10 @@ impl Site {
             for wall in level.walls.values() {
                 wall_count += 1;
                 // TODO(luca) materials for walls
-                let start = get_anchor(wall.anchors.start(), level, self).ok_or(SdfConversionError::BrokenAnchorReference)?;
-                let end = get_anchor(wall.anchors.end(), level, self).ok_or(SdfConversionError::BrokenAnchorReference)?;
+                let start = get_anchor(wall.anchors.start(), level, self)
+                    .ok_or(SdfConversionError::BrokenAnchorReference)?;
+                let end = get_anchor(wall.anchors.end(), level, self)
+                    .ok_or(SdfConversionError::BrokenAnchorReference)?;
                 let start = start.translation_for_category(Category::General);
                 let end = end.translation_for_category(Category::General);
                 let length = ((start[0] - end[0]).powi(2) + (start[1] - end[1]).powi(2)).sqrt();

From f0006a893e1530077967c183f19bd18ee4bc695d Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 11 Dec 2023 14:36:54 +0800
Subject: [PATCH 03/71] Point floors / walls to generated model

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml               |  2 +-
 rmf_site_editor/src/site/sdf_exporter.rs |  8 +-
 rmf_site_format/src/sdf.rs               | 95 ++++++------------------
 3 files changed, 28 insertions(+), 77 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index bee9740b..01f80a0f 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -21,7 +21,7 @@ bevy_mod_raycast = "0.13"
 bevy_mod_outline = "0.5"
 bevy_infinite_grid = "0.8"
 bevy_polyline = "0.7"
-bevy_gltf_export = { path = "../../bevy_gltf_export" }
+bevy_gltf_export = { git = "https://github.com/luca-della-vedova/bevy_gltf_export", branch = "main"}
 bevy_stl = "0.11"
 bevy_obj = { version = "0.11", features = ["scene"] }
 bevy_rapier3d = "0.22.0"
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 7efc8048..b5ad8557 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -66,11 +66,13 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
                 }
             }
             let mut meshes = mesh_data.into_iter();
-            let Some(mut combined) = meshes.next() else {
+            let Some(mut mesh) = meshes.next() else {
                 continue;
             };
-            let combined = combined.combine_with(meshes, CompressGltfOptions::maximum());
-            let Ok(bytes) = combined.to_bytes() else {
+            let Ok(bytes) = mesh
+                .combine_with(meshes, CompressGltfOptions::maximum())
+                .to_bytes()
+            else {
                 error!("Error converting glb to bytes");
                 continue;
             };
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index e8ef2539..2d050268 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -113,85 +113,34 @@ impl Site {
                     pose: Some(model.pose.to_sdf(z)),
                     r#static: Some(model.is_static.0),
                     ..Default::default()
-                })
-            }
-            for floor in level.floors.values() {
-                // TODO(luca) materials for floors
-                floor_count += 1;
-                let anchors = floor
-                    .anchors
-                    .0
-                    .iter()
-                    .map(|id| {
-                        let anchor = get_anchor(*id, level, self)
-                            .ok_or(SdfConversionError::BrokenAnchorReference)?;
-                        let pose = anchor.translation_for_category(Category::General);
-                        Ok(format!("{} {}", pose[0], pose[1]))
-                    })
-                    .collect::<Result<Vec<_>, _>>()?;
-                let geometry = SdfGeometry::Polyline(SdfPolylineShape {
-                    point: anchors,
-                    height: floor_thickness as f64,
                 });
-                models.push(SdfModel {
-                    name: format!("Floor_{}", floor_count),
-                    r#static: Some(true),
-                    pose: Some(Pose::default().to_sdf(z - floor_thickness / 2.0)),
-                    link: vec![SdfLink {
-                        name: format!("Floor_{}", floor_count),
-                        collision: vec![SdfCollision {
-                            name: "collision".into(),
-                            geometry: geometry.clone(),
-                            ..Default::default()
-                        }],
-                        visual: vec![SdfVisual {
-                            name: "visual".into(),
-                            geometry,
-                            ..Default::default()
-                        }],
+            }
+            // Floors and walls are included in the static level mesh
+            models.push(SdfModel {
+                name: format!("level_{}", level.properties.name.0),
+                r#static: Some(true),
+                link: vec![SdfLink {
+                    name: "link".into(),
+                    collision: vec![SdfCollision {
+                        name: "collision".into(),
+                        geometry: SdfGeometry::Mesh(SdfMeshShape {
+                                uri: format!("meshes://level_{}.glb", level.properties.name.0),
+                                ..Default::default()
+                            }),
                         ..Default::default()
                     }],
-                    ..Default::default()
-                })
-            }
-            for wall in level.walls.values() {
-                wall_count += 1;
-                // TODO(luca) materials for walls
-                let start = get_anchor(wall.anchors.start(), level, self)
-                    .ok_or(SdfConversionError::BrokenAnchorReference)?;
-                let end = get_anchor(wall.anchors.end(), level, self)
-                    .ok_or(SdfConversionError::BrokenAnchorReference)?;
-                let start = start.translation_for_category(Category::General);
-                let end = end.translation_for_category(Category::General);
-                let length = ((start[0] - end[0]).powi(2) + (start[1] - end[1]).powi(2)).sqrt();
-                let geometry = SdfGeometry::r#Box(SdfBoxShape {
-                    size: Vector3d::new(length.into(), wall_thickness, wall_height),
-                });
-                let pose = Pose {
-                    trans: Default::default(),
-                    rot: Rotation::Yaw(Angle::Rad((start[0] - end[0]).atan2(start[1] - end[1]))),
-                };
-                models.push(SdfModel {
-                    name: format!("Wall_{}", wall_count),
-                    r#static: Some(true),
-                    pose: Some(pose.to_sdf(0.0)),
-                    link: vec![SdfLink {
-                        name: format!("Wall_{}", wall_count),
-                        collision: vec![SdfCollision {
-                            name: "collision".into(),
-                            geometry: geometry.clone(),
-                            ..Default::default()
-                        }],
-                        visual: vec![SdfVisual {
-                            name: "visual".into(),
-                            geometry,
-                            ..Default::default()
-                        }],
+                    visual: vec![SdfVisual {
+                        name: "visual".into(),
+                        geometry: SdfGeometry::Mesh(SdfMeshShape {
+                                uri: format!("meshes/level_{}.glb", level.properties.name.0),
+                                ..Default::default()
+                            }),
                         ..Default::default()
                     }],
                     ..Default::default()
-                })
-            }
+                }],
+                ..Default::default()
+            })
         }
 
         Ok(SdfRoot {

From 2796b55d069f5dc53e0e24e48c0303b094f03f7a Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 13 Dec 2023 15:51:50 +0800
Subject: [PATCH 04/71] Split collision and visual meshes, add sun

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/save.rs         |   6 +-
 rmf_site_editor/src/site/sdf_exporter.rs | 186 +++++++++++++++++------
 rmf_site_format/src/sdf.rs               |  51 +++----
 3 files changed, 170 insertions(+), 73 deletions(-)

diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index b05edd52..6578c189 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1267,8 +1267,10 @@ pub fn save_site(world: &mut World) {
                     }
                 };
 
-                std::fs::create_dir("meshes").ok();
-                collect_site_meshes(world, save_event.site, "meshes");
+                let mut meshes_dir = PathBuf::from(new_path.parent().unwrap());
+                meshes_dir.push("meshes");
+                std::fs::create_dir(&meshes_dir).ok();
+                collect_site_meshes(world, save_event.site, &meshes_dir);
 
                 let old_default_path = world.get::<DefaultFile>(save_event.site).cloned();
                 migrate_relative_paths(save_event.site, &new_path, world);
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index b5ad8557..eeece4ca 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -1,82 +1,180 @@
 use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
-use bevy_gltf_export::{export_mesh, CompressGltfOptions, GltfPose};
+use bevy_gltf_export::{export_meshes, CompressGltfOptions, GltfPose, MeshData};
 
 use std::collections::BTreeMap;
+use std::path::Path;
 
-use crate::site::FloorSegments;
-use rmf_site_format::{FloorMarker, LevelElevation, NameInSite, Pose, WallMarker};
+use crate::site::{CollisionMeshMarker, FloorSegments, VisualMeshMarker};
+use rmf_site_format::{FloorMarker, LevelElevation, ModelMarker, NameInSite, Pose, WallMarker};
 
-pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &str) {
+pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
     let mut state: SystemState<(
         Query<&Children>,
         Query<(&NameInSite, &LevelElevation, &Children)>,
         Query<Entity, With<WallMarker>>,
         Query<&FloorSegments, With<FloorMarker>>,
+        Query<Entity, With<ModelMarker>>,
+        Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
+        Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
+        Query<&GlobalTransform>,
     )> = SystemState::new(world);
-    let (q_children, q_levels, q_walls, q_floors, q_pbr) = state.get(world);
+    let (q_children, q_levels, q_walls, q_floors, q_models, q_collisions, q_visuals, q_pbr, q_tfs) =
+        state.get(world);
 
     let image_assets = world.resource::<Assets<Image>>();
     let mesh_assets = world.resource::<Assets<Mesh>>();
     let material_assets = world.resource::<Assets<StandardMaterial>>();
     let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
 
+    let get_mesh_and_material = |entity: Entity| -> Option<(&Mesh, &StandardMaterial)> {
+        let Ok((mesh, material)) = q_pbr.get(entity) else {
+            return None;
+        };
+        let Some(mesh) = mesh_assets.get(mesh) else {
+            warn!("Mesh asset not found");
+            return None;
+        };
+        let Some(material) = material_assets.get(material) else {
+            warn!("Material asset not found");
+            return None;
+        };
+        Some((mesh, material))
+    };
+
     let Ok(site_children) = q_children.get(site) else {
         return;
     };
     for site_child in site_children.iter() {
-        let mut mesh_data = Vec::new();
+        let mut collision_data = Vec::new();
+        let mut visual_data = Vec::new();
         if let Ok((level_name, elevation, children)) = q_levels.get(*site_child) {
+            let level_pose = GltfPose {
+                translation: [0.0, 0.0, **elevation],
+                ..Default::default()
+            };
             for child in children.iter() {
-                let entity = if let Ok(res) = q_walls.get(*child) {
-                    res
+                if let Ok(res) = q_walls.get(*child) {
+                    let Some((mesh, material)) = get_mesh_and_material(res) else {
+                        continue;
+                    };
+                    collision_data.push(MeshData {
+                        mesh,
+                        material,
+                        pose: Some(level_pose.clone()),
+                    });
+                    visual_data.push(MeshData {
+                        mesh,
+                        material,
+                        pose: Some(level_pose.clone()),
+                    });
                 } else if let Ok(res) = q_floors.get(*child) {
-                    res.mesh
+                    let Some((mesh, material)) = get_mesh_and_material(res.mesh) else {
+                        continue;
+                    };
+                    collision_data.push(MeshData {
+                        mesh,
+                        material,
+                        pose: Some(level_pose.clone()),
+                    });
+                    visual_data.push(MeshData {
+                        mesh,
+                        material,
+                        pose: Some(level_pose.clone()),
+                    });
+                } else if let Ok(model) = q_models.get(*child) {
+                    // TODO(luca) don't do full descendant iter here or we might add twice?
+                    // Iterate through children and select all meshes
+                    for model_child in DescendantIter::new(&q_children, model) {
+                        if let Ok((entity, tf)) = q_collisions.get(model_child) {
+                            // Now iterate through the children of the collision and add them
+                            for entity in DescendantIter::new(&q_children, model_child) {
+                                let Some((mesh, material)) = get_mesh_and_material(entity) else {
+                                    continue;
+                                };
+                                let Ok(tf) = q_tfs.get(entity) else {
+                                    continue;
+                                };
+                                let tf = tf.compute_transform();
+                                let pose = GltfPose {
+                                    translation: [
+                                        tf.translation.x,
+                                        tf.translation.y,
+                                        tf.translation.z + **elevation,
+                                    ],
+                                    rotation: tf.rotation.to_array(),
+                                    scale: Some(tf.scale.to_array()),
+                                };
+                                collision_data.push(MeshData {
+                                    mesh,
+                                    material,
+                                    pose: Some(pose),
+                                });
+                            }
+                        } else if let Ok((entity, tf)) = q_visuals.get(model_child) {
+                            // Now iterate through the children of the visuals and add them
+                            for entity in DescendantIter::new(&q_children, model_child) {
+                                let Some((mesh, material)) = get_mesh_and_material(entity) else {
+                                    continue;
+                                };
+                                let Ok(tf) = q_tfs.get(entity) else {
+                                    continue;
+                                };
+                                let tf = tf.compute_transform();
+                                let pose = GltfPose {
+                                    translation: [
+                                        tf.translation.x,
+                                        tf.translation.y,
+                                        tf.translation.z + **elevation,
+                                    ],
+                                    rotation: tf.rotation.to_array(),
+                                    scale: Some(tf.scale.to_array()),
+                                };
+                                visual_data.push(MeshData {
+                                    mesh,
+                                    material,
+                                    pose: Some(pose),
+                                });
+                            }
+                        }
+                    }
                 } else {
                     continue;
                 };
-                // Get mesh and material
-                let Ok((mesh, material)) = q_pbr.get(entity) else {
-                    // This shouldn't happen
-                    warn!("Wall or floor {:?} without a mesh", entity);
-                    continue;
-                };
-                let Some(mesh) = mesh_assets.get(mesh) else {
-                    warn!("Mesh asset not found");
-                    continue;
-                };
-                let Some(material) = material_assets.get(material) else {
-                    warn!("Material asset not found");
-                    continue;
-                };
-
-                let pose = GltfPose {
-                    translation: [0.0, 0.0, **elevation],
-                    ..Default::default()
-                };
-                match bevy_gltf_export::export_mesh(
-                    mesh.clone(),
-                    material.clone(),
-                    Some(pose),
-                    image_getter,
-                ) {
-                    Ok(exported) => mesh_data.push(exported),
-                    Err(e) => error!("Error exporting mesh to glb"),
-                }
             }
-            let mut meshes = mesh_data.into_iter();
-            let Some(mut mesh) = meshes.next() else {
+            let Ok(collisions) = export_meshes(
+                collision_data,
+                None,
+                image_getter,
+                CompressGltfOptions::skip_materials(),
+            ) else {
+                error!("Failed exporting collision data");
+                continue;
+            };
+            let Ok(bytes) = collisions.to_bytes() else {
+                error!("Error converting glb to bytes");
+                continue;
+            };
+            let filename = format!("{}/level_{}_collision.glb", folder.display(), **level_name);
+            let Ok(e) = std::fs::write(filename, bytes) else {
+                error!("Error writing mesh to file");
+                continue;
+            };
+            let Ok(visuals) = export_meshes(
+                visual_data,
+                Some(format!("level_{}_visuals", **level_name)),
+                image_getter,
+                CompressGltfOptions::default(),
+            ) else {
+                error!("Failed exporting visual data");
                 continue;
             };
-            let Ok(bytes) = mesh
-                .combine_with(meshes, CompressGltfOptions::maximum())
-                .to_bytes()
-            else {
+            let Ok(bytes) = visuals.to_bytes() else {
                 error!("Error converting glb to bytes");
                 continue;
             };
-            let filename = format!("{}/level_{}.glb", folder, **level_name);
+            let filename = format!("{}/level_{}_visual.glb", folder.display(), **level_name);
             let Ok(e) = std::fs::write(filename, bytes) else {
                 error!("Error writing mesh to file");
                 continue;
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 2d050268..834d313b 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -92,30 +92,10 @@ impl Site {
                 .or_else(|| self.anchors.get(&id))
                 .cloned()
         };
-        //let mut levels = Vec::new();
         let mut includes = Vec::new();
         let mut models = Vec::new();
-        // Models must have a unique name, use this to add a counter
-        let mut model_counts = HashMap::new();
-        let mut floor_count = 0;
-        let mut wall_count = 0;
-        let floor_thickness = 0.01_f32;
-        let wall_thickness = 0.01;
-        let wall_height = 2.5;
         for level in self.levels.values() {
-            let z = level.properties.elevation.0;
-            // TODO(luca) meshes for floor, walls,
-            for model in level.models.values() {
-                let source = model.source.to_sdf()?;
-                includes.push(SdfWorldInclude {
-                    name: Some(model.name.to_sdf(&mut model_counts)),
-                    uri: source,
-                    pose: Some(model.pose.to_sdf(z)),
-                    r#static: Some(model.is_static.0),
-                    ..Default::default()
-                });
-            }
-            // Floors and walls are included in the static level mesh
+            // Floors walls and static models are included in the level mesh
             models.push(SdfModel {
                 name: format!("level_{}", level.properties.name.0),
                 r#static: Some(true),
@@ -124,17 +104,17 @@ impl Site {
                     collision: vec![SdfCollision {
                         name: "collision".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
-                                uri: format!("meshes://level_{}.glb", level.properties.name.0),
-                                ..Default::default()
-                            }),
+                            uri: format!("meshes/level_{}_collision.glb", level.properties.name.0),
+                            ..Default::default()
+                        }),
                         ..Default::default()
                     }],
                     visual: vec![SdfVisual {
                         name: "visual".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
-                                uri: format!("meshes/level_{}.glb", level.properties.name.0),
-                                ..Default::default()
-                            }),
+                            uri: format!("meshes/level_{}_visual.glb", level.properties.name.0),
+                            ..Default::default()
+                        }),
                         ..Default::default()
                     }],
                     ..Default::default()
@@ -143,6 +123,22 @@ impl Site {
             })
         }
 
+        let sun = SdfLight {
+            name: "sun".into(),
+            r#type: "directional".into(),
+            cast_shadows: Some(true),
+            diffuse: Some("1 1 1 1".into()),
+            pose: Some(Pose::default().to_sdf(10.0)),
+            specular: Some("0.2 0.2 0.2 1".into()),
+            attenuation: Some(SdfLightAttenuation {
+                range: 1000.0,
+                constant: Some(0.09),
+                linear: Some(0.001),
+                quadratic: Some(0.001),
+            }),
+            direction: Vector3d::new(-0.5, 0.1, -0.9),
+            ..Default::default()
+        };
         Ok(SdfRoot {
             version: "1.7".to_string(),
             world: vec![SdfWorld {
@@ -158,6 +154,7 @@ impl Site {
                     background: "0.8 0.8 0.8".to_string(),
                     ..Default::default()
                 },
+                light: vec![sun],
                 ..Default::default()
             }],
             ..Default::default()

From 39335236ac30d5dd678778a8878e909ed32243f4 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 13 Dec 2023 15:59:33 +0800
Subject: [PATCH 05/71] Cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 48 +-------------------------------------
 1 file changed, 1 insertion(+), 47 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 834d313b..e47102ac 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -15,9 +15,8 @@
  *
 */
 
-use crate::{Anchor, Angle, AssetSource, Category, Level, NameInSite, Pose, Rotation, Site};
+use crate::{Pose, Rotation, Site};
 use sdformat_rs::*;
-use std::collections::{hash_map::Entry, HashMap};
 
 #[derive(Debug)]
 pub enum SdfConversionError {
@@ -27,26 +26,6 @@ pub enum SdfConversionError {
     BrokenAnchorReference,
 }
 
-impl AssetSource {
-    fn to_sdf(&self) -> Result<String, SdfConversionError> {
-        // TODO(luca) check this function
-        match self {
-            AssetSource::Local(path) => Ok(path.clone()),
-            AssetSource::Remote(name) => Ok(name.clone()),
-            AssetSource::Search(name) => {
-                let name = name
-                    .rsplit("/")
-                    .next()
-                    .ok_or(SdfConversionError::UnsupportedAssetType)?;
-                Ok("model://".to_owned() + name)
-            }
-            AssetSource::Bundled(_) | AssetSource::Package(_) | AssetSource::OSMTile { .. } => {
-                Err(SdfConversionError::UnsupportedAssetType)
-            }
-        }
-    }
-}
-
 impl Pose {
     fn to_sdf(&self, elevation: f32) -> SdfPose {
         let p = &self.trans;
@@ -67,32 +46,8 @@ impl Pose {
     }
 }
 
-impl NameInSite {
-    fn to_sdf(&self, model_counts: &mut HashMap<String, i64>) -> String {
-        match model_counts.entry(self.0.to_string()) {
-            Entry::Occupied(mut entry) => {
-                let name = format!("{}_{}", self.0, entry.get());
-                *entry.get_mut() += 1;
-                name
-            }
-            Entry::Vacant(entry) => {
-                entry.insert(1);
-                self.0.clone()
-            }
-        }
-    }
-}
-
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
-        let get_anchor = |id: u32, level: &Level, site: &Site| -> Option<Anchor> {
-            level
-                .anchors
-                .get(&id)
-                .or_else(|| self.anchors.get(&id))
-                .cloned()
-        };
-        let mut includes = Vec::new();
         let mut models = Vec::new();
         for level in self.levels.values() {
             // Floors walls and static models are included in the level mesh
@@ -143,7 +98,6 @@ impl Site {
             version: "1.7".to_string(),
             world: vec![SdfWorld {
                 name: self.properties.name.0.clone(),
-                include: includes,
                 model: models,
                 atmosphere: SdfAtmosphere {
                     r#type: "adiabatic".to_string(),

From 955f2ec4e10c9b6fbd2880b074b9fe16d87d7997 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 15 Dec 2023 09:14:57 +0800
Subject: [PATCH 06/71] WIP adding doors

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 117 +++++++++-----
 rmf_site_format/src/sdf.rs               | 188 ++++++++++++++++++++++-
 2 files changed, 261 insertions(+), 44 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index eeece4ca..4157507a 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -1,32 +1,64 @@
 use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
-use bevy_gltf_export::{export_meshes, CompressGltfOptions, GltfPose, MeshData};
+use bevy_gltf_export::{export_meshes, CompressGltfOptions, GltfPose, MeshData, MeshExportError};
 
 use std::collections::BTreeMap;
 use std::path::Path;
 
-use crate::site::{CollisionMeshMarker, FloorSegments, VisualMeshMarker};
-use rmf_site_format::{FloorMarker, LevelElevation, ModelMarker, NameInSite, Pose, WallMarker};
+use crate::site::{CollisionMeshMarker, DoorSegments, FloorSegments, VisualMeshMarker};
+use rmf_site_format::{LevelElevation, ModelMarker, NameInSite, Pose, WallMarker};
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
     let mut state: SystemState<(
         Query<&Children>,
         Query<(&NameInSite, &LevelElevation, &Children)>,
         Query<Entity, With<WallMarker>>,
-        Query<&FloorSegments, With<FloorMarker>>,
+        Query<&FloorSegments>,
+        Query<(&NameInSite, &DoorSegments)>,
         Query<Entity, With<ModelMarker>>,
         Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
         Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
         Query<&GlobalTransform>,
     )> = SystemState::new(world);
-    let (q_children, q_levels, q_walls, q_floors, q_models, q_collisions, q_visuals, q_pbr, q_tfs) =
-        state.get(world);
+    let (
+        q_children,
+        q_levels,
+        q_walls,
+        q_floors,
+        q_doors,
+        q_models,
+        q_collisions,
+        q_visuals,
+        q_pbr,
+        q_tfs,
+    ) = state.get(world);
 
     let image_assets = world.resource::<Assets<Image>>();
     let mesh_assets = world.resource::<Assets<Mesh>>();
     let material_assets = world.resource::<Assets<StandardMaterial>>();
-    let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
+    let write_meshes_to_file = |
+        meshes: Vec<MeshData>,
+        name: Option<String>,
+        options: CompressGltfOptions,
+        filename: String,
+    | -> Result<(), MeshExportError> {
+        let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
+        let meshes = export_meshes(
+            meshes,
+            name,
+            image_getter,
+            options,
+        )?;
+        let bytes = meshes.to_bytes()?;
+        let Ok(_) = std::fs::write(filename, bytes) else {
+            // TODO(luca) make this an error
+            error!("Error writing mesh to file");
+            return Ok(());
+        };
+        Ok(())
+    };
+
 
     let get_mesh_and_material = |entity: Entity| -> Option<(&Mesh, &StandardMaterial)> {
         let Ok((mesh, material)) = q_pbr.get(entity) else {
@@ -139,46 +171,49 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             }
                         }
                     }
+                } else if let Ok((door_name, segments)) = q_doors.get(*child) {
+                    // TODO(luca) get rid of reverse by changing the entities function
+                    for (entity, segment_name) in segments
+                        .body
+                        .entities()
+                        .iter()
+                        .rev()
+                        .zip(["right", "left"].into_iter())
+                    {
+                        // Generate the visual and collisions here
+                        let Some((mesh, material)) = get_mesh_and_material(*entity) else {
+                            continue;
+                        };
+                        let Ok(tf) = q_tfs.get(*entity) else {
+                            continue;
+                        };
+                        let tf = tf.compute_transform();
+                        let pose = GltfPose {
+                            translation: [
+                                tf.translation.x,
+                                tf.translation.y,
+                                tf.translation.z + **elevation,
+                            ],
+                            rotation: tf.rotation.to_array(),
+                            scale: Some(tf.scale.to_array()),
+                        };
+
+                        let data = MeshData {
+                            mesh,
+                            material,
+                            pose: Some(pose.clone()),
+                        };
+                        let filename = format!("{}/{}_{}.glb", folder.display(), **door_name, segment_name);
+                        write_meshes_to_file(vec![data], None, CompressGltfOptions::default(), filename);
+                    }
                 } else {
                     continue;
                 };
             }
-            let Ok(collisions) = export_meshes(
-                collision_data,
-                None,
-                image_getter,
-                CompressGltfOptions::skip_materials(),
-            ) else {
-                error!("Failed exporting collision data");
-                continue;
-            };
-            let Ok(bytes) = collisions.to_bytes() else {
-                error!("Error converting glb to bytes");
-                continue;
-            };
             let filename = format!("{}/level_{}_collision.glb", folder.display(), **level_name);
-            let Ok(e) = std::fs::write(filename, bytes) else {
-                error!("Error writing mesh to file");
-                continue;
-            };
-            let Ok(visuals) = export_meshes(
-                visual_data,
-                Some(format!("level_{}_visuals", **level_name)),
-                image_getter,
-                CompressGltfOptions::default(),
-            ) else {
-                error!("Failed exporting visual data");
-                continue;
-            };
-            let Ok(bytes) = visuals.to_bytes() else {
-                error!("Error converting glb to bytes");
-                continue;
-            };
+            write_meshes_to_file(collision_data, None, CompressGltfOptions::skip_materials(), filename);
             let filename = format!("{}/level_{}_visual.glb", folder.display(), **level_name);
-            let Ok(e) = std::fs::write(filename, bytes) else {
-                error!("Error writing mesh to file");
-                continue;
-            };
+            write_meshes_to_file(visual_data, Some(format!("level_{}_visuals", **level_name)), CompressGltfOptions::default(), filename);
         }
     }
 }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index e47102ac..fa1740db 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -15,7 +15,7 @@
  *
 */
 
-use crate::{Pose, Rotation, Site};
+use crate::{Category, Side, Anchor, DoorType, Level, Pose, Rotation, Site};
 use sdformat_rs::*;
 
 #[derive(Debug)]
@@ -23,7 +23,7 @@ pub enum SdfConversionError {
     /// An asset that can't be converted to an sdf world was found.
     UnsupportedAssetType,
     /// Entity referenced a non existing anchor.
-    BrokenAnchorReference,
+    BrokenAnchorReference(u32),
 }
 
 impl Pose {
@@ -48,6 +48,15 @@ impl Pose {
 
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
+        let get_anchor = |id: u32, level: &Level| -> Result<Anchor, SdfConversionError> {
+            level
+                .anchors
+                .get(&id)
+                .or_else(|| self.anchors.get(&id))
+                .ok_or(SdfConversionError::BrokenAnchorReference(id))
+                .cloned()
+        };
+        let door_mass = 50.0;
         let mut models = Vec::new();
         for level in self.levels.values() {
             // Floors walls and static models are included in the level mesh
@@ -62,6 +71,13 @@ impl Site {
                             uri: format!("meshes/level_{}_collision.glb", level.properties.name.0),
                             ..Default::default()
                         }),
+                        surface: Some(SdfSurface {
+                            contact: Some(SdfSurfaceContact {
+                                collide_bitmask: Some("0x01".into()),
+                                ..Default::default()
+                            }),
+                            ..Default::default()
+                        }),
                         ..Default::default()
                     }],
                     visual: vec![SdfVisual {
@@ -75,7 +91,173 @@ impl Site {
                     ..Default::default()
                 }],
                 ..Default::default()
-            })
+            });
+            // Now add all the doors
+            for door in level.doors.values() {
+                let labels = match door.kind {
+                    DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
+                        Vec::from(["right"])
+                    }
+                    DoorType::DoubleSliding(_) | DoorType::DoubleSwing(_) => {
+                        Vec::from(["right", "left"])
+                    }
+                };
+                let mut door_model = SdfModel {
+                    name: door.name.0.clone(),
+                    r#static: Some(false),
+                    ..Default::default()
+                };
+                for label in labels.iter() {
+                    door_model.link.push(SdfLink {
+                        name: label.to_string(),
+                        collision: vec![SdfCollision {
+                            name: format!("{}_collision", label),
+                            geometry: SdfGeometry::Mesh(SdfMeshShape {
+                                uri: format!("meshes/{}_{}.glb", door.name.0, label),
+                                ..Default::default()
+                            }),
+                            surface: Some(SdfSurface {
+                                contact: Some(SdfSurfaceContact {
+                                    collide_bitmask: Some("0x02".into()),
+                                    ..Default::default()
+                                }),
+                                ..Default::default()
+                            }),
+                            ..Default::default()
+                        }],
+                        visual: vec![SdfVisual {
+                            name: format!("{}_visual", label),
+                            geometry: SdfGeometry::Mesh(SdfMeshShape {
+                                uri: format!("meshes/{}_{}.glb", door.name.0, label),
+                                ..Default::default()
+                            }),
+                            ..Default::default()
+                        }],
+                        // TODO(luca) calculate inertia based on door properties
+                        inertial: Some(SdfInertial {
+                            mass: Some(door_mass),
+                            inertia: Some(SdfInertialInertia {
+                                ixx: 20.0,
+                                iyy: 20.0,
+                                izz: 5.0,
+                                ..Default::default()
+                            }),
+                            ..Default::default()
+                        }),
+                        ..Default::default()
+                    });
+                }
+                let joints = match &door.kind {
+                    DoorType::SingleSliding(door) => {
+                        // TODO(luca) set pose
+                        vec![
+                            SdfJoint {
+                                name: "right_joint".into(),
+                                parent: "world".into(),
+                                child: "right".into(),
+                                r#type: "prismatic".into(),
+                                axis: Some(SdfJointAxis {
+                                    xyz: Vector3d::new(1.0, 0.0, 0.0),
+                                    limit: SdfJointaxisLimit {
+                                        lower: 0.0,
+                                        // TODO(luca) this is length of the door
+                                        upper: 0.0,
+                                        ..Default::default()
+                                    },
+                                    ..Default::default()
+                                }),
+                                ..Default::default()
+                            }
+                        ]
+                    }
+                    DoorType::SingleSwing(swing) => {
+                        let (lower, upper) = swing.swing.swing_on_pivot(swing.pivot_on);
+                        // TODO(check this logic)
+                        let lower = 0.0;
+                        let upper = lower - upper.radians() as f64;
+                        let pivot = door.anchors.side(swing.pivot_on);
+                        let start_anchor = get_anchor(pivot, level)?;
+                        let trans = start_anchor.translation_for_category(Category::Level);
+                        let end_anchor = get_anchor(door.anchors.side(swing.pivot_on.opposite()), level)?;
+                        let end = end_anchor.translation_for_category(Category::Level);
+                        let pose = Pose {
+                            trans: [trans[0], trans[1], level.properties.elevation.0 + 1.0],
+                            ..Default::default()
+                        }.to_sdf(0.0);
+                        vec![
+                            SdfJoint {
+                                name: "right_joint".into(),
+                                parent: "world".into(),
+                                child: "right".into(),
+                                r#type: "revolute".into(),
+                                axis: Some(SdfJointAxis {
+                                    xyz: Vector3d::new(0.0, 0.0, -1.0),
+                                    limit: SdfJointaxisLimit {
+                                        lower,
+                                        upper,
+                                        ..Default::default()
+                                    },
+                                    ..Default::default()
+                                }),
+                                pose: Some(pose),
+                                ..Default::default()
+                            }
+                        ]
+                    }
+                    DoorType::DoubleSliding(door) => {
+                        vec![
+                            SdfJoint {
+                                name: "right_joint".into(),
+                                parent: "world".into(),
+                                child: "right".into(),
+                                r#type: "prismatic".into(),
+                                ..Default::default()
+                            },
+                            SdfJoint {
+                                name: "left_joint".into(),
+                                parent: "world".into(),
+                                child: "left".into(),
+                                r#type: "prismatic".into(),
+                                ..Default::default()
+                            },
+                        ]
+                    }
+                    DoorType::DoubleSwing(door) => {
+                        vec![
+                            SdfJoint {
+                                name: "right_joint".into(),
+                                parent: "world".into(),
+                                child: "right".into(),
+                                r#type: "revolute".into(),
+                                ..Default::default()
+                            },
+                            SdfJoint {
+                                name: "left_joint".into(),
+                                parent: "world".into(),
+                                child: "left".into(),
+                                r#type: "revolute".into(),
+                                ..Default::default()
+                            },
+                        ]
+                    }
+                    DoorType::Model(_) => {
+                        // Unimplemented! Use a fixed joint for now
+                        vec![
+                            SdfJoint {
+                                name: "right_joint".into(),
+                                parent: "world".into(),
+                                child: "right".into(),
+                                r#type: "fixed".into(),
+                                ..Default::default()
+                            }
+                        ]
+                    }
+                };
+                door_model.joint.extend(joints);
+                // TODO(luca) Plugin element
+
+                models.push(door_model);
+            }
         }
 
         let sun = SdfLight {

From edae72aa792da75a1c71a13ebd5e418fb5670a44 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 15 Dec 2023 15:16:35 +0800
Subject: [PATCH 07/71] Tuning to door exports

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs |  50 +++---
 rmf_site_format/src/sdf.rs               | 209 ++++++++++++++++-------
 2 files changed, 182 insertions(+), 77 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 4157507a..731271ea 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -20,6 +20,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
         Query<&GlobalTransform>,
+        Query<&Transform>,
     )> = SystemState::new(world);
     let (
         q_children,
@@ -31,25 +32,20 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         q_collisions,
         q_visuals,
         q_pbr,
+        q_global_tfs,
         q_tfs,
     ) = state.get(world);
 
     let image_assets = world.resource::<Assets<Image>>();
     let mesh_assets = world.resource::<Assets<Mesh>>();
     let material_assets = world.resource::<Assets<StandardMaterial>>();
-    let write_meshes_to_file = |
-        meshes: Vec<MeshData>,
-        name: Option<String>,
-        options: CompressGltfOptions,
-        filename: String,
-    | -> Result<(), MeshExportError> {
+    let write_meshes_to_file = |meshes: Vec<MeshData>,
+                                name: Option<String>,
+                                options: CompressGltfOptions,
+                                filename: String|
+     -> Result<(), MeshExportError> {
         let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
-        let meshes = export_meshes(
-            meshes,
-            name,
-            image_getter,
-            options,
-        )?;
+        let meshes = export_meshes(meshes, name, image_getter, options)?;
         let bytes = meshes.to_bytes()?;
         let Ok(_) = std::fs::write(filename, bytes) else {
             // TODO(luca) make this an error
@@ -59,7 +55,6 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Ok(())
     };
 
-
     let get_mesh_and_material = |entity: Entity| -> Option<(&Mesh, &StandardMaterial)> {
         let Ok((mesh, material)) = q_pbr.get(entity) else {
             return None;
@@ -125,7 +120,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                 let Some((mesh, material)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
-                                let Ok(tf) = q_tfs.get(entity) else {
+                                let Ok(tf) = q_global_tfs.get(entity) else {
                                     continue;
                                 };
                                 let tf = tf.compute_transform();
@@ -150,7 +145,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                 let Some((mesh, material)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
-                                let Ok(tf) = q_tfs.get(entity) else {
+                                let Ok(tf) = q_global_tfs.get(entity) else {
                                     continue;
                                 };
                                 let tf = tf.compute_transform();
@@ -187,7 +182,6 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                         let Ok(tf) = q_tfs.get(*entity) else {
                             continue;
                         };
-                        let tf = tf.compute_transform();
                         let pose = GltfPose {
                             translation: [
                                 tf.translation.x,
@@ -203,17 +197,33 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             material,
                             pose: Some(pose.clone()),
                         };
-                        let filename = format!("{}/{}_{}.glb", folder.display(), **door_name, segment_name);
-                        write_meshes_to_file(vec![data], None, CompressGltfOptions::default(), filename);
+                        let filename =
+                            format!("{}/{}_{}.glb", folder.display(), **door_name, segment_name);
+                        write_meshes_to_file(
+                            vec![data],
+                            None,
+                            CompressGltfOptions::default(),
+                            filename,
+                        );
                     }
                 } else {
                     continue;
                 };
             }
             let filename = format!("{}/level_{}_collision.glb", folder.display(), **level_name);
-            write_meshes_to_file(collision_data, None, CompressGltfOptions::skip_materials(), filename);
+            write_meshes_to_file(
+                collision_data,
+                None,
+                CompressGltfOptions::skip_materials(),
+                filename,
+            );
             let filename = format!("{}/level_{}_visual.glb", folder.display(), **level_name);
-            write_meshes_to_file(visual_data, Some(format!("level_{}_visuals", **level_name)), CompressGltfOptions::default(), filename);
+            write_meshes_to_file(
+                visual_data,
+                Some(format!("level_{}_visuals", **level_name)),
+                CompressGltfOptions::default(),
+                filename,
+            );
         }
     }
 }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index fa1740db..4fb80f6a 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -15,7 +15,7 @@
  *
 */
 
-use crate::{Category, Side, Anchor, DoorType, Level, Pose, Rotation, Site};
+use crate::{Anchor, Angle, Category, DoorType, Level, Pose, Rotation, Side, Site, Swing};
 use sdformat_rs::*;
 
 #[derive(Debug)]
@@ -94,6 +94,18 @@ impl Site {
             });
             // Now add all the doors
             for door in level.doors.values() {
+                let left_anchor = get_anchor(door.anchors.left(), level)?;
+                let left_trans = left_anchor.translation_for_category(Category::Door);
+                let right_anchor = get_anchor(door.anchors.right(), level)?;
+                let right_trans = right_anchor.translation_for_category(Category::Door);
+                let center = [
+                    (left_trans[0] + right_trans[0]) / 2.0,
+                    (left_trans[1] + right_trans[1]) / 2.0,
+                ];
+                let dx = left_trans[0] - right_trans[0];
+                let dy = left_trans[1] - right_trans[1];
+                let door_length = (dx * dx + dy * dy).sqrt();
+                let yaw = -dx.atan2(dy);
                 let labels = match door.kind {
                     DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
                         Vec::from(["right"])
@@ -104,6 +116,13 @@ impl Site {
                 };
                 let mut door_model = SdfModel {
                     name: door.name.0.clone(),
+                    pose: Some(
+                        Pose {
+                            trans: [center[0], center[1], level.properties.elevation.0],
+                            rot: Rotation::Yaw(Angle::Rad(yaw)),
+                        }
+                        .to_sdf(0.0),
+                    ),
                     r#static: Some(false),
                     ..Default::default()
                 };
@@ -149,68 +168,92 @@ impl Site {
                 }
                 let joints = match &door.kind {
                     DoorType::SingleSliding(door) => {
-                        // TODO(luca) set pose
-                        vec![
-                            SdfJoint {
-                                name: "right_joint".into(),
-                                parent: "world".into(),
-                                child: "right".into(),
-                                r#type: "prismatic".into(),
-                                axis: Some(SdfJointAxis {
-                                    xyz: Vector3d::new(1.0, 0.0, 0.0),
-                                    limit: SdfJointaxisLimit {
-                                        lower: 0.0,
-                                        // TODO(luca) this is length of the door
-                                        upper: 0.0,
-                                        ..Default::default()
-                                    },
+                        let pose = Pose {
+                            trans: [0.0, (door_length / 2.0) * door.towards.sign(), 1.25],
+                            ..Default::default()
+                        }
+                        .to_sdf(0.0);
+                        vec![SdfJoint {
+                            name: "right_joint".into(),
+                            parent: "world".into(),
+                            child: "right".into(),
+                            r#type: "prismatic".into(),
+                            pose: Some(pose),
+                            axis: Some(SdfJointAxis {
+                                xyz: Vector3d::new(0.0, door.towards.sign(), 0.0),
+                                limit: SdfJointaxisLimit {
+                                    lower: 0.0,
+                                    upper: door_length as f64,
                                     ..Default::default()
-                                }),
+                                },
                                 ..Default::default()
-                            }
-                        ]
+                            }),
+                            ..Default::default()
+                        }]
                     }
-                    DoorType::SingleSwing(swing) => {
-                        let (lower, upper) = swing.swing.swing_on_pivot(swing.pivot_on);
-                        // TODO(check this logic)
-                        let lower = 0.0;
-                        let upper = lower - upper.radians() as f64;
-                        let pivot = door.anchors.side(swing.pivot_on);
-                        let start_anchor = get_anchor(pivot, level)?;
-                        let trans = start_anchor.translation_for_category(Category::Level);
-                        let end_anchor = get_anchor(door.anchors.side(swing.pivot_on.opposite()), level)?;
-                        let end = end_anchor.translation_for_category(Category::Level);
+                    DoorType::SingleSwing(door) => {
+                        let side = door.pivot_on.sign() as f64;
+                        let closed = 0.0_f64;
+                        let (open, z) = match door.swing {
+                            Swing::Forward(angle) => (side * angle.radians() as f64, 1.0),
+                            Swing::Backward(angle) => (-side * angle.radians() as f64, -1.0),
+                            // Only use the forward position for double doors
+                            Swing::Both { forward, backward } => {
+                                (side * forward.radians() as f64, 1.0)
+                            }
+                        };
+                        let lower = closed.min(closed + open);
+                        let upper = closed.max(closed + open);
                         let pose = Pose {
-                            trans: [trans[0], trans[1], level.properties.elevation.0 + 1.0],
+                            trans: [0.0, (door_length / 2.0) * door.pivot_on.sign(), 1.25],
                             ..Default::default()
-                        }.to_sdf(0.0);
+                        }
+                        .to_sdf(0.0);
+                        vec![SdfJoint {
+                            name: "right_joint".into(),
+                            parent: "world".into(),
+                            child: "right".into(),
+                            r#type: "revolute".into(),
+                            axis: Some(SdfJointAxis {
+                                xyz: Vector3d::new(0.0, 0.0, z),
+                                limit: SdfJointaxisLimit {
+                                    lower,
+                                    upper,
+                                    ..Default::default()
+                                },
+                                ..Default::default()
+                            }),
+                            pose: Some(pose),
+                            ..Default::default()
+                        }]
+                    }
+                    DoorType::DoubleSliding(door) => {
+                        let right_pose = Pose {
+                            trans: [0.0, -door_length / 2.0, 1.25],
+                            ..Default::default()
+                        }
+                        .to_sdf(0.0);
+                        let left_pose = Pose {
+                            trans: [0.0, door_length / 2.0, 1.25],
+                            ..Default::default()
+                        }
+                        .to_sdf(0.0);
                         vec![
                             SdfJoint {
                                 name: "right_joint".into(),
                                 parent: "world".into(),
                                 child: "right".into(),
-                                r#type: "revolute".into(),
+                                r#type: "prismatic".into(),
+                                pose: Some(right_pose),
                                 axis: Some(SdfJointAxis {
-                                    xyz: Vector3d::new(0.0, 0.0, -1.0),
+                                    xyz: Vector3d::new(0.0, 1.0, 0.0),
                                     limit: SdfJointaxisLimit {
-                                        lower,
-                                        upper,
+                                        lower: 0.0,
+                                        upper: door_length as f64 / 2.0,
                                         ..Default::default()
                                     },
                                     ..Default::default()
                                 }),
-                                pose: Some(pose),
-                                ..Default::default()
-                            }
-                        ]
-                    }
-                    DoorType::DoubleSliding(door) => {
-                        vec![
-                            SdfJoint {
-                                name: "right_joint".into(),
-                                parent: "world".into(),
-                                child: "right".into(),
-                                r#type: "prismatic".into(),
                                 ..Default::default()
                             },
                             SdfJoint {
@@ -218,17 +261,56 @@ impl Site {
                                 parent: "world".into(),
                                 child: "left".into(),
                                 r#type: "prismatic".into(),
+                                pose: Some(left_pose),
+                                axis: Some(SdfJointAxis {
+                                    xyz: Vector3d::new(0.0, -1.0, 0.0),
+                                    limit: SdfJointaxisLimit {
+                                        lower: -door_length as f64 / 2.0,
+                                        upper: 0.0,
+                                        ..Default::default()
+                                    },
+                                    ..Default::default()
+                                }),
                                 ..Default::default()
                             },
                         ]
                     }
                     DoorType::DoubleSwing(door) => {
+                        let closed = 0.0_f64;
+                        let (open, z) = match door.swing {
+                            Swing::Forward(angle) => (-angle.radians() as f64, 1.0),
+                            Swing::Backward(angle) => (angle.radians() as f64, -1.0),
+                            // Only use the forward position for double doors
+                            Swing::Both { forward, backward } => (-forward.radians() as f64, 1.0),
+                        };
+                        let lower = closed.min(open);
+                        let upper = closed.max(open);
+                        let right_pose = Pose {
+                            trans: [0.0, -door_length / 2.0, 1.25],
+                            ..Default::default()
+                        }
+                        .to_sdf(0.0);
+                        let left_pose = Pose {
+                            trans: [0.0, door_length / 2.0, 1.25],
+                            ..Default::default()
+                        }
+                        .to_sdf(0.0);
                         vec![
                             SdfJoint {
                                 name: "right_joint".into(),
                                 parent: "world".into(),
                                 child: "right".into(),
                                 r#type: "revolute".into(),
+                                axis: Some(SdfJointAxis {
+                                    xyz: Vector3d::new(0.0, 0.0, z),
+                                    limit: SdfJointaxisLimit {
+                                        lower,
+                                        upper,
+                                        ..Default::default()
+                                    },
+                                    ..Default::default()
+                                }),
+                                pose: Some(right_pose),
                                 ..Default::default()
                             },
                             SdfJoint {
@@ -236,26 +318,39 @@ impl Site {
                                 parent: "world".into(),
                                 child: "left".into(),
                                 r#type: "revolute".into(),
+                                axis: Some(SdfJointAxis {
+                                    xyz: Vector3d::new(0.0, 0.0, -z),
+                                    limit: SdfJointaxisLimit {
+                                        lower: -upper,
+                                        upper: -lower,
+                                        ..Default::default()
+                                    },
+                                    ..Default::default()
+                                }),
+                                pose: Some(left_pose),
                                 ..Default::default()
                             },
                         ]
                     }
                     DoorType::Model(_) => {
                         // Unimplemented! Use a fixed joint for now
-                        vec![
-                            SdfJoint {
-                                name: "right_joint".into(),
-                                parent: "world".into(),
-                                child: "right".into(),
-                                r#type: "fixed".into(),
-                                ..Default::default()
-                            }
-                        ]
+                        let pose = Pose {
+                            trans: [0.0, door_length / 2.0, 1.25],
+                            ..Default::default()
+                        }
+                        .to_sdf(0.0);
+                        vec![SdfJoint {
+                            name: "right_joint".into(),
+                            parent: "world".into(),
+                            child: "right".into(),
+                            r#type: "fixed".into(),
+                            pose: Some(pose),
+                            ..Default::default()
+                        }]
                     }
                 };
                 door_model.joint.extend(joints);
                 // TODO(luca) Plugin element
-
                 models.push(door_model);
             }
         }

From 7587db5cbc623e882ffaf23d5f40178142b91a82 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Dec 2023 14:55:27 +0800
Subject: [PATCH 08/71] Fix door transform

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 6 +-----
 rmf_site_format/src/sdf.rs               | 2 +-
 2 files changed, 2 insertions(+), 6 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 731271ea..e6076403 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -183,11 +183,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             continue;
                         };
                         let pose = GltfPose {
-                            translation: [
-                                tf.translation.x,
-                                tf.translation.y,
-                                tf.translation.z + **elevation,
-                            ],
+                            translation: tf.translation.to_array(),
                             rotation: tf.rotation.to_array(),
                             scale: Some(tf.scale.to_array()),
                         };
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 4fb80f6a..1dc7e043 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -180,7 +180,7 @@ impl Site {
                             r#type: "prismatic".into(),
                             pose: Some(pose),
                             axis: Some(SdfJointAxis {
-                                xyz: Vector3d::new(0.0, door.towards.sign(), 0.0),
+                                xyz: Vector3d::new(0.0, door.towards.sign().into(), 0.0),
                                 limit: SdfJointaxisLimit {
                                     lower: 0.0,
                                     upper: door_length as f64,

From 946a2ab290669a2f36a9564562ff8fa1e36ba0de Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 22 Dec 2023 15:07:49 +0800
Subject: [PATCH 09/71] Finish edge cases for door plugins

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/door.rs         |  12 +
 rmf_site_editor/src/site/sdf_exporter.rs |   3 +-
 rmf_site_format/src/sdf.rs               | 613 +++++++++++++----------
 3 files changed, 368 insertions(+), 260 deletions(-)

diff --git a/rmf_site_editor/src/site/door.rs b/rmf_site_editor/src/site/door.rs
index af904e1c..3cf212d7 100644
--- a/rmf_site_editor/src/site/door.rs
+++ b/rmf_site_editor/src/site/door.rs
@@ -72,6 +72,18 @@ impl DoorBodyType {
             | DoorBodyType::DoubleSliding { left, right } => vec![*left, *right],
         }
     }
+
+    pub fn labels(&self) -> Vec<&str> {
+        match self {
+            DoorBodyType::SingleSwing { body }
+            | DoorBodyType::SingleSliding { body }
+            | DoorBodyType::Model { body } => {
+                vec!["body"]
+            }
+            DoorBodyType::DoubleSwing { left, right }
+            | DoorBodyType::DoubleSliding { left, right } => vec!["left", "right"],
+        }
+    }
 }
 
 #[derive(Debug, Clone, Copy, Component)]
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index e6076403..993af776 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -172,8 +172,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                         .body
                         .entities()
                         .iter()
-                        .rev()
-                        .zip(["right", "left"].into_iter())
+                        .zip(segments.body.labels().into_iter())
                     {
                         // Generate the visual and collisions here
                         let Some((mesh, material)) = get_mesh_and_material(*entity) else {
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 1dc7e043..17a7b7a1 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -15,7 +15,7 @@
  *
 */
 
-use crate::{Anchor, Angle, Category, DoorType, Level, Pose, Rotation, Side, Site, Swing};
+use crate::{Anchor, Angle, Category, Door, DoorType, Level, Pose, Rotation, Side, Site, Swing};
 use sdformat_rs::*;
 
 #[derive(Debug)]
@@ -46,6 +46,355 @@ impl Pose {
     }
 }
 
+fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink {
+    let door_mass = 50.0;
+    SdfLink {
+        name: link_name.to_string(),
+        collision: vec![SdfCollision {
+            name: format!("{}_collision", link_name),
+            geometry: SdfGeometry::Mesh(SdfMeshShape {
+                uri: format!("meshes/{}_{}.glb", door_name, link_name),
+                ..Default::default()
+            }),
+            surface: Some(SdfSurface {
+                contact: Some(SdfSurfaceContact {
+                    collide_bitmask: Some("0x02".into()),
+                    ..Default::default()
+                }),
+                ..Default::default()
+            }),
+            ..Default::default()
+        }],
+        visual: vec![SdfVisual {
+            name: format!("{}_visual", link_name),
+            geometry: SdfGeometry::Mesh(SdfMeshShape {
+                uri: format!("meshes/{}_{}.glb", door_name, link_name),
+                ..Default::default()
+            }),
+            ..Default::default()
+        }],
+        // TODO(luca) calculate inertia based on door properties
+        inertial: Some(SdfInertial {
+            mass: Some(door_mass),
+            inertia: Some(SdfInertialInertia {
+                ixx: 20.0,
+                iyy: 20.0,
+                izz: 5.0,
+                ..Default::default()
+            }),
+            ..Default::default()
+        }),
+        ..Default::default()
+    }
+}
+
+impl Door<u32> {
+    pub fn to_sdf(
+        &self,
+        left_anchor: Anchor,
+        right_anchor: Anchor,
+        elevation: f32,
+    ) -> Result<SdfModel, SdfConversionError> {
+        let door_mass = 50.0;
+        let left_trans = left_anchor.translation_for_category(Category::Door);
+        let right_trans = right_anchor.translation_for_category(Category::Door);
+        let center = [
+            (left_trans[0] + right_trans[0]) / 2.0,
+            (left_trans[1] + right_trans[1]) / 2.0,
+        ];
+        let dx = left_trans[0] - right_trans[0];
+        let dy = left_trans[1] - right_trans[1];
+        let door_length = (dx * dx + dy * dy).sqrt();
+        let yaw = -dx.atan2(dy);
+        let labels = match self.kind {
+            DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
+                Vec::from(["body"])
+            }
+            DoorType::DoubleSliding(_) | DoorType::DoubleSwing(_) => Vec::from(["right", "left"]),
+        };
+        let mut plugin = SdfPlugin {
+            name: "door".into(),
+            filename: "libdoor.so".into(),
+            ..Default::default()
+        };
+        let mut door_plugin_inner = XmlElement {
+            name: "door".into(),
+            ..Default::default()
+        };
+        door_plugin_inner
+            .attributes
+            .insert("name".to_string(), self.name.0.clone());
+        let mut door_model = SdfModel {
+            name: self.name.0.clone(),
+            pose: Some(
+                Pose {
+                    trans: [center[0], center[1], elevation],
+                    rot: Rotation::Yaw(Angle::Rad(yaw)),
+                }
+                .to_sdf(0.0),
+            ),
+            r#static: Some(false),
+            ..Default::default()
+        };
+        for label in labels.iter() {
+            door_model
+                .link
+                .push(make_sdf_door_link(&self.name.0, label));
+        }
+        let mut door_motion_params = vec![];
+        let joints = match &self.kind {
+            DoorType::SingleSliding(door) => {
+                door_plugin_inner
+                    .attributes
+                    .insert("type".into(), "SlidingDoor".into());
+                door_plugin_inner
+                    .attributes
+                    .insert("left_joint_name".into(), "empty_joint".into());
+                door_plugin_inner
+                    .attributes
+                    .insert("right_joint_name".into(), "joint".into());
+                door_motion_params.push(("v_max_door", "0.2"));
+                door_motion_params.push(("a_max_door", "0.2"));
+                door_motion_params.push(("a_nom_door", "0.08"));
+                door_motion_params.push(("dx_min_door", "0.001"));
+                door_motion_params.push(("f_max_door", "100.0"));
+                let pose = Pose {
+                    trans: [0.0, (door_length / 2.0) * door.towards.sign(), 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                vec![SdfJoint {
+                    name: "joint".into(),
+                    parent: "world".into(),
+                    child: "body".into(),
+                    r#type: "prismatic".into(),
+                    pose: Some(pose),
+                    axis: Some(SdfJointAxis {
+                        xyz: Vector3d::new(0.0, door.towards.sign().into(), 0.0),
+                        limit: SdfJointaxisLimit {
+                            lower: 0.0,
+                            upper: door_length as f64,
+                            ..Default::default()
+                        },
+                        ..Default::default()
+                    }),
+                    ..Default::default()
+                }]
+            }
+            DoorType::SingleSwing(door) => {
+                door_plugin_inner
+                    .attributes
+                    .insert("type".into(), "SwingDoor".into());
+                door_motion_params.push(("v_max_door", "0.5"));
+                door_motion_params.push(("a_max_door", "0.3"));
+                door_motion_params.push(("a_nom_door", "0.15"));
+                door_motion_params.push(("dx_min_door", "0.01"));
+                door_motion_params.push(("f_max_door", "500.0"));
+                let side = door.pivot_on.sign() as f64;
+                let (open, z) = match door.swing {
+                    Swing::Forward(angle) => (angle.radians() as f64, side),
+                    Swing::Backward(angle) => (angle.radians() as f64, -side),
+                    // Only use the forward position for double doors
+                    Swing::Both { forward, backward } => (forward.radians() as f64, side),
+                };
+                let lower = 0.0;
+                let upper = open.abs();
+                let pose = Pose {
+                    trans: [0.0, (door_length / 2.0) * door.pivot_on.sign(), 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                let (left_joint_name, right_joint_name) = ("empty_joint", "joint");
+                door_plugin_inner
+                    .attributes
+                    .insert("left_joint_name".into(), left_joint_name.into());
+                door_plugin_inner
+                    .attributes
+                    .insert("right_joint_name".into(), right_joint_name.into());
+                vec![SdfJoint {
+                    name: "joint".into(),
+                    parent: "world".into(),
+                    child: "body".into(),
+                    r#type: "revolute".into(),
+                    axis: Some(SdfJointAxis {
+                        xyz: Vector3d::new(0.0, 0.0, z),
+                        limit: SdfJointaxisLimit {
+                            lower,
+                            upper,
+                            ..Default::default()
+                        },
+                        ..Default::default()
+                    }),
+                    pose: Some(pose),
+                    ..Default::default()
+                }]
+            }
+            DoorType::DoubleSliding(door) => {
+                door_plugin_inner
+                    .attributes
+                    .insert("type".into(), "DoubleSlidingDoor".into());
+                door_plugin_inner
+                    .attributes
+                    .insert("left_joint_name".into(), "left_joint".into());
+                door_plugin_inner
+                    .attributes
+                    .insert("right_joint_name".into(), "right_joint".into());
+                door_motion_params.push(("v_max_door", "0.2"));
+                door_motion_params.push(("a_max_door", "0.2"));
+                door_motion_params.push(("a_nom_door", "0.08"));
+                door_motion_params.push(("dx_min_door", "0.001"));
+                door_motion_params.push(("f_max_door", "100.0"));
+                let right_pose = Pose {
+                    trans: [0.0, -door_length / 2.0, 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                let left_pose = Pose {
+                    trans: [0.0, door_length / 2.0, 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                let left_length =
+                    (door.left_right_ratio / (1.0 + door.left_right_ratio)) * door_length;
+                let right_length = door_length - left_length;
+                vec![
+                    SdfJoint {
+                        name: "right_joint".into(),
+                        parent: "world".into(),
+                        child: "right".into(),
+                        r#type: "prismatic".into(),
+                        pose: Some(right_pose),
+                        axis: Some(SdfJointAxis {
+                            xyz: Vector3d::new(0.0, -1.0, 0.0),
+                            limit: SdfJointaxisLimit {
+                                lower: 0.0,
+                                upper: right_length as f64,
+                                ..Default::default()
+                            },
+                            ..Default::default()
+                        }),
+                        ..Default::default()
+                    },
+                    SdfJoint {
+                        name: "left_joint".into(),
+                        parent: "world".into(),
+                        child: "left".into(),
+                        r#type: "prismatic".into(),
+                        pose: Some(left_pose),
+                        axis: Some(SdfJointAxis {
+                            xyz: Vector3d::new(0.0, -1.0, 0.0),
+                            limit: SdfJointaxisLimit {
+                                lower: -left_length as f64,
+                                upper: 0.0,
+                                ..Default::default()
+                            },
+                            ..Default::default()
+                        }),
+                        ..Default::default()
+                    },
+                ]
+            }
+            DoorType::DoubleSwing(door) => {
+                door_plugin_inner
+                    .attributes
+                    .insert("type".into(), "DoubleSwingDoor".into());
+                door_plugin_inner
+                    .attributes
+                    .insert("left_joint_name".into(), "left_joint".into());
+                door_plugin_inner
+                    .attributes
+                    .insert("right_joint_name".into(), "right_joint".into());
+                door_motion_params.push(("v_max_door", "0.5"));
+                door_motion_params.push(("a_max_door", "0.3"));
+                door_motion_params.push(("a_nom_door", "0.15"));
+                door_motion_params.push(("dx_min_door", "0.01"));
+                door_motion_params.push(("f_max_door", "500.0"));
+                let (open, z) = match door.swing {
+                    Swing::Forward(angle) => (angle.radians() as f64, -1.0),
+                    Swing::Backward(angle) => (angle.radians() as f64, 1.0),
+                    // Only use the forward position for double doors
+                    Swing::Both { forward, backward } => (forward.radians() as f64, -1.0),
+                };
+                let lower = 0.0;
+                let upper = open.abs();
+                let right_pose = Pose {
+                    trans: [0.0, -door_length / 2.0, 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                let left_pose = Pose {
+                    trans: [0.0, door_length / 2.0, 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                vec![
+                    SdfJoint {
+                        name: "right_joint".into(),
+                        parent: "world".into(),
+                        child: "right".into(),
+                        r#type: "revolute".into(),
+                        axis: Some(SdfJointAxis {
+                            xyz: Vector3d::new(0.0, 0.0, z),
+                            limit: SdfJointaxisLimit {
+                                lower: 0.0,
+                                upper,
+                                ..Default::default()
+                            },
+                            ..Default::default()
+                        }),
+                        pose: Some(right_pose),
+                        ..Default::default()
+                    },
+                    SdfJoint {
+                        name: "left_joint".into(),
+                        parent: "world".into(),
+                        child: "left".into(),
+                        r#type: "revolute".into(),
+                        axis: Some(SdfJointAxis {
+                            xyz: Vector3d::new(0.0, 0.0, z),
+                            limit: SdfJointaxisLimit {
+                                lower: -upper,
+                                upper: 0.0,
+                                ..Default::default()
+                            },
+                            ..Default::default()
+                        }),
+                        pose: Some(left_pose),
+                        ..Default::default()
+                    },
+                ]
+            }
+            DoorType::Model(_) => {
+                // Unimplemented! Use a fixed joint for now
+                let pose = Pose {
+                    trans: [0.0, door_length / 2.0, 1.25],
+                    ..Default::default()
+                }
+                .to_sdf(0.0);
+                vec![SdfJoint {
+                    name: "joint".into(),
+                    parent: "world".into(),
+                    child: "body".into(),
+                    r#type: "fixed".into(),
+                    pose: Some(pose),
+                    ..Default::default()
+                }]
+            }
+        };
+        door_model.joint.extend(joints);
+        for (name, value) in door_motion_params.into_iter() {
+            plugin.elements.push(XmlElement {
+                name: name.into(),
+                data: ElementData::String(value.to_string()),
+                ..Default::default()
+            });
+        }
+        plugin.elements.push(door_plugin_inner);
+        door_model.plugin = vec![plugin];
+        Ok(door_model)
+    }
+}
+
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
         let get_anchor = |id: u32, level: &Level| -> Result<Anchor, SdfConversionError> {
@@ -56,7 +405,6 @@ impl Site {
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
         };
-        let door_mass = 50.0;
         let mut models = Vec::new();
         for level in self.levels.values() {
             // Floors walls and static models are included in the level mesh
@@ -95,263 +443,12 @@ impl Site {
             // Now add all the doors
             for door in level.doors.values() {
                 let left_anchor = get_anchor(door.anchors.left(), level)?;
-                let left_trans = left_anchor.translation_for_category(Category::Door);
                 let right_anchor = get_anchor(door.anchors.right(), level)?;
-                let right_trans = right_anchor.translation_for_category(Category::Door);
-                let center = [
-                    (left_trans[0] + right_trans[0]) / 2.0,
-                    (left_trans[1] + right_trans[1]) / 2.0,
-                ];
-                let dx = left_trans[0] - right_trans[0];
-                let dy = left_trans[1] - right_trans[1];
-                let door_length = (dx * dx + dy * dy).sqrt();
-                let yaw = -dx.atan2(dy);
-                let labels = match door.kind {
-                    DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
-                        Vec::from(["right"])
-                    }
-                    DoorType::DoubleSliding(_) | DoorType::DoubleSwing(_) => {
-                        Vec::from(["right", "left"])
-                    }
-                };
-                let mut door_model = SdfModel {
-                    name: door.name.0.clone(),
-                    pose: Some(
-                        Pose {
-                            trans: [center[0], center[1], level.properties.elevation.0],
-                            rot: Rotation::Yaw(Angle::Rad(yaw)),
-                        }
-                        .to_sdf(0.0),
-                    ),
-                    r#static: Some(false),
-                    ..Default::default()
-                };
-                for label in labels.iter() {
-                    door_model.link.push(SdfLink {
-                        name: label.to_string(),
-                        collision: vec![SdfCollision {
-                            name: format!("{}_collision", label),
-                            geometry: SdfGeometry::Mesh(SdfMeshShape {
-                                uri: format!("meshes/{}_{}.glb", door.name.0, label),
-                                ..Default::default()
-                            }),
-                            surface: Some(SdfSurface {
-                                contact: Some(SdfSurfaceContact {
-                                    collide_bitmask: Some("0x02".into()),
-                                    ..Default::default()
-                                }),
-                                ..Default::default()
-                            }),
-                            ..Default::default()
-                        }],
-                        visual: vec![SdfVisual {
-                            name: format!("{}_visual", label),
-                            geometry: SdfGeometry::Mesh(SdfMeshShape {
-                                uri: format!("meshes/{}_{}.glb", door.name.0, label),
-                                ..Default::default()
-                            }),
-                            ..Default::default()
-                        }],
-                        // TODO(luca) calculate inertia based on door properties
-                        inertial: Some(SdfInertial {
-                            mass: Some(door_mass),
-                            inertia: Some(SdfInertialInertia {
-                                ixx: 20.0,
-                                iyy: 20.0,
-                                izz: 5.0,
-                                ..Default::default()
-                            }),
-                            ..Default::default()
-                        }),
-                        ..Default::default()
-                    });
-                }
-                let joints = match &door.kind {
-                    DoorType::SingleSliding(door) => {
-                        let pose = Pose {
-                            trans: [0.0, (door_length / 2.0) * door.towards.sign(), 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        vec![SdfJoint {
-                            name: "right_joint".into(),
-                            parent: "world".into(),
-                            child: "right".into(),
-                            r#type: "prismatic".into(),
-                            pose: Some(pose),
-                            axis: Some(SdfJointAxis {
-                                xyz: Vector3d::new(0.0, door.towards.sign().into(), 0.0),
-                                limit: SdfJointaxisLimit {
-                                    lower: 0.0,
-                                    upper: door_length as f64,
-                                    ..Default::default()
-                                },
-                                ..Default::default()
-                            }),
-                            ..Default::default()
-                        }]
-                    }
-                    DoorType::SingleSwing(door) => {
-                        let side = door.pivot_on.sign() as f64;
-                        let closed = 0.0_f64;
-                        let (open, z) = match door.swing {
-                            Swing::Forward(angle) => (side * angle.radians() as f64, 1.0),
-                            Swing::Backward(angle) => (-side * angle.radians() as f64, -1.0),
-                            // Only use the forward position for double doors
-                            Swing::Both { forward, backward } => {
-                                (side * forward.radians() as f64, 1.0)
-                            }
-                        };
-                        let lower = closed.min(closed + open);
-                        let upper = closed.max(closed + open);
-                        let pose = Pose {
-                            trans: [0.0, (door_length / 2.0) * door.pivot_on.sign(), 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        vec![SdfJoint {
-                            name: "right_joint".into(),
-                            parent: "world".into(),
-                            child: "right".into(),
-                            r#type: "revolute".into(),
-                            axis: Some(SdfJointAxis {
-                                xyz: Vector3d::new(0.0, 0.0, z),
-                                limit: SdfJointaxisLimit {
-                                    lower,
-                                    upper,
-                                    ..Default::default()
-                                },
-                                ..Default::default()
-                            }),
-                            pose: Some(pose),
-                            ..Default::default()
-                        }]
-                    }
-                    DoorType::DoubleSliding(door) => {
-                        let right_pose = Pose {
-                            trans: [0.0, -door_length / 2.0, 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        let left_pose = Pose {
-                            trans: [0.0, door_length / 2.0, 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        vec![
-                            SdfJoint {
-                                name: "right_joint".into(),
-                                parent: "world".into(),
-                                child: "right".into(),
-                                r#type: "prismatic".into(),
-                                pose: Some(right_pose),
-                                axis: Some(SdfJointAxis {
-                                    xyz: Vector3d::new(0.0, 1.0, 0.0),
-                                    limit: SdfJointaxisLimit {
-                                        lower: 0.0,
-                                        upper: door_length as f64 / 2.0,
-                                        ..Default::default()
-                                    },
-                                    ..Default::default()
-                                }),
-                                ..Default::default()
-                            },
-                            SdfJoint {
-                                name: "left_joint".into(),
-                                parent: "world".into(),
-                                child: "left".into(),
-                                r#type: "prismatic".into(),
-                                pose: Some(left_pose),
-                                axis: Some(SdfJointAxis {
-                                    xyz: Vector3d::new(0.0, -1.0, 0.0),
-                                    limit: SdfJointaxisLimit {
-                                        lower: -door_length as f64 / 2.0,
-                                        upper: 0.0,
-                                        ..Default::default()
-                                    },
-                                    ..Default::default()
-                                }),
-                                ..Default::default()
-                            },
-                        ]
-                    }
-                    DoorType::DoubleSwing(door) => {
-                        let closed = 0.0_f64;
-                        let (open, z) = match door.swing {
-                            Swing::Forward(angle) => (-angle.radians() as f64, 1.0),
-                            Swing::Backward(angle) => (angle.radians() as f64, -1.0),
-                            // Only use the forward position for double doors
-                            Swing::Both { forward, backward } => (-forward.radians() as f64, 1.0),
-                        };
-                        let lower = closed.min(open);
-                        let upper = closed.max(open);
-                        let right_pose = Pose {
-                            trans: [0.0, -door_length / 2.0, 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        let left_pose = Pose {
-                            trans: [0.0, door_length / 2.0, 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        vec![
-                            SdfJoint {
-                                name: "right_joint".into(),
-                                parent: "world".into(),
-                                child: "right".into(),
-                                r#type: "revolute".into(),
-                                axis: Some(SdfJointAxis {
-                                    xyz: Vector3d::new(0.0, 0.0, z),
-                                    limit: SdfJointaxisLimit {
-                                        lower,
-                                        upper,
-                                        ..Default::default()
-                                    },
-                                    ..Default::default()
-                                }),
-                                pose: Some(right_pose),
-                                ..Default::default()
-                            },
-                            SdfJoint {
-                                name: "left_joint".into(),
-                                parent: "world".into(),
-                                child: "left".into(),
-                                r#type: "revolute".into(),
-                                axis: Some(SdfJointAxis {
-                                    xyz: Vector3d::new(0.0, 0.0, -z),
-                                    limit: SdfJointaxisLimit {
-                                        lower: -upper,
-                                        upper: -lower,
-                                        ..Default::default()
-                                    },
-                                    ..Default::default()
-                                }),
-                                pose: Some(left_pose),
-                                ..Default::default()
-                            },
-                        ]
-                    }
-                    DoorType::Model(_) => {
-                        // Unimplemented! Use a fixed joint for now
-                        let pose = Pose {
-                            trans: [0.0, door_length / 2.0, 1.25],
-                            ..Default::default()
-                        }
-                        .to_sdf(0.0);
-                        vec![SdfJoint {
-                            name: "right_joint".into(),
-                            parent: "world".into(),
-                            child: "right".into(),
-                            r#type: "fixed".into(),
-                            pose: Some(pose),
-                            ..Default::default()
-                        }]
-                    }
-                };
-                door_model.joint.extend(joints);
-                // TODO(luca) Plugin element
-                models.push(door_model);
+                models.push(door.to_sdf(
+                    left_anchor,
+                    right_anchor,
+                    level.properties.elevation.0,
+                )?);
             }
         }
 

From e9e7e45ec83e0f483006b67d73b63b52cdd6b388 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 22 Dec 2023 17:26:24 +0800
Subject: [PATCH 10/71] Add lift meshes

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 33 +++++++++++-
 rmf_site_format/src/sdf.rs               | 67 +++++++++++++++++++++---
 2 files changed, 93 insertions(+), 7 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 993af776..31e824a4 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -5,7 +5,10 @@ use bevy_gltf_export::{export_meshes, CompressGltfOptions, GltfPose, MeshData, M
 use std::collections::BTreeMap;
 use std::path::Path;
 
-use crate::site::{CollisionMeshMarker, DoorSegments, FloorSegments, VisualMeshMarker};
+use crate::site::{
+    ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, FloorSegments, LiftDoormat,
+    VisualMeshMarker,
+};
 use rmf_site_format::{LevelElevation, ModelMarker, NameInSite, Pose, WallMarker};
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
@@ -19,6 +22,8 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
         Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
+        Query<(&NameInSite, &ChildLiftCabinGroup)>,
+        Query<((), With<LiftDoormat>)>,
         Query<&GlobalTransform>,
         Query<&Transform>,
     )> = SystemState::new(world);
@@ -32,6 +37,8 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         q_collisions,
         q_visuals,
         q_pbr,
+        q_lift_cabins,
+        q_lift_door_mats,
         q_global_tfs,
         q_tfs,
     ) = state.get(world);
@@ -220,5 +227,29 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                 filename,
             );
         }
+        // Lifts
+        if let Ok((lift_name, lift_cabin)) = q_lift_cabins.get(*site_child) {
+            // The children of this entity have the mesh for the lift cabin
+            info!("New lift");
+            let mut lift_data = vec![];
+            for entity in DescendantIter::new(&q_children, **lift_cabin) {
+                if q_lift_door_mats.get(entity).is_ok() {
+                    // Just visual cues, not exported
+                    continue;
+                }
+                let Some((mesh, material)) = get_mesh_and_material(entity) else {
+                    info!("Cabin Child without mesh!");
+                    continue;
+                };
+                info!("Found mesh for cabin child");
+                lift_data.push(MeshData {
+                    mesh,
+                    material,
+                    pose: None,
+                });
+            }
+            let filename = format!("{}/{}.glb", folder.display(), **lift_name);
+            write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename);
+        }
     }
 }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 17a7b7a1..c2bd9c35 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -15,7 +15,9 @@
  *
 */
 
-use crate::{Anchor, Angle, Category, Door, DoorType, Level, Pose, Rotation, Side, Site, Swing};
+use crate::{
+    Anchor, Angle, Category, Door, DoorType, Level, LiftCabin, Pose, Rotation, Side, Site, Swing,
+};
 use sdformat_rs::*;
 
 #[derive(Debug)]
@@ -397,10 +399,9 @@ impl Door<u32> {
 
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
-        let get_anchor = |id: u32, level: &Level| -> Result<Anchor, SdfConversionError> {
+        let get_anchor = |id: u32, level: Option<&Level>| -> Result<Anchor, SdfConversionError> {
             level
-                .anchors
-                .get(&id)
+                .and_then(|l| l.anchors.get(&id))
                 .or_else(|| self.anchors.get(&id))
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
@@ -442,8 +443,8 @@ impl Site {
             });
             // Now add all the doors
             for door in level.doors.values() {
-                let left_anchor = get_anchor(door.anchors.left(), level)?;
-                let right_anchor = get_anchor(door.anchors.right(), level)?;
+                let left_anchor = get_anchor(door.anchors.left(), Some(level))?;
+                let right_anchor = get_anchor(door.anchors.right(), Some(level))?;
                 models.push(door.to_sdf(
                     left_anchor,
                     right_anchor,
@@ -451,6 +452,60 @@ impl Site {
                 )?);
             }
         }
+        for lift in self.lifts.values() {
+            // Cabin
+            let LiftCabin::Rect(ref cabin) = lift.properties.cabin else {
+                continue;
+            };
+            let center = cabin.aabb().center;
+            let left_anchor = get_anchor(lift.properties.reference_anchors.left(), None)?;
+            let right_anchor = get_anchor(lift.properties.reference_anchors.right(), None)?;
+            let left_trans = left_anchor.translation_for_category(Category::Level);
+            let right_trans = right_anchor.translation_for_category(Category::Level);
+            let midpoint = [
+                (left_trans[0] + right_trans[0]) / 2.0,
+                (left_trans[1] + right_trans[1]) / 2.0,
+            ];
+            // TODO(luca) initial level
+            let pose = Pose {
+                trans: [midpoint[0] + center.x, midpoint[1] + center.y, 0.0],
+                ..Default::default()
+            };
+            models.push(SdfModel {
+                name: lift.properties.name.0.clone(),
+                // TODO(luca) remove static
+                r#static: Some(true),
+                pose: Some(pose.to_sdf(0.0)),
+                link: vec![SdfLink {
+                    name: "link".into(),
+                    collision: vec![SdfCollision {
+                        name: "collision".into(),
+                        geometry: SdfGeometry::Mesh(SdfMeshShape {
+                            uri: format!("meshes/{}.glb", lift.properties.name.0),
+                            ..Default::default()
+                        }),
+                        surface: Some(SdfSurface {
+                            contact: Some(SdfSurfaceContact {
+                                collide_bitmask: Some("0x02".into()),
+                                ..Default::default()
+                            }),
+                            ..Default::default()
+                        }),
+                        ..Default::default()
+                    }],
+                    visual: vec![SdfVisual {
+                        name: "visual".into(),
+                        geometry: SdfGeometry::Mesh(SdfMeshShape {
+                            uri: format!("meshes/{}.glb", lift.properties.name.0),
+                            ..Default::default()
+                        }),
+                        ..Default::default()
+                    }],
+                    ..Default::default()
+                }],
+                ..Default::default()
+            });
+        }
 
         let sun = SdfLight {
             name: "sun".into(),

From dfaf379f90f5849849d1ab96fa6b2b694ee0958c Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 9 Jan 2024 12:42:45 +0800
Subject: [PATCH 11/71] WIP adding lifts

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 Cargo.lock                 | 464 ++++++++++++++++++-------------------
 rmf_site_format/src/sdf.rs |  27 ++-
 2 files changed, 247 insertions(+), 244 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 449e0e6e..99d5070e 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -142,9 +142,9 @@ dependencies = [
 
 [[package]]
 name = "ahash"
-version = "0.8.6"
+version = "0.8.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "91429305e9f0a25f6205c5b8e0d2db09e0708a7a6df0f42212bb56c32c8ac97a"
+checksum = "77c3a9648d43b9cd48db467b3f87fdd6e146bcc88ab0180006cef2179fe11d01"
 dependencies = [
  "cfg-if",
  "getrandom 0.2.11",
@@ -231,9 +231,9 @@ dependencies = [
 
 [[package]]
 name = "anstream"
-version = "0.6.4"
+version = "0.6.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2ab91ebe16eb252986481c5b62f6098f3b698a45e34b5b98200cf20dd2484a44"
+checksum = "d664a92ecae85fd0a7392615844904654d1d5f5514837f471ddef4a057aba1b6"
 dependencies = [
  "anstyle",
  "anstyle-parse",
@@ -251,37 +251,37 @@ checksum = "7079075b41f533b8c61d2a4d073c4676e1f8b249ff94a393b0595db304e0dd87"
 
 [[package]]
 name = "anstyle-parse"
-version = "0.2.2"
+version = "0.2.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "317b9a89c1868f5ea6ff1d9539a69f45dffc21ce321ac1fd1160dfa48c8e2140"
+checksum = "c75ac65da39e5fe5ab759307499ddad880d724eed2f6ce5b5e8a26f4f387928c"
 dependencies = [
  "utf8parse",
 ]
 
 [[package]]
 name = "anstyle-query"
-version = "1.0.0"
+version = "1.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5ca11d4be1bab0c8bc8734a9aa7bf4ee8316d462a08c6ac5052f888fef5b494b"
+checksum = "e28923312444cdd728e4738b3f9c9cac739500909bb3d3c94b43551b16517648"
 dependencies = [
- "windows-sys 0.48.0",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
 name = "anstyle-wincon"
-version = "3.0.1"
+version = "3.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f0699d10d2f4d628a98ee7b57b289abbc98ff3bad977cb3152709d4bf2330628"
+checksum = "1cd54b81ec8d6180e24654d0b371ad22fc3dd083b6ff8ba325b72e00c87660a7"
 dependencies = [
  "anstyle",
- "windows-sys 0.48.0",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
 name = "anyhow"
-version = "1.0.75"
+version = "1.0.79"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a4668cab20f66d8d020e1fbc0ebe47217433c1b6c8f2040faf858554e394ace6"
+checksum = "080e9890a082662b09c1ad45f567faeeb47f22b5fb23895fbe1e651e718e25ca"
 
 [[package]]
 name = "approx"
@@ -344,7 +344,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "1ca33f4bc4ed1babef42cad36cc1f51fa88be00420404e5b1e80ab1b18f7678c"
 dependencies = [
  "concurrent-queue",
- "event-listener 4.0.0",
+ "event-listener 4.0.3",
  "event-listener-strategy",
  "futures-core",
  "pin-project-lite",
@@ -356,26 +356,26 @@ version = "1.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "17ae5ebefcc48e7452b4987947920dac9450be1110cadf34d1b8c116bdbaf97c"
 dependencies = [
- "async-lock 3.1.2",
+ "async-lock 3.2.0",
  "async-task",
  "concurrent-queue",
  "fastrand 2.0.1",
- "futures-lite 2.0.1",
+ "futures-lite 2.2.0",
  "slab",
 ]
 
 [[package]]
 name = "async-global-executor"
-version = "2.4.0"
+version = "2.4.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9b4353121d5644cdf2beb5726ab752e79a8db1ebb52031770ec47db31d245526"
+checksum = "05b1b633a2115cd122d73b955eadd9916c18c8f510ec9cd1686404c60ad1c29c"
 dependencies = [
  "async-channel 2.1.1",
  "async-executor",
- "async-io 2.2.1",
- "async-lock 3.1.2",
+ "async-io 2.2.2",
+ "async-lock 3.2.0",
  "blocking",
- "futures-lite 2.0.1",
+ "futures-lite 2.2.0",
  "once_cell",
 ]
 
@@ -401,18 +401,18 @@ dependencies = [
 
 [[package]]
 name = "async-io"
-version = "2.2.1"
+version = "2.2.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d6d3b15875ba253d1110c740755e246537483f152fa334f91abd7fe84c88b3ff"
+checksum = "6afaa937395a620e33dc6a742c593c01aced20aa376ffb0f628121198578ccc7"
 dependencies = [
- "async-lock 3.1.2",
+ "async-lock 3.2.0",
  "cfg-if",
  "concurrent-queue",
  "futures-io",
- "futures-lite 2.0.1",
+ "futures-lite 2.2.0",
  "parking",
  "polling 3.3.1",
- "rustix 0.38.25",
+ "rustix 0.38.28",
  "slab",
  "tracing",
  "windows-sys 0.52.0",
@@ -429,11 +429,11 @@ dependencies = [
 
 [[package]]
 name = "async-lock"
-version = "3.1.2"
+version = "3.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dea8b3453dd7cc96711834b75400d671b73e3656975fa68d9f277163b7f7e316"
+checksum = "7125e42787d53db9dd54261812ef17e937c95a51e4d291373b670342fa44310c"
 dependencies = [
- "event-listener 4.0.0",
+ "event-listener 4.0.3",
  "event-listener-strategy",
  "pin-project-lite",
 ]
@@ -466,19 +466,19 @@ dependencies = [
 
 [[package]]
 name = "async-task"
-version = "4.5.0"
+version = "4.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b4eb2cdb97421e01129ccb49169d8279ed21e829929144f4a22a6e54ac549ca1"
+checksum = "fbb36e985947064623dbd357f727af08ffd077f93d696782f3c56365fa2e2799"
 
 [[package]]
 name = "async-trait"
-version = "0.1.74"
+version = "0.1.77"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a66537f1bb974b254c98ed142ff995236e81b9d0fe4db0575f46612cb15eb0f9"
+checksum = "c980ee35e870bd1a4d2c8294d4c04d0499e67bca1e4b5cefcc693c2fa00caea9"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -540,9 +540,9 @@ checksum = "9e1b586273c5702936fe7b7d6896644d8be71e6314cfe09d3167c95f712589e8"
 
 [[package]]
 name = "base64"
-version = "0.21.5"
+version = "0.21.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "35636a1494ede3b646cc98f74f8e62c773a38a659ebc777a2cf26b9b74171df9"
+checksum = "c79fed4cdb43e993fcdadc7e58a09fd0e3e649c4436fa11da71c9f1f3ee7feb9"
 
 [[package]]
 name = "bevy"
@@ -694,7 +694,7 @@ checksum = "a44e4e2784a81430199e4157e02903a987a32127c773985506f020e7d501b62e"
 dependencies = [
  "bevy_macro_utils",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -751,7 +751,7 @@ dependencies = [
  "bevy_macro_utils",
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -847,6 +847,7 @@ dependencies = [
 [[package]]
 name = "bevy_gltf_export"
 version = "0.1.0"
+source = "git+https://github.com/luca-della-vedova/bevy_gltf_export?branch=luca/primitive_instantiation#c73fbc0cc7405d4bfd9992496d7c460ef633df72"
 dependencies = [
  "bevy_asset",
  "bevy_pbr",
@@ -960,7 +961,7 @@ checksum = "23ddc18d489b4e57832d4958cde7cd2f349f0ad91e5892ac9e2f2ee16546b981"
 dependencies = [
  "quote",
  "rustc-hash",
- "syn 2.0.39",
+ "syn 2.0.48",
  "toml_edit 0.19.15",
 ]
 
@@ -1108,7 +1109,7 @@ dependencies = [
  "bit-set",
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
  "uuid",
 ]
 
@@ -1171,7 +1172,7 @@ dependencies = [
  "bevy_macro_utils",
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -1352,7 +1353,7 @@ checksum = "5391b242c36f556db01d5891444730c83aa9dd648b6a8fd2b755d22cb3bddb57"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -1412,7 +1413,7 @@ dependencies = [
  "regex",
  "rustc-hash",
  "shlex",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -1498,11 +1499,11 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6a37913e8dc4ddcc604f0c6d3bf2887c995153af3611de9e23c352b44c1b9118"
 dependencies = [
  "async-channel 2.1.1",
- "async-lock 3.1.2",
+ "async-lock 3.2.0",
  "async-task",
  "fastrand 2.0.1",
  "futures-io",
- "futures-lite 2.0.1",
+ "futures-lite 2.2.0",
  "piper",
  "tracing",
 ]
@@ -1530,7 +1531,7 @@ checksum = "965ab7eb5f8f97d2a083c799f3a1b994fc397b2fe2da5d1da1626ce15a39f2b1"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -1588,9 +1589,9 @@ dependencies = [
 
 [[package]]
 name = "cfg-expr"
-version = "0.15.5"
+version = "0.15.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "03915af431787e6ffdcc74c645077518c6b6e01f80b761e0fbbfa288536311b3"
+checksum = "6100bc57b6209840798d95cb2775684849d332f7bd788db2a8c8caf7ef82a41a"
 dependencies = [
  "smallvec",
  "target-lexicon",
@@ -1619,20 +1620,20 @@ dependencies = [
 
 [[package]]
 name = "clang-sys"
-version = "1.6.1"
+version = "1.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c688fc74432808e3eb684cae8830a86be1d66a2bd58e1f248ed0960a590baf6f"
+checksum = "67523a3b4be3ce1989d607a828d036249522dd9c1c8de7f4dd2dae43a37369d1"
 dependencies = [
  "glob",
  "libc",
- "libloading 0.7.4",
+ "libloading 0.8.1",
 ]
 
 [[package]]
 name = "clap"
-version = "4.4.10"
+version = "4.4.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "41fffed7514f420abec6d183b1d3acfd9099c79c3a10a06ade4f8203f1411272"
+checksum = "33e92c5c1a78c62968ec57dbc2440366a2d6e5a23faf829970ff1585dc6b18e2"
 dependencies = [
  "clap_builder",
  "clap_derive",
@@ -1640,9 +1641,9 @@ dependencies = [
 
 [[package]]
 name = "clap_builder"
-version = "4.4.9"
+version = "4.4.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "63361bae7eef3771745f02d8d892bec2fee5f6e34af316ba556e7f97a7069ff1"
+checksum = "f4323769dc8a61e2c39ad7dc26f6f2800524691a44d74fe3d1071a5c24db6370"
 dependencies = [
  "anstream",
  "anstyle",
@@ -1659,7 +1660,7 @@ dependencies = [
  "heck 0.4.1",
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -1719,9 +1720,9 @@ dependencies = [
 
 [[package]]
 name = "concurrent-queue"
-version = "2.3.0"
+version = "2.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f057a694a54f12365049b0958a1685bb52d567f5593b355fbf685838e873d400"
+checksum = "d16048cd947b08fa32c24458a22f5dc5e835264f689f4f5653210c69fd107363"
 dependencies = [
  "crossbeam-utils",
 ]
@@ -1791,9 +1792,9 @@ dependencies = [
 
 [[package]]
 name = "core-foundation"
-version = "0.9.3"
+version = "0.9.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "194a7a9e6de53fa55116934067c844d9d749312f75c6f6d0980e8c252f8c2146"
+checksum = "91e195e091a93c46f7102ec7818a2aa394e1e1771c3ab4825963fa03e45afb8f"
 dependencies = [
  "core-foundation-sys",
  "libc",
@@ -1801,9 +1802,9 @@ dependencies = [
 
 [[package]]
 name = "core-foundation-sys"
-version = "0.8.4"
+version = "0.8.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e496a50fda8aacccc86d7529e2c1e0892dbd0f898a6b5645b5561b89c3210efa"
+checksum = "06ea2b9bc92be3c2baa9334a323ebca2d6f074ff852cd1d7b11064035cd3868f"
 
 [[package]]
 name = "core-graphics"
@@ -1820,9 +1821,9 @@ dependencies = [
 
 [[package]]
 name = "core-graphics-types"
-version = "0.1.2"
+version = "0.1.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2bb142d41022986c1d8ff29103a1411c8a3dfad3552f87a4f8dc50d61d4f4e33"
+checksum = "45390e6114f68f718cc7a830514a96f903cccd70d02a8f6d9f643ac4ba45afaf"
 dependencies = [
  "bitflags 1.3.2",
  "core-foundation",
@@ -1842,9 +1843,9 @@ dependencies = [
 
 [[package]]
 name = "coreaudio-sys"
-version = "0.2.14"
+version = "0.2.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f3120ebb80a9de008e638ad833d4127d50ea3d3a960ea23ea69bc66d9358a028"
+checksum = "7f01585027057ff5f0a5bf276174ae4c1594a2c5bde93d5f46a016d76270f5a9"
 dependencies = [
  "bindgen",
 ]
@@ -1876,9 +1877,9 @@ dependencies = [
 
 [[package]]
 name = "cpufeatures"
-version = "0.2.11"
+version = "0.2.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ce420fe07aecd3e67c5f910618fe65e94158f6dcc0adf44e00d69ce2bdfe0fd0"
+checksum = "53fe5e26ff1b7aef8bca9c6080520cfb8d9333c7568e1829cef191a9723e5504"
 dependencies = [
  "libc",
 ]
@@ -1900,11 +1901,10 @@ dependencies = [
 
 [[package]]
 name = "crossbeam"
-version = "0.8.2"
+version = "0.8.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "2801af0d36612ae591caa9568261fddce32ce6e08a7275ea334a06a4ad021a2c"
+checksum = "1137cd7e7fc0fb5d3c5a8678be38ec56e819125d8d7907411fe24ccb943faca8"
 dependencies = [
- "cfg-if",
  "crossbeam-channel",
  "crossbeam-deque",
  "crossbeam-epoch",
@@ -1914,56 +1914,46 @@ dependencies = [
 
 [[package]]
 name = "crossbeam-channel"
-version = "0.5.8"
+version = "0.5.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a33c2bf77f2df06183c3aa30d1e96c0695a313d4f9c453cc3762a6db39f99200"
+checksum = "176dc175b78f56c0f321911d9c8eb2b77a78a4860b9c19db83835fea1a46649b"
 dependencies = [
- "cfg-if",
  "crossbeam-utils",
 ]
 
 [[package]]
 name = "crossbeam-deque"
-version = "0.8.3"
+version = "0.8.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ce6fd6f855243022dcecf8702fef0c297d4338e226845fe067f6341ad9fa0cef"
+checksum = "613f8cc01fe9cf1a3eb3d7f488fd2fa8388403e97039e2f73692932e291a770d"
 dependencies = [
- "cfg-if",
  "crossbeam-epoch",
  "crossbeam-utils",
 ]
 
 [[package]]
 name = "crossbeam-epoch"
-version = "0.9.15"
+version = "0.9.18"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ae211234986c545741a7dc064309f67ee1e5ad243d0e48335adc0484d960bcc7"
+checksum = "5b82ac4a3c2ca9c3460964f020e1402edd5753411d7737aa39c3714ad1b5420e"
 dependencies = [
- "autocfg",
- "cfg-if",
  "crossbeam-utils",
- "memoffset 0.9.0",
- "scopeguard",
 ]
 
 [[package]]
 name = "crossbeam-queue"
-version = "0.3.8"
+version = "0.3.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d1cfb3ea8a53f37c40dea2c7bedcbd88bdfae54f5e2175d6ecaff1c988353add"
+checksum = "df0346b5d5e76ac2fe4e327c5fd1118d6be7c51dfb18f9b7922923f287471e35"
 dependencies = [
- "cfg-if",
  "crossbeam-utils",
 ]
 
 [[package]]
 name = "crossbeam-utils"
-version = "0.8.16"
+version = "0.8.19"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a22b2d63d4d1dc0b7f1b6b2747dd0088008a9be28b6ddf0b1e7d335e3037294"
-dependencies = [
- "cfg-if",
-]
+checksum = "248e3bacc7dc6baa3b21e405ee045c3047101a49145e7e9eca583ab4c2ca5345"
 
 [[package]]
 name = "crunchy"
@@ -2007,9 +1997,9 @@ dependencies = [
 
 [[package]]
 name = "curl-sys"
-version = "0.4.68+curl-8.4.0"
+version = "0.4.70+curl-8.5.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b4a0d18d88360e374b16b2273c832b5e57258ffc1d4aa4f96b108e0738d5752f"
+checksum = "3c0333d8849afe78a4c8102a429a446bfdd055832af071945520e835ae2d841e"
 dependencies = [
  "cc",
  "libc",
@@ -2176,7 +2166,7 @@ checksum = "3fe2568f851fd6144a45fa91cfed8fe5ca8fc0b56ba6797bfc1ed2771b90e37c"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -2256,9 +2246,9 @@ checksum = "0206175f82b8d6bf6652ff7d71a1e27fd2e4efde587fd368662814d6ec1d9ce0"
 
 [[package]]
 name = "event-listener"
-version = "4.0.0"
+version = "4.0.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "770d968249b5d99410d61f5bf89057f3199a077a04d087092f58e7d10692baae"
+checksum = "67b215c49b2b248c855fb73579eb1f4f26c38ffdc12973e20e07b91d78d5646e"
 dependencies = [
  "concurrent-queue",
  "parking",
@@ -2271,7 +2261,7 @@ version = "0.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "958e4d70b6d5e81971bebec42271ec641e7ff4e170a6fa605f2b8a8b65cb97d3"
 dependencies = [
- "event-listener 4.0.0",
+ "event-listener 4.0.3",
  "pin-project-lite",
 ]
 
@@ -2308,23 +2298,23 @@ checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5"
 
 [[package]]
 name = "fdeflate"
-version = "0.3.1"
+version = "0.3.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "64d6dafc854908ff5da46ff3f8f473c6984119a2876a383a860246dd7841a868"
+checksum = "209098dd6dfc4445aa6111f0e98653ac323eaa4dfd212c9ca3931bf9955c31bd"
 dependencies = [
  "simd-adler32",
 ]
 
 [[package]]
 name = "filetime"
-version = "0.2.22"
+version = "0.2.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d4029edd3e734da6fe05b6cd7bd2960760a616bd2ddd0d59a0124746d6272af0"
+checksum = "1ee447700ac8aa0b2f2bd7bc4462ad686ba06baa6727ac149a2d6277f0d240fd"
 dependencies = [
  "cfg-if",
  "libc",
- "redox_syscall 0.3.5",
- "windows-sys 0.48.0",
+ "redox_syscall 0.4.1",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
@@ -2419,9 +2409,9 @@ dependencies = [
 
 [[package]]
 name = "futures"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "da0290714b38af9b4a7b094b8a37086d1b4e61f2df9122c3cad2577669145335"
+checksum = "645c6916888f6cb6350d2550b80fb63e734897a8498abe35cfb732b6487804b0"
 dependencies = [
  "futures-channel",
  "futures-core",
@@ -2434,9 +2424,9 @@ dependencies = [
 
 [[package]]
 name = "futures-channel"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ff4dd66668b557604244583e3e1e1eada8c5c2e96a6d0d6653ede395b78bbacb"
+checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78"
 dependencies = [
  "futures-core",
  "futures-sink",
@@ -2444,15 +2434,15 @@ dependencies = [
 
 [[package]]
 name = "futures-core"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "eb1d22c66e66d9d72e1758f0bd7d4fd0bee04cad842ee34587d68c07e45d088c"
+checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d"
 
 [[package]]
 name = "futures-executor"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0f4fb8693db0cf099eadcca0efe2a5a22e4550f98ed16aba6c48700da29597bc"
+checksum = "a576fc72ae164fca6b9db127eaa9a9dda0d61316034f33a0a0d4eda41f02b01d"
 dependencies = [
  "futures-core",
  "futures-task",
@@ -2461,9 +2451,9 @@ dependencies = [
 
 [[package]]
 name = "futures-io"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8bf34a163b5c4c52d0478a4d757da8fb65cabef42ba90515efee0f6f9fa45aaa"
+checksum = "a44623e20b9681a318efdd71c299b6b222ed6f231972bfe2f224ebad6311f0c1"
 
 [[package]]
 name = "futures-lite"
@@ -2482,46 +2472,45 @@ dependencies = [
 
 [[package]]
 name = "futures-lite"
-version = "2.0.1"
+version = "2.2.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d3831c2651acb5177cbd83943f3d9c8912c5ad03c76afcc0e9511ba568ec5ebb"
+checksum = "445ba825b27408685aaecefd65178908c36c6e96aaf6d8599419d46e624192ba"
 dependencies = [
  "fastrand 2.0.1",
  "futures-core",
  "futures-io",
- "memchr",
  "parking",
  "pin-project-lite",
 ]
 
 [[package]]
 name = "futures-macro"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "53b153fd91e4b0147f4aced87be237c98248656bb01050b96bf3ee89220a8ddb"
+checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
 name = "futures-sink"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e36d3378ee38c2a36ad710c5d30c2911d752cb941c00c72dbabfb786a7970817"
+checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5"
 
 [[package]]
 name = "futures-task"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "efd193069b0ddadc69c46389b740bbccdd97203899b48d09c5f7969591d6bae2"
+checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004"
 
 [[package]]
 name = "futures-util"
-version = "0.3.29"
+version = "0.3.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a19526d624e703a3179b3d322efec918b6246ea0fa51d41124525f00f1cc8104"
+checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48"
 dependencies = [
  "futures-channel",
  "futures-core",
@@ -2631,9 +2620,9 @@ dependencies = [
 
 [[package]]
 name = "gilrs"
-version = "0.10.3"
+version = "0.10.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9e9eec02069fcbd7abe00a28adf216547774889129a777cb5e53fdfb75d59f09"
+checksum = "d8b2e57a9cb946b5d04ae8638c5f554abb5a9f82c4c950fd5b1fee6d119592fb"
 dependencies = [
  "fnv",
  "gilrs-core",
@@ -2644,9 +2633,9 @@ dependencies = [
 
 [[package]]
 name = "gilrs-core"
-version = "0.5.9"
+version = "0.5.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "178769da179a47b187837d1ab2b5b9b684a21180166a77a4ca37e7e58ee3833d"
+checksum = "0af1827b7dd2f36d740ae804c1b3ea0d64c12533fb61ff91883005143a0e8c5a"
 dependencies = [
  "core-foundation",
  "inotify 0.10.2",
@@ -2660,7 +2649,7 @@ dependencies = [
  "vec_map",
  "wasm-bindgen",
  "web-sys",
- "windows 0.51.1",
+ "windows 0.52.0",
 ]
 
 [[package]]
@@ -2734,32 +2723,33 @@ dependencies = [
 
 [[package]]
 name = "gltf"
-version = "1.3.0"
+version = "1.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ad2dcfb6dd7a66f9eb3d181a29dcfb22d146b0bcdc2e1ed1713cbf03939a88ea"
+checksum = "3b78f069cf941075835822953c345b9e1edd67ae347b81ace3aea9de38c2ef33"
 dependencies = [
  "byteorder",
  "gltf-json",
  "lazy_static",
+ "serde_json",
 ]
 
 [[package]]
 name = "gltf-derive"
-version = "1.3.0"
+version = "1.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f2cbcea5dd47e7ad4e9ee6f040384fcd7204bbf671aa4f9e7ca7dfc9bfa1de20"
+checksum = "438ffe1a5540d75403feaf23636b164e816e93f6f03131674722b3886ce32a57"
 dependencies = [
  "inflections",
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
 name = "gltf-json"
-version = "1.3.0"
+version = "1.4.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7d5b810806b78dde4b71a95cc0e6fdcab34c4c617da3574df166f9987be97d03"
+checksum = "655951ba557f2bc69ea4b0799446bae281fa78efae6319968bdd2c3e9a06d8e1"
 dependencies = [
  "gltf-derive",
  "serde",
@@ -2881,7 +2871,7 @@ version = "0.1.0"
 source = "git+https://github.com/open-rmf/gz-fuel-rs?branch=first_implementation#44c2e1e074df4499d3c40eedaa386d9d612a9314"
 dependencies = [
  "dirs 5.0.1",
- "futures-lite 2.0.1",
+ "futures-lite 2.2.0",
  "itertools",
  "serde",
  "serde_json",
@@ -2988,11 +2978,11 @@ dependencies = [
 
 [[package]]
 name = "home"
-version = "0.5.5"
+version = "0.5.9"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5444c27eef6923071f7ebcc33e3444508466a76f7a2b93da00ed6e19f30c1ddb"
+checksum = "e3d1354bf6b7235cb4a0576c2619fd4ed18183f689b12b006a0ee7329eeff9a5"
 dependencies = [
- "windows-sys 0.48.0",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
@@ -3212,9 +3202,9 @@ dependencies = [
 
 [[package]]
 name = "itoa"
-version = "1.0.9"
+version = "1.0.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "af150ab688ff2122fcef229be89cb50dd66af9e01a4ff320cc137eecc9bacc38"
+checksum = "b1a46d1a171d865aa5f83f92695765caa047a9b4cbae2cbf37dbd613a793fd4c"
 
 [[package]]
 name = "jni"
@@ -3373,9 +3363,9 @@ dependencies = [
 
 [[package]]
 name = "libc"
-version = "0.2.150"
+version = "0.2.152"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c"
+checksum = "13e3bf6590cbc649f4d1a3eefc9d5d6eb746f5200ffb04e5e142700b8faa56e7"
 
 [[package]]
 name = "libloading"
@@ -3405,9 +3395,9 @@ checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058"
 
 [[package]]
 name = "libnghttp2-sys"
-version = "0.1.8+1.55.1"
+version = "0.1.9+1.58.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4fae956c192dadcdb5dace96db71fa0b827333cce7c7b38dc71446f024d8a340"
+checksum = "b57e858af2798e167e709b9d969325b6d8e9d50232fcbc494d7d54f976854a64"
 dependencies = [
  "cc",
  "libc",
@@ -3447,9 +3437,9 @@ dependencies = [
 
 [[package]]
 name = "libz-sys"
-version = "1.1.12"
+version = "1.1.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d97137b25e321a73eef1418d1d5d2eda4d77e12813f8e6dead84bc52c5870a7b"
+checksum = "5f526fdd09d99e19742883e43de41e1aa9e36db0c7ab7f935165d611c5cccc66"
 dependencies = [
  "cc",
  "libc",
@@ -3471,9 +3461,9 @@ checksum = "ef53942eb7bf7ff43a617b3e2c1c4a5ecf5944a7c1bc12d7ee39bbb15e5c1519"
 
 [[package]]
 name = "linux-raw-sys"
-version = "0.4.11"
+version = "0.4.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "969488b55f8ac402214f3f5fd243ebb7206cf82de60d3172994707a4bcc2b829"
+checksum = "c4cd1a83af159aa67994778be9070f0ae1bd732942279cabb14f86f986a21456"
 
 [[package]]
 name = "lock_api"
@@ -3516,9 +3506,9 @@ dependencies = [
 
 [[package]]
 name = "lyon_geom"
-version = "1.0.4"
+version = "1.0.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "74df1ff0a0147282eb10699537a03baa7d31972b58984a1d44ce0624043fe8ad"
+checksum = "edecfb8d234a2b0be031ab02ebcdd9f3b9ee418fb35e265f7a540a48d197bff9"
 dependencies = [
  "arrayvec",
  "euclid",
@@ -3537,20 +3527,20 @@ dependencies = [
 
 [[package]]
 name = "lyon_tessellation"
-version = "1.0.11"
+version = "1.0.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "23bcac20d47825850fabf1e869bf7c2bbe2daefa0776c3cd2eb7cb74635f6e4a"
+checksum = "8c7c67b5bc8123b352b2e7e742b47d1f236a13fe77619433be9568fbd888e9c0"
 dependencies = [
  "float_next_after",
  "lyon_path",
- "thiserror",
+ "num-traits",
 ]
 
 [[package]]
 name = "mach2"
-version = "0.4.1"
+version = "0.4.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6d0d1830bcd151a6fc4aea1369af235b36c1528fe976b8ff678683c9995eade8"
+checksum = "19b955cdeb2a02b9117f121ce63aa52d08ade45de53e48fe6a38b39c10f6f709"
 dependencies = [
  "libc",
 ]
@@ -3585,9 +3575,9 @@ dependencies = [
 
 [[package]]
 name = "memchr"
-version = "2.6.4"
+version = "2.7.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f665ee40bc4a3c5590afb1e9677db74a508659dfd71e126420da8274909a0167"
+checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149"
 
 [[package]]
 name = "memoffset"
@@ -3598,15 +3588,6 @@ dependencies = [
  "autocfg",
 ]
 
-[[package]]
-name = "memoffset"
-version = "0.9.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5a634b1c61a95585bd15607c6ab0c4e5b226e695ff2800ba0cdccddf208c406c"
-dependencies = [
- "autocfg",
-]
-
 [[package]]
 name = "metal"
 version = "0.24.0"
@@ -3655,9 +3636,9 @@ dependencies = [
 
 [[package]]
 name = "mio"
-version = "0.8.9"
+version = "0.8.10"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3dce281c5e46beae905d4de1870d8b1509a9142b62eedf18b443b011ca8343d0"
+checksum = "8f3d0b296e374a4e6f3c7b0a1f5a51d748a0d34c85e7dc48fc3fa9a87657fe09"
 dependencies = [
  "libc",
  "log",
@@ -3783,7 +3764,7 @@ dependencies = [
  "bitflags 1.3.2",
  "cfg-if",
  "libc",
- "memoffset 0.7.1",
+ "memoffset",
 ]
 
 [[package]]
@@ -3941,7 +3922,7 @@ dependencies = [
  "proc-macro-crate",
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -4011,9 +3992,9 @@ dependencies = [
 
 [[package]]
 name = "object"
-version = "0.32.1"
+version = "0.32.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9cf5f9dd3933bd50a9e1f149ec995f39ae2c496d31fd772c1fd45ebc27e902b0"
+checksum = "a6a622008b6e321afc04970976f62ee297fdbaa6f95318ca343e3eebb9648441"
 dependencies = [
  "memchr",
 ]
@@ -4052,9 +4033,9 @@ dependencies = [
 
 [[package]]
 name = "once_cell"
-version = "1.18.0"
+version = "1.19.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dd8b5dd2ae5ed71462c540258bedcb51965123ad7e7ccf4b9a8cafaa4a63576d"
+checksum = "3fdb12b2476b595f9358c5161aa467c2438859caa136dec86c26fdd2efe17b92"
 
 [[package]]
 name = "opaque-debug"
@@ -4070,9 +4051,9 @@ checksum = "ff011a302c396a5197692431fc1948019154afc178baf7d8e37367442a4601cf"
 
 [[package]]
 name = "openssl-sys"
-version = "0.9.96"
+version = "0.9.98"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3812c071ba60da8b5677cc12bcb1d42989a65553772897a7e0355545a819838f"
+checksum = "c1665caf8ab2dc9aef43d1c0023bd904633a6a05cb30b0ad59bec2ae986e57a7"
 dependencies = [
  "cc",
  "libc",
@@ -4223,7 +4204,7 @@ checksum = "4359fd9c9171ec6e8c62926d6faaf553a8dc3f64e1507e76da7911b4f6a04405"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -4251,9 +4232,9 @@ dependencies = [
 
 [[package]]
 name = "pkg-config"
-version = "0.3.27"
+version = "0.3.28"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "26072860ba924cbfa98ea39c8c19b4dd6a4a25423dbdf219c1eca91aa0cf6964"
+checksum = "69d3587f8a9e599cc7ec2c00e331f71c4e69a5f9a4b8a6efd5b07466b9736f9a"
 
 [[package]]
 name = "png"
@@ -4293,7 +4274,7 @@ dependencies = [
  "cfg-if",
  "concurrent-queue",
  "pin-project-lite",
- "rustix 0.38.25",
+ "rustix 0.38.28",
  "tracing",
  "windows-sys 0.52.0",
 ]
@@ -4342,18 +4323,18 @@ checksum = "dc375e1527247fe1a97d8b7156678dfe7c1af2fc075c9a4db3690ecd2a148068"
 
 [[package]]
 name = "proc-macro2"
-version = "1.0.70"
+version = "1.0.76"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "39278fbbf5fb4f646ce651690877f89d1c5811a3d4acb27700c1cb3cdb78fd3b"
+checksum = "95fc56cda0b5c3325f5fbbd7ff9fda9e02bb00bb3dac51252d2f1bfa1cb8cc8c"
 dependencies = [
  "unicode-ident",
 ]
 
 [[package]]
 name = "profiling"
-version = "1.0.11"
+version = "1.0.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f89dff0959d98c9758c88826cc002e2c3d0b9dfac4139711d1f30de442f1139b"
+checksum = "d135ede8821cf6376eb7a64148901e1690b788c11ae94dc297ae917dbc91dc0e"
 
 [[package]]
 name = "qoi"
@@ -4366,9 +4347,9 @@ dependencies = [
 
 [[package]]
 name = "quote"
-version = "1.0.33"
+version = "1.0.35"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae"
+checksum = "291ec9ab5efd934aaf503a6466c5d5251535d108ee747472c3977cc5acc868ef"
 dependencies = [
  "proc-macro2",
 ]
@@ -4699,7 +4680,7 @@ version = "0.8.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b91f7eff05f748767f183df4320a63d6936e9c6107d97c9e6bdd9784f4289c94"
 dependencies = [
- "base64 0.21.5",
+ "base64 0.21.6",
  "bitflags 2.4.1",
  "serde",
  "serde_derive",
@@ -4742,15 +4723,15 @@ dependencies = [
 
 [[package]]
 name = "rustix"
-version = "0.38.25"
+version = "0.38.28"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dc99bc2d4f1fed22595588a013687477aedf3cdcfb26558c559edb67b4d9b22e"
+checksum = "72e572a5e8ca657d7366229cdde4bd14c4eb5499a9573d4d366fe1b599daa316"
 dependencies = [
  "bitflags 2.4.1",
  "errno",
  "libc",
- "linux-raw-sys 0.4.11",
- "windows-sys 0.48.0",
+ "linux-raw-sys 0.4.12",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
@@ -4766,9 +4747,9 @@ dependencies = [
 
 [[package]]
 name = "ryu"
-version = "1.0.15"
+version = "1.0.16"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "1ad4cc8da4ef723ed60bced201181d83791ad433213d8c24efffda1eec85d741"
+checksum = "f98d2aa92eebf49b69786be48e4477826b256916e84a57ff2a4f21923b48eb4c"
 
 [[package]]
 name = "safe_arch"
@@ -4790,11 +4771,11 @@ dependencies = [
 
 [[package]]
 name = "schannel"
-version = "0.1.22"
+version = "0.1.23"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "0c3733bf4cf7ea0880754e19cb5a462007c4a8c1914bff372ccc95b464f1df88"
+checksum = "fbc91545643bcf3a0bbb6569265615222618bdf33ce4ffbbd13c4bbd4c093534"
 dependencies = [
- "windows-sys 0.48.0",
+ "windows-sys 0.52.0",
 ]
 
 [[package]]
@@ -4806,7 +4787,6 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
 [[package]]
 name = "sdformat_rs"
 version = "0.1.0"
-source = "git+https://github.com/open-rmf/sdf_rust_experimental?branch=luca/serde_plugin#9ab9e0da9dfd84114dba7c2e6ca9db4b3f0ad08c"
 dependencies = [
  "convert_case",
  "nalgebra",
@@ -4832,29 +4812,29 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
 
 [[package]]
 name = "serde"
-version = "1.0.193"
+version = "1.0.195"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "25dd9975e68d0cb5aa1120c288333fc98731bd1dd12f561e468ea4728c042b89"
+checksum = "63261df402c67811e9ac6def069e4786148c4563f4b50fd4bf30aa370d626b02"
 dependencies = [
  "serde_derive",
 ]
 
 [[package]]
 name = "serde_derive"
-version = "1.0.193"
+version = "1.0.195"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "43576ca501357b9b071ac53cdc7da8ef0cbd9493d8df094cd821777ea6e894d3"
+checksum = "46fe8f8603d81ba86327b23a2e9cdf49e1255fb94a4c5f297f6ee0547178ea2c"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
 name = "serde_json"
-version = "1.0.108"
+version = "1.0.111"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "3d1c7e3eac408d115102c4c24ad393e0821bb3a5df4d506a80f85f7a742a526b"
+checksum = "176e46fa42316f18edd598015a5166857fc835ec732f5215eac6b7bdbf0a84f4"
 dependencies = [
  "itoa",
  "ryu",
@@ -4874,9 +4854,9 @@ dependencies = [
 
 [[package]]
 name = "serde_spanned"
-version = "0.6.4"
+version = "0.6.5"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "12022b835073e5b11e90a14f86838ceb1c8fb0325b72416845c487ac0fa95e80"
+checksum = "eb3622f419d1296904700073ea6cc23ad690adbd66f13ea683df73298736f0c1"
 dependencies = [
  "serde",
 ]
@@ -4978,9 +4958,9 @@ dependencies = [
 
 [[package]]
 name = "slotmap"
-version = "1.0.6"
+version = "1.0.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e1e08e261d0e8f5c43123b7adf3e4ca1690d655377ac93a03b2c9d3e98de1342"
+checksum = "dbff4acf519f630b3a3ddcfaea6c06b42174d9a44bc70c620e9ed1649d58b82a"
 dependencies = [
  "version_check",
 ]
@@ -5026,9 +5006,9 @@ dependencies = [
 
 [[package]]
 name = "spade"
-version = "2.4.0"
+version = "2.5.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c3d3bf265ec2d5dd1ddf87863252123447c550491adba2c70c574173a95cd8ba"
+checksum = "bd774eb23cff002036706e6ea83c3f4ab4c80dad89da76fe16d49f77ab71682f"
 dependencies = [
  "hashbrown 0.14.3",
  "num-traits",
@@ -5198,9 +5178,9 @@ dependencies = [
 
 [[package]]
 name = "syn"
-version = "2.0.39"
+version = "2.0.48"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "23e78b90f2fcf45d3e842032ce32e3f2d1545ba6636271dcbf24fa306d87be7a"
+checksum = "0f3531638e407dfc0814761abb7c00a5b54992b849452a0646b7f65c9f770f3f"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -5248,9 +5228,9 @@ dependencies = [
 
 [[package]]
 name = "target-lexicon"
-version = "0.12.12"
+version = "0.12.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "14c39fd04924ca3a864207c66fc2cd7d22d7c016007f9ce846cbb9326331930a"
+checksum = "69758bda2e78f098e4ccb393021a0963bb3442eac05f135c30f61b7370bbafae"
 
 [[package]]
 name = "termcolor"
@@ -5263,9 +5243,9 @@ dependencies = [
 
 [[package]]
 name = "thiserror"
-version = "1.0.50"
+version = "1.0.56"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f9a7210f5c9a7156bb50aa36aed4c95afb51df0df00713949448cf9e97d382d2"
+checksum = "d54378c645627613241d077a3a79db965db602882668f9136ac42af9ecb730ad"
 dependencies = [
  "thiserror-impl",
 ]
@@ -5287,18 +5267,18 @@ checksum = "e4c60d69f36615a077cc7663b9cb8e42275722d23e58a7fa3d2c7f2915d09d04"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
 name = "thiserror-impl"
-version = "1.0.50"
+version = "1.0.56"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "266b2e40bc00e5a6c09c3584011e08b06f123c00362c92b975ba9843aaaa14b8"
+checksum = "fa0faa943b50f3db30a20aa7e265dbc66076993efed8463e8de414e5d06d3471"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -5450,7 +5430,7 @@ checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
@@ -5557,9 +5537,9 @@ dependencies = [
 
 [[package]]
 name = "unicode-bidi"
-version = "0.3.13"
+version = "0.3.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "92888ba5573ff080736b3648696b70cafad7d250551175acbaa4e0385b3e1460"
+checksum = "6f2528f27a9eb2b21e69c95319b30bd0efd85d09c379741b0f78ea1d86be2416"
 
 [[package]]
 name = "unicode-ident"
@@ -5659,9 +5639,9 @@ checksum = "830b7e5d4d90034032940e4ace0d9a9a057e7a45cd94e6c007832e39edb82f6d"
 
 [[package]]
 name = "value-bag"
-version = "1.4.2"
+version = "1.6.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4a72e1902dde2bd6441347de2b70b7f5d59bf157c6c62f0c44572607a1d55bbe"
+checksum = "7cdbaf5e132e593e9fc1de6a15bbec912395b11fb9719e061cf64f804524c503"
 
 [[package]]
 name = "vcpkg"
@@ -6012,21 +5992,21 @@ dependencies = [
 
 [[package]]
 name = "windows"
-version = "0.51.1"
+version = "0.52.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ca229916c5ee38c2f2bc1e9d8f04df975b4bd93f9955dc69fabb5d91270045c9"
+checksum = "e48a53791691ab099e5e2ad123536d0fff50652600abaf43bbf952894110d0be"
 dependencies = [
  "windows-core",
- "windows-targets 0.48.5",
+ "windows-targets 0.52.0",
 ]
 
 [[package]]
 name = "windows-core"
-version = "0.51.1"
+version = "0.52.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f1f8cf84f35d2db49a46868f947758c7a1138116f7fac3bc844f43ade1292e64"
+checksum = "33ab640c8d7e35bf8ba19b884ba838ceb4fba93a4e8c65a9059d08afcfc683d9"
 dependencies = [
- "windows-targets 0.48.5",
+ "windows-targets 0.52.0",
 ]
 
 [[package]]
@@ -6281,9 +6261,9 @@ dependencies = [
 
 [[package]]
 name = "winnow"
-version = "0.5.19"
+version = "0.5.33"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "829846f3e3db426d4cee4510841b71a8e58aa2a76b1132579487ae430ccd9c7b"
+checksum = "b7520bbdec7211caa7c4e682eb1fbe07abe20cee6756b6e00f537c82c11816aa"
 dependencies = [
  "memchr",
 ]
@@ -6377,22 +6357,22 @@ dependencies = [
 
 [[package]]
 name = "zerocopy"
-version = "0.7.26"
+version = "0.7.32"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e97e415490559a91254a2979b4829267a57d2fcd741a98eee8b722fb57289aa0"
+checksum = "74d4d3961e53fa4c9a25a8637fc2bfaf2595b3d3ae34875568a5cf64787716be"
 dependencies = [
  "zerocopy-derive",
 ]
 
 [[package]]
 name = "zerocopy-derive"
-version = "0.7.26"
+version = "0.7.32"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dd7e48ccf166952882ca8bd778a43502c64f33bf94c12ebe2a7f08e5a0f6689f"
+checksum = "9ce1b18ccd8e73a9321186f97e46f9f04b778851177567b1975109d26a08d2a6"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.39",
+ "syn 2.0.48",
 ]
 
 [[package]]
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index c2bd9c35..667782c9 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -471,10 +471,32 @@ impl Site {
                 trans: [midpoint[0] + center.x, midpoint[1] + center.y, 0.0],
                 ..Default::default()
             };
+            let mut plugin = SdfPlugin {
+                name: "lift".into(),
+                filename: "liblift.so".into(),
+                ..Default::default()
+            };
+            let mut elements = vec![];
+            elements.push(("lift_name", lift.properties.name.0.clone()));
+            // TODO(luca) remove unwrap here for missing initial level
+            let initial_level = lift.properties.initial_level.0.and_then(|id| self.levels.get(&id)).map(|level| level.properties.name.0.clone()).unwrap();
+            elements.push(("initial_level", initial_level));
+            elements.push(("v_max_cabin", "2.0".to_string()));
+            elements.push(("a_max_cabin", "1.2".to_string()));
+            elements.push(("a_nom_cabin", "1.0".to_string()));
+            elements.push(("dx_min_cabin", "0.001".to_string()));
+            elements.push(("f_max_cabin", "25323.0".to_string()));
+            for (name, value) in elements.into_iter() {
+                plugin.elements.push(XmlElement {
+                    name: name.into(),
+                    data: ElementData::String(value),
+                    ..Default::default()
+                });
+            }
+            // Now add the floors
             models.push(SdfModel {
                 name: lift.properties.name.0.clone(),
-                // TODO(luca) remove static
-                r#static: Some(true),
+                r#static: Some(lift.properties.is_static.0),
                 pose: Some(pose.to_sdf(0.0)),
                 link: vec![SdfLink {
                     name: "link".into(),
@@ -503,6 +525,7 @@ impl Site {
                     }],
                     ..Default::default()
                 }],
+                plugin: vec![plugin],
                 ..Default::default()
             });
         }

From 873042801cd780d43ec24011ff3295a98de0226c Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 24 Jan 2024 11:10:58 +0800
Subject: [PATCH 12/71] WIP lift doors not working yet

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs |  20 ++-
 rmf_site_format/src/sdf.rs               | 164 ++++++++++++++++++++---
 2 files changed, 161 insertions(+), 23 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 31e824a4..916ea88b 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -9,7 +9,7 @@ use crate::site::{
     ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, FloorSegments, LiftDoormat,
     VisualMeshMarker,
 };
-use rmf_site_format::{LevelElevation, ModelMarker, NameInSite, Pose, WallMarker};
+use rmf_site_format::{SiteID, LevelElevation, LiftCabin, ModelMarker, NameInSite, Pose, WallMarker};
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
     let mut state: SystemState<(
@@ -17,7 +17,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Query<(&NameInSite, &LevelElevation, &Children)>,
         Query<Entity, With<WallMarker>>,
         Query<&FloorSegments>,
-        Query<(&NameInSite, &DoorSegments)>,
+        Query<(Option<&NameInSite>, Option<&SiteID>, &DoorSegments)>,
         Query<Entity, With<ModelMarker>>,
         Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
         Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
@@ -173,8 +173,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             }
                         }
                     }
-                } else if let Ok((door_name, segments)) = q_doors.get(*child) {
-                    // TODO(luca) get rid of reverse by changing the entities function
+                } else if let Ok((door_name, _, segments)) = q_doors.get(*child) {
                     for (entity, segment_name) in segments
                         .body
                         .entities()
@@ -188,6 +187,9 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                         let Ok(tf) = q_tfs.get(*entity) else {
                             continue;
                         };
+                        let Some(door_name) = door_name else {
+                            continue;
+                        };
                         let pose = GltfPose {
                             translation: tf.translation.to_array(),
                             rotation: tf.rotation.to_array(),
@@ -250,6 +252,16 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
             }
             let filename = format!("{}/{}.glb", folder.display(), **lift_name);
             write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename);
+            // Now generate the lift doors
+            let Ok(children) = q_children.get(*site_child) else {
+                continue;
+            };
+            for child in children.iter() {
+                if let Ok((_, site_id, segments)) = q_doors.get(*child) {
+                    println!("Found lift door with id");
+                    dbg!(site_id);
+                }
+            }
         }
     }
 }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 667782c9..1d19cc6e 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -16,9 +16,10 @@
 */
 
 use crate::{
-    Anchor, Angle, Category, Door, DoorType, Level, LiftCabin, Pose, Rotation, Side, Site, Swing,
+    Anchor, Angle, Category, Door, DoorType, Level, LiftCabin, Pose, Rotation, Side, Site, Swing, NameInSite, DoorMarker,
 };
 use sdformat_rs::*;
+use std::collections::BTreeMap;
 
 #[derive(Debug)]
 pub enum SdfConversionError {
@@ -26,6 +27,8 @@ pub enum SdfConversionError {
     UnsupportedAssetType,
     /// Entity referenced a non existing anchor.
     BrokenAnchorReference(u32),
+    /// Entity referenced a non existing level.
+    BrokenLevelReference(u32),
 }
 
 impl Pose {
@@ -115,14 +118,20 @@ impl Door<u32> {
             DoorType::DoubleSliding(_) | DoorType::DoubleSwing(_) => Vec::from(["right", "left"]),
         };
         let mut plugin = SdfPlugin {
-            name: "door".into(),
-            filename: "libdoor.so".into(),
+            name: "register_component".into(),
+            filename: "libregister_component.so".into(),
+            ..Default::default()
+        };
+        let mut component = XmlElement {
+            name: "component".into(),
             ..Default::default()
         };
         let mut door_plugin_inner = XmlElement {
             name: "door".into(),
             ..Default::default()
         };
+        component.attributes.insert("name".into(), "Door".into());
+        let mut component_data = ElementMap::default();
         door_plugin_inner
             .attributes
             .insert("name".to_string(), self.name.0.clone());
@@ -383,15 +392,18 @@ impl Door<u32> {
                 }]
             }
         };
+        door_motion_params.push(("ros_interface", "true"));
         door_model.joint.extend(joints);
         for (name, value) in door_motion_params.into_iter() {
-            plugin.elements.push(XmlElement {
+            component_data.push(XmlElement {
                 name: name.into(),
                 data: ElementData::String(value.to_string()),
                 ..Default::default()
             });
         }
-        plugin.elements.push(door_plugin_inner);
+        component_data.push(door_plugin_inner);
+        component.data = ElementData::Nested(component_data);
+        plugin.elements.push(component);
         door_model.plugin = vec![plugin];
         Ok(door_model)
     }
@@ -399,10 +411,9 @@ impl Door<u32> {
 
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
-        let get_anchor = |id: u32, level: Option<&Level>| -> Result<Anchor, SdfConversionError> {
-            level
-                .and_then(|l| l.anchors.get(&id))
-                .or_else(|| self.anchors.get(&id))
+        let get_anchor = |id: u32| -> Result<Anchor, SdfConversionError> {
+            self.anchors.get(&id)
+                .or_else(|| self.levels.values().find_map(|l| l.anchors.get(&id)))
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
         };
@@ -443,8 +454,8 @@ impl Site {
             });
             // Now add all the doors
             for door in level.doors.values() {
-                let left_anchor = get_anchor(door.anchors.left(), Some(level))?;
-                let right_anchor = get_anchor(door.anchors.right(), Some(level))?;
+                let left_anchor = get_anchor(door.anchors.left())?;
+                let right_anchor = get_anchor(door.anchors.right())?;
                 models.push(door.to_sdf(
                     left_anchor,
                     right_anchor,
@@ -458,15 +469,14 @@ impl Site {
                 continue;
             };
             let center = cabin.aabb().center;
-            let left_anchor = get_anchor(lift.properties.reference_anchors.left(), None)?;
-            let right_anchor = get_anchor(lift.properties.reference_anchors.right(), None)?;
+            let left_anchor = get_anchor(lift.properties.reference_anchors.left())?;
+            let right_anchor = get_anchor(lift.properties.reference_anchors.right())?;
             let left_trans = left_anchor.translation_for_category(Category::Level);
             let right_trans = right_anchor.translation_for_category(Category::Level);
             let midpoint = [
                 (left_trans[0] + right_trans[0]) / 2.0,
                 (left_trans[1] + right_trans[1]) / 2.0,
             ];
-            // TODO(luca) initial level
             let pose = Pose {
                 trans: [midpoint[0] + center.x, midpoint[1] + center.y, 0.0],
                 ..Default::default()
@@ -476,24 +486,113 @@ impl Site {
                 filename: "liblift.so".into(),
                 ..Default::default()
             };
+            let mut component = XmlElement {
+                name: "component".into(),
+                ..Default::default()
+            };
+            component.attributes.insert("name".into(), "Lift".into());
+            let mut component_data = ElementMap::default();
             let mut elements = vec![];
-            elements.push(("lift_name", lift.properties.name.0.clone()));
+            let lift_name = &lift.properties.name.0;
+            elements.push(("lift_name", lift_name.clone()));
             // TODO(luca) remove unwrap here for missing initial level
-            let initial_level = lift.properties.initial_level.0.and_then(|id| self.levels.get(&id)).map(|level| level.properties.name.0.clone()).unwrap();
+            let initial_level = lift
+                .properties
+                .initial_level
+                .0
+                .and_then(|id| self.levels.get(&id))
+                .map(|level| level.properties.name.0.clone())
+                .unwrap();
             elements.push(("initial_level", initial_level));
             elements.push(("v_max_cabin", "2.0".to_string()));
             elements.push(("a_max_cabin", "1.2".to_string()));
             elements.push(("a_nom_cabin", "1.0".to_string()));
             elements.push(("dx_min_cabin", "0.001".to_string()));
             elements.push(("f_max_cabin", "25323.0".to_string()));
-            for (name, value) in elements.into_iter() {
+            elements.push(("cabin_joint_name", "cabin_joint".to_string()));
+            let mut levels: BTreeMap<u32, ElementMap> = BTreeMap::new();
+            //let mut lift_models = Vec::new();
+            for (door_idx, door) in lift.cabin_doors.iter() {
+                let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, door_idx);
+                // Create a dummy cabin door first
+                let dummy_cabin = Door {
+                    anchors: door.reference_anchors.clone(),
+                    name: NameInSite(cabin_door_name.clone()),
+                    kind: door.kind.clone(),
+                    marker: DoorMarker,
+                };
+                // TODO(luca) remove unwrap here
+                let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
+                let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
+                // TODO(luca) do this when nested models are supported
+                //lift_models.push(dummy_cabin.to_sdf(
+                models.push(dummy_cabin.to_sdf(
+                    left_anchor,
+                    right_anchor,
+                    0.0
+                    //level.properties.elevation.0,
+                )?);
+                for visit in door.visits.0.iter() {
+                    let level = self
+                        .levels
+                        .get(visit)
+                        .ok_or(SdfConversionError::BrokenLevelReference(*visit))?;
+                    let shaft_door_name = format!("ShaftDoor_{}_{}_door_{}", level.properties.name.0, lift_name, door_idx);
+                    // TODO(luca) proper pose for shaft doors
+                    let dummy_shaft  = Door {
+                        anchors: door.reference_anchors.clone(),
+                        name: NameInSite(shaft_door_name.clone()),
+                        kind: door.kind.clone(),
+                        marker: DoorMarker,
+                    };
+                    let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
+                    let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
+                    models.push(dummy_shaft.to_sdf(
+                        left_anchor,
+                        right_anchor,
+                        level.properties.elevation.0,
+                    )?);
+                    let mut level = levels.entry(*visit).or_default();
+                    let element = XmlElement {
+                        name: "door_pair".into(),
+                        attributes: [
+                            ("cabin_door".to_string(), cabin_door_name.clone()),
+                            ("shaft_door".to_string(), shaft_door_name.clone()),
+                        ]
+                        .into(),
+                        ..Default::default()
+                    };
+                    level.push(element);
+                }
+            }
+            for (key, door_pairs) in levels.into_iter() {
+                let level = self
+                    .levels
+                    .get(&key)
+                    .ok_or(SdfConversionError::BrokenLevelReference(key))?;
                 plugin.elements.push(XmlElement {
+                    name: "floor".into(),
+                    attributes: [
+                        ("name".to_string(), level.properties.name.0.clone()),
+                        (
+                            "elevation".to_string(),
+                            level.properties.elevation.0.to_string(),
+                        ),
+                    ]
+                    .into(),
+                    data: ElementData::Nested(door_pairs),
+                    ..Default::default()
+                });
+            }
+            for (name, value) in elements.into_iter() {
+                component_data.push(XmlElement {
                     name: name.into(),
                     data: ElementData::String(value),
                     ..Default::default()
                 });
             }
-            // Now add the floors
+            component.data = ElementData::Nested(component_data);
+            plugin.elements.push(component);
             models.push(SdfModel {
                 name: lift.properties.name.0.clone(),
                 r#static: Some(lift.properties.is_static.0),
@@ -525,7 +624,8 @@ impl Site {
                     }],
                     ..Default::default()
                 }],
-                plugin: vec![plugin],
+                //model: lift_models,
+                //plugin: vec![plugin],
                 ..Default::default()
             });
         }
@@ -546,6 +646,31 @@ impl Site {
             direction: Vector3d::new(-0.5, 0.1, -0.9),
             ..Default::default()
         };
+        let lift_plugin = SdfPlugin {
+            name: "lift".into(),
+            filename: "liblift.so".into(),
+            ..Default::default()
+        };
+        let door_plugin = SdfPlugin {
+            name: "door".into(),
+            filename: "libdoor.so".into(),
+            ..Default::default()
+        };
+        let physics_plugin = SdfPlugin {
+            name: "gz::sim::systems::Physics".into(),
+            filename: "libgz-sim-physics-system.so".into(),
+            ..Default::default()
+        };
+        let user_commands_plugin = SdfPlugin {
+            name: "gz::sim::systems::UserCommands".into(),
+            filename: "libgz-sim-user-commands-system.so".into(),
+            ..Default::default()
+        };
+        let scene_broadcaster_plugin = SdfPlugin {
+            name: "gz::sim::systems::SceneBroadcaster".into(),
+            filename: "libgz-sim-scene-broadcaster-system.so".into(),
+            ..Default::default()
+        };
         Ok(SdfRoot {
             version: "1.7".to_string(),
             world: vec![SdfWorld {
@@ -561,6 +686,7 @@ impl Site {
                     ..Default::default()
                 },
                 light: vec![sun],
+                plugin: vec![physics_plugin, user_commands_plugin, scene_broadcaster_plugin, door_plugin, lift_plugin],
                 ..Default::default()
             }],
             ..Default::default()

From 2ecb4186d9b2d685f3b9a3584a0a6d18aef1244d Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 24 Jan 2024 11:59:26 +0800
Subject: [PATCH 13/71] WIP, lift door meshes generated, parenthood TODO

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 59 +++++++++++++++++++-----
 rmf_site_format/src/sdf.rs               | 33 ++++++++++---
 2 files changed, 74 insertions(+), 18 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 916ea88b..0ec1d9c0 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -17,12 +17,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Query<(&NameInSite, &LevelElevation, &Children)>,
         Query<Entity, With<WallMarker>>,
         Query<&FloorSegments>,
-        Query<(Option<&NameInSite>, Option<&SiteID>, &DoorSegments)>,
+        Query<(Option<&NameInSite>, &DoorSegments)>,
         Query<Entity, With<ModelMarker>>,
         Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
         Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
-        Query<(&NameInSite, &ChildLiftCabinGroup)>,
+        Query<(&NameInSite, &LiftCabin<Entity>, &ChildLiftCabinGroup)>,
         Query<((), With<LiftDoormat>)>,
         Query<&GlobalTransform>,
         Query<&Transform>,
@@ -173,7 +173,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             }
                         }
                     }
-                } else if let Ok((door_name, _, segments)) = q_doors.get(*child) {
+                } else if let Ok((door_name, segments)) = q_doors.get(*child) {
                     for (entity, segment_name) in segments
                         .body
                         .entities()
@@ -230,11 +230,11 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
             );
         }
         // Lifts
-        if let Ok((lift_name, lift_cabin)) = q_lift_cabins.get(*site_child) {
+        if let Ok((lift_name, cabin, cabin_children)) = q_lift_cabins.get(*site_child) {
             // The children of this entity have the mesh for the lift cabin
             info!("New lift");
             let mut lift_data = vec![];
-            for entity in DescendantIter::new(&q_children, **lift_cabin) {
+            for entity in DescendantIter::new(&q_children, **cabin_children) {
                 if q_lift_door_mats.get(entity).is_ok() {
                     // Just visual cues, not exported
                     continue;
@@ -253,13 +253,48 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
             let filename = format!("{}/{}.glb", folder.display(), **lift_name);
             write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename);
             // Now generate the lift doors
-            let Ok(children) = q_children.get(*site_child) else {
-                continue;
-            };
-            for child in children.iter() {
-                if let Ok((_, site_id, segments)) = q_doors.get(*child) {
-                    println!("Found lift door with id");
-                    dbg!(site_id);
+            let LiftCabin::Rect(cabin) = cabin;
+            for (face, door) in cabin.doors().iter() {
+                let Some(door) = door else {
+                    continue;
+                };
+                println!("Found door with label {}", face.label());
+                if let Ok((_, segments)) = q_doors.get(door.door) {
+                    println!("Segments found");
+                    // TODO(luca) this is duplicated with door generation, refactor
+                    for (entity, segment_name) in segments
+                        .body
+                        .entities()
+                        .iter()
+                        .zip(segments.body.labels().into_iter())
+                    {
+                        // Generate the visual and collisions here
+                        let Some((mesh, material)) = get_mesh_and_material(*entity) else {
+                            continue;
+                        };
+                        let Ok(tf) = q_tfs.get(*entity) else {
+                            continue;
+                        };
+                        let pose = GltfPose {
+                            translation: tf.translation.to_array(),
+                            rotation: tf.rotation.to_array(),
+                            scale: Some(tf.scale.to_array()),
+                        };
+
+                        let data = MeshData {
+                            mesh,
+                            material,
+                            pose: Some(pose.clone()),
+                        };
+                        let filename =
+                            format!("{}/{}_{}_{}.glb", folder.display(), **lift_name, face.label(), segment_name);
+                        write_meshes_to_file(
+                            vec![data],
+                            None,
+                            CompressGltfOptions::default(),
+                            filename,
+                        );
+                    }
                 }
             }
         }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 1d19cc6e..96d4afa1 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -99,6 +99,8 @@ impl Door<u32> {
         left_anchor: Anchor,
         right_anchor: Anchor,
         elevation: f32,
+        ros_interface: bool,
+        name_override: Option<String>,
     ) -> Result<SdfModel, SdfConversionError> {
         let door_mass = 50.0;
         let left_trans = left_anchor.translation_for_category(Category::Door);
@@ -147,10 +149,15 @@ impl Door<u32> {
             r#static: Some(false),
             ..Default::default()
         };
+        let link_name = if let Some(name_override) = name_override.as_ref() {
+            name_override
+        } else {
+            &self.name.0
+        };
         for label in labels.iter() {
             door_model
                 .link
-                .push(make_sdf_door_link(&self.name.0, label));
+                .push(make_sdf_door_link(link_name, label));
         }
         let mut door_motion_params = vec![];
         let joints = match &self.kind {
@@ -392,7 +399,8 @@ impl Door<u32> {
                 }]
             }
         };
-        door_motion_params.push(("ros_interface", "true"));
+        let b = ros_interface.to_string();
+        door_motion_params.push(("ros_interface", &b));
         door_model.joint.extend(joints);
         for (name, value) in door_motion_params.into_iter() {
             component_data.push(XmlElement {
@@ -460,6 +468,8 @@ impl Site {
                     left_anchor,
                     right_anchor,
                     level.properties.elevation.0,
+                    true,
+                    None,
                 )?);
             }
         }
@@ -512,8 +522,15 @@ impl Site {
             elements.push(("cabin_joint_name", "cabin_joint".to_string()));
             let mut levels: BTreeMap<u32, ElementMap> = BTreeMap::new();
             //let mut lift_models = Vec::new();
-            for (door_idx, door) in lift.cabin_doors.iter() {
-                let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, door_idx);
+            for (face, door) in cabin.doors().iter() {
+                let Some(door) = door else {
+                    continue;
+                };
+                // TODO(luca) use door struct for offset / shift
+                // TODO(luca) remove unwrap
+                let door = lift.cabin_doors.get(&door.door).unwrap();
+                let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, face.label());
+                let cabin_mesh_prefix = format!("{}_{}", lift_name, face.label());
                 // Create a dummy cabin door first
                 let dummy_cabin = Door {
                     anchors: door.reference_anchors.clone(),
@@ -529,7 +546,9 @@ impl Site {
                 models.push(dummy_cabin.to_sdf(
                     left_anchor,
                     right_anchor,
-                    0.0
+                    0.0,
+                    false,
+                    Some(cabin_mesh_prefix.clone()),
                     //level.properties.elevation.0,
                 )?);
                 for visit in door.visits.0.iter() {
@@ -537,7 +556,7 @@ impl Site {
                         .levels
                         .get(visit)
                         .ok_or(SdfConversionError::BrokenLevelReference(*visit))?;
-                    let shaft_door_name = format!("ShaftDoor_{}_{}_door_{}", level.properties.name.0, lift_name, door_idx);
+                    let shaft_door_name = format!("ShaftDoor_{}_{}_door_{}", level.properties.name.0, lift_name, face.label());
                     // TODO(luca) proper pose for shaft doors
                     let dummy_shaft  = Door {
                         anchors: door.reference_anchors.clone(),
@@ -551,6 +570,8 @@ impl Site {
                         left_anchor,
                         right_anchor,
                         level.properties.elevation.0,
+                        false,
+                        Some(cabin_mesh_prefix.clone()),
                     )?);
                     let mut level = levels.entry(*visit).or_default();
                     let element = XmlElement {

From 52583421b892ce77a095052320b4d72ecfe0f046 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 25 Jan 2024 17:05:08 +0800
Subject: [PATCH 14/71] WIP, lift doors working

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 Cargo.lock                       | 245 ++++++++++++++++---------------
 rmf_site_editor/Cargo.toml       |   2 +-
 rmf_site_editor/src/site/save.rs |   1 +
 rmf_site_format/Cargo.toml       |   2 +-
 rmf_site_format/src/sdf.rs       |  73 +++++----
 5 files changed, 177 insertions(+), 146 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 99d5070e..500bc87c 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -147,7 +147,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "77c3a9648d43b9cd48db467b3f87fdd6e146bcc88ab0180006cef2179fe11d01"
 dependencies = [
  "cfg-if",
- "getrandom 0.2.11",
+ "getrandom 0.2.12",
  "once_cell",
  "version_check",
  "zerocopy",
@@ -231,9 +231,9 @@ dependencies = [
 
 [[package]]
 name = "anstream"
-version = "0.6.5"
+version = "0.6.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d664a92ecae85fd0a7392615844904654d1d5f5514837f471ddef4a057aba1b6"
+checksum = "6e2e1ebcb11de5c03c67de28a7df593d32191b44939c482e97702baaaa6ab6a5"
 dependencies = [
  "anstyle",
  "anstyle-parse",
@@ -356,7 +356,7 @@ version = "1.8.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "17ae5ebefcc48e7452b4987947920dac9450be1110cadf34d1b8c116bdbaf97c"
 dependencies = [
- "async-lock 3.2.0",
+ "async-lock 3.3.0",
  "async-task",
  "concurrent-queue",
  "fastrand 2.0.1",
@@ -372,8 +372,8 @@ checksum = "05b1b633a2115cd122d73b955eadd9916c18c8f510ec9cd1686404c60ad1c29c"
 dependencies = [
  "async-channel 2.1.1",
  "async-executor",
- "async-io 2.2.2",
- "async-lock 3.2.0",
+ "async-io 2.3.0",
+ "async-lock 3.3.0",
  "blocking",
  "futures-lite 2.2.0",
  "once_cell",
@@ -401,18 +401,18 @@ dependencies = [
 
 [[package]]
 name = "async-io"
-version = "2.2.2"
+version = "2.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6afaa937395a620e33dc6a742c593c01aced20aa376ffb0f628121198578ccc7"
+checksum = "fb41eb19024a91746eba0773aa5e16036045bbf45733766661099e182ea6a744"
 dependencies = [
- "async-lock 3.2.0",
+ "async-lock 3.3.0",
  "cfg-if",
  "concurrent-queue",
  "futures-io",
  "futures-lite 2.2.0",
  "parking",
- "polling 3.3.1",
- "rustix 0.38.28",
+ "polling 3.3.2",
+ "rustix 0.38.30",
  "slab",
  "tracing",
  "windows-sys 0.52.0",
@@ -429,9 +429,9 @@ dependencies = [
 
 [[package]]
 name = "async-lock"
-version = "3.2.0"
+version = "3.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "7125e42787d53db9dd54261812ef17e937c95a51e4d291373b670342fa44310c"
+checksum = "d034b430882f8381900d3fe6f0aaa3ad94f2cb4ac519b429692a1bc2dda4ae7b"
 dependencies = [
  "event-listener 4.0.3",
  "event-listener-strategy",
@@ -540,9 +540,9 @@ checksum = "9e1b586273c5702936fe7b7d6896644d8be71e6314cfe09d3167c95f712589e8"
 
 [[package]]
 name = "base64"
-version = "0.21.6"
+version = "0.21.7"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c79fed4cdb43e993fcdadc7e58a09fd0e3e649c4436fa11da71c9f1f3ee7feb9"
+checksum = "9d297deb1925b89f2ccc13d7635fa0714f12c87adce1c75356b39ca9b7178567"
 
 [[package]]
 name = "bevy"
@@ -681,7 +681,7 @@ dependencies = [
  "bevy_render",
  "bevy_transform",
  "bevy_utils",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "radsort",
  "serde",
 ]
@@ -847,7 +847,7 @@ dependencies = [
 [[package]]
 name = "bevy_gltf_export"
 version = "0.1.0"
-source = "git+https://github.com/luca-della-vedova/bevy_gltf_export?branch=luca/primitive_instantiation#c73fbc0cc7405d4bfd9992496d7c460ef633df72"
+source = "git+https://github.com/luca-della-vedova/bevy_gltf_export?branch=main#88817fb1e7e3f6f3bf1872d2696fa0117d2a4535"
 dependencies = [
  "bevy_asset",
  "bevy_pbr",
@@ -1042,7 +1042,7 @@ dependencies = [
  "bevy_transform",
  "bevy_utils",
  "bevy_window",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "bytemuck",
  "naga_oil",
  "radsort",
@@ -1055,7 +1055,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "8b82c222b756d1d41ca808595934fc02edc5b05604151930bc7d00bb4fd8d1bd"
 dependencies = [
  "bevy",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "naga",
 ]
 
@@ -1138,7 +1138,7 @@ dependencies = [
  "bevy_transform",
  "bevy_utils",
  "bevy_window",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "bytemuck",
  "codespan-reporting",
  "downcast-rs",
@@ -1214,7 +1214,7 @@ dependencies = [
  "bevy_render",
  "bevy_transform",
  "bevy_utils",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "bytemuck",
  "fixedbitset",
  "guillotiere",
@@ -1336,7 +1336,7 @@ checksum = "08d9484e32434ea84dc548cff246ce0c6f756c1336f5ea03f24ac120a48595c7"
 dependencies = [
  "ahash",
  "bevy_utils_proc_macros",
- "getrandom 0.2.11",
+ "getrandom 0.2.12",
  "hashbrown 0.14.3",
  "instant",
  "petgraph",
@@ -1398,11 +1398,11 @@ dependencies = [
 
 [[package]]
 name = "bindgen"
-version = "0.69.1"
+version = "0.69.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9ffcebc3849946a7170a05992aac39da343a90676ab392c51a4280981d6379c2"
+checksum = "a4c69fae65a523209d34240b60abe0c42d33d1045d445c0839d8a4894a736e2d"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "cexpr",
  "clang-sys",
  "lazy_static",
@@ -1451,9 +1451,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a"
 
 [[package]]
 name = "bitflags"
-version = "2.4.1"
+version = "2.4.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "327762f6e5a765692301e5bb513e0d9fef63be86bbc14528052b1cd3e6f03e07"
+checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf"
 dependencies = [
  "serde",
 ]
@@ -1499,7 +1499,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6a37913e8dc4ddcc604f0c6d3bf2887c995153af3611de9e23c352b44c1b9118"
 dependencies = [
  "async-channel 2.1.1",
- "async-lock 3.2.0",
+ "async-lock 3.3.0",
  "async-task",
  "fastrand 2.0.1",
  "futures-io",
@@ -1516,9 +1516,9 @@ checksum = "7f30e7476521f6f8af1a1c4c0b8cc94f0bee37d91763d0ca2665f299b6cd8aec"
 
 [[package]]
 name = "bytemuck"
-version = "1.14.0"
+version = "1.14.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "374d28ec25809ee0e23827c2ab573d729e293f281dfe393500e7ad618baa61c6"
+checksum = "ed2490600f404f2b94c167e31d3ed1d5f3c225a0f3b80230053b3e0b7b962bd9"
 dependencies = [
  "bytemuck_derive",
 ]
@@ -1631,9 +1631,9 @@ dependencies = [
 
 [[package]]
 name = "clap"
-version = "4.4.14"
+version = "4.4.18"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "33e92c5c1a78c62968ec57dbc2440366a2d6e5a23faf829970ff1585dc6b18e2"
+checksum = "1e578d6ec4194633722ccf9544794b71b1385c3c027efe0c55db226fc880865c"
 dependencies = [
  "clap_builder",
  "clap_derive",
@@ -1641,9 +1641,9 @@ dependencies = [
 
 [[package]]
 name = "clap_builder"
-version = "4.4.14"
+version = "4.4.18"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "f4323769dc8a61e2c39ad7dc26f6f2800524691a44d74fe3d1071a5c24db6370"
+checksum = "4df4df40ec50c46000231c914968278b1eb05098cf8f1b3a518a95030e71d1c7"
 dependencies = [
  "anstream",
  "anstyle",
@@ -2298,9 +2298,9 @@ checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5"
 
 [[package]]
 name = "fdeflate"
-version = "0.3.3"
+version = "0.3.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "209098dd6dfc4445aa6111f0e98653ac323eaa4dfd212c9ca3931bf9955c31bd"
+checksum = "4f9bfee30e4dedf0ab8b422f03af778d9612b63f502710fc500a334ebe2de645"
 dependencies = [
  "simd-adler32",
 ]
@@ -2587,9 +2587,9 @@ dependencies = [
 
 [[package]]
 name = "getrandom"
-version = "0.2.11"
+version = "0.2.12"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "fe9006bed769170c11f845cf00c7c1e9092aeb3f268e007c3e760ac68008070f"
+checksum = "190092ea657667030ac6a35e305e62fc4dd69fd98ac98631e5d3a2b1575a12b5"
 dependencies = [
  "cfg-if",
  "js-sys",
@@ -2817,7 +2817,7 @@ version = "0.2.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "cc11df1ace8e7e564511f53af41f3e42ddc95b56fd07b3f4445d2a6048bc682c"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "gpu-descriptor-types",
  "hashbrown 0.14.3",
 ]
@@ -2828,7 +2828,7 @@ version = "0.1.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6bf0b36e6f090b7e1d8a4b49c0cb81c1f8376f72198c65dd3ad9ff3556b8b78c"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
 ]
 
 [[package]]
@@ -2936,9 +2936,9 @@ checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8"
 
 [[package]]
 name = "hermit-abi"
-version = "0.3.3"
+version = "0.3.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "d77f7ec81a6d05a3abb01ab6eb7590f6083d08449fe5a1c8b1e620283546ccb7"
+checksum = "5d3d0e0f38255e7fa3cf31335b3a56f05febd18025f4db5ef7a0cfb4f8da651f"
 
 [[package]]
 name = "hexasphere"
@@ -3049,9 +3049,9 @@ dependencies = [
 
 [[package]]
 name = "image"
-version = "0.24.7"
+version = "0.24.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6f3dfdbdd72063086ff443e297b61695500514b1e41095b6fb9a5ab48a70a711"
+checksum = "034bbe799d1909622a74d1193aa50147769440040ff36cb2baa947609b0a4e23"
 dependencies = [
  "bytemuck",
  "byteorder",
@@ -3059,7 +3059,6 @@ dependencies = [
  "exr",
  "gif",
  "jpeg-decoder",
- "num-rational",
  "num-traits",
  "png",
  "qoi",
@@ -3267,9 +3266,9 @@ dependencies = [
 
 [[package]]
 name = "jpeg-decoder"
-version = "0.3.0"
+version = "0.3.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bc0000e42512c92e31c2252315bda326620a4e034105e900c98ec492fa077b3e"
+checksum = "f5d4a7da358eff58addd2877a45865158f0d78c911d43a5784ceb7bbf52833b0"
 dependencies = [
  "rayon",
 ]
@@ -3409,7 +3408,7 @@ version = "0.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "85c833ca1e66078851dba29046874e38f08b2c883700aa29a03ddd3b23814ee8"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "libc",
  "redox_syscall 0.4.1",
 ]
@@ -3420,7 +3419,7 @@ version = "0.0.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "3af92c55d7d839293953fcd0fda5ecfe93297cfde6ffbdec13b41d99c0ba6607"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "libc",
  "redox_syscall 0.4.1",
 ]
@@ -3437,9 +3436,9 @@ dependencies = [
 
 [[package]]
 name = "libz-sys"
-version = "1.1.13"
+version = "1.1.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5f526fdd09d99e19742883e43de41e1aa9e36db0c7ab7f935165d611c5cccc66"
+checksum = "295c17e837573c8c821dbaeb3cceb3d745ad082f7572191409e69cbc1b3fd050"
 dependencies = [
  "cc",
  "libc",
@@ -3461,9 +3460,9 @@ checksum = "ef53942eb7bf7ff43a617b3e2c1c4a5ecf5944a7c1bc12d7ee39bbb15e5c1519"
 
 [[package]]
 name = "linux-raw-sys"
-version = "0.4.12"
+version = "0.4.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c4cd1a83af159aa67994778be9070f0ae1bd732942279cabb14f86f986a21456"
+checksum = "01cda141df6706de531b6c46c3a33ecca755538219bd484262fa09410c13539c"
 
 [[package]]
 name = "lock_api"
@@ -3773,7 +3772,7 @@ version = "0.27.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "2eb04e9c688eff1c89d72b407f168cf79bb9e867a9d3323ed6c01519eb9cc053"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "cfg-if",
  "libc",
 ]
@@ -3800,7 +3799,7 @@ version = "6.1.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "6205bd8bb1e454ad2e27422015fb5e4f2bcc7e08fa8f27058670d208324a4d2d"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "crossbeam-channel",
  "filetime",
  "fsevent-sys",
@@ -3852,6 +3851,17 @@ dependencies = [
  "syn 1.0.109",
 ]
 
+[[package]]
+name = "num-derive"
+version = "0.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cfb77679af88f8b125209d354a202862602672222e7f2313fdd6dc349bad4712"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn 2.0.48",
+]
+
 [[package]]
 name = "num-integer"
 version = "0.1.45"
@@ -4008,7 +4018,7 @@ dependencies = [
  "jni 0.20.0",
  "ndk",
  "ndk-context",
- "num-derive",
+ "num-derive 0.3.3",
  "num-traits",
  "oboe-sys",
 ]
@@ -4051,9 +4061,9 @@ checksum = "ff011a302c396a5197692431fc1948019154afc178baf7d8e37367442a4601cf"
 
 [[package]]
 name = "openssl-sys"
-version = "0.9.98"
+version = "0.9.99"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c1665caf8ab2dc9aef43d1c0023bd904633a6a05cb30b0ad59bec2ae986e57a7"
+checksum = "22e1bf214306098e4832460f797824c05d25aacdf896f64a985fb0fd992454ae"
 dependencies = [
  "cc",
  "libc",
@@ -4134,9 +4144,9 @@ dependencies = [
 
 [[package]]
 name = "parry3d"
-version = "0.13.5"
+version = "0.13.6"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b55dc0e6db79bddbc5fd583569f7356cdcc63e1e9b2b93a9ab70dd8e717160e0"
+checksum = "13d0bdaf533851feec5cba9af11cefcc753ecefba05f758cf6abe886086bc3f5"
 dependencies = [
  "approx",
  "arrayvec",
@@ -4144,7 +4154,7 @@ dependencies = [
  "downcast-rs",
  "either",
  "nalgebra",
- "num-derive",
+ "num-derive 0.4.1",
  "num-traits",
  "rustc-hash",
  "simba",
@@ -4232,15 +4242,15 @@ dependencies = [
 
 [[package]]
 name = "pkg-config"
-version = "0.3.28"
+version = "0.3.29"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "69d3587f8a9e599cc7ec2c00e331f71c4e69a5f9a4b8a6efd5b07466b9736f9a"
+checksum = "2900ede94e305130c13ddd391e0ab7cbaeb783945ae07a279c268cb05109c6cb"
 
 [[package]]
 name = "png"
-version = "0.17.10"
+version = "0.17.11"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "dd75bf2d8dd3702b9707cdbc56a5b9ef42cec752eb8b3bafc01234558442aa64"
+checksum = "1f6c3c3e617595665b8ea2ff95a86066be38fb121ff920a9c0eb282abcd1da5a"
 dependencies = [
  "bitflags 1.3.2",
  "crc32fast",
@@ -4267,14 +4277,14 @@ dependencies = [
 
 [[package]]
 name = "polling"
-version = "3.3.1"
+version = "3.3.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cf63fa624ab313c11656b4cda960bfc46c410187ad493c41f6ba2d8c1e991c9e"
+checksum = "545c980a3880efd47b2e262f6a4bb6daad6555cf3367aa9c4e52895f69537a41"
 dependencies = [
  "cfg-if",
  "concurrent-queue",
  "pin-project-lite",
- "rustix 0.38.28",
+ "rustix 0.38.30",
  "tracing",
  "windows-sys 0.52.0",
 ]
@@ -4323,9 +4333,9 @@ checksum = "dc375e1527247fe1a97d8b7156678dfe7c1af2fc075c9a4db3690ecd2a148068"
 
 [[package]]
 name = "proc-macro2"
-version = "1.0.76"
+version = "1.0.78"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "95fc56cda0b5c3325f5fbbd7ff9fda9e02bb00bb3dac51252d2f1bfa1cb8cc8c"
+checksum = "e2422ad645d89c99f8f3e6b88a9fdeca7fabeac836b1002371c4367c8f984aae"
 dependencies = [
  "unicode-ident",
 ]
@@ -4419,7 +4429,7 @@ version = "0.6.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c"
 dependencies = [
- "getrandom 0.2.11",
+ "getrandom 0.2.12",
 ]
 
 [[package]]
@@ -4450,7 +4460,7 @@ dependencies = [
  "crossbeam",
  "downcast-rs",
  "nalgebra",
- "num-derive",
+ "num-derive 0.3.3",
  "num-traits",
  "parry3d",
  "rustc-hash",
@@ -4471,9 +4481,9 @@ checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
 
 [[package]]
 name = "rayon"
-version = "1.8.0"
+version = "1.8.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9c27db03db7734835b3f53954b534c91069375ce6ccaa2e065441e07d9b6cdb1"
+checksum = "fa7237101a77a10773db45d62004a272517633fbcc3df19d96455ede1122e051"
 dependencies = [
  "either",
  "rayon-core",
@@ -4481,9 +4491,9 @@ dependencies = [
 
 [[package]]
 name = "rayon-core"
-version = "1.12.0"
+version = "1.12.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5ce3fb6ad83f861aac485e76e1985cd109d9a3713802152be56c3b1f0e0658ed"
+checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2"
 dependencies = [
  "crossbeam-deque",
  "crossbeam-utils",
@@ -4519,20 +4529,20 @@ version = "0.4.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "a18479200779601e498ada4e8c1e1f50e3ee19deb0259c25825a98b5603b2cb4"
 dependencies = [
- "getrandom 0.2.11",
+ "getrandom 0.2.12",
  "libredox 0.0.1",
  "thiserror",
 ]
 
 [[package]]
 name = "regex"
-version = "1.10.2"
+version = "1.10.3"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "380b951a9c5e80ddfd6136919eef32310721aa4aacd4889a8d39124b026ab343"
+checksum = "b62dbe01f0b06f9d8dc7d49e05a0785f153b00b2c227856282f671e0318c9b15"
 dependencies = [
  "aho-corasick",
  "memchr",
- "regex-automata 0.4.3",
+ "regex-automata 0.4.4",
  "regex-syntax 0.8.2",
 ]
 
@@ -4547,9 +4557,9 @@ dependencies = [
 
 [[package]]
 name = "regex-automata"
-version = "0.4.3"
+version = "0.4.4"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5f804c7828047e88b2d32e2d7fe5a105da8ee3264f01902f796c8e067dc2483f"
+checksum = "3b7fa1134405e2ec9353fd416b17f8dacd46c473d7d3fd1cf202706a14eb792a"
 dependencies = [
  "aho-corasick",
  "memchr",
@@ -4680,8 +4690,8 @@ version = "0.8.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "b91f7eff05f748767f183df4320a63d6936e9c6107d97c9e6bdd9784f4289c94"
 dependencies = [
- "base64 0.21.6",
- "bitflags 2.4.1",
+ "base64 0.21.7",
+ "bitflags 2.4.2",
  "serde",
  "serde_derive",
 ]
@@ -4723,14 +4733,14 @@ dependencies = [
 
 [[package]]
 name = "rustix"
-version = "0.38.28"
+version = "0.38.30"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "72e572a5e8ca657d7366229cdde4bd14c4eb5499a9573d4d366fe1b599daa316"
+checksum = "322394588aaf33c24007e8bb3238ee3e4c5c09c084ab32bc73890b99ff326bca"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "errno",
  "libc",
- "linux-raw-sys 0.4.12",
+ "linux-raw-sys 0.4.13",
  "windows-sys 0.52.0",
 ]
 
@@ -4787,6 +4797,7 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
 [[package]]
 name = "sdformat_rs"
 version = "0.1.0"
+source = "git+https://github.com/open-rmf/sdf_rust_experimental?branch=luca/yaserde_nested_models#605ffb7f3c821e919e6eda6b5224e58649d6e46d"
 dependencies = [
  "convert_case",
  "nalgebra",
@@ -4924,9 +4935,9 @@ dependencies = [
 
 [[package]]
 name = "shlex"
-version = "1.2.0"
+version = "1.3.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a7cee0529a6d40f580e7a5e6c495c8fbfe21b7b52795ed4bb5e62cdf92bc6380"
+checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64"
 
 [[package]]
 name = "simba"
@@ -4978,18 +4989,18 @@ dependencies = [
 
 [[package]]
 name = "smallvec"
-version = "1.11.2"
+version = "1.13.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4dccd0940a2dcdf68d092b8cbab7dc0ad8fa938bf95787e1b916b0e3d0e8e970"
+checksum = "e6ecd384b10a64542d77071bd64bd7b231f4ed5940fba55e98c3de13824cf3d7"
 dependencies = [
  "serde",
 ]
 
 [[package]]
 name = "smol_str"
-version = "0.2.0"
+version = "0.2.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "74212e6bbe9a4352329b2f68ba3130c15a3f26fe88ff22dbdc6cdd58fa85e99c"
+checksum = "e6845563ada680337a52d43bb0b29f396f2d911616f6573012645b9e3d048a49"
 dependencies = [
  "serde",
 ]
@@ -5006,9 +5017,9 @@ dependencies = [
 
 [[package]]
 name = "spade"
-version = "2.5.1"
+version = "2.6.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "bd774eb23cff002036706e6ea83c3f4ab4c80dad89da76fe16d49f77ab71682f"
+checksum = "61addf9117b11d1f5b4bf6fe94242ba25f59d2d4b2080544b771bd647024fd00"
 dependencies = [
  "hashbrown 0.14.3",
  "num-traits",
@@ -5147,7 +5158,7 @@ dependencies = [
  "cfg-if",
  "encoding_rs",
  "futures-util",
- "getrandom 0.2.11",
+ "getrandom 0.2.12",
  "http-client",
  "http-types",
  "log",
@@ -5234,9 +5245,9 @@ checksum = "69758bda2e78f098e4ccb393021a0963bb3442eac05f135c30f61b7370bbafae"
 
 [[package]]
 name = "termcolor"
-version = "1.4.0"
+version = "1.4.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "ff1bc3d3f05aff0403e8ac0d92ced918ec05b666a43f83297ccef5bea8a3d449"
+checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755"
 dependencies = [
  "winapi-util",
 ]
@@ -5293,9 +5304,9 @@ dependencies = [
 
 [[package]]
 name = "tiff"
-version = "0.9.0"
+version = "0.9.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6d172b0f4d3fba17ba89811858b9d3d97f928aece846475bbda076ca46736211"
+checksum = "ba1310fcea54c6a9a4fd1aad794ecc02c31682f6bfbecdf460bf19533eed1e3e"
 dependencies = [
  "flate2",
  "jpeg-decoder",
@@ -5357,9 +5368,9 @@ checksum = "1f3ccbac311fea05f86f61904b462b55fb3df8837a366dfc601a0161d0532f20"
 
 [[package]]
 name = "tobj"
-version = "4.0.0"
+version = "4.0.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b450e3ba06251ec4fc76917dafeaf55805ffb26dbf7d5500bfb9511ce63a0d1f"
+checksum = "5f7ca3ec0405b0f2f95e0dbcced28882190dc79b0dac63ff82533d256d770223"
 dependencies = [
  "ahash",
  "log",
@@ -5537,9 +5548,9 @@ dependencies = [
 
 [[package]]
 name = "unicode-bidi"
-version = "0.3.14"
+version = "0.3.15"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "6f2528f27a9eb2b21e69c95319b30bd0efd85d09c379741b0f78ea1d86be2416"
+checksum = "08f95100a766bf4f8f28f90d77e0a5461bbdb219042e7679bebe79004fed8d75"
 
 [[package]]
 name = "unicode-ident"
@@ -5623,11 +5634,11 @@ checksum = "95b09e3b3a0abd1ccb77673a6b7b8875d9d1c80626154add451cf18392dc4c3c"
 
 [[package]]
 name = "uuid"
-version = "1.6.1"
+version = "1.7.0"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "5e395fcf16a7a3d8127ec99782007af141946b4795001f876d54fb0d55978560"
+checksum = "f00cc9702ca12d3c81455259621e676d0f7251cec66a21e98fe2e9a37db93b2a"
 dependencies = [
- "getrandom 0.2.11",
+ "getrandom 0.2.12",
  "serde",
 ]
 
@@ -5801,9 +5812,9 @@ dependencies = [
 
 [[package]]
 name = "weezl"
-version = "0.1.7"
+version = "0.1.8"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "9193164d4de03a926d909d3bc7c30543cecb35400c02114792c2cae20d5e2dbb"
+checksum = "53a85b86a771b1c87058196170769dd264f66c0782acf1ae6cc51bfd64b39082"
 
 [[package]]
 name = "wgpu"
@@ -5837,7 +5848,7 @@ checksum = "8f478237b4bf0d5b70a39898a66fa67ca3a007d79f2520485b8b0c3dfc46f8c2"
 dependencies = [
  "arrayvec",
  "bit-vec",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "codespan-reporting",
  "log",
  "naga",
@@ -5862,7 +5873,7 @@ dependencies = [
  "arrayvec",
  "ash",
  "bit-set",
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "block",
  "core-graphics-types",
  "d3d12",
@@ -5900,16 +5911,16 @@ version = "0.16.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "d0c153280bb108c2979eb5c7391cb18c56642dd3c072e55f52065e13e2a1252a"
 dependencies = [
- "bitflags 2.4.1",
+ "bitflags 2.4.2",
  "js-sys",
  "web-sys",
 ]
 
 [[package]]
 name = "wide"
-version = "0.7.13"
+version = "0.7.14"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c68938b57b33da363195412cfc5fc37c9ed49aa9cfe2156fde64b8d2c9498242"
+checksum = "b31891d644eba1789fb6715f27fbc322e4bdf2ecdc412ede1993246159271613"
 dependencies = [
  "bytemuck",
  "safe_arch",
@@ -6261,9 +6272,9 @@ dependencies = [
 
 [[package]]
 name = "winnow"
-version = "0.5.33"
+version = "0.5.34"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "b7520bbdec7211caa7c4e682eb1fbe07abe20cee6756b6e00f537c82c11816aa"
+checksum = "b7cf47b659b318dccbd69cc4797a39ae128f533dce7902a1096044d1967b9c16"
 dependencies = [
  "memchr",
 ]
diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 01f80a0f..072a0701 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -48,7 +48,7 @@ rfd = "0.12"
 urdf-rs = "0.7"
 yaserde = "0.7"
 utm = "0.1.6"
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/serde_plugin"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/yaserde_nested_models"}
 gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "first_implementation" }
 pathdiff = "*"
 
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 6578c189..8e449442 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1287,6 +1287,7 @@ pub fn save_site(world: &mut World) {
                     Err(err) => {
                         //error!("Unable to convert site to sdf: {err}");
                         error!("Unable to convert site to sdf");
+                        dbg!(err);
                         continue;
                     }
                 };
diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index 93621283..3b1d0943 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -20,4 +20,4 @@ bevy = { version = "0.11", optional = true }
 urdf-rs = "0.7"
 yaserde = "0.7"
 pathdiff = "*"
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/serde_plugin"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/yaserde_nested_models"}
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 96d4afa1..c7326fba 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -18,6 +18,7 @@
 use crate::{
     Anchor, Angle, Category, Door, DoorType, Level, LiftCabin, Pose, Rotation, Side, Site, Swing, NameInSite, DoorMarker,
 };
+use glam::Vec3;
 use sdformat_rs::*;
 use std::collections::BTreeMap;
 
@@ -98,7 +99,7 @@ impl Door<u32> {
         &self,
         left_anchor: Anchor,
         right_anchor: Anchor,
-        elevation: f32,
+        offset: Vec3,
         ros_interface: bool,
         name_override: Option<String>,
     ) -> Result<SdfModel, SdfConversionError> {
@@ -141,7 +142,7 @@ impl Door<u32> {
             name: self.name.0.clone(),
             pose: Some(
                 Pose {
-                    trans: [center[0], center[1], elevation],
+                    trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(),
                     rot: Rotation::Yaw(Angle::Rad(yaw)),
                 }
                 .to_sdf(0.0),
@@ -189,7 +190,7 @@ impl Door<u32> {
                     pose: Some(pose),
                     axis: Some(SdfJointAxis {
                         xyz: Vector3d::new(0.0, door.towards.sign().into(), 0.0),
-                        limit: SdfJointaxisLimit {
+                        limit: SdfJointAxisLimit {
                             lower: 0.0,
                             upper: door_length as f64,
                             ..Default::default()
@@ -236,7 +237,7 @@ impl Door<u32> {
                     r#type: "revolute".into(),
                     axis: Some(SdfJointAxis {
                         xyz: Vector3d::new(0.0, 0.0, z),
-                        limit: SdfJointaxisLimit {
+                        limit: SdfJointAxisLimit {
                             lower,
                             upper,
                             ..Default::default()
@@ -284,7 +285,7 @@ impl Door<u32> {
                         pose: Some(right_pose),
                         axis: Some(SdfJointAxis {
                             xyz: Vector3d::new(0.0, -1.0, 0.0),
-                            limit: SdfJointaxisLimit {
+                            limit: SdfJointAxisLimit {
                                 lower: 0.0,
                                 upper: right_length as f64,
                                 ..Default::default()
@@ -301,7 +302,7 @@ impl Door<u32> {
                         pose: Some(left_pose),
                         axis: Some(SdfJointAxis {
                             xyz: Vector3d::new(0.0, -1.0, 0.0),
-                            limit: SdfJointaxisLimit {
+                            limit: SdfJointAxisLimit {
                                 lower: -left_length as f64,
                                 upper: 0.0,
                                 ..Default::default()
@@ -353,7 +354,7 @@ impl Door<u32> {
                         r#type: "revolute".into(),
                         axis: Some(SdfJointAxis {
                             xyz: Vector3d::new(0.0, 0.0, z),
-                            limit: SdfJointaxisLimit {
+                            limit: SdfJointAxisLimit {
                                 lower: 0.0,
                                 upper,
                                 ..Default::default()
@@ -370,7 +371,7 @@ impl Door<u32> {
                         r#type: "revolute".into(),
                         axis: Some(SdfJointAxis {
                             xyz: Vector3d::new(0.0, 0.0, z),
-                            limit: SdfJointaxisLimit {
+                            limit: SdfJointAxisLimit {
                                 lower: -upper,
                                 upper: 0.0,
                                 ..Default::default()
@@ -467,7 +468,7 @@ impl Site {
                 models.push(door.to_sdf(
                     left_anchor,
                     right_anchor,
-                    level.properties.elevation.0,
+                    Vec3::new(0.0, 0.0, level.properties.elevation.0),
                     true,
                     None,
                 )?);
@@ -492,8 +493,8 @@ impl Site {
                 ..Default::default()
             };
             let mut plugin = SdfPlugin {
-                name: "lift".into(),
-                filename: "liblift.so".into(),
+                name: "register_component".into(),
+                filename: "libregister_component.so".into(),
                 ..Default::default()
             };
             let mut component = XmlElement {
@@ -506,14 +507,14 @@ impl Site {
             let lift_name = &lift.properties.name.0;
             elements.push(("lift_name", lift_name.clone()));
             // TODO(luca) remove unwrap here for missing initial level
-            let initial_level = lift
+            let initial_floor = lift
                 .properties
                 .initial_level
                 .0
                 .and_then(|id| self.levels.get(&id))
                 .map(|level| level.properties.name.0.clone())
                 .unwrap();
-            elements.push(("initial_level", initial_level));
+            elements.push(("initial_floor", initial_floor));
             elements.push(("v_max_cabin", "2.0".to_string()));
             elements.push(("a_max_cabin", "1.2".to_string()));
             elements.push(("a_nom_cabin", "1.0".to_string()));
@@ -521,7 +522,18 @@ impl Site {
             elements.push(("f_max_cabin", "25323.0".to_string()));
             elements.push(("cabin_joint_name", "cabin_joint".to_string()));
             let mut levels: BTreeMap<u32, ElementMap> = BTreeMap::new();
-            //let mut lift_models = Vec::new();
+            let mut lift_models = Vec::new();
+            let mut lift_joints = vec![SdfJoint {
+                name: "cabin_joint".into(),
+                r#type: "prismatic".into(),
+                parent: "world".into(),
+                child: "platform".into(),
+                axis: Some(SdfJointAxis {
+                    xyz: Vector3d::new(0.0, 0.0, 1.0),
+                    ..Default::default()
+                }),
+                ..Default::default()
+                }];
             for (face, door) in cabin.doors().iter() {
                 let Some(door) = door else {
                     continue;
@@ -541,16 +553,20 @@ impl Site {
                 // TODO(luca) remove unwrap here
                 let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
                 let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
-                // TODO(luca) do this when nested models are supported
-                //lift_models.push(dummy_cabin.to_sdf(
-                models.push(dummy_cabin.to_sdf(
+                let mut dummy_cabin = dummy_cabin.to_sdf(
                     left_anchor,
                     right_anchor,
-                    0.0,
+                    Vec3::default(),
                     false,
                     Some(cabin_mesh_prefix.clone()),
-                    //level.properties.elevation.0,
-                )?);
+                )?;
+                for mut joint in dummy_cabin.joint.drain(..) {
+                    // Move the joint to the lift and change its parenthood accordingly
+                    joint.parent = "platform".into();
+                    joint.child = dummy_cabin.name.clone() + "::" + &joint.child;
+                    lift_joints.push(joint);
+                }
+                lift_models.push(dummy_cabin.into());
                 for visit in door.visits.0.iter() {
                     let level = self
                         .levels
@@ -566,13 +582,15 @@ impl Site {
                     };
                     let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
                     let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
-                    models.push(dummy_shaft.to_sdf(
+                    let mut dummy_shaft = dummy_shaft.to_sdf(
                         left_anchor,
                         right_anchor,
-                        level.properties.elevation.0,
+                        Vec3::from(pose.trans),
                         false,
                         Some(cabin_mesh_prefix.clone()),
-                    )?);
+                    )?;
+                    // Add the pose of the lift to have world coordinates
+                    models.push(dummy_shaft);
                     let mut level = levels.entry(*visit).or_default();
                     let element = XmlElement {
                         name: "door_pair".into(),
@@ -591,7 +609,7 @@ impl Site {
                     .levels
                     .get(&key)
                     .ok_or(SdfConversionError::BrokenLevelReference(key))?;
-                plugin.elements.push(XmlElement {
+                component_data.push(XmlElement {
                     name: "floor".into(),
                     attributes: [
                         ("name".to_string(), level.properties.name.0.clone()),
@@ -619,7 +637,7 @@ impl Site {
                 r#static: Some(lift.properties.is_static.0),
                 pose: Some(pose.to_sdf(0.0)),
                 link: vec![SdfLink {
-                    name: "link".into(),
+                    name: "platform".into(),
                     collision: vec![SdfCollision {
                         name: "collision".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
@@ -645,8 +663,9 @@ impl Site {
                     }],
                     ..Default::default()
                 }],
-                //model: lift_models,
-                //plugin: vec![plugin],
+                joint: lift_joints,
+                model: lift_models,
+                plugin: vec![plugin],
                 ..Default::default()
             });
         }

From fe4d6759504eb783804886b2477f1cd4b913b955 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 26 Jan 2024 16:05:44 +0800
Subject: [PATCH 15/71] Lifts fully working

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml               |  2 +-
 rmf_site_editor/src/site/sdf_exporter.rs | 19 +++++------
 rmf_site_format/src/sdf.rs               | 43 +++++++++++++-----------
 3 files changed, 33 insertions(+), 31 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 072a0701..241441cc 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -21,7 +21,7 @@ bevy_mod_raycast = "0.13"
 bevy_mod_outline = "0.5"
 bevy_infinite_grid = "0.8"
 bevy_polyline = "0.7"
-bevy_gltf_export = { git = "https://github.com/luca-della-vedova/bevy_gltf_export", branch = "main"}
+bevy_gltf_export = { git = "https://github.com/luca-della-vedova/bevy_gltf_export", branch = "luca/primitive_instantiation"}
 bevy_stl = "0.11"
 bevy_obj = { version = "0.11", features = ["scene"] }
 bevy_rapier3d = "0.22.0"
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 0ec1d9c0..199b7d5d 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -95,12 +95,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                     };
                     collision_data.push(MeshData {
                         mesh,
-                        material,
+                        material: None,
                         pose: Some(level_pose.clone()),
                     });
                     visual_data.push(MeshData {
                         mesh,
-                        material,
+                        material: Some(material),
                         pose: Some(level_pose.clone()),
                     });
                 } else if let Ok(res) = q_floors.get(*child) {
@@ -109,12 +109,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                     };
                     collision_data.push(MeshData {
                         mesh,
-                        material,
+                        material: None,
                         pose: Some(level_pose.clone()),
                     });
                     visual_data.push(MeshData {
                         mesh,
-                        material,
+                        material: Some(material),
                         pose: Some(level_pose.clone()),
                     });
                 } else if let Ok(model) = q_models.get(*child) {
@@ -142,7 +142,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                 };
                                 collision_data.push(MeshData {
                                     mesh,
-                                    material,
+                                    material: None,
                                     pose: Some(pose),
                                 });
                             }
@@ -167,7 +167,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                 };
                                 visual_data.push(MeshData {
                                     mesh,
-                                    material,
+                                    material: Some(material),
                                     pose: Some(pose),
                                 });
                             }
@@ -198,7 +198,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
 
                         let data = MeshData {
                             mesh,
-                            material,
+                            material: Some(material),
                             pose: Some(pose.clone()),
                         };
                         let filename =
@@ -232,7 +232,6 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         // Lifts
         if let Ok((lift_name, cabin, cabin_children)) = q_lift_cabins.get(*site_child) {
             // The children of this entity have the mesh for the lift cabin
-            info!("New lift");
             let mut lift_data = vec![];
             for entity in DescendantIter::new(&q_children, **cabin_children) {
                 if q_lift_door_mats.get(entity).is_ok() {
@@ -246,7 +245,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                 info!("Found mesh for cabin child");
                 lift_data.push(MeshData {
                     mesh,
-                    material,
+                    material: Some(material),
                     pose: None,
                 });
             }
@@ -283,7 +282,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
 
                         let data = MeshData {
                             mesh,
-                            material,
+                            material: Some(material),
                             pose: Some(pose.clone()),
                         };
                         let filename =
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index c7326fba..1b9d8f90 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -114,6 +114,7 @@ impl Door<u32> {
         let dy = left_trans[1] - right_trans[1];
         let door_length = (dx * dx + dy * dy).sqrt();
         let yaw = -dx.atan2(dy);
+        let prefix = name_override.clone().unwrap_or_default();
         let labels = match self.kind {
             DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
                 Vec::from(["body"])
@@ -171,7 +172,7 @@ impl Door<u32> {
                     .insert("left_joint_name".into(), "empty_joint".into());
                 door_plugin_inner
                     .attributes
-                    .insert("right_joint_name".into(), "joint".into());
+                    .insert("right_joint_name".into(), prefix.clone() + "joint");
                 door_motion_params.push(("v_max_door", "0.2"));
                 door_motion_params.push(("a_max_door", "0.2"));
                 door_motion_params.push(("a_nom_door", "0.08"));
@@ -183,7 +184,7 @@ impl Door<u32> {
                 }
                 .to_sdf(0.0);
                 vec![SdfJoint {
-                    name: "joint".into(),
+                    name: prefix.clone() + "joint",
                     parent: "world".into(),
                     child: "body".into(),
                     r#type: "prismatic".into(),
@@ -223,15 +224,15 @@ impl Door<u32> {
                     ..Default::default()
                 }
                 .to_sdf(0.0);
-                let (left_joint_name, right_joint_name) = ("empty_joint", "joint");
+                let (left_joint_name, right_joint_name) = ("empty_joint", prefix.clone() + "joint");
                 door_plugin_inner
                     .attributes
                     .insert("left_joint_name".into(), left_joint_name.into());
                 door_plugin_inner
                     .attributes
-                    .insert("right_joint_name".into(), right_joint_name.into());
+                    .insert("right_joint_name".into(), right_joint_name);
                 vec![SdfJoint {
-                    name: "joint".into(),
+                    name: prefix.clone() + "joint",
                     parent: "world".into(),
                     child: "body".into(),
                     r#type: "revolute".into(),
@@ -254,10 +255,10 @@ impl Door<u32> {
                     .insert("type".into(), "DoubleSlidingDoor".into());
                 door_plugin_inner
                     .attributes
-                    .insert("left_joint_name".into(), "left_joint".into());
+                    .insert("left_joint_name".into(), prefix.clone() + "left_joint");
                 door_plugin_inner
                     .attributes
-                    .insert("right_joint_name".into(), "right_joint".into());
+                    .insert("right_joint_name".into(), prefix.clone() + "right_joint");
                 door_motion_params.push(("v_max_door", "0.2"));
                 door_motion_params.push(("a_max_door", "0.2"));
                 door_motion_params.push(("a_nom_door", "0.08"));
@@ -278,7 +279,7 @@ impl Door<u32> {
                 let right_length = door_length - left_length;
                 vec![
                     SdfJoint {
-                        name: "right_joint".into(),
+                        name: prefix.clone() + "right_joint",
                         parent: "world".into(),
                         child: "right".into(),
                         r#type: "prismatic".into(),
@@ -295,7 +296,7 @@ impl Door<u32> {
                         ..Default::default()
                     },
                     SdfJoint {
-                        name: "left_joint".into(),
+                        name: prefix.clone() + "left_joint",
                         parent: "world".into(),
                         child: "left".into(),
                         r#type: "prismatic".into(),
@@ -319,10 +320,10 @@ impl Door<u32> {
                     .insert("type".into(), "DoubleSwingDoor".into());
                 door_plugin_inner
                     .attributes
-                    .insert("left_joint_name".into(), "left_joint".into());
+                    .insert("left_joint_name".into(), prefix.clone() + "left_joint");
                 door_plugin_inner
                     .attributes
-                    .insert("right_joint_name".into(), "right_joint".into());
+                    .insert("right_joint_name".into(), prefix.clone() + "right_joint");
                 door_motion_params.push(("v_max_door", "0.5"));
                 door_motion_params.push(("a_max_door", "0.3"));
                 door_motion_params.push(("a_nom_door", "0.15"));
@@ -348,7 +349,7 @@ impl Door<u32> {
                 .to_sdf(0.0);
                 vec![
                     SdfJoint {
-                        name: "right_joint".into(),
+                        name: prefix.clone() + "right_joint",
                         parent: "world".into(),
                         child: "right".into(),
                         r#type: "revolute".into(),
@@ -365,7 +366,7 @@ impl Door<u32> {
                         ..Default::default()
                     },
                     SdfJoint {
-                        name: "left_joint".into(),
+                        name: prefix.clone() + "left_joint",
                         parent: "world".into(),
                         child: "left".into(),
                         r#type: "revolute".into(),
@@ -391,7 +392,7 @@ impl Door<u32> {
                 }
                 .to_sdf(0.0);
                 vec![SdfJoint {
-                    name: "joint".into(),
+                    name: prefix.clone() + "joint",
                     parent: "world".into(),
                     child: "body".into(),
                     r#type: "fixed".into(),
@@ -534,13 +535,13 @@ impl Site {
                 }),
                 ..Default::default()
                 }];
-            for (face, door) in cabin.doors().iter() {
-                let Some(door) = door else {
+            for (face, door_placement) in cabin.doors().iter() {
+                let Some(door_placement) = door_placement else {
                     continue;
                 };
                 // TODO(luca) use door struct for offset / shift
                 // TODO(luca) remove unwrap
-                let door = lift.cabin_doors.get(&door.door).unwrap();
+                let door = lift.cabin_doors.get(&door_placement.door).unwrap();
                 let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, face.label());
                 let cabin_mesh_prefix = format!("{}_{}", lift_name, face.label());
                 // Create a dummy cabin door first
@@ -553,10 +554,12 @@ impl Site {
                 // TODO(luca) remove unwrap here
                 let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
                 let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
+                let x_offset = -face.u() * (door_placement.thickness() / 2.0 + door_placement.custom_gap.unwrap_or_else(|| cabin.gap.unwrap_or(0.01)));
+                dbg!(&x_offset);
                 let mut dummy_cabin = dummy_cabin.to_sdf(
                     left_anchor,
                     right_anchor,
-                    Vec3::default(),
+                    x_offset,
                     false,
                     Some(cabin_mesh_prefix.clone()),
                 )?;
@@ -585,7 +588,7 @@ impl Site {
                     let mut dummy_shaft = dummy_shaft.to_sdf(
                         left_anchor,
                         right_anchor,
-                        Vec3::from(pose.trans),
+                        Vec3::from(pose.trans) + Vec3::new(0.0, 0.0, level.properties.elevation.0),
                         false,
                         Some(cabin_mesh_prefix.clone()),
                     )?;
@@ -646,7 +649,7 @@ impl Site {
                         }),
                         surface: Some(SdfSurface {
                             contact: Some(SdfSurfaceContact {
-                                collide_bitmask: Some("0x02".into()),
+                                collide_bitmask: Some("0x04".into()),
                                 ..Default::default()
                             }),
                             ..Default::default()

From 7314c763d8cd2337164541224aa2e0f66311a214 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 26 Jan 2024 16:06:09 +0800
Subject: [PATCH 16/71] Format

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 13 ++++--
 rmf_site_format/src/sdf.rs               | 59 ++++++++++++++++++------
 2 files changed, 55 insertions(+), 17 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 199b7d5d..fa6f90c3 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -9,7 +9,9 @@ use crate::site::{
     ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, FloorSegments, LiftDoormat,
     VisualMeshMarker,
 };
-use rmf_site_format::{SiteID, LevelElevation, LiftCabin, ModelMarker, NameInSite, Pose, WallMarker};
+use rmf_site_format::{
+    LevelElevation, LiftCabin, ModelMarker, NameInSite, Pose, SiteID, WallMarker,
+};
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
     let mut state: SystemState<(
@@ -285,8 +287,13 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             material: Some(material),
                             pose: Some(pose.clone()),
                         };
-                        let filename =
-                            format!("{}/{}_{}_{}.glb", folder.display(), **lift_name, face.label(), segment_name);
+                        let filename = format!(
+                            "{}/{}_{}_{}.glb",
+                            folder.display(),
+                            **lift_name,
+                            face.label(),
+                            segment_name
+                        );
                         write_meshes_to_file(
                             vec![data],
                             None,
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 1b9d8f90..5c41d27d 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -16,7 +16,8 @@
 */
 
 use crate::{
-    Anchor, Angle, Category, Door, DoorType, Level, LiftCabin, Pose, Rotation, Side, Site, Swing, NameInSite, DoorMarker,
+    Anchor, Angle, Category, Door, DoorMarker, DoorType, Level, LiftCabin, NameInSite, Pose,
+    Rotation, Side, Site, Swing,
 };
 use glam::Vec3;
 use sdformat_rs::*;
@@ -157,9 +158,7 @@ impl Door<u32> {
             &self.name.0
         };
         for label in labels.iter() {
-            door_model
-                .link
-                .push(make_sdf_door_link(link_name, label));
+            door_model.link.push(make_sdf_door_link(link_name, label));
         }
         let mut door_motion_params = vec![];
         let joints = match &self.kind {
@@ -422,7 +421,8 @@ impl Door<u32> {
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
         let get_anchor = |id: u32| -> Result<Anchor, SdfConversionError> {
-            self.anchors.get(&id)
+            self.anchors
+                .get(&id)
                 .or_else(|| self.levels.values().find_map(|l| l.anchors.get(&id)))
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
@@ -534,7 +534,7 @@ impl Site {
                     ..Default::default()
                 }),
                 ..Default::default()
-                }];
+            }];
             for (face, door_placement) in cabin.doors().iter() {
                 let Some(door_placement) = door_placement else {
                     continue;
@@ -552,9 +552,21 @@ impl Site {
                     marker: DoorMarker,
                 };
                 // TODO(luca) remove unwrap here
-                let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
-                let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
-                let x_offset = -face.u() * (door_placement.thickness() / 2.0 + door_placement.custom_gap.unwrap_or_else(|| cabin.gap.unwrap_or(0.01)));
+                let left_anchor = lift
+                    .cabin_anchors
+                    .get(&door.reference_anchors.left())
+                    .unwrap()
+                    .clone();
+                let right_anchor = lift
+                    .cabin_anchors
+                    .get(&door.reference_anchors.right())
+                    .unwrap()
+                    .clone();
+                let x_offset = -face.u()
+                    * (door_placement.thickness() / 2.0
+                        + door_placement
+                            .custom_gap
+                            .unwrap_or_else(|| cabin.gap.unwrap_or(0.01)));
                 dbg!(&x_offset);
                 let mut dummy_cabin = dummy_cabin.to_sdf(
                     left_anchor,
@@ -575,16 +587,29 @@ impl Site {
                         .levels
                         .get(visit)
                         .ok_or(SdfConversionError::BrokenLevelReference(*visit))?;
-                    let shaft_door_name = format!("ShaftDoor_{}_{}_door_{}", level.properties.name.0, lift_name, face.label());
+                    let shaft_door_name = format!(
+                        "ShaftDoor_{}_{}_door_{}",
+                        level.properties.name.0,
+                        lift_name,
+                        face.label()
+                    );
                     // TODO(luca) proper pose for shaft doors
-                    let dummy_shaft  = Door {
+                    let dummy_shaft = Door {
                         anchors: door.reference_anchors.clone(),
                         name: NameInSite(shaft_door_name.clone()),
                         kind: door.kind.clone(),
                         marker: DoorMarker,
                     };
-                    let left_anchor = lift.cabin_anchors.get(&door.reference_anchors.left()).unwrap().clone();
-                    let right_anchor = lift.cabin_anchors.get(&door.reference_anchors.right()).unwrap().clone();
+                    let left_anchor = lift
+                        .cabin_anchors
+                        .get(&door.reference_anchors.left())
+                        .unwrap()
+                        .clone();
+                    let right_anchor = lift
+                        .cabin_anchors
+                        .get(&door.reference_anchors.right())
+                        .unwrap()
+                        .clone();
                     let mut dummy_shaft = dummy_shaft.to_sdf(
                         left_anchor,
                         right_anchor,
@@ -729,7 +754,13 @@ impl Site {
                     ..Default::default()
                 },
                 light: vec![sun],
-                plugin: vec![physics_plugin, user_commands_plugin, scene_broadcaster_plugin, door_plugin, lift_plugin],
+                plugin: vec![
+                    physics_plugin,
+                    user_commands_plugin,
+                    scene_broadcaster_plugin,
+                    door_plugin,
+                    lift_plugin,
+                ],
                 ..Default::default()
             }],
             ..Default::default()

From 4d0c4c54d04de42560821609092759cc04a18944 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 30 Jan 2024 12:13:05 +0800
Subject: [PATCH 17/71] Add robot spawning

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 47 ++++++++++++++++++++++++++++++++++++--
 1 file changed, 45 insertions(+), 2 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 5c41d27d..4a9abccf 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -16,8 +16,8 @@
 */
 
 use crate::{
-    Anchor, Angle, Category, Door, DoorMarker, DoorType, Level, LiftCabin, NameInSite, Pose,
-    Rotation, Side, Site, Swing,
+    Anchor, Angle, AssetSource, Category, Door, DoorMarker, DoorType, Level, LiftCabin, NameInSite,
+    Pose, Rotation, Side, Site, Swing,
 };
 use glam::Vec3;
 use sdformat_rs::*;
@@ -739,11 +739,47 @@ impl Site {
             filename: "libgz-sim-scene-broadcaster-system.so".into(),
             ..Default::default()
         };
+        // Spawn the robots now
+        // TODO(luca) use robot properties and export this like other meshes
+        // instead of including a tag
+        let mut world_includes = Vec::new();
+        for location in self.navigation.guided.locations.values() {
+            let Some(robot) = location.tags.0.iter().find_map(|l| l.spawn_robot()) else {
+                continue;
+            };
+            // For now all robots are just Search type
+            let AssetSource::Search(ref robot_type) = robot.source else {
+                continue;
+            };
+            let Some(level) = self
+                .levels
+                .values()
+                .find(|l| l.anchors.get(&location.anchor.0).is_some())
+            else {
+                // TODO(luca) this would fail if the robot was on a site anchor
+                continue;
+            };
+            // Get the location
+            let anchor = get_anchor(location.anchor.0)?;
+            let tf = anchor.translation_for_category(Category::Level);
+            let pose = Pose {
+                trans: [tf[0], tf[1], level.properties.elevation.0],
+                ..Default::default()
+            };
+            world_includes.push(SdfWorldInclude {
+                uri: "model://".to_string() + robot_type,
+                name: Some(robot.name.0.clone()),
+                pose: Some(pose.to_sdf(0.0)),
+                r#static: Some(robot.is_static.0),
+                ..Default::default()
+            });
+        }
         Ok(SdfRoot {
             version: "1.7".to_string(),
             world: vec![SdfWorld {
                 name: self.properties.name.0.clone(),
                 model: models,
+                include: world_includes,
                 atmosphere: SdfAtmosphere {
                     r#type: "adiabatic".to_string(),
                     ..Default::default()
@@ -753,6 +789,13 @@ impl Site {
                     background: "0.8 0.8 0.8".to_string(),
                     ..Default::default()
                 },
+                physics: vec![SdfPhysics {
+                    r#type: "ode".into(),
+                    max_step_size: 0.01,
+                    real_time_factor: 1.0,
+                    real_time_update_rate: 100.0,
+                    ..Default::default()
+                }],
                 light: vec![sun],
                 plugin: vec![
                     physics_plugin,

From 614a9a3619cbc929a689c6364a4b4866113bac7f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 30 Jan 2024 17:25:57 +0800
Subject: [PATCH 18/71] Add orientation constraint, doors and lifts

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/legacy/nav_graph.rs | 148 ++++++++++++++++++++----
 rmf_site_format/src/lift.rs             |  54 +++++++++
 rmf_site_format/src/misc.rs             |   2 +-
 rmf_site_format/src/site.rs             |   6 +
 4 files changed, 189 insertions(+), 21 deletions(-)

diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs
index 17316410..fe0e806b 100644
--- a/rmf_site_format/src/legacy/nav_graph.rs
+++ b/rmf_site_format/src/legacy/nav_graph.rs
@@ -6,6 +6,27 @@ use std::collections::{HashMap, HashSet};
 pub struct NavGraph {
     pub building_name: String,
     pub levels: HashMap<String, NavLevel>,
+    pub doors: HashMap<String, NavDoor>,
+    pub lifts: HashMap<String, NavLift>,
+}
+
+// Readapted from legacy traffic editor implementation
+fn segments_intersect(p1: [f32; 2], p2: [f32; 2], p3: [f32; 2], p4: [f32; 2]) -> bool {
+    // line segments are (x1,y1),(x2,y2) and (x3,y3),(x4,y4)
+    let [x1, y1] = p1;
+    let [x2, y2] = p2;
+    let [x3, y3] = p3;
+    let [x4, y4] = p4;
+    let det = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+    if det.abs() < 0.01 {
+        return false;
+    }
+    let t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / det;
+    let u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / det;
+    if u < 0.0 || t < 0.0 || u > 1.0 || t > 1.0 {
+        return false;
+    }
+    true
 }
 
 impl NavGraph {
@@ -39,6 +60,7 @@ impl NavGraph {
 
             // TODO(MXG): Make this work for lifts
 
+            let mut doors = HashMap::new();
             let mut levels = HashMap::new();
             for (_, level) in &site.levels {
                 let mut anchor_to_vertex = HashMap::new();
@@ -55,6 +77,33 @@ impl NavGraph {
                     vertices.push(NavVertex::from_anchor(anchor, location_at_anchor.get(id)));
                 }
 
+                let mut level_doors = HashMap::new();
+                for (_, door) in &level.doors {
+                    let door_name = &door.name.0;
+                    let (v0, v1) = match (
+                        level.anchors.get(&door.anchors.start()),
+                        level.anchors.get(&door.anchors.end()),
+                    ) {
+                        (Some(v0), Some(v1)) => (
+                            v0.translation_for_category(Category::Level),
+                            v1.translation_for_category(Category::Level),
+                        ),
+                        _ => {
+                            println!(
+                                "ERROR: Skipping door {door_name} due to broken anchor reference"
+                            );
+                            continue;
+                        }
+                    };
+                    level_doors.insert(
+                        door_name.clone(),
+                        NavDoor {
+                            map: level.properties.name.0.clone(),
+                            endpoints: [*v0, *v1],
+                        },
+                    );
+                }
+
                 let mut lanes = Vec::new();
                 for lane_id in &lanes_to_include {
                     let lane = site.navigation.guided.lanes.get(lane_id).unwrap();
@@ -69,14 +118,27 @@ impl NavGraph {
                         }
                     };
 
-                    let props = NavLaneProperties::from_motion(&lane.forward);
+                    let mut door_name = None;
+                    let l0 = [vertices[v0].0, vertices[v0].1];
+                    let l1 = [vertices[v1].0, vertices[v1].1];
+                    for (name, door) in &level_doors {
+                        if segments_intersect(l0, l1, door.endpoints[0], door.endpoints[1]) {
+                            door_name = Some(name);
+                        }
+                    }
+
+                    let props = NavLaneProperties::from_motion(&lane.forward, door_name.cloned());
                     lanes.push(NavLane(v0, v1, props.clone()));
                     match &lane.reverse {
                         ReverseLane::Same => {
                             lanes.push(NavLane(v1, v0, props));
                         }
                         ReverseLane::Different(motion) => {
-                            lanes.push(NavLane(v1, v0, NavLaneProperties::from_motion(motion)));
+                            lanes.push(NavLane(
+                                v1,
+                                v0,
+                                NavLaneProperties::from_motion(motion, door_name.cloned()),
+                            ));
                         }
                         ReverseLane::Disable => {
                             // Do nothing
@@ -84,17 +146,46 @@ impl NavGraph {
                     }
                 }
 
+                doors.extend(level_doors);
                 levels.insert(
                     level.properties.name.clone().0,
                     NavLevel { lanes, vertices },
                 );
             }
 
+            let mut lifts = HashMap::new();
+            for (_, lift) in &site.lifts {
+                let lift_name = &lift.properties.name.0;
+                let Some(pose) = lift.properties.center(site) else {
+                    println!("ERROR: Skipping lift {lift_name} due to broken anchor reference");
+                    continue;
+                };
+                let Rotation::Yaw(yaw) = pose.rot else {
+                    println!("ERROR: Skipping lift {lift_name} due to rotation not being pure yaw");
+                    continue;
+                };
+                match &lift.properties.cabin {
+                    LiftCabin::Rect(params) => {
+                        lifts.insert(
+                            lift_name.clone(),
+                            NavLift {
+                                position: [pose.trans[0], pose.trans[1], yaw.radians()],
+                                // Note depth and width are inverted between legacy and site editor
+                                dims: [params.depth, params.width],
+                            },
+                        );
+                    }
+                }
+                // TODO(luca) lift property for vertices
+            }
+
             graphs.push((
                 graph.name.0.clone(),
                 Self {
                     building_name: site.properties.name.clone().0,
                     levels,
+                    doors,
+                    lifts,
                 },
             ))
         }
@@ -117,17 +208,32 @@ pub struct NavLaneProperties {
     pub speed_limit: f32,
     #[serde(skip_serializing_if = "Option::is_none")]
     pub dock_name: Option<String>,
-    // TODO(MXG): Add other lane properties
-    // door_name,
-    // orientation_constraint,
-    // demo_mock_floor_name
+    #[serde(skip_serializing_if = "Option::is_none")]
+    pub door_name: Option<String>,
+    #[serde(skip_serializing_if = "Option::is_none")]
+    pub orientation_constraint: Option<String>, // TODO(MXG): Add other lane properties
+                                                // demo_mock_floor_name
 }
 
 impl NavLaneProperties {
-    fn from_motion(motion: &Motion) -> Self {
+    fn from_motion(motion: &Motion, door_name: Option<String>) -> Self {
+        let orientation_constraint = match &motion.orientation_constraint {
+            OrientationConstraint::None => None,
+            OrientationConstraint::Forwards => Some("forward".to_owned()),
+            OrientationConstraint::Backwards => Some("backward".to_owned()),
+            OrientationConstraint::RelativeYaw(_) | OrientationConstraint::AbsoluteYaw(_) => {
+                println!(
+                    "Skipping orientation constraint [{:?}] because of incompatibility",
+                    motion.orientation_constraint
+                );
+                None
+            }
+        };
         Self {
             speed_limit: motion.speed_limit.unwrap_or(0.0),
             dock_name: motion.dock.as_ref().map(|d| d.name.clone()),
+            orientation_constraint,
+            door_name,
         }
     }
 }
@@ -142,7 +248,7 @@ impl NavVertex {
     }
 }
 
-#[derive(Serialize, Deserialize, Clone)]
+#[derive(Serialize, Deserialize, Clone, Default)]
 pub struct NavVertexProperties {
     #[serde(skip_serializing_if = "Option::is_none")]
     pub lift: Option<String>,
@@ -152,21 +258,11 @@ pub struct NavVertexProperties {
     pub is_holding_point: bool,
     #[serde(skip_serializing_if = "is_false")]
     pub is_parking_spot: bool,
+    #[serde(skip_serializing_if = "Option::is_none")]
+    pub merge_radius: Option<f32>,
     pub name: String,
 }
 
-impl Default for NavVertexProperties {
-    fn default() -> Self {
-        Self {
-            lift: None,
-            is_charger: false,
-            is_holding_point: false,
-            is_parking_spot: false,
-            name: "".to_owned(),
-        }
-    }
-}
-
 impl NavVertexProperties {
     fn from_location(location: Option<&Location<u32>>) -> Self {
         let mut props = Self::default();
@@ -196,3 +292,15 @@ impl NavVertexProperties {
 fn is_false(b: &bool) -> bool {
     !b
 }
+
+#[derive(Serialize, Deserialize, Clone)]
+pub struct NavDoor {
+    pub endpoints: [[f32; 2]; 2],
+    pub map: String,
+}
+
+#[derive(Serialize, Deserialize, Clone)]
+pub struct NavLift {
+    pub position: [f32; 3],
+    pub dims: [f32; 2],
+}
diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs
index 97d22d38..90d16040 100644
--- a/rmf_site_format/src/lift.rs
+++ b/rmf_site_format/src/lift.rs
@@ -115,6 +115,60 @@ pub struct LiftProperties<T: RefTrait> {
     pub initial_level: InitialLevel<T>,
 }
 
+impl LiftProperties<u32> {
+    /// Returns the pose of the lift cabin center in global coordinates.
+    pub fn center(&self, site: &Site) -> Option<Pose> {
+        // Center of the aabb
+        let center = match &self.cabin {
+            LiftCabin::Rect(params) => {
+                let front_door_t = params
+                    .front_door
+                    .as_ref()
+                    .map(|d| d.thickness())
+                    .unwrap_or(DEFAULT_CABIN_DOOR_THICKNESS);
+
+                [
+                    -params.depth / 2.0 - params.thickness() - params.gap() - front_door_t / 2.0,
+                    params.shift(),
+                    DEFAULT_LEVEL_HEIGHT / 2.0,
+                ]
+            }
+        };
+        // Get the vector between the reference anchors
+        let left_anchor = site.get_anchor(self.reference_anchors.left())?;
+        let right_anchor = site.get_anchor(self.reference_anchors.right())?;
+        let left_trans = left_anchor.translation_for_category(Category::Level);
+        let right_trans = right_anchor.translation_for_category(Category::Level);
+        let yaw = (left_trans[1] - right_trans[1]).atan2(left_trans[0] - right_trans[0]);
+        let midpoint = [
+            (left_trans[0] + right_trans[0]) / 2.0,
+            (left_trans[1] + right_trans[1]) / 2.0,
+        ];
+        let elevation = match &self.initial_level.0 {
+            Some(l) => site
+                .levels
+                .get(l)
+                .map(|level| level.properties.elevation.0)?,
+            None => {
+                let mut min_elevation = site
+                    .levels
+                    .first_key_value()
+                    .map(|(_, l)| l.properties.elevation.0)?;
+                for l in site.levels.values().skip(1) {
+                    if l.properties.elevation.0 < min_elevation {
+                        min_elevation = l.properties.elevation.0;
+                    }
+                }
+                min_elevation
+            }
+        };
+        Some(Pose {
+            trans: [midpoint[0] + center[0], midpoint[1] + center[1], elevation],
+            rot: Rotation::Yaw(Angle::Rad(yaw)),
+        })
+    }
+}
+
 #[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
 #[serde(transparent)]
 #[cfg_attr(feature = "bevy", derive(Component, Deref, DerefMut))]
diff --git a/rmf_site_format/src/misc.rs b/rmf_site_format/src/misc.rs
index 236065e1..85d4d175 100644
--- a/rmf_site_format/src/misc.rs
+++ b/rmf_site_format/src/misc.rs
@@ -15,7 +15,7 @@
  *
 */
 
-use crate::{Recall, RefTrait};
+use crate::RefTrait;
 #[cfg(feature = "bevy")]
 use bevy::prelude::*;
 use glam::{Quat, Vec2, Vec3};
diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs
index b5b6f17b..9c6e8c0b 100644
--- a/rmf_site_format/src/site.rs
+++ b/rmf_site_format/src/site.rs
@@ -183,6 +183,12 @@ impl Site {
     pub fn from_bytes<'a>(s: &'a [u8]) -> ron::error::SpannedResult<Self> {
         ron::de::from_bytes(s)
     }
+
+    pub fn get_anchor(&self, id: u32) -> Option<&Anchor> {
+        self.anchors
+            .get(&id)
+            .or_else(|| self.levels.values().find_map(|l| l.anchors.get(&id)))
+    }
 }
 
 pub trait RefTrait: Ord + Eq + Copy + Send + Sync + Hash + 'static {}

From 287356599f5b7e84a4014f74889ec32af2188c55 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 31 Jan 2024 10:43:26 +0800
Subject: [PATCH 19/71] Add TODOs for documentation

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/legacy/nav_graph.rs | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs
index fe0e806b..93d24360 100644
--- a/rmf_site_format/src/legacy/nav_graph.rs
+++ b/rmf_site_format/src/legacy/nav_graph.rs
@@ -12,7 +12,7 @@ pub struct NavGraph {
 
 // Readapted from legacy traffic editor implementation
 fn segments_intersect(p1: [f32; 2], p2: [f32; 2], p3: [f32; 2], p4: [f32; 2]) -> bool {
-    // line segments are (x1,y1),(x2,y2) and (x3,y3),(x4,y4)
+    // line segments are [p1-p2] and [p3-p4]
     let [x1, y1] = p1;
     let [x2, y2] = p2;
     let [x3, y3] = p3;
@@ -164,6 +164,7 @@ impl NavGraph {
                     println!("ERROR: Skipping lift {lift_name} due to rotation not being pure yaw");
                     continue;
                 };
+                // TODO(luca) check that the lift position is correct when doing end to end testing
                 match &lift.properties.cabin {
                     LiftCabin::Rect(params) => {
                         lifts.insert(
@@ -176,7 +177,7 @@ impl NavGraph {
                         );
                     }
                 }
-                // TODO(luca) lift property for vertices
+                // TODO(luca) lift property for vertices contained in lifts
             }
 
             graphs.push((
@@ -211,8 +212,10 @@ pub struct NavLaneProperties {
     #[serde(skip_serializing_if = "Option::is_none")]
     pub door_name: Option<String>,
     #[serde(skip_serializing_if = "Option::is_none")]
-    pub orientation_constraint: Option<String>, // TODO(MXG): Add other lane properties
-                                                // demo_mock_floor_name
+    pub orientation_constraint: Option<String>,
+    // TODO(luca): Add other lane properties
+    // demo_mock_floor_name
+    // mutex
 }
 
 impl NavLaneProperties {
@@ -250,6 +253,7 @@ impl NavVertex {
 
 #[derive(Serialize, Deserialize, Clone, Default)]
 pub struct NavVertexProperties {
+    // TODO(luca) serialize lift and merge_radius, they are currently skipped
     #[serde(skip_serializing_if = "Option::is_none")]
     pub lift: Option<String>,
     #[serde(skip_serializing_if = "is_false")]

From d76f218b70ebdf9aa1c9bcf0259a9a1e6ce2652e Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 31 Jan 2024 13:53:04 +0800
Subject: [PATCH 20/71] Fix lanes being skipped when they share an anchor

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/legacy/nav_graph.rs | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs
index 93d24360..1da3b9ac 100644
--- a/rmf_site_format/src/legacy/nav_graph.rs
+++ b/rmf_site_format/src/legacy/nav_graph.rs
@@ -46,13 +46,14 @@ impl NavGraph {
             };
 
             let lanes_with_anchor = {
-                let mut lanes_with_anchor = HashMap::new();
+                let mut lanes_with_anchor: HashMap<u32, Vec<u32>> = HashMap::new();
                 for (lane_id, lane) in &site.navigation.guided.lanes {
                     if !lane.graphs.includes(graph_id) {
                         continue;
                     }
                     for a in lane.anchors.array() {
-                        lanes_with_anchor.insert(a, (*lane_id, lane));
+                        let entry = lanes_with_anchor.entry(a).or_default();
+                        entry.push(*lane_id);
                     }
                 }
                 lanes_with_anchor
@@ -67,12 +68,14 @@ impl NavGraph {
                 let mut vertices = Vec::new();
                 let mut lanes_to_include = HashSet::new();
                 for (id, anchor) in &level.anchors {
-                    let (lane, _) = match lanes_with_anchor.get(id) {
-                        Some(v) => v,
-                        None => continue,
+                    let Some(lanes) = lanes_with_anchor.get(id) else {
+                        continue;
                     };
 
-                    lanes_to_include.insert(*lane);
+                    for lane in lanes.iter() {
+                        lanes_to_include.insert(*lane);
+                    }
+
                     anchor_to_vertex.insert(*id, vertices.len());
                     vertices.push(NavVertex::from_anchor(anchor, location_at_anchor.get(id)));
                 }

From 3e9497484ca3d0ec9d9098800ad7810d0456d753 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 31 Jan 2024 13:55:31 +0800
Subject: [PATCH 21/71] Fix duplicated unnamed waypoints when exporting

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/legacy/vertex.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_site_format/src/legacy/vertex.rs b/rmf_site_format/src/legacy/vertex.rs
index fa1ea453..3e0d7897 100644
--- a/rmf_site_format/src/legacy/vertex.rs
+++ b/rmf_site_format/src/legacy/vertex.rs
@@ -80,7 +80,7 @@ impl Vertex {
             return Some(Location {
                 anchor: anchor.into(),
                 tags: LocationTags(tags),
-                name: NameInSite(name.unwrap_or("<Unnamed>".to_string())),
+                name: NameInSite(name.unwrap_or_default()),
                 graphs: AssociatedGraphs::All,
             });
         }

From 0009908bcd6a3fdbf21c1737d96ba2c0823717af Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 31 Jan 2024 16:08:52 +0800
Subject: [PATCH 22/71] Fix compilation without bevy

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 16 +++-------------
 1 file changed, 3 insertions(+), 13 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 4a9abccf..117583fa 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -31,6 +31,8 @@ pub enum SdfConversionError {
     BrokenAnchorReference(u32),
     /// Entity referenced a non existing level.
     BrokenLevelReference(u32),
+    /// Parsing of a lift cabin failed
+    LiftParsingError(String),
 }
 
 impl Pose {
@@ -480,19 +482,7 @@ impl Site {
             let LiftCabin::Rect(ref cabin) = lift.properties.cabin else {
                 continue;
             };
-            let center = cabin.aabb().center;
-            let left_anchor = get_anchor(lift.properties.reference_anchors.left())?;
-            let right_anchor = get_anchor(lift.properties.reference_anchors.right())?;
-            let left_trans = left_anchor.translation_for_category(Category::Level);
-            let right_trans = right_anchor.translation_for_category(Category::Level);
-            let midpoint = [
-                (left_trans[0] + right_trans[0]) / 2.0,
-                (left_trans[1] + right_trans[1]) / 2.0,
-            ];
-            let pose = Pose {
-                trans: [midpoint[0] + center.x, midpoint[1] + center.y, 0.0],
-                ..Default::default()
-            };
+            let pose = lift.properties.center(self).ok_or(SdfConversionError::LiftParsingError(lift.properties.name.0.clone()))?;
             let mut plugin = SdfPlugin {
                 name: "register_component".into(),
                 filename: "libregister_component.so".into(),

From b4d0644cc2cf52c2e4aa9ab3d2d735c18843d2ac Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 1 Feb 2024 18:12:10 +0800
Subject: [PATCH 23/71] Add anchors in lifts, fix lift center rotation

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/legacy/nav_graph.rs | 90 ++++++++++++++++---------
 rmf_site_format/src/lift.rs             |  2 +-
 2 files changed, 59 insertions(+), 33 deletions(-)

diff --git a/rmf_site_format/src/legacy/nav_graph.rs b/rmf_site_format/src/legacy/nav_graph.rs
index 1da3b9ac..f0dd1801 100644
--- a/rmf_site_format/src/legacy/nav_graph.rs
+++ b/rmf_site_format/src/legacy/nav_graph.rs
@@ -59,15 +59,68 @@ impl NavGraph {
                 lanes_with_anchor
             };
 
-            // TODO(MXG): Make this work for lifts
-
             let mut doors = HashMap::new();
             let mut levels = HashMap::new();
+            let mut lifts = HashMap::new();
             for (_, level) in &site.levels {
                 let mut anchor_to_vertex = HashMap::new();
                 let mut vertices = Vec::new();
                 let mut lanes_to_include = HashSet::new();
-                for (id, anchor) in &level.anchors {
+                // Add vertices for anchors that are in lifts
+                for lift in site.lifts.values() {
+                    let lift_name = &lift.properties.name.0;
+                    let Some(center) = lift.properties.center(site) else {
+                        println!("ERROR: Skipping lift {lift_name} due to broken anchor reference");
+                        continue;
+                    };
+                    let Rotation::Yaw(yaw) = center.rot else {
+                        println!(
+                            "ERROR: Skipping lift {lift_name} due to rotation not being pure yaw"
+                        );
+                        continue;
+                    };
+                    let yaw = yaw.radians();
+                    // Note this will overwrite the entry in the map but that is OK
+                    // TODO(luca) check that the lift position is correct when doing end to end testing
+                    match &lift.properties.cabin {
+                        LiftCabin::Rect(params) => {
+                            lifts.insert(
+                                lift_name.clone(),
+                                NavLift {
+                                    position: [center.trans[0], center.trans[1], yaw],
+                                    // Note depth and width are inverted between legacy and site editor
+                                    dims: [params.depth, params.width],
+                                },
+                            );
+                        }
+                    }
+                    for (id, anchor) in &lift.cabin_anchors {
+                        let Some(lanes) = lanes_with_anchor.get(id) else {
+                            continue;
+                        };
+
+                        for lane in lanes.iter() {
+                            lanes_to_include.insert(*lane);
+                        }
+
+                        // The anchor is in lift coordinates, make it in global coordinates
+                        let trans = anchor.translation_for_category(Category::General);
+                        // TODO(luca) check that this translation is correct, most cases just have
+                        // an anchor in the origin.
+                        let anchor = Anchor::Translate2D([
+                            center.trans[0] + yaw.cos() * trans[0],
+                            center.trans[1] + yaw.sin() * trans[1],
+                        ]);
+
+                        anchor_to_vertex.insert(*id, vertices.len());
+                        let mut vertex =
+                            NavVertex::from_anchor(&anchor, location_at_anchor.get(id));
+                        vertex.2.lift = Some(lift_name.clone());
+                        vertices.push(vertex);
+                    }
+                }
+                // Add site and level anchors
+                for (id, anchor) in site.anchors.iter().chain(level.anchors.iter()) {
                     let Some(lanes) = lanes_with_anchor.get(id) else {
                         continue;
                     };
@@ -84,8 +137,8 @@ impl NavGraph {
                 for (_, door) in &level.doors {
                     let door_name = &door.name.0;
                     let (v0, v1) = match (
-                        level.anchors.get(&door.anchors.start()),
-                        level.anchors.get(&door.anchors.end()),
+                        site.get_anchor(door.anchors.start()),
+                        site.get_anchor(door.anchors.end()),
                     ) {
                         (Some(v0), Some(v1)) => (
                             v0.translation_for_category(Category::Level),
@@ -156,33 +209,6 @@ impl NavGraph {
                 );
             }
 
-            let mut lifts = HashMap::new();
-            for (_, lift) in &site.lifts {
-                let lift_name = &lift.properties.name.0;
-                let Some(pose) = lift.properties.center(site) else {
-                    println!("ERROR: Skipping lift {lift_name} due to broken anchor reference");
-                    continue;
-                };
-                let Rotation::Yaw(yaw) = pose.rot else {
-                    println!("ERROR: Skipping lift {lift_name} due to rotation not being pure yaw");
-                    continue;
-                };
-                // TODO(luca) check that the lift position is correct when doing end to end testing
-                match &lift.properties.cabin {
-                    LiftCabin::Rect(params) => {
-                        lifts.insert(
-                            lift_name.clone(),
-                            NavLift {
-                                position: [pose.trans[0], pose.trans[1], yaw.radians()],
-                                // Note depth and width are inverted between legacy and site editor
-                                dims: [params.depth, params.width],
-                            },
-                        );
-                    }
-                }
-                // TODO(luca) lift property for vertices contained in lifts
-            }
-
             graphs.push((
                 graph.name.0.clone(),
                 Self {
diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs
index 90d16040..5644403a 100644
--- a/rmf_site_format/src/lift.rs
+++ b/rmf_site_format/src/lift.rs
@@ -139,7 +139,7 @@ impl LiftProperties<u32> {
         let right_anchor = site.get_anchor(self.reference_anchors.right())?;
         let left_trans = left_anchor.translation_for_category(Category::Level);
         let right_trans = right_anchor.translation_for_category(Category::Level);
-        let yaw = (left_trans[1] - right_trans[1]).atan2(left_trans[0] - right_trans[0]);
+        let yaw = (left_trans[0] - right_trans[0]).atan2(left_trans[1] - right_trans[1]);
         let midpoint = [
             (left_trans[0] + right_trans[0]) / 2.0,
             (left_trans[1] + right_trans[1]) / 2.0,

From 1d6537f09d938ea94506ebf0cfbdb1e171c1f88a Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 2 Feb 2024 15:54:53 +0800
Subject: [PATCH 24/71] Add joint limit for lift cabins

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 117583fa..0f6ee150 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -430,7 +430,11 @@ impl Site {
                 .cloned()
         };
         let mut models = Vec::new();
+        let mut min_elevation = f32::MAX;
+        let mut max_elevation = f32::MIN;
         for level in self.levels.values() {
+            max_elevation = max_elevation.max(level.properties.elevation.0);
+            min_elevation = min_elevation.min(level.properties.elevation.0);
             // Floors walls and static models are included in the level mesh
             models.push(SdfModel {
                 name: format!("level_{}", level.properties.name.0),
@@ -482,7 +486,12 @@ impl Site {
             let LiftCabin::Rect(ref cabin) = lift.properties.cabin else {
                 continue;
             };
-            let pose = lift.properties.center(self).ok_or(SdfConversionError::LiftParsingError(lift.properties.name.0.clone()))?;
+            let pose = lift
+                .properties
+                .center(self)
+                .ok_or(SdfConversionError::LiftParsingError(
+                    lift.properties.name.0.clone(),
+                ))?;
             let mut plugin = SdfPlugin {
                 name: "register_component".into(),
                 filename: "libregister_component.so".into(),
@@ -521,6 +530,11 @@ impl Site {
                 child: "platform".into(),
                 axis: Some(SdfJointAxis {
                     xyz: Vector3d::new(0.0, 0.0, 1.0),
+                    limit: SdfJointAxisLimit {
+                        lower: min_elevation.into(),
+                        upper: max_elevation.into(),
+                        ..Default::default()
+                    },
                     ..Default::default()
                 }),
                 ..Default::default()

From ddea381f2029bd87188323af1b7b6cee4bee2192 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 5 Feb 2024 14:33:10 +0800
Subject: [PATCH 25/71] Export graphs together with SDF

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml       |  2 +-
 rmf_site_editor/src/site/save.rs | 21 +++++++++++++++++++++
 rmf_site_format/src/sdf.rs       |  4 +---
 3 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 0d9aaaab..14de115f 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -47,7 +47,7 @@ rfd = "0.12"
 urdf-rs = "0.7"
 yaserde = "0.7"
 utm = "0.1.6"
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/yaserde_nested_models"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "2d20fe0"}
 gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "first_implementation" }
 pathdiff = "*"
 tera = "1.19.1"
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 8e449442..b7cbc4c1 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1282,6 +1282,7 @@ pub fn save_site(world: &mut World) {
                         continue;
                     }
                 };
+                let graphs = legacy::nav_graph::NavGraph::from_site(&site);
                 let sdf = match site.to_sdf() {
                     Ok(sdf) => sdf,
                     Err(err) => {
@@ -1297,6 +1298,26 @@ pub fn save_site(world: &mut World) {
                     ..Default::default()
                 };
                 let s = yaserde::ser::serialize_with_writer(&sdf, f, &config).unwrap();
+                let mut navgraph_dir = PathBuf::from(new_path.parent().unwrap());
+                navgraph_dir.push("nav");
+                for (name, graph) in &graphs {
+                    let mut graph_file = navgraph_dir.clone();
+                    graph_file.set_file_name(name.to_owned() + ".yaml");
+                    info!(
+                        "Saving legacy nav graph to {}",
+                        graph_file.to_str().unwrap_or("<failed to render??>")
+                    );
+                    let f = match std::fs::File::create(graph_file) {
+                        Ok(f) => f,
+                        Err(err) => {
+                            error!("Unable to save nav graph: {err}");
+                            continue;
+                        }
+                    };
+                    if let Err(err) = serde_yaml::to_writer(f, &graph) {
+                        error!("Failed to save nav graph: {err}");
+                    }
+                }
             }
             ExportFormat::Urdf => {
                 warn!("Site exporting to Urdf is not supported.");
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 0f6ee150..950e1f46 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -423,9 +423,7 @@ impl Door<u32> {
 impl Site {
     pub fn to_sdf(&self) -> Result<SdfRoot, SdfConversionError> {
         let get_anchor = |id: u32| -> Result<Anchor, SdfConversionError> {
-            self.anchors
-                .get(&id)
-                .or_else(|| self.levels.values().find_map(|l| l.anchors.get(&id)))
+            self.get_anchor(id)
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
         };

From 6b30bee36bb73de5fb8da3e72e6f9e5ca302e736 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 5 Feb 2024 17:25:38 +0800
Subject: [PATCH 26/71] First draft of headless support, app can run

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml               | 11 +++-
 rmf_site_editor/src/interaction/mod.rs   | 11 +++-
 rmf_site_editor/src/keyboard.rs          | 11 +++-
 rmf_site_editor/src/lib.rs               | 70 ++++++++++++++++--------
 rmf_site_editor/src/log.rs               |  2 +-
 rmf_site_editor/src/main_menu.rs         | 13 ++++-
 rmf_site_editor/src/widgets/mod.rs       | 56 ++++++++++---------
 rmf_site_editor/src/workcell/keyboard.rs | 12 +++-
 rmf_site_format/Cargo.toml               |  2 +-
 9 files changed, 127 insertions(+), 61 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 96173862..fbb4b901 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -30,8 +30,15 @@ serde_json = "1.0"
 # wasm-bindgen 0.2.85 introduces a compile error in stdweb
 wasm-bindgen = "=0.2.84"
 futures-lite = "1.12.0"
-bevy = { version = "0.11", features = ["pnm", "jpeg", "tga"] }
-bevy_utils = "0.11"
+bevy = { version = "0.11", default-features = false, features = [
+    "pnm",
+    "jpeg",
+    "tga",
+    "bevy_core_pipeline",
+    "bevy_gltf",
+    "bevy_render",
+    "bevy_scene",
+] }
 dirs = "5.0"
 thread_local = "*"
 lyon = "1"
diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs
index 7debd4fd..29c354b0 100644
--- a/rmf_site_editor/src/interaction/mod.rs
+++ b/rmf_site_editor/src/interaction/mod.rs
@@ -97,7 +97,9 @@ use bevy_polyline::PolylinePlugin;
 pub struct SiteRaycastSet;
 
 #[derive(Default)]
-pub struct InteractionPlugin;
+pub struct InteractionPlugin {
+    pub headless: bool,
+}
 
 #[derive(Debug, Clone, Copy, Default, Hash, PartialEq, Eq, States)]
 pub enum InteractionState {
@@ -171,8 +173,10 @@ impl Plugin for InteractionPlugin {
                 CategoryVisibilityPlugin::<WallMarker>::visible(true),
                 CategoryVisibilityPlugin::<WorkcellVisualizationMarker>::visible(true),
             ))
-            .add_plugins((CameraControlsPlugin, ModelPreviewPlugin))
-            .add_systems(
+            .add_plugins((CameraControlsPlugin, ModelPreviewPlugin));
+
+        if !self.headless {
+            app.add_systems(
                 Update,
                 (
                     make_lift_doormat_gizmo,
@@ -255,6 +259,7 @@ impl Plugin for InteractionPlugin {
                     .run_if(in_state(InteractionState::Enable)),
             )
             .add_systems(First, (update_picked, update_interaction_mode));
+        }
     }
 }
 
diff --git a/rmf_site_editor/src/keyboard.rs b/rmf_site_editor/src/keyboard.rs
index b6e1a7c8..e5139df9 100644
--- a/rmf_site_editor/src/keyboard.rs
+++ b/rmf_site_editor/src/keyboard.rs
@@ -23,7 +23,7 @@ use crate::{
     site::{AlignSiteDrawings, CurrentLevel, Delete},
     CreateNewWorkspace, CurrentWorkspace, LoadWorkspace, SaveWorkspace,
 };
-use bevy::{ecs::system::SystemParam, prelude::*};
+use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow};
 use bevy_egui::EguiContexts;
 
 #[derive(Debug, Clone, Copy, Resource)]
@@ -59,8 +59,15 @@ fn handle_keyboard_input(
     mut debug_mode: ResMut<DebugMode>,
     mut align_site: EventWriter<AlignSiteDrawings>,
     current_workspace: Res<CurrentWorkspace>,
+    primary_windows: Query<Entity, With<PrimaryWindow>>,
 ) {
-    let egui_context = egui_context.ctx_mut();
+    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();
diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 00717a9b..58a4f107 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -1,5 +1,6 @@
 use bevy::{
-    log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*, render::renderer::RenderAdapterInfo,
+    app::ScheduleRunnerPlugin, log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*,
+    render::renderer::RenderAdapterInfo,
 };
 use bevy_egui::EguiPlugin;
 use main_menu::MainMenuPlugin;
@@ -74,6 +75,9 @@ pub struct CommandLineArgs {
     /// 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<String>,
+    /// Run in headless mode.
+    #[arg(long)]
+    pub headless: bool,
 }
 
 #[derive(Clone, Default, Eq, PartialEq, Debug, Hash, States)]
@@ -117,22 +121,27 @@ pub fn run_js() {
 pub fn run(command_line_args: Vec<String>) {
     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),
-            ));
-        }
+    //#[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(SiteEditor);
+    app.add_plugins(SiteEditor {
+        headless: command_line_args.headless,
+    });
     app.run();
 }
 
-pub struct SiteEditor;
+pub struct SiteEditor {
+    /// Whether to run the site editor in headless mode.
+    pub headless: bool,
+}
 
 impl Plugin for SiteEditor {
     fn build(&self, app: &mut App) {
@@ -165,18 +174,27 @@ impl Plugin for SiteEditor {
 
         #[cfg(not(target_arch = "wasm32"))]
         {
+            let window_plugin = if self.headless {
+                WindowPlugin {
+                    primary_window: None,
+                    exit_condition: bevy::window::ExitCondition::DontExit,
+                    close_when_requested: false,
+                }
+            } else {
+                WindowPlugin {
+                    primary_window: Some(Window {
+                        title: "RMF Site Editor".to_owned(),
+                        resolution: (1600., 900.).into(),
+                        ..default()
+                    }),
+                    ..default()
+                }
+            };
             app.add_plugins(
                 DefaultPlugins
                     .build()
                     .disable::<LogPlugin>()
-                    .set(WindowPlugin {
-                        primary_window: Some(Window {
-                            title: "RMF Site Editor".to_owned(),
-                            resolution: (1600., 900.).into(),
-                            ..default()
-                        }),
-                        ..default()
-                    })
+                    .set(window_plugin)
                     .set(ImagePlugin {
                         default_sampler: SamplerDescriptor {
                             address_mode_u: AddressMode::Repeat,
@@ -206,8 +224,12 @@ impl Plugin for SiteEditor {
                 MainMenuPlugin,
                 WorkcellEditorPlugin,
                 SitePlugin,
-                InteractionPlugin,
-                StandardUiLayout,
+                InteractionPlugin {
+                    headless: self.headless,
+                },
+                StandardUiLayout {
+                    headless: self.headless,
+                },
                 AnimationPlugin,
                 OccupancyPlugin,
                 WorkspacePlugin,
@@ -219,5 +241,9 @@ impl Plugin for SiteEditor {
                 OSMViewPlugin,
                 SiteWireframePlugin,
             ));
+
+        if self.headless {
+            app.add_plugins(ScheduleRunnerPlugin::default());
+        }
     }
 }
diff --git a/rmf_site_editor/src/log.rs b/rmf_site_editor/src/log.rs
index 5070bf2c..ff6313ea 100644
--- a/rmf_site_editor/src/log.rs
+++ b/rmf_site_editor/src/log.rs
@@ -15,8 +15,8 @@
  *
 */
 
+use bevy::utils::tracing::{field::Field, span::Record, Event, Id, Level, Subscriber};
 use bevy::{ecs::system::SystemParam, prelude::*};
-use bevy_utils::tracing::{field::Field, span::Record, Event, Id, Level, Subscriber};
 use crossbeam_channel::{unbounded, Receiver, SendError, Sender, TryRecvError};
 use std::collections::HashMap;
 use std::fmt::{self, Debug, Write};
diff --git a/rmf_site_editor/src/main_menu.rs b/rmf_site_editor/src/main_menu.rs
index 4989277e..a6f67a1a 100644
--- a/rmf_site_editor/src/main_menu.rs
+++ b/rmf_site_editor/src/main_menu.rs
@@ -17,7 +17,7 @@
 
 use super::demo_world::*;
 use crate::{AppState, LoadWorkspace, WorkspaceData};
-use bevy::{app::AppExit, prelude::*, tasks::Task};
+use bevy::{app::AppExit, prelude::*, tasks::Task, window::PrimaryWindow};
 use bevy_egui::{egui, EguiContexts};
 use std::path::PathBuf;
 
@@ -44,6 +44,7 @@ fn egui_ui(
     mut _load_workspace: EventWriter<LoadWorkspace>,
     mut _app_state: ResMut<State<AppState>>,
     autoload: Option<ResMut<Autoload>>,
+    primary_windows: Query<Entity, With<PrimaryWindow>>,
 ) {
     if let Some(mut autoload) = autoload {
         #[cfg(not(target_arch = "wasm32"))]
@@ -56,12 +57,20 @@ fn egui_ui(
         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(egui_context.ctx_mut(), |ui| {
+        .show(ctx, |ui| {
             ui.heading("Welcome to The RMF Site Editor!");
             ui.add_space(10.);
 
diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs
index 20128c92..127deeb6 100644
--- a/rmf_site_editor/src/widgets/mod.rs
+++ b/rmf_site_editor/src/widgets/mod.rs
@@ -96,7 +96,9 @@ pub struct PendingModel {
 }
 
 #[derive(Default)]
-pub struct StandardUiLayout;
+pub struct StandardUiLayout {
+    pub headless: bool,
+}
 
 impl Plugin for StandardUiLayout {
     fn build(&self, app: &mut App) {
@@ -112,32 +114,34 @@ impl Plugin for StandardUiLayout {
             .init_resource::<SearchForFiducial>()
             .add_plugins(MenuPluginManager)
             .init_resource::<SearchForTexture>()
-            .init_resource::<GroupViewModes>()
-            .add_systems(Startup, init_ui_style)
-            .add_systems(
-                Update,
-                site_ui_layout.run_if(in_state(AppState::SiteEditor)),
-            )
-            .add_systems(
-                Update,
-                workcell_ui_layout.run_if(in_state(AppState::WorkcellEditor)),
-            )
-            .add_systems(
-                Update,
-                site_drawing_ui_layout.run_if(in_state(AppState::SiteDrawingEditor)),
-            )
-            .add_systems(
-                Update,
-                site_visualizer_ui_layout.run_if(in_state(AppState::SiteVisualizer)),
-            )
-            .add_systems(
-                PostUpdate,
-                (
-                    resolve_light_export_file,
-                    resolve_nav_graph_import_export_files,
+            .init_resource::<GroupViewModes>();
+        if !self.headless {
+            app.add_systems(Startup, init_ui_style)
+                .add_systems(
+                    Update,
+                    site_ui_layout.run_if(in_state(AppState::SiteEditor)),
+                )
+                .add_systems(
+                    Update,
+                    workcell_ui_layout.run_if(in_state(AppState::WorkcellEditor)),
+                )
+                .add_systems(
+                    Update,
+                    site_drawing_ui_layout.run_if(in_state(AppState::SiteDrawingEditor)),
                 )
-                    .run_if(AppState::in_site_mode()),
-            );
+                .add_systems(
+                    Update,
+                    site_visualizer_ui_layout.run_if(in_state(AppState::SiteVisualizer)),
+                )
+                .add_systems(
+                    PostUpdate,
+                    (
+                        resolve_light_export_file,
+                        resolve_nav_graph_import_export_files,
+                    )
+                        .run_if(AppState::in_site_mode()),
+                );
+        }
     }
 }
 
diff --git a/rmf_site_editor/src/workcell/keyboard.rs b/rmf_site_editor/src/workcell/keyboard.rs
index 6327d5ae..0832f076 100644
--- a/rmf_site_editor/src/workcell/keyboard.rs
+++ b/rmf_site_editor/src/workcell/keyboard.rs
@@ -16,15 +16,23 @@
 */
 
 use crate::{ExportFormat, SaveWorkspace, SaveWorkspaceDestination};
-use bevy::prelude::*;
+use bevy::{prelude::*, window::PrimaryWindow};
 use bevy_egui::EguiContexts;
 
 pub fn handle_workcell_keyboard_input(
     keyboard_input: Res<Input<KeyCode>>,
     mut egui_context: EguiContexts,
     mut save_events: EventWriter<SaveWorkspace>,
+    primary_windows: Query<Entity, With<PrimaryWindow>>,
 ) {
-    let egui_context = egui_context.ctx_mut();
+    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();
diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index 67def6f4..10279bee 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -17,7 +17,7 @@ glam = { version = "0.24", features = ["serde"] }
 uuid = { version = "1.1", features = ["v4", "serde"] }
 # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features
 # We depend on a bugfix released specifically in 0.7.3
-bevy = { version = "0.11", optional = true }
+bevy = { version = "0.11", default_features = false, optional = true }
 urdf-rs = "0.7.3"
 pathdiff = "*"
 

From e57f962d2607a770a11afb297fc51191e980c657 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 5 Feb 2024 17:43:52 +0800
Subject: [PATCH 27/71] Restore features

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index fbb4b901..0badd51b 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -30,14 +30,10 @@ serde_json = "1.0"
 # wasm-bindgen 0.2.85 introduces a compile error in stdweb
 wasm-bindgen = "=0.2.84"
 futures-lite = "1.12.0"
-bevy = { version = "0.11", default-features = false, features = [
+bevy = { version = "0.11", features = [
     "pnm",
     "jpeg",
     "tga",
-    "bevy_core_pipeline",
-    "bevy_gltf",
-    "bevy_render",
-    "bevy_scene",
 ] }
 dirs = "5.0"
 thread_local = "*"

From 8c2ff616fc9b5bedf1fc4f30d7df65e27bc2abb4 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 6 Feb 2024 16:46:39 +0800
Subject: [PATCH 28/71] Fix running when no X server is available

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 58a4f107..425ec9e3 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -174,7 +174,9 @@ impl Plugin for SiteEditor {
 
         #[cfg(not(target_arch = "wasm32"))]
         {
+            let mut plugins = DefaultPlugins.build();
             let window_plugin = if self.headless {
+                plugins = plugins.disable::<bevy::winit::WinitPlugin>();
                 WindowPlugin {
                     primary_window: None,
                     exit_condition: bevy::window::ExitCondition::DontExit,
@@ -191,8 +193,7 @@ impl Plugin for SiteEditor {
                 }
             };
             app.add_plugins(
-                DefaultPlugins
-                    .build()
+                plugins
                     .disable::<LogPlugin>()
                     .set(window_plugin)
                     .set(ImagePlugin {

From 3b6b3345229e2b4aad546ee86150b5d160d636e5 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 6 Feb 2024 16:53:05 +0800
Subject: [PATCH 29/71] Minor cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 425ec9e3..beac1248 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -175,27 +175,27 @@ impl Plugin for SiteEditor {
         #[cfg(not(target_arch = "wasm32"))]
         {
             let mut plugins = DefaultPlugins.build();
-            let window_plugin = if self.headless {
-                plugins = plugins.disable::<bevy::winit::WinitPlugin>();
-                WindowPlugin {
-                    primary_window: None,
-                    exit_condition: bevy::window::ExitCondition::DontExit,
-                    close_when_requested: false,
-                }
+            let plugins = if self.headless {
+                plugins
+                    .set(WindowPlugin {
+                        primary_window: None,
+                        exit_condition: bevy::window::ExitCondition::DontExit,
+                        close_when_requested: false,
+                    })
+                    .disable::<bevy::winit::WinitPlugin>()
             } else {
-                WindowPlugin {
+                plugins.set(WindowPlugin {
                     primary_window: Some(Window {
                         title: "RMF Site Editor".to_owned(),
                         resolution: (1600., 900.).into(),
                         ..default()
                     }),
                     ..default()
-                }
+                })
             };
             app.add_plugins(
                 plugins
                     .disable::<LogPlugin>()
-                    .set(window_plugin)
                     .set(ImagePlugin {
                         default_sampler: SamplerDescriptor {
                             address_mode_u: AddressMode::Repeat,
@@ -243,6 +243,8 @@ impl Plugin for SiteEditor {
                 SiteWireframePlugin,
             ));
 
+        // TODO(luca) This schedule runner plugin runs forever.
+        // We might need to have a custom one where we can inject exit conditions.
         if self.headless {
             app.add_plugins(ScheduleRunnerPlugin::default());
         }

From a5b40d6cf5527263f521ae3106f536a6f52ba138 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 6 Feb 2024 16:54:51 +0800
Subject: [PATCH 30/71] Remove unneeded changes

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml | 7 ++-----
 rmf_site_format/Cargo.toml | 2 +-
 2 files changed, 3 insertions(+), 6 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 0badd51b..96173862 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -30,11 +30,8 @@ serde_json = "1.0"
 # wasm-bindgen 0.2.85 introduces a compile error in stdweb
 wasm-bindgen = "=0.2.84"
 futures-lite = "1.12.0"
-bevy = { version = "0.11", features = [
-    "pnm",
-    "jpeg",
-    "tga",
-] }
+bevy = { version = "0.11", features = ["pnm", "jpeg", "tga"] }
+bevy_utils = "0.11"
 dirs = "5.0"
 thread_local = "*"
 lyon = "1"
diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index 10279bee..67def6f4 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -17,7 +17,7 @@ glam = { version = "0.24", features = ["serde"] }
 uuid = { version = "1.1", features = ["v4", "serde"] }
 # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features
 # We depend on a bugfix released specifically in 0.7.3
-bevy = { version = "0.11", default_features = false, optional = true }
+bevy = { version = "0.11", optional = true }
 urdf-rs = "0.7.3"
 pathdiff = "*"
 

From 9c510799ba7432060a71315e98f775704db32584 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 6 Feb 2024 17:00:27 +0800
Subject: [PATCH 31/71] Fix tests

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/examples/extending_menu.rs |  2 +-
 rmf_site_editor/src/lib.rs                 | 21 ++++++++++++---------
 2 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/rmf_site_editor/examples/extending_menu.rs b/rmf_site_editor/examples/extending_menu.rs
index 1454bb1f..d00c6fb0 100644
--- a/rmf_site_editor/examples/extending_menu.rs
+++ b/rmf_site_editor/examples/extending_menu.rs
@@ -86,7 +86,7 @@ impl Plugin for MyMenuPlugin {
 /// Lets embed site editor in our application with our own plugin
 fn main() {
     App::new()
-        .add_plugin(SiteEditor)
+        .add_plugin(SiteEditor::default())
         .add_plugin(MyMenuPlugin)
         .run();
 }
diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index beac1248..fa95d883 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -120,24 +120,27 @@ pub fn run_js() {
 
 pub fn run(command_line_args: Vec<String>) {
     let mut app = App::new();
+    let mut headless = false;
 
-    //#[cfg(not(target_arch = "wasm32"))]
-    //{
+    #[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),
-        ));
+        if let Some(path) = command_line_args.filename {
+            app.insert_resource(Autoload::file(
+                path.into(),
+                command_line_args.import.map(Into::into),
+            ));
+        }
+        headless = command_line_args.headless;
     }
-    //}
 
     app.add_plugins(SiteEditor {
-        headless: command_line_args.headless,
+        headless,
     });
     app.run();
 }
 
+#[derive(Default)]
 pub struct SiteEditor {
     /// Whether to run the site editor in headless mode.
     pub headless: bool,

From 16bb3b9f30cef697a0d7ef4b7057812ce5a4fdb8 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 6 Feb 2024 17:10:51 +0800
Subject: [PATCH 32/71] Fix web

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index fa95d883..9254b2a8 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -76,7 +76,7 @@ pub struct CommandLineArgs {
     #[cfg_attr(not(target_arch = "wasm32"), arg(short, long))]
     pub import: Option<String>,
     /// Run in headless mode.
-    #[arg(long)]
+    #[cfg_attr(not(target_arch = "wasm32"), arg(long))]
     pub headless: bool,
 }
 

From 6d3a4b62b45f364fe09769fc3e606e6805f80fa8 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 6 Feb 2024 17:12:20 +0800
Subject: [PATCH 33/71] Style

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 9254b2a8..a2975bfb 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -124,7 +124,7 @@ pub fn run(command_line_args: Vec<String>) {
 
     #[cfg(not(target_arch = "wasm32"))]
     {
-    let command_line_args = CommandLineArgs::parse_from(command_line_args);
+        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(),
@@ -134,9 +134,7 @@ pub fn run(command_line_args: Vec<String>) {
         headless = command_line_args.headless;
     }
 
-    app.add_plugins(SiteEditor {
-        headless,
-    });
+    app.add_plugins(SiteEditor { headless });
     app.run();
 }
 

From 1065ae5d06ecf7fb6e9274c3d19837dac25ec374 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 29 Feb 2024 18:22:02 +0800
Subject: [PATCH 34/71] Skeleton of GUI plugins

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 31 ++++++++++++++++++++++++++++++-
 1 file changed, 30 insertions(+), 1 deletion(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 950e1f46..e9eabbc1 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -430,12 +430,28 @@ impl Site {
         let mut models = Vec::new();
         let mut min_elevation = f32::MAX;
         let mut max_elevation = f32::MIN;
+        let mut toggle_floors_plugin = SdfPlugin {
+            name: "toggle_floors".into(),
+            filename: "libtoggle_floors.so".into(),
+            ..Default::default()
+        };
         for level in self.levels.values() {
             max_elevation = max_elevation.max(level.properties.elevation.0);
             min_elevation = min_elevation.min(level.properties.elevation.0);
+            let mut floor_models_ele = XmlElement {
+                name: "floor".into(),
+                ..Default::default()
+            };
+            let level_model_name = format!("level_{}", level.properties.name.0);
+            floor_models_ele
+                .attributes
+                .insert("name".into(), level.properties.name.0.clone());
+            floor_models_ele
+                .attributes
+                .insert("model_name".into(), level_model_name.clone());
             // Floors walls and static models are included in the level mesh
             models.push(SdfModel {
-                name: format!("level_{}", level.properties.name.0),
+                name: level_model_name,
                 r#static: Some(true),
                 link: vec![SdfLink {
                     name: "link".into(),
@@ -468,6 +484,7 @@ impl Site {
             });
             // Now add all the doors
             for door in level.doors.values() {
+                // TODO(luca) doors into toggle floors
                 let left_anchor = get_anchor(door.anchors.left())?;
                 let right_anchor = get_anchor(door.anchors.right())?;
                 models.push(door.to_sdf(
@@ -478,6 +495,7 @@ impl Site {
                     None,
                 )?);
             }
+            toggle_floors_plugin.elements.push(floor_models_ele);
         }
         for lift in self.lifts.values() {
             // Cabin
@@ -726,6 +744,11 @@ impl Site {
             filename: "libdoor.so".into(),
             ..Default::default()
         };
+        let charging_plugin = SdfPlugin {
+            name: "toggle_charging".into(),
+            filename: "libtoggle_charging.so".into(),
+            ..Default::default()
+        };
         let physics_plugin = SdfPlugin {
             name: "gz::sim::systems::Physics".into(),
             filename: "libgz-sim-physics-system.so".into(),
@@ -799,6 +822,12 @@ impl Site {
                     ..Default::default()
                 }],
                 light: vec![sun],
+                // TODO(luca) check why these plugins are not being applied, we might need to
+                // override more than only plugins
+                gui: Some(SdfGui {
+                    plugin: vec![charging_plugin, toggle_floors_plugin],
+                    ..Default::default()
+                }),
                 plugin: vec![
                     physics_plugin,
                     user_commands_plugin,

From 80062f40c5346a45ff6b777dc4951f79b4b45ed7 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 29 Feb 2024 18:29:32 +0800
Subject: [PATCH 35/71] Minor cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 25 ++++++++++++++++---------
 1 file changed, 16 insertions(+), 9 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index e9eabbc1..e97ed49c 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -33,6 +33,8 @@ pub enum SdfConversionError {
     BrokenLevelReference(u32),
     /// Parsing of a lift cabin failed
     LiftParsingError(String),
+    /// A lift had no initial level where it could be spawned
+    MissingInitialLevel(String),
 }
 
 impl Pose {
@@ -522,14 +524,13 @@ impl Site {
             let mut elements = vec![];
             let lift_name = &lift.properties.name.0;
             elements.push(("lift_name", lift_name.clone()));
-            // TODO(luca) remove unwrap here for missing initial level
             let initial_floor = lift
                 .properties
                 .initial_level
                 .0
                 .and_then(|id| self.levels.get(&id))
                 .map(|level| level.properties.name.0.clone())
-                .unwrap();
+                .ok_or(SdfConversionError::MissingInitialLevel(lift_name.clone()))?;
             elements.push(("initial_floor", initial_floor));
             elements.push(("v_max_cabin", "2.0".to_string()));
             elements.push(("a_max_cabin", "1.2".to_string()));
@@ -571,23 +572,25 @@ impl Site {
                     kind: door.kind.clone(),
                     marker: DoorMarker,
                 };
-                // TODO(luca) remove unwrap here
                 let left_anchor = lift
                     .cabin_anchors
                     .get(&door.reference_anchors.left())
-                    .unwrap()
+                    .ok_or(SdfConversionError::BrokenAnchorReference(
+                        door.reference_anchors.left(),
+                    ))?
                     .clone();
                 let right_anchor = lift
                     .cabin_anchors
                     .get(&door.reference_anchors.right())
-                    .unwrap()
+                    .ok_or(SdfConversionError::BrokenAnchorReference(
+                        door.reference_anchors.right(),
+                    ))?
                     .clone();
                 let x_offset = -face.u()
                     * (door_placement.thickness() / 2.0
                         + door_placement
                             .custom_gap
                             .unwrap_or_else(|| cabin.gap.unwrap_or(0.01)));
-                dbg!(&x_offset);
                 let mut dummy_cabin = dummy_cabin.to_sdf(
                     left_anchor,
                     right_anchor,
@@ -623,14 +626,18 @@ impl Site {
                     let left_anchor = lift
                         .cabin_anchors
                         .get(&door.reference_anchors.left())
-                        .unwrap()
+                        .ok_or(SdfConversionError::BrokenAnchorReference(
+                            door.reference_anchors.left(),
+                        ))?
                         .clone();
                     let right_anchor = lift
                         .cabin_anchors
                         .get(&door.reference_anchors.right())
-                        .unwrap()
+                        .ok_or(SdfConversionError::BrokenAnchorReference(
+                            door.reference_anchors.right(),
+                        ))?
                         .clone();
-                    let mut dummy_shaft = dummy_shaft.to_sdf(
+                    let dummy_shaft = dummy_shaft.to_sdf(
                         left_anchor,
                         right_anchor,
                         Vec3::from(pose.trans) + Vec3::new(0.0, 0.0, level.properties.elevation.0),

From 26f9af38bf9fb806dd840f463ba4b18bc8de5f45 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 1 Mar 2024 11:50:13 +0800
Subject: [PATCH 36/71] Add template, fix preliminary implementation of toggle
 floors

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/Cargo.toml                 |   5 +-
 rmf_site_format/src/sdf.rs                 | 137 +++-------
 rmf_site_format/src/templates/gz_world.sdf | 298 +++++++++++++++++++++
 3 files changed, 338 insertions(+), 102 deletions(-)
 create mode 100644 rmf_site_format/src/templates/gz_world.sdf

diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index 86083e34..c9a76b2d 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -18,8 +18,11 @@ uuid = { version = "1.1", features = ["v4", "serde"] }
 # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features
 # We depend on a bugfix released specifically in 0.7.3
 bevy = { version = "0.11", optional = true }
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", branch = "luca/yaserde_nested_models"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "2d20fe0"}
+yaserde = "0.7"
 urdf-rs = "0.7.3"
+# Used for lazy initialization of static variable when they are non const
+once_cell = "1"
 pathdiff = "*"
 
 [dev-dependencies]
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index e97ed49c..edabc9c9 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -20,9 +20,15 @@ use crate::{
     Pose, Rotation, Side, Site, Swing,
 };
 use glam::Vec3;
+use once_cell::sync::Lazy;
 use sdformat_rs::*;
 use std::collections::BTreeMap;
 
+static WORLD_TEMPLATE: Lazy<SdfRoot> = Lazy::new(|| {
+    yaserde::de::from_str(include_str!("templates/gz_world.sdf"))
+        .expect("Failed deserializing template")
+});
+
 #[derive(Debug)]
 pub enum SdfConversionError {
     /// An asset that can't be converted to an sdf world was found.
@@ -429,12 +435,13 @@ impl Site {
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
         };
-        let mut models = Vec::new();
+        let mut root = WORLD_TEMPLATE.clone();
+        let world = &mut root.world[0];
         let mut min_elevation = f32::MAX;
         let mut max_elevation = f32::MIN;
         let mut toggle_floors_plugin = SdfPlugin {
             name: "toggle_floors".into(),
-            filename: "libtoggle_floors.so".into(),
+            filename: "toggle_floors".into(),
             ..Default::default()
         };
         for level in self.levels.values() {
@@ -452,7 +459,7 @@ impl Site {
                 .attributes
                 .insert("model_name".into(), level_model_name.clone());
             // Floors walls and static models are included in the level mesh
-            models.push(SdfModel {
+            world.model.push(SdfModel {
                 name: level_model_name,
                 r#static: Some(true),
                 link: vec![SdfLink {
@@ -489,7 +496,7 @@ impl Site {
                 // TODO(luca) doors into toggle floors
                 let left_anchor = get_anchor(door.anchors.left())?;
                 let right_anchor = get_anchor(door.anchors.right())?;
-                models.push(door.to_sdf(
+                world.model.push(door.to_sdf(
                     left_anchor,
                     right_anchor,
                     Vec3::new(0.0, 0.0, level.properties.elevation.0),
@@ -501,9 +508,7 @@ impl Site {
         }
         for lift in self.lifts.values() {
             // Cabin
-            let LiftCabin::Rect(ref cabin) = lift.properties.cabin else {
-                continue;
-            };
+            let LiftCabin::Rect(ref cabin) = lift.properties.cabin;
             let pose = lift
                 .properties
                 .center(self)
@@ -591,20 +596,20 @@ impl Site {
                         + door_placement
                             .custom_gap
                             .unwrap_or_else(|| cabin.gap.unwrap_or(0.01)));
-                let mut dummy_cabin = dummy_cabin.to_sdf(
+                let mut cabin = dummy_cabin.to_sdf(
                     left_anchor,
                     right_anchor,
                     x_offset,
                     false,
                     Some(cabin_mesh_prefix.clone()),
                 )?;
-                for mut joint in dummy_cabin.joint.drain(..) {
+                for mut joint in cabin.joint.drain(..) {
                     // Move the joint to the lift and change its parenthood accordingly
                     joint.parent = "platform".into();
-                    joint.child = dummy_cabin.name.clone() + "::" + &joint.child;
+                    joint.child = format!("{}::{}", cabin.name, joint.child);
                     lift_joints.push(joint);
                 }
-                lift_models.push(dummy_cabin.into());
+                lift_models.push(cabin.into());
                 for visit in door.visits.0.iter() {
                     let level = self
                         .levels
@@ -616,7 +621,6 @@ impl Site {
                         lift_name,
                         face.label()
                     );
-                    // TODO(luca) proper pose for shaft doors
                     let dummy_shaft = Door {
                         anchors: door.reference_anchors.clone(),
                         name: NameInSite(shaft_door_name.clone()),
@@ -645,7 +649,7 @@ impl Site {
                         Some(cabin_mesh_prefix.clone()),
                     )?;
                     // Add the pose of the lift to have world coordinates
-                    models.push(dummy_shaft);
+                    world.model.push(dummy_shaft);
                     let mut level = levels.entry(*visit).or_default();
                     let element = XmlElement {
                         name: "door_pair".into(),
@@ -687,7 +691,7 @@ impl Site {
             }
             component.data = ElementData::Nested(component_data);
             plugin.elements.push(component);
-            models.push(SdfModel {
+            world.model.push(SdfModel {
                 name: lift.properties.name.0.clone(),
                 r#static: Some(lift.properties.is_static.0),
                 pose: Some(pose.to_sdf(0.0)),
@@ -723,58 +727,15 @@ impl Site {
                 plugin: vec![plugin],
                 ..Default::default()
             });
+            // TODO(luca) lifts in the legacy pipeline seem to also have a "ramp" joint to allow
+            // easier transition of robots into lifts.
+            // From tests simulation seems to also work without it, probably due to having changed
+            // from full joint with wheel torques to just kinematic simulation for whole robots.
         }
 
-        let sun = SdfLight {
-            name: "sun".into(),
-            r#type: "directional".into(),
-            cast_shadows: Some(true),
-            diffuse: Some("1 1 1 1".into()),
-            pose: Some(Pose::default().to_sdf(10.0)),
-            specular: Some("0.2 0.2 0.2 1".into()),
-            attenuation: Some(SdfLightAttenuation {
-                range: 1000.0,
-                constant: Some(0.09),
-                linear: Some(0.001),
-                quadratic: Some(0.001),
-            }),
-            direction: Vector3d::new(-0.5, 0.1, -0.9),
-            ..Default::default()
-        };
-        let lift_plugin = SdfPlugin {
-            name: "lift".into(),
-            filename: "liblift.so".into(),
-            ..Default::default()
-        };
-        let door_plugin = SdfPlugin {
-            name: "door".into(),
-            filename: "libdoor.so".into(),
-            ..Default::default()
-        };
-        let charging_plugin = SdfPlugin {
-            name: "toggle_charging".into(),
-            filename: "libtoggle_charging.so".into(),
-            ..Default::default()
-        };
-        let physics_plugin = SdfPlugin {
-            name: "gz::sim::systems::Physics".into(),
-            filename: "libgz-sim-physics-system.so".into(),
-            ..Default::default()
-        };
-        let user_commands_plugin = SdfPlugin {
-            name: "gz::sim::systems::UserCommands".into(),
-            filename: "libgz-sim-user-commands-system.so".into(),
-            ..Default::default()
-        };
-        let scene_broadcaster_plugin = SdfPlugin {
-            name: "gz::sim::systems::SceneBroadcaster".into(),
-            filename: "libgz-sim-scene-broadcaster-system.so".into(),
-            ..Default::default()
-        };
         // Spawn the robots now
         // TODO(luca) use robot properties and export this like other meshes
         // instead of including a tag
-        let mut world_includes = Vec::new();
         for location in self.navigation.guided.locations.values() {
             let Some(robot) = location.tags.0.iter().find_map(|l| l.spawn_robot()) else {
                 continue;
@@ -798,7 +759,7 @@ impl Site {
                 trans: [tf[0], tf[1], level.properties.elevation.0],
                 ..Default::default()
             };
-            world_includes.push(SdfWorldInclude {
+            world.include.push(SdfWorldInclude {
                 uri: "model://".to_string() + robot_type,
                 name: Some(robot.name.0.clone()),
                 pose: Some(pose.to_sdf(0.0)),
@@ -806,46 +767,20 @@ impl Site {
                 ..Default::default()
             });
         }
-        Ok(SdfRoot {
-            version: "1.7".to_string(),
-            world: vec![SdfWorld {
-                name: self.properties.name.0.clone(),
-                model: models,
-                include: world_includes,
-                atmosphere: SdfAtmosphere {
-                    r#type: "adiabatic".to_string(),
-                    ..Default::default()
-                },
-                scene: SdfScene {
-                    ambient: "1 1 1".to_string(),
-                    background: "0.8 0.8 0.8".to_string(),
-                    ..Default::default()
-                },
-                physics: vec![SdfPhysics {
-                    r#type: "ode".into(),
-                    max_step_size: 0.01,
-                    real_time_factor: 1.0,
-                    real_time_update_rate: 100.0,
-                    ..Default::default()
-                }],
-                light: vec![sun],
-                // TODO(luca) check why these plugins are not being applied, we might need to
-                // override more than only plugins
-                gui: Some(SdfGui {
-                    plugin: vec![charging_plugin, toggle_floors_plugin],
-                    ..Default::default()
-                }),
-                plugin: vec![
-                    physics_plugin,
-                    user_commands_plugin,
-                    scene_broadcaster_plugin,
-                    door_plugin,
-                    lift_plugin,
-                ],
-                ..Default::default()
-            }],
+        world.name = self.properties.name.0.clone();
+        if let Some(gui) = world.gui.as_mut() {
+            gui.plugin.push(toggle_floors_plugin);
+        }
+        // TODO(luca) these fields are set as required in the specification but seem not to be in
+        // practice (rightly so because not everyone wants to manually specify gravity, earth
+        // magnetic field and atmosphere model)
+        world.atmosphere = SdfAtmosphere {
+            r#type: "adiabatic".to_string(),
             ..Default::default()
-        })
+        };
+        world.gravity = Vector3d::new(0.0, 0.0, -9.80);
+        world.magnetic_field = Vector3d::new(5.64e-6, 2.29e-5, -4.24e-5);
+        Ok(root)
     }
 }
 
diff --git a/rmf_site_format/src/templates/gz_world.sdf b/rmf_site_format/src/templates/gz_world.sdf
new file mode 100644
index 00000000..32562659
--- /dev/null
+++ b/rmf_site_format/src/templates/gz_world.sdf
@@ -0,0 +1,298 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="sim_world" xmlns:rmf="rmf">
+    <physics name="10ms" type="ode">
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="libgz-sim-physics-system.so"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="libgz-sim-user-commands-system.so"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="libgz-sim-scene-broadcaster-system.so"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="libdoor.so"
+      name="door">
+    </plugin>
+    <plugin
+      filename="liblift.so"
+      name="lift">
+    </plugin>
+
+    <scene>
+      <ambient>1 1 1</ambient>
+      <background>0.8 0.8 0.8</background>
+      <grid>false</grid>
+    </scene>
+
+    <gui fullscreen="0">
+
+      <window>
+        <width>1000</width>
+        <height>845</height>
+        <style
+          material_theme="Light"
+          material_primary="DeepOrange"
+          material_accent="LightBlue"
+          toolbar_color_light="#f3f3f3"
+          toolbar_text_color_light="#111111"
+          toolbar_color_dark="#414141"
+          toolbar_text_color_dark="#f3f3f3"
+          plugin_toolbar_color_light="#bbdefb"
+          plugin_toolbar_text_color_light="#111111"
+          plugin_toolbar_color_dark="#607d8b"
+          plugin_toolbar_text_color_dark="#eeeeee"
+        />
+        <menus>
+          <drawer default="false">
+          </drawer>
+        </menus>
+      </window>
+
+      <!-- GUI plugins -->
+      <plugin filename="MinimalScene" name="3D View">
+          <gz-gui>
+              <title>3D View</title>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="string" key="state">docked</property>
+          </gz-gui>
+          <engine>ogre2</engine>
+          <scene>scene</scene>
+          <ambient_light>0.4 0.4 0.4</ambient_light>
+          <background_color>0.8 0.8 0.8</background_color>
+          <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
+
+      <!-- Plugins that add functionality to the scene -->
+      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+          <gz-gui>
+              <property key="state" type="string">floating</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="GzSceneManager" name="Scene Manager">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="InteractiveViewControl" name="Interactive view control">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="CameraTracking" name="Camera Tracking">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="MarkerManager" name="Marker manager">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="SelectEntities" name="Select Entities">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="Spawn" name="Spawn Entities">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+          <gz-gui>
+              <title>World control</title>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="bool" key="resizable">false</property>
+              <property type="double" key="height">72</property>
+              <property type="double" key="z">1</property>
+              <property type="string" key="state">floating</property>
+              <anchors target="3D View">
+                  <line own="left" target="left"/>
+                  <line own="bottom" target="bottom"/>
+              </anchors>
+          </gz-gui>
+          <play_pause>true</play_pause>
+          <step>true</step>
+          <start_paused>true</start_paused>
+          <use_event>true</use_event>
+      </plugin>
+
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+          <gz-gui>
+              <title>World stats</title>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="bool" key="resizable">false</property>
+              <property type="double" key="height">110</property>
+              <property type="double" key="width">290</property>
+              <property type="double" key="z">1</property>
+              <property type="string" key="state">floating</property>
+              <anchors target="3D View">
+                  <line own="right" target="right"/>
+                  <line own="bottom" target="bottom"/>
+              </anchors>
+          </gz-gui>
+          <sim_time>true</sim_time>
+          <real_time>true</real_time>
+          <real_time_factor>true</real_time_factor>
+          <iterations>true</iterations>
+      </plugin>
+
+      <!-- Insert simple shapes -->
+      <plugin filename="Shapes" name="Shapes">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">0</property>
+              <property key="y" type="double">0</property>
+              <property key="width" type="double">250</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#666666</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Insert lights -->
+      <plugin filename="Lights" name="Lights">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">250</property>
+              <property key="y" type="double">0</property>
+              <property key="width" type="double">150</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#666666</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Translate / rotate -->
+      <plugin filename="TransformControl" name="Transform control">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">0</property>
+              <property key="y" type="double">50</property>
+              <property key="width" type="double">250</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#777777</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Screenshot -->
+      <plugin filename="Screenshot" name="Screenshot">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">250</property>
+              <property key="y" type="double">50</property>
+              <property key="width" type="double">50</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#777777</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Copy/Paste -->
+      <plugin filename="CopyPaste" name="CopyPaste">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">300</property>
+              <property key="y" type="double">50</property>
+              <property key="width" type="double">100</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#777777</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Inspector -->
+      <plugin filename="ComponentInspector" name="Component inspector">
+          <gz-gui>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="string" key="state">docked</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Entity tree -->
+      <plugin filename="EntityTree" name="Entity tree">
+          <gz-gui>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="string" key="state">docked</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="toggle_charging" name="toggle_charging"/>
+    </gui>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.09</constant>
+        <linear>0.001</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+  </world>
+</sdf>

From c9dff668e38d6353c55f4af1622fff00cc8a61d6 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 1 Mar 2024 12:33:17 +0800
Subject: [PATCH 37/71] Navgraph fixes for legacy compatibility

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 Cargo.lock                                 | 4 +++-
 rmf_site_editor/src/site/save.rs           | 5 +++--
 rmf_site_format/src/legacy/building_map.rs | 4 ++--
 3 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 8b66f06d..1e426d62 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -4804,6 +4804,7 @@ dependencies = [
  "bevy",
  "float_eq",
  "glam",
+ "once_cell",
  "pathdiff",
  "ron",
  "sdformat_rs",
@@ -4813,6 +4814,7 @@ dependencies = [
  "thiserror",
  "urdf-rs",
  "uuid",
+ "yaserde",
 ]
 
 [[package]]
@@ -4938,7 +4940,7 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
 [[package]]
 name = "sdformat_rs"
 version = "0.1.0"
-source = "git+https://github.com/open-rmf/sdf_rust_experimental?branch=luca/yaserde_nested_models#605ffb7f3c821e919e6eda6b5224e58649d6e46d"
+source = "git+https://github.com/open-rmf/sdf_rust_experimental?rev=2d20fe0#2d20fe051a285e7a14f1948ebeb2a78c7e642da1"
 dependencies = [
  "convert_case",
  "nalgebra",
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index b7cbc4c1..8beac37e 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1299,10 +1299,11 @@ pub fn save_site(world: &mut World) {
                 };
                 let s = yaserde::ser::serialize_with_writer(&sdf, f, &config).unwrap();
                 let mut navgraph_dir = PathBuf::from(new_path.parent().unwrap());
-                navgraph_dir.push("nav");
+                navgraph_dir.push("nav_graphs");
+                std::fs::create_dir(&navgraph_dir).ok();
                 for (name, graph) in &graphs {
                     let mut graph_file = navgraph_dir.clone();
-                    graph_file.set_file_name(name.to_owned() + ".yaml");
+                    graph_file.push(name.to_owned() + ".yaml");
                     info!(
                         "Saving legacy nav graph to {}",
                         graph_file.to_str().unwrap_or("<failed to render??>")
diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs
index fef34f47..abd63132 100644
--- a/rmf_site_format/src/legacy/building_map.rs
+++ b/rmf_site_format/src/legacy/building_map.rs
@@ -153,7 +153,7 @@ impl BuildingMap {
         let mut wall_texture_map: HashMap<WallProperties, u32> = HashMap::new();
         let mut lift_cabin_anchors: BTreeMap<String, Vec<(u32, Anchor)>> = BTreeMap::new();
 
-        let mut building_id_to_nav_graph_id = HashMap::new();
+        let mut building_id_to_nav_graph_id = BTreeMap::new();
 
         let mut fiducial_groups: BTreeMap<u32, FiducialGroup> = BTreeMap::new();
         let mut cartesian_fiducials: HashMap<u32, Vec<DVec2>> = HashMap::new();
@@ -599,7 +599,7 @@ impl BuildingMap {
             nav_graphs.insert(
                 *graph_id,
                 NavGraph {
-                    name: NameInSite("unnamed_graph_#".to_string() + &i.to_string()),
+                    name: NameInSite(i.to_string()),
                     color: DisplayColor(DEFAULT_NAV_GRAPH_COLORS[color_index]),
                     marker: Default::default(),
                 },

From 739c4179fc3893aa5a0341824b2596a9ed48a2f3 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 1 Mar 2024 17:24:29 +0800
Subject: [PATCH 38/71] POC of headless mode for SDF export

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs       | 59 ++++++++++++++++++++++++++++----
 rmf_site_editor/src/workspace.rs |  5 +++
 2 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index d7c780ef..3161a03c 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -77,7 +77,7 @@ pub struct CommandLineArgs {
     pub import: Option<String>,
     /// Run in headless mode.
     #[cfg_attr(not(target_arch = "wasm32"), arg(long))]
-    pub headless: bool,
+    pub headless: Option<String>,
 }
 
 #[derive(Clone, Default, Eq, PartialEq, Debug, Hash, States)]
@@ -120,7 +120,7 @@ pub fn run_js() {
 
 pub fn run(command_line_args: Vec<String>) {
     let mut app = App::new();
-    let mut headless = false;
+    let mut headless = None;
 
     #[cfg(not(target_arch = "wasm32"))]
     {
@@ -141,7 +141,7 @@ pub fn run(command_line_args: Vec<String>) {
 #[derive(Default)]
 pub struct SiteEditor {
     /// Whether to run the site editor in headless mode.
-    pub headless: bool,
+    pub headless: Option<String>,
 }
 
 impl Plugin for SiteEditor {
@@ -176,7 +176,7 @@ impl Plugin for SiteEditor {
         #[cfg(not(target_arch = "wasm32"))]
         {
             let mut plugins = DefaultPlugins.build();
-            let plugins = if self.headless {
+            let plugins = if self.headless.is_some() {
                 plugins
                     .set(WindowPlugin {
                         primary_window: None,
@@ -227,10 +227,10 @@ impl Plugin for SiteEditor {
                 WorkcellEditorPlugin,
                 SitePlugin,
                 InteractionPlugin {
-                    headless: self.headless,
+                    headless: self.headless.is_some(),
                 },
                 StandardUiLayout {
-                    headless: self.headless,
+                    headless: self.headless.is_some(),
                 },
                 AnimationPlugin,
                 OccupancyPlugin,
@@ -247,8 +247,53 @@ impl Plugin for SiteEditor {
 
         // TODO(luca) This schedule runner plugin runs forever.
         // We might need to have a custom one where we can inject exit conditions.
-        if self.headless {
+        if let Some(path) = &self.headless {
             app.add_plugins(ScheduleRunnerPlugin::default());
+            app.insert_resource(HeadlessExport(path.clone()));
+            app.add_systems(Last, headless_sdf_export.run_if(AppState::in_site_mode()));
+        }
+    }
+}
+
+#[derive(Debug, Resource, Default)]
+pub struct HeadlessExport(String);
+
+#[derive(Debug, Resource, Default)]
+pub struct Counter(u32, bool);
+
+pub fn headless_sdf_export(
+    mut export: EventWriter<SaveWorkspace>,
+    mut exit: EventWriter<bevy::app::AppExit>,
+    missing_models: Query<
+        (),
+        (
+            With<site::TentativeModelFormat>,
+            Without<site::ModelSceneRoot>,
+        ),
+    >,
+    mut export_state: Local<Counter>,
+    path: Res<HeadlessExport>,
+    sites: Query<(Entity, &rmf_site_format::NameOfSite)>,
+) {
+    export_state.0 += 1;
+    if export_state.0 < 5 {
+        return;
+    }
+    if !missing_models.is_empty() {
+        info!("Waiting for models to spawn");
+    } else {
+        dbg!(&export_state);
+        if !export_state.1 {
+            export_state.1 = true;
+            info!("All models spawned, sending export event");
+            export_state.0 = 0;
+            for (e, name) in &sites {
+                let path = std::path::PathBuf::from(path.0.clone());
+                export.send(SaveWorkspace::new().to_sdf().to_path(&path));
+            }
+        }
+        if export_state.0 > 5 {
+            exit.send(bevy::app::AppExit);
         }
     }
 }
diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs
index d7180b4c..ebf0ba1a 100644
--- a/rmf_site_editor/src/workspace.rs
+++ b/rmf_site_editor/src/workspace.rs
@@ -145,6 +145,11 @@ impl SaveWorkspace {
         self.format = ExportFormat::Urdf;
         self
     }
+
+    pub fn to_sdf(mut self) -> Self {
+        self.format = ExportFormat::Sdf;
+        self
+    }
 }
 
 #[derive(Default, Debug, Clone)]

From a16a41fe0896bf96983165911cde712a94d2f851 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Mon, 4 Mar 2024 17:30:43 +0800
Subject: [PATCH 39/71] Make sure reference level is not translated

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/alignment.rs           | 43 ++++++++++++----------
 rmf_site_format/src/legacy/building_map.rs |  6 +--
 2 files changed, 26 insertions(+), 23 deletions(-)

diff --git a/rmf_site_format/src/alignment.rs b/rmf_site_format/src/alignment.rs
index 63adab56..1755b706 100644
--- a/rmf_site_format/src/alignment.rs
+++ b/rmf_site_format/src/alignment.rs
@@ -548,25 +548,28 @@ fn traverse_locations<F: FnMut(usize, DVec2, usize, DVec2)>(
 }
 
 fn calculate_center_adjustment(building: &BuildingMap, u: &mut [f64]) {
-    let mut center = DVec2::ZERO;
-    let mut weight = 0.0;
-    for (i, level) in building.levels.values().enumerate() {
-        let range = 4 * i..4 * (i + 1);
-        let vars = LevelVariables::new(&u[range], i);
-        for v in &level.vertices {
-            let v = vars.transform(v.to_vec());
-            center += v;
-            weight += 1.0;
-        }
-    }
-
-    if weight >= 0.0 {
-        center /= weight;
-        for level in 0..u.len() / 4 {
-            let x = 4 * level;
-            let y = 4 * level + 1;
-            u[x] -= center.x;
-            u[y] -= center.y;
-        }
+    if building.levels.is_empty() {
+        return;
+    }
+    let reference_idx = building
+        .reference_level_name
+        .as_ref()
+        .and_then(|name| {
+            building
+                .levels
+                .iter()
+                .position(|(level_name, _)| name == level_name)
+        })
+        .unwrap_or(0);
+    let range = 4 * reference_idx..4 * (reference_idx + 1);
+    let center = LevelVariables::new(&u[range], reference_idx);
+    let dx = center.dx().clone();
+    let dy = center.dy().clone();
+
+    for level in 0..u.len() / 4 {
+        let x = 4 * level;
+        let y = 4 * level + 1;
+        u[x] -= dx;
+        u[y] -= dy;
     }
 }
diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs
index abd63132..56193271 100644
--- a/rmf_site_format/src/legacy/building_map.rs
+++ b/rmf_site_format/src/legacy/building_map.rs
@@ -33,6 +33,8 @@ pub struct BuildingMap {
     pub name: String,
     #[serde(default)]
     pub coordinate_system: CoordinateSystem,
+    #[serde(default)]
+    pub reference_level_name: Option<String>,
     pub levels: BTreeMap<String, Level>,
     // TODO(MXG): Consider parsing legacy crowdsim data and converting it to
     // a format that will have future support.
@@ -349,13 +351,11 @@ impl BuildingMap {
             }
 
             for (name, layer) in &level.layers {
-                // TODO(luca) coordinates in site and traffic editor might be different, use
-                // optimization engine instead of parsing
                 let drawing_id = site_id.next().unwrap();
                 let pose = Pose {
                     trans: [
                         layer.transform.translation_x as f32,
-                        layer.transform.translation_y as f32,
+                        -layer.transform.translation_y as f32,
                         0.0 as f32,
                     ],
                     rot: Rotation::Yaw(Angle::Rad(layer.transform.yaw as f32)),

From f7ca197268ff57614c152f3fa8ab79e778452121 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 7 Mar 2024 11:56:06 +0800
Subject: [PATCH 40/71] Add saved camera poses field and computation

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/level.rs          | 32 ++++++++-
 rmf_site_editor/src/site/mod.rs            |  1 +
 rmf_site_editor/src/site/save.rs           |  3 +
 rmf_site_editor/src/site/site.rs           |  1 +
 rmf_site_format/src/camera_poses.rs        | 61 ++++++++++++++++++
 rmf_site_format/src/legacy/building_map.rs | 75 ++++++++++++++++------
 rmf_site_format/src/level.rs               |  3 +
 rmf_site_format/src/lib.rs                 |  3 +
 8 files changed, 159 insertions(+), 20 deletions(-)
 create mode 100644 rmf_site_format/src/camera_poses.rs

diff --git a/rmf_site_editor/src/site/level.rs b/rmf_site_editor/src/site/level.rs
index fda3ac9a..fcaebac9 100644
--- a/rmf_site_editor/src/site/level.rs
+++ b/rmf_site_editor/src/site/level.rs
@@ -16,7 +16,7 @@
 */
 
 use crate::site::*;
-use crate::CurrentWorkspace;
+use crate::{interaction::CameraControls, CurrentWorkspace};
 use bevy::prelude::*;
 
 pub fn update_level_visibility(
@@ -66,3 +66,33 @@ pub fn assign_orphan_elements_to_level<T: Component>(
         commands.entity(current_level).add_child(orphan);
     }
 }
+
+pub fn set_camera_transform_on_level_change(
+    current_level: Res<CurrentLevel>,
+    mut camera_controls: ResMut<CameraControls>,
+    camera_poses: Query<&CameraPoses>,
+    mut transforms: Query<&mut Transform>,
+) {
+    if current_level.is_changed() {
+        let Some(level) = current_level.0 else {
+            return;
+        };
+
+        if let Ok(poses) = camera_poses.get(level) {
+            // TODO(luca) Add an actual default pose rather than first in map
+            let Some(pose) = poses.0.values().next() else {
+                return;
+            };
+            if let Ok(mut tf) = transforms.get_mut(camera_controls.perspective_camera_entities[0]) {
+                *tf = pose.transform();
+            }
+            let mut translation = pose.transform().translation;
+            // TODO(luca) these are the same value that are in rmf_site_format, should we change
+            // them?
+            translation.x = translation.x + 10.0;
+            translation.y = translation.y + 10.0;
+            translation.z = 0.0;
+            camera_controls.orbit_center = translation;
+        }
+    }
+}
diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs
index c9bb1f43..11694dc9 100644
--- a/rmf_site_editor/src/site/mod.rs
+++ b/rmf_site_editor/src/site/mod.rs
@@ -330,6 +330,7 @@ impl Plugin for SitePlugin {
                 align_site_drawings,
                 clear_old_issues_on_new_validate_event,
                 export_lights,
+                set_camera_transform_on_level_change,
             )
                 .run_if(AppState::in_site_mode())
                 .in_set(SiteUpdateSet::BetweenVisibilityAndTransform),
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 8beac37e..d4809a40 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -346,6 +346,7 @@ fn generate_levels(
                 &Children,
                 Option<&RecencyRanking<FloorMarker>>,
                 Option<&RecencyRanking<DrawingMarker>>,
+                &CameraPoses,
             ),
             Without<Pending>,
         >,
@@ -411,6 +412,7 @@ fn generate_levels(
                 level_children,
                 floor_ranking,
                 drawing_ranking,
+                camera_poses,
             )) = q_levels.get(*c)
             {
                 let mut level = Level::new(
@@ -419,6 +421,7 @@ fn generate_levels(
                         elevation: elevation.clone(),
                         global_floor_visibility: floor_vis.clone(),
                         global_drawing_visibility: drawing_vis.clone(),
+                        camera_poses: camera_poses.clone(),
                     },
                     RankingsInLevel {
                         floors: floor_ranking
diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs
index a9a21ad1..6aad8ff2 100644
--- a/rmf_site_editor/src/site/site.rs
+++ b/rmf_site_editor/src/site/site.rs
@@ -123,6 +123,7 @@ pub fn change_site(
                                 elevation: LevelElevation(0.),
                                 global_floor_visibility: default(),
                                 global_drawing_visibility: default(),
+                                camera_poses: default(),
                             })
                             .set_parent(cmd.site)
                             .id();
diff --git a/rmf_site_format/src/camera_poses.rs b/rmf_site_format/src/camera_poses.rs
new file mode 100644
index 00000000..185e0556
--- /dev/null
+++ b/rmf_site_format/src/camera_poses.rs
@@ -0,0 +1,61 @@
+/*
+ * 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::{Anchor, Category, Pose, Rotation};
+#[cfg(feature = "bevy")]
+use bevy::prelude::Component;
+use glam::{Affine3A, Quat, Vec3};
+use serde::{Deserialize, Serialize};
+use std::collections::HashMap;
+
+#[derive(Serialize, Deserialize, Default, Debug, Clone, PartialEq)]
+#[cfg_attr(feature = "bevy", derive(Component))]
+pub struct CameraPoses(pub HashMap<String, Pose>);
+
+impl CameraPoses {
+    pub fn from_anchors<'a, I>(anchors: I) -> Self
+    where
+        I: Iterator<Item = &'a Anchor>,
+    {
+        // Center the camera in the centroid of the anchors
+        let mut count = 0;
+        let mut trans = Vec3::default();
+        for anchor in anchors {
+            let anchor_trans = *anchor.translation_for_category(Category::Level);
+            trans[0] = trans[0] + anchor_trans[0];
+            trans[1] = trans[1] + anchor_trans[1];
+            count += 1;
+        }
+        if count > 0 {
+            trans = trans / count as f32;
+        }
+        let offset = Vec3::new(-10.0, -10.0, 10.0);
+        let affine = Affine3A::look_at_rh(trans + offset, trans, Vec3::Z);
+        let rotation = Quat::from_mat3a(&affine.matrix3);
+        // TODO(luca) check why the signs are inverted and fix the API call
+        let mut rot = rotation.to_array();
+        rot[0] = -rot[0];
+        rot[1] = -rot[1];
+        rot[2] = -rot[2];
+
+        let pose = Pose {
+            trans: (trans + offset).into(),
+            rot: Rotation::Quat(rot),
+        };
+        Self(HashMap::from([("default".to_string(), pose)]))
+    }
+}
diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs
index 56193271..6383fb75 100644
--- a/rmf_site_format/src/legacy/building_map.rs
+++ b/rmf_site_format/src/legacy/building_map.rs
@@ -3,12 +3,13 @@ use super::{
 };
 use crate::{
     alignment::align_legacy_building, Affiliation, Anchor, Angle, AssetSource, AssociatedGraphs,
-    Category, DisplayColor, Dock as SiteDock, Drawing as SiteDrawing, DrawingProperties,
-    Fiducial as SiteFiducial, FiducialGroup, FiducialMarker, Guided, Lane as SiteLane, LaneMarker,
-    Level as SiteLevel, LevelElevation, LevelProperties as SiteLevelProperties, Motion, NameInSite,
-    NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose,
-    PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, Site, SiteProperties,
-    Texture as SiteTexture, TextureGroup, DEFAULT_NAV_GRAPH_COLORS,
+    CameraPoses, Category, DisplayColor, Dock as SiteDock, Drawing as SiteDrawing,
+    DrawingProperties, Fiducial as SiteFiducial, FiducialGroup, FiducialMarker, Guided,
+    Lane as SiteLane, LaneMarker, Level as SiteLevel, LevelElevation,
+    LevelProperties as SiteLevelProperties, Motion, NameInSite, NameOfSite, NavGraph, Navigation,
+    OrientationConstraint, PixelsPerMeter, Pose, PreferredSemiTransparency, RankingsInLevel,
+    ReverseLane, Rotation, Site, SiteProperties, Texture as SiteTexture, TextureGroup,
+    DEFAULT_NAV_GRAPH_COLORS,
 };
 use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot};
 use serde::{Deserialize, Serialize};
@@ -53,6 +54,28 @@ impl BuildingMap {
         }
     }
 
+    /// Collects all vertices that are used in cartesian features (i.e. floors, walls, doors).
+    /// Doesn't include pixel anchors (i.e. drawings)
+    fn collect_level_cartesian_vertices(level: &Level) -> HashSet<usize> {
+        let mut anchors: HashSet<usize> = HashSet::new();
+        for door in &level.doors {
+            anchors.extend([door.0, door.1]);
+        }
+
+        for floor in &level.floors {
+            anchors.extend(&floor.vertices);
+        }
+
+        for wall in &level.walls {
+            anchors.extend([wall.0, wall.1]);
+        }
+
+        for lane in &level.lanes {
+            anchors.extend([lane.0, lane.1]);
+        }
+        anchors
+    }
+
     /// Converts a map from the oldest legacy format, which uses pixel coordinates.
     fn from_pixel_coordinates(mut map: BuildingMap) -> BuildingMap {
         let alignments = align_legacy_building(&map);
@@ -72,8 +95,27 @@ impl BuildingMap {
             let tf = alignment.to_affine();
             level.alignment = Some(alignment.clone());
             // We need to keep the vertices associated with measurements in image coordinates
+            let level_vertices = Self::collect_level_cartesian_vertices(level);
             let mut drawing_vertices = HashSet::new();
-            for measurement in &level.measurements {
+            for measurement in &mut level.measurements {
+                match level_vertices.get(&measurement.0) {
+                    Some(vertex) => {
+                        // Vertex is shared, duplicate it
+                        let idx = level.vertices.len();
+                        level.vertices.push(level.vertices[*vertex].clone());
+                        measurement.0 = idx;
+                    }
+                    None => {}
+                }
+                match level_vertices.get(&measurement.1) {
+                    Some(vertex) => {
+                        // Vertex is shared, duplicate it
+                        let idx = level.vertices.len();
+                        level.vertices.push(level.vertices[*vertex].clone());
+                        measurement.1 = idx;
+                    }
+                    None => {}
+                }
                 drawing_vertices.insert(measurement.0);
                 drawing_vertices.insert(measurement.1);
             }
@@ -316,19 +358,13 @@ impl BuildingMap {
                 for measurement in &level.measurements {
                     let mut site_measurement = measurement.to_site(&vertex_to_anchor_id)?;
                     let edge = &mut site_measurement.anchors;
-                    let (start_anchor, end_anchor) = (
-                        level_anchors.get(&edge.left()).unwrap(),
-                        level_anchors.get(&edge.right()).unwrap(),
-                    );
-                    // Now get the anchors and duplicate them in the drawing
-                    let anchor_id = site_id.next().unwrap();
-                    drawing_anchors.insert(anchor_id, start_anchor.clone());
-                    *edge.left_mut() = anchor_id;
-                    let anchor_id = site_id.next().unwrap();
-                    drawing_anchors.insert(anchor_id, end_anchor.clone());
-                    *edge.right_mut() = anchor_id;
+                    // Remove the measurement anchors from the level anchors, since they belong to
+                    // the drawing
+                    let left = level_anchors.remove_entry(&edge.left()).unwrap();
+                    let right = level_anchors.remove_entry(&edge.right()).unwrap();
+                    drawing_anchors.insert(left.0, left.1);
+                    drawing_anchors.insert(right.0, right.1);
                     measurements.insert(site_id.next().unwrap(), site_measurement);
-                    // TODO(luca) remove original anchors if they have no other dependents
                     // TODO(MXG): Have rankings for measurements
                 }
 
@@ -503,6 +539,7 @@ impl BuildingMap {
                         elevation: LevelElevation(elevation),
                         global_floor_visibility: Default::default(),
                         global_drawing_visibility: Default::default(),
+                        camera_poses: CameraPoses::from_anchors(level_anchors.values()),
                     },
                     anchors: level_anchors,
                     doors,
diff --git a/rmf_site_format/src/level.rs b/rmf_site_format/src/level.rs
index b4486773..f7f8a7c0 100644
--- a/rmf_site_format/src/level.rs
+++ b/rmf_site_format/src/level.rs
@@ -30,6 +30,8 @@ pub struct LevelProperties {
     pub global_floor_visibility: GlobalFloorVisibility,
     #[serde(default, skip_serializing_if = "is_default")]
     pub global_drawing_visibility: GlobalDrawingVisibility,
+    #[serde(default, skip_serializing_if = "is_default")]
+    pub camera_poses: CameraPoses,
 }
 
 impl Default for LevelProperties {
@@ -39,6 +41,7 @@ impl Default for LevelProperties {
             elevation: LevelElevation(0.0),
             global_floor_visibility: Default::default(),
             global_drawing_visibility: Default::default(),
+            camera_poses: Default::default(),
         }
     }
 }
diff --git a/rmf_site_format/src/lib.rs b/rmf_site_format/src/lib.rs
index bf0f6fda..cf8f0e49 100644
--- a/rmf_site_format/src/lib.rs
+++ b/rmf_site_format/src/lib.rs
@@ -29,6 +29,9 @@ pub use asset_source::*;
 pub mod category;
 pub use category::*;
 
+pub mod camera_poses;
+pub use camera_poses::*;
+
 pub mod constraint;
 pub use constraint::*;
 

From dd4c3e55c073860c6e2f505ffe662426c964f76a Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 7 Mar 2024 15:33:11 +0800
Subject: [PATCH 41/71] Abort if site loading failed

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 3161a03c..f502bab3 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -250,7 +250,7 @@ impl Plugin for SiteEditor {
         if let Some(path) = &self.headless {
             app.add_plugins(ScheduleRunnerPlugin::default());
             app.insert_resource(HeadlessExport(path.clone()));
-            app.add_systems(Last, headless_sdf_export.run_if(AppState::in_site_mode()));
+            app.add_systems(Last, headless_sdf_export);
         }
     }
 }
@@ -279,10 +279,13 @@ pub fn headless_sdf_export(
     if export_state.0 < 5 {
         return;
     }
+    if sites.is_empty() {
+        warn!("Site loading failed, aborting");
+        exit.send(bevy::app::AppExit);
+    }
     if !missing_models.is_empty() {
         info!("Waiting for models to spawn");
     } else {
-        dbg!(&export_state);
         if !export_state.1 {
             export_state.1 = true;
             info!("All models spawned, sending export event");

From 8890a523f389b1d4f9fa833c723c0c3da9dd4e52 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 7 Mar 2024 16:04:21 +0800
Subject: [PATCH 42/71] Update camera pose

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/Cargo.toml |  2 +-
 rmf_site_format/src/sdf.rs | 16 ++++++++++++++++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index c9a76b2d..108b6efa 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -18,7 +18,7 @@ uuid = { version = "1.1", features = ["v4", "serde"] }
 # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features
 # We depend on a bugfix released specifically in 0.7.3
 bevy = { version = "0.11", optional = true }
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "2d20fe0"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "48ebcad"}
 yaserde = "0.7"
 urdf-rs = "0.7.3"
 # Used for lazy initialization of static variable when they are non const
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index edabc9c9..ad5513d1 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -770,6 +770,22 @@ impl Site {
         world.name = self.properties.name.0.clone();
         if let Some(gui) = world.gui.as_mut() {
             gui.plugin.push(toggle_floors_plugin);
+            if let Some(minimal_scene) = gui
+                .plugin
+                .iter_mut()
+                .find(|plugin| plugin.filename == "MinimalScene")
+            {
+                if let Some(camera_pose) = minimal_scene.elements.get_mut("camera_pose") {
+                    if let Some(default_camera) = self
+                        .levels
+                        .first_key_value()
+                        .and_then(|(_, level)| level.properties.camera_poses.0.iter().next())
+                    {
+                        // TODO(luca) use level elevation here?
+                        camera_pose.data = ElementData::String(default_camera.1.to_sdf(0.0).data);
+                    }
+                }
+            }
         }
         // TODO(luca) these fields are set as required in the specification but seem not to be in
         // practice (rightly so because not everyone wants to manually specify gravity, earth

From 21c1426e493e7b2aa164436de0ccb8c7501ec2b8 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 7 Mar 2024 17:50:28 +0800
Subject: [PATCH 43/71] Additional wait before saving

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs       | 23 +++++++++++++----------
 rmf_site_editor/src/site/save.rs |  8 ++++++--
 2 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index f502bab3..24087d0b 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -259,7 +259,7 @@ impl Plugin for SiteEditor {
 pub struct HeadlessExport(String);
 
 #[derive(Debug, Resource, Default)]
-pub struct Counter(u32, bool);
+pub struct Counter(u32, bool, bool);
 
 pub fn headless_sdf_export(
     mut export: EventWriter<SaveWorkspace>,
@@ -284,19 +284,22 @@ pub fn headless_sdf_export(
         exit.send(bevy::app::AppExit);
     }
     if !missing_models.is_empty() {
-        info!("Waiting for models to spawn");
+        // TODO(luca) implement a timeout logic?
     } else {
         if !export_state.1 {
-            export_state.1 = true;
-            info!("All models spawned, sending export event");
             export_state.0 = 0;
-            for (e, name) in &sites {
-                let path = std::path::PathBuf::from(path.0.clone());
-                export.send(SaveWorkspace::new().to_sdf().to_path(&path));
+            export_state.1 = true;
+        } else {
+            if !export_state.2 && export_state.0 > 5 {
+                export_state.2 = true;
+                for (e, name) in &sites {
+                    let path = std::path::PathBuf::from(path.0.clone());
+                    export.send(SaveWorkspace::new().to_sdf().to_path(&path));
+                    info!("Sent");
+                }
+            } else if export_state.2 && export_state.0 > 15 {
+                exit.send(bevy::app::AppExit);
             }
         }
-        if export_state.0 > 5 {
-            exit.send(bevy::app::AppExit);
-        }
     }
 }
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index d4809a40..b3e690ea 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1262,10 +1262,14 @@ pub fn save_site(world: &mut World) {
                     new_path = new_path.with_extension("world");
                 }
                 info!("Saving to {}", new_path.display());
-                let f = match std::fs::File::create(new_path.clone()) {
+                let parent_folder = new_path.parent().unwrap();
+                if !parent_folder.exists() {
+                    std::fs::create_dir_all(new_path.parent().unwrap());
+                }
+                let f = match std::fs::File::create(&new_path) {
                     Ok(f) => f,
                     Err(err) => {
-                        error!("Unable to save file: {err}");
+                        error!("Unable to save file {}: {err}", new_path.display());
                         continue;
                     }
                 };

From 28d28d19f12e54992df991913542edb003598d81 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 19 Mar 2024 15:52:48 +0800
Subject: [PATCH 44/71] Fix camera pose, add dispensers

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 34 +++++++++++++++++++++++++++++-----
 1 file changed, 29 insertions(+), 5 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index ad5513d1..7132eae9 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -54,7 +54,7 @@ impl Pose {
                 rpy[1].radians(),
                 rpy[2].radians()
             ),
-            Rotation::Quat(quat) => format!("{} {} {} {}", quat[0], quat[1], quat[2], quat[3]),
+            Rotation::Quat(quat) => format!("{} {} {} {}", quat[3], quat[0], quat[1], quat[2]),
         };
         SdfPose {
             data: format!("{} {} {} {}", p[0], p[1], p[2] + elevation, r),
@@ -491,6 +491,26 @@ impl Site {
                 }],
                 ..Default::default()
             });
+            // TODO(luca) We need this because there is no concept of ingestor or dispenser in
+            // rmf_site yet. Remove when there is
+            for model in level.models.values() {
+                if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) {
+                    world.include.push(SdfWorldInclude {
+                        uri: "model://TeleportIngestor".to_string(),
+                        name: Some(model.name.0.clone()),
+                        pose: Some(model.pose.to_sdf(0.0)),
+                        ..Default::default()
+                    });
+                }
+                else if model.source == AssetSource::Search("OpenRobotics/TeleportDispenser".to_string()) {
+                    world.include.push(SdfWorldInclude {
+                        uri: "model://TeleportDispenser".to_string(),
+                        name: Some(model.name.0.clone()),
+                        pose: Some(model.pose.to_sdf(0.0)),
+                        ..Default::default()
+                    });
+                }
+            }
             // Now add all the doors
             for door in level.doors.values() {
                 // TODO(luca) doors into toggle floors
@@ -776,13 +796,17 @@ impl Site {
                 .find(|plugin| plugin.filename == "MinimalScene")
             {
                 if let Some(camera_pose) = minimal_scene.elements.get_mut("camera_pose") {
-                    if let Some(default_camera) = self
+                    if let Some(pose) = self
                         .levels
                         .first_key_value()
-                        .and_then(|(_, level)| level.properties.camera_poses.0.iter().next())
+                        .and_then(|(_, level)| level.properties.camera_poses.0.values().next())
                     {
-                        // TODO(luca) use level elevation here?
-                        camera_pose.data = ElementData::String(default_camera.1.to_sdf(0.0).data);
+                        // TODO(luca) use level elevation here? It also seems that quaternion
+                        // notation in Gazebo and Bevy is different, check
+                        let mut pose = pose.clone();
+                        pose.rot = Rotation::EulerExtrinsicXYZ([Angle::Rad(0.0), Angle::Rad(0.6), Angle::Rad(1.57)]);
+                        pose.trans[0] = pose.trans[0] + 10.0;
+                        camera_pose.data = ElementData::String(pose.to_sdf(0.0).data);
                     }
                 }
             }

From 31a66e650809ef7ad459ff66c0b00896fb06ea23 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 19 Mar 2024 16:22:03 +0800
Subject: [PATCH 45/71] Add non static meshes export

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 33 ++++++++++++++++++++----
 rmf_site_format/src/sdf.rs               | 26 ++++++++++++++++---
 2 files changed, 50 insertions(+), 9 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index fa6f90c3..f47dbf07 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -10,7 +10,7 @@ use crate::site::{
     VisualMeshMarker,
 };
 use rmf_site_format::{
-    LevelElevation, LiftCabin, ModelMarker, NameInSite, Pose, SiteID, WallMarker,
+    IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, Pose, SiteID, WallMarker,
 };
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
@@ -20,7 +20,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Query<Entity, With<WallMarker>>,
         Query<&FloorSegments>,
         Query<(Option<&NameInSite>, &DoorSegments)>,
-        Query<Entity, With<ModelMarker>>,
+        Query<(Entity, &IsStatic, &NameInSite), With<ModelMarker>>,
         Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
         Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
@@ -119,7 +119,9 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                         material: Some(material),
                         pose: Some(level_pose.clone()),
                     });
-                } else if let Ok(model) = q_models.get(*child) {
+                } else if let Ok((model, is_static, name)) = q_models.get(*child) {
+                    let mut model_collisions = vec![];
+                    let mut model_visuals = vec![];
                     // TODO(luca) don't do full descendant iter here or we might add twice?
                     // Iterate through children and select all meshes
                     for model_child in DescendantIter::new(&q_children, model) {
@@ -142,7 +144,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                     rotation: tf.rotation.to_array(),
                                     scale: Some(tf.scale.to_array()),
                                 };
-                                collision_data.push(MeshData {
+                                model_collisions.push(MeshData {
                                     mesh,
                                     material: None,
                                     pose: Some(pose),
@@ -167,7 +169,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                     rotation: tf.rotation.to_array(),
                                     scale: Some(tf.scale.to_array()),
                                 };
-                                visual_data.push(MeshData {
+                                model_visuals.push(MeshData {
                                     mesh,
                                     material: Some(material),
                                     pose: Some(pose),
@@ -175,6 +177,27 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             }
                         }
                     }
+                    if **is_static {
+                        // This is part of the static world, add it to the static mesh
+                        collision_data.extend(model_collisions);
+                        visual_data.extend(model_visuals);
+                    } else {
+                        // Create a new mesh for it
+                        let filename = format!("{}/{}_collision.glb", folder.display(), **name);
+                        write_meshes_to_file(
+                            model_collisions,
+                            None,
+                            CompressGltfOptions::skip_materials(),
+                            filename,
+                        );
+                        let filename = format!("{}/{}_visual.glb", folder.display(), **name);
+                        write_meshes_to_file(
+                            model_visuals,
+                            Some(format!("{}_visual", **name)),
+                            CompressGltfOptions::default(),
+                            filename,
+                        );
+                    }
                 } else if let Ok((door_name, segments)) = q_doors.get(*child) {
                     for (entity, segment_name) in segments
                         .body
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 7132eae9..fc3c2ccf 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -494,15 +494,17 @@ impl Site {
             // TODO(luca) We need this because there is no concept of ingestor or dispenser in
             // rmf_site yet. Remove when there is
             for model in level.models.values() {
-                if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string()) {
+                if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string())
+                {
                     world.include.push(SdfWorldInclude {
                         uri: "model://TeleportIngestor".to_string(),
                         name: Some(model.name.0.clone()),
                         pose: Some(model.pose.to_sdf(0.0)),
                         ..Default::default()
                     });
-                }
-                else if model.source == AssetSource::Search("OpenRobotics/TeleportDispenser".to_string()) {
+                } else if model.source
+                    == AssetSource::Search("OpenRobotics/TeleportDispenser".to_string())
+                {
                     world.include.push(SdfWorldInclude {
                         uri: "model://TeleportDispenser".to_string(),
                         name: Some(model.name.0.clone()),
@@ -510,6 +512,18 @@ impl Site {
                         ..Default::default()
                     });
                 }
+                // Non static models are included separately and are not part of the static world
+                // TODO(luca) this will duplicate multiple instances of the model since it uses
+                // NameInSite instead of AssetSource for the URI, fix
+                else if !model.is_static.0 {
+                    world.include.push(SdfWorldInclude {
+                        uri: format!("model://{}", model.name.0.clone()),
+                        name: Some(model.name.0.clone()),
+                        pose: Some(model.pose.to_sdf(0.0)),
+                        r#static: Some(model.is_static.0),
+                        ..Default::default()
+                    });
+                }
             }
             // Now add all the doors
             for door in level.doors.values() {
@@ -804,7 +818,11 @@ impl Site {
                         // TODO(luca) use level elevation here? It also seems that quaternion
                         // notation in Gazebo and Bevy is different, check
                         let mut pose = pose.clone();
-                        pose.rot = Rotation::EulerExtrinsicXYZ([Angle::Rad(0.0), Angle::Rad(0.6), Angle::Rad(1.57)]);
+                        pose.rot = Rotation::EulerExtrinsicXYZ([
+                            Angle::Rad(0.0),
+                            Angle::Rad(0.6),
+                            Angle::Rad(1.57),
+                        ]);
                         pose.trans[0] = pose.trans[0] + 10.0;
                         camera_pose.data = ElementData::String(pose.to_sdf(0.0).data);
                     }

From c65e1e0879a772fbd2237d13d57115990ce583c6 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 19 Mar 2024 17:32:35 +0800
Subject: [PATCH 46/71] Clear simple warnings

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/door.rs         | 11 ++++++-----
 rmf_site_editor/src/site/file_menu.rs    |  3 +--
 rmf_site_editor/src/site/save.rs         |  4 +---
 rmf_site_editor/src/site/sdf_exporter.rs | 15 ++++++---------
 rmf_site_format/src/sdf.rs               | 12 +++++-------
 5 files changed, 19 insertions(+), 26 deletions(-)

diff --git a/rmf_site_editor/src/site/door.rs b/rmf_site_editor/src/site/door.rs
index b0ca773e..41457cc1 100644
--- a/rmf_site_editor/src/site/door.rs
+++ b/rmf_site_editor/src/site/door.rs
@@ -75,13 +75,14 @@ impl DoorBodyType {
 
     pub fn labels(&self) -> Vec<&str> {
         match self {
-            DoorBodyType::SingleSwing { body }
-            | DoorBodyType::SingleSliding { body }
-            | DoorBodyType::Model { body } => {
+            DoorBodyType::SingleSwing { .. }
+            | DoorBodyType::SingleSliding { .. }
+            | DoorBodyType::Model { .. } => {
                 vec!["body"]
             }
-            DoorBodyType::DoubleSwing { left, right }
-            | DoorBodyType::DoubleSliding { left, right } => vec!["left", "right"],
+            DoorBodyType::DoubleSwing { .. } | DoorBodyType::DoubleSliding { .. } => {
+                vec!["left", "right"]
+            }
         }
     }
 }
diff --git a/rmf_site_editor/src/site/file_menu.rs b/rmf_site_editor/src/site/file_menu.rs
index 1e7e7800..f067c284 100644
--- a/rmf_site_editor/src/site/file_menu.rs
+++ b/rmf_site_editor/src/site/file_menu.rs
@@ -47,12 +47,11 @@ impl FromWorld for ExportSdfMenu {
 }
 
 pub fn handle_export_sdf_menu_events(
-    mut commands: Commands,
     mut menu_events: EventReader<MenuEvent>,
     sdf_menu: Res<ExportSdfMenu>,
     mut save_events: EventWriter<SaveWorkspace>,
 ) {
-    for event in menu_events.iter() {
+    for event in menu_events.read() {
         if event.clicked() && event.source() == sdf_menu.export_sdf {
             save_events.send(SaveWorkspace {
                 destination: SaveWorkspaceDestination::Dialog,
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index d4809a40..94f17389 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -21,7 +21,6 @@ use bevy::{
 };
 use std::{
     collections::{BTreeMap, BTreeSet},
-    io::Write,
     path::PathBuf,
 };
 use thiserror::Error as ThisError;
@@ -1275,7 +1274,6 @@ pub fn save_site(world: &mut World) {
                 std::fs::create_dir(&meshes_dir).ok();
                 collect_site_meshes(world, save_event.site, &meshes_dir);
 
-                let old_default_path = world.get::<DefaultFile>(save_event.site).cloned();
                 migrate_relative_paths(save_event.site, &new_path, world);
 
                 let site = match generate_site(world, save_event.site) {
@@ -1300,7 +1298,7 @@ pub fn save_site(world: &mut World) {
                     write_document_declaration: true,
                     ..Default::default()
                 };
-                let s = yaserde::ser::serialize_with_writer(&sdf, f, &config).unwrap();
+                yaserde::ser::serialize_with_writer(&sdf, f, &config).unwrap();
                 let mut navgraph_dir = PathBuf::from(new_path.parent().unwrap());
                 navgraph_dir.push("nav_graphs");
                 std::fs::create_dir(&navgraph_dir).ok();
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index f47dbf07..63cd2053 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -2,16 +2,13 @@ use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
 use bevy_gltf_export::{export_meshes, CompressGltfOptions, GltfPose, MeshData, MeshExportError};
 
-use std::collections::BTreeMap;
 use std::path::Path;
 
 use crate::site::{
     ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, FloorSegments, LiftDoormat,
     VisualMeshMarker,
 };
-use rmf_site_format::{
-    IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, Pose, SiteID, WallMarker,
-};
+use rmf_site_format::{IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, WallMarker};
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
     let mut state: SystemState<(
@@ -21,8 +18,8 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
         Query<&FloorSegments>,
         Query<(Option<&NameInSite>, &DoorSegments)>,
         Query<(Entity, &IsStatic, &NameInSite), With<ModelMarker>>,
-        Query<(Entity, &GlobalTransform), With<CollisionMeshMarker>>,
-        Query<(Entity, &GlobalTransform), With<VisualMeshMarker>>,
+        Query<(), With<CollisionMeshMarker>>,
+        Query<(), With<VisualMeshMarker>>,
         Query<(&Handle<Mesh>, &Handle<StandardMaterial>)>,
         Query<(&NameInSite, &LiftCabin<Entity>, &ChildLiftCabinGroup)>,
         Query<((), With<LiftDoormat>)>,
@@ -125,10 +122,10 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                     // TODO(luca) don't do full descendant iter here or we might add twice?
                     // Iterate through children and select all meshes
                     for model_child in DescendantIter::new(&q_children, model) {
-                        if let Ok((entity, tf)) = q_collisions.get(model_child) {
+                        if q_collisions.get(model_child).is_ok() {
                             // Now iterate through the children of the collision and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
-                                let Some((mesh, material)) = get_mesh_and_material(entity) else {
+                                let Some((mesh, _)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
                                 let Ok(tf) = q_global_tfs.get(entity) else {
@@ -150,7 +147,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                     pose: Some(pose),
                                 });
                             }
-                        } else if let Ok((entity, tf)) = q_visuals.get(model_child) {
+                        } else if q_visuals.get(model_child).is_ok() {
                             // Now iterate through the children of the visuals and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
                                 let Some((mesh, material)) = get_mesh_and_material(entity) else {
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index fc3c2ccf..ff476467 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -16,8 +16,8 @@
 */
 
 use crate::{
-    Anchor, Angle, AssetSource, Category, Door, DoorMarker, DoorType, Level, LiftCabin, NameInSite,
-    Pose, Rotation, Side, Site, Swing,
+    Anchor, Angle, AssetSource, Category, Door, DoorMarker, DoorType, LiftCabin, NameInSite, Pose,
+    Rotation, Site, Swing,
 };
 use glam::Vec3;
 use once_cell::sync::Lazy;
@@ -114,7 +114,6 @@ impl Door<u32> {
         ros_interface: bool,
         name_override: Option<String>,
     ) -> Result<SdfModel, SdfConversionError> {
-        let door_mass = 50.0;
         let left_trans = left_anchor.translation_for_category(Category::Door);
         let right_trans = right_anchor.translation_for_category(Category::Door);
         let center = [
@@ -224,7 +223,7 @@ impl Door<u32> {
                     Swing::Forward(angle) => (angle.radians() as f64, side),
                     Swing::Backward(angle) => (angle.radians() as f64, -side),
                     // Only use the forward position for double doors
-                    Swing::Both { forward, backward } => (forward.radians() as f64, side),
+                    Swing::Both { forward, .. } => (forward.radians() as f64, side),
                 };
                 let lower = 0.0;
                 let upper = open.abs();
@@ -342,9 +341,8 @@ impl Door<u32> {
                     Swing::Forward(angle) => (angle.radians() as f64, -1.0),
                     Swing::Backward(angle) => (angle.radians() as f64, 1.0),
                     // Only use the forward position for double doors
-                    Swing::Both { forward, backward } => (forward.radians() as f64, -1.0),
+                    Swing::Both { forward, .. } => (forward.radians() as f64, -1.0),
                 };
-                let lower = 0.0;
                 let upper = open.abs();
                 let right_pose = Pose {
                     trans: [0.0, -door_length / 2.0, 1.25],
@@ -684,7 +682,7 @@ impl Site {
                     )?;
                     // Add the pose of the lift to have world coordinates
                     world.model.push(dummy_shaft);
-                    let mut level = levels.entry(*visit).or_default();
+                    let level = levels.entry(*visit).or_default();
                     let element = XmlElement {
                         name: "door_pair".into(),
                         attributes: [

From af648f16679aa0d1459a16d1bb0600b0e56d39d1 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 20 Mar 2024 10:25:10 +0800
Subject: [PATCH 47/71] Propagate errors when exporting SDF meshes

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/save.rs         |  4 ++-
 rmf_site_editor/src/site/sdf_exporter.rs | 32 +++++++++++-------------
 2 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 94f17389..5226e9ea 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1272,7 +1272,9 @@ pub fn save_site(world: &mut World) {
                 let mut meshes_dir = PathBuf::from(new_path.parent().unwrap());
                 meshes_dir.push("meshes");
                 std::fs::create_dir(&meshes_dir).ok();
-                collect_site_meshes(world, save_event.site, &meshes_dir);
+                if let Err(e) = collect_site_meshes(world, save_event.site, &meshes_dir) {
+                    error!("Unable to collect site meshes {e}");
+                }
 
                 migrate_relative_paths(save_event.site, &new_path, world);
 
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 63cd2053..749c18e0 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -10,7 +10,7 @@ use crate::site::{
 };
 use rmf_site_format::{IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, WallMarker};
 
-pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
+pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Result<(), String> {
     let mut state: SystemState<(
         Query<&Children>,
         Query<(&NameInSite, &LevelElevation, &Children)>,
@@ -49,16 +49,11 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                                 name: Option<String>,
                                 options: CompressGltfOptions,
                                 filename: String|
-     -> Result<(), MeshExportError> {
+     -> Result<(), String> {
         let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
-        let meshes = export_meshes(meshes, name, image_getter, options)?;
-        let bytes = meshes.to_bytes()?;
-        let Ok(_) = std::fs::write(filename, bytes) else {
-            // TODO(luca) make this an error
-            error!("Error writing mesh to file");
-            return Ok(());
-        };
-        Ok(())
+        let meshes = export_meshes(meshes, name, image_getter, options).map_err(|e| e.to_string())?;
+        let bytes = meshes.to_bytes().map_err(|e| e.to_string())?;
+        std::fs::write(filename, bytes).map_err(|e| e.to_string())
     };
 
     let get_mesh_and_material = |entity: Entity| -> Option<(&Mesh, &StandardMaterial)> {
@@ -77,7 +72,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
     };
 
     let Ok(site_children) = q_children.get(site) else {
-        return;
+        return Ok(());
     };
     for site_child in site_children.iter() {
         let mut collision_data = Vec::new();
@@ -186,14 +181,14 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             None,
                             CompressGltfOptions::skip_materials(),
                             filename,
-                        );
+                        )?;
                         let filename = format!("{}/{}_visual.glb", folder.display(), **name);
                         write_meshes_to_file(
                             model_visuals,
                             Some(format!("{}_visual", **name)),
                             CompressGltfOptions::default(),
                             filename,
-                        );
+                        )?;
                     }
                 } else if let Ok((door_name, segments)) = q_doors.get(*child) {
                     for (entity, segment_name) in segments
@@ -230,7 +225,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             None,
                             CompressGltfOptions::default(),
                             filename,
-                        );
+                        )?;
                     }
                 } else {
                     continue;
@@ -242,14 +237,14 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                 None,
                 CompressGltfOptions::skip_materials(),
                 filename,
-            );
+            )?;
             let filename = format!("{}/level_{}_visual.glb", folder.display(), **level_name);
             write_meshes_to_file(
                 visual_data,
                 Some(format!("level_{}_visuals", **level_name)),
                 CompressGltfOptions::default(),
                 filename,
-            );
+            )?;
         }
         // Lifts
         if let Ok((lift_name, cabin, cabin_children)) = q_lift_cabins.get(*site_child) {
@@ -272,7 +267,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                 });
             }
             let filename = format!("{}/{}.glb", folder.display(), **lift_name);
-            write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename);
+            write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename)?;
             // Now generate the lift doors
             let LiftCabin::Rect(cabin) = cabin;
             for (face, door) in cabin.doors().iter() {
@@ -319,10 +314,11 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) {
                             None,
                             CompressGltfOptions::default(),
                             filename,
-                        );
+                        )?;
                     }
                 }
             }
         }
     }
+    Ok(())
 }

From 87ddccebc77f9d74f453479f15da0d92844c7d45 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 20 Mar 2024 17:01:33 +0800
Subject: [PATCH 48/71] Add remaining models to level transparency plugin

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs |  3 +-
 rmf_site_format/src/sdf.rs               | 39 +++++++++++++++++++++++-
 2 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 749c18e0..da981a1c 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -51,7 +51,8 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                 filename: String|
      -> Result<(), String> {
         let image_getter = |id: &Handle<Image>| image_assets.get(id).cloned();
-        let meshes = export_meshes(meshes, name, image_getter, options).map_err(|e| e.to_string())?;
+        let meshes =
+            export_meshes(meshes, name, image_getter, options).map_err(|e| e.to_string())?;
         let bytes = meshes.to_bytes().map_err(|e| e.to_string())?;
         std::fs::write(filename, bytes).map_err(|e| e.to_string())
     };
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index ff476467..4d58ce44 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -443,6 +443,8 @@ impl Site {
             ..Default::default()
         };
         for level in self.levels.values() {
+            let mut level_model_names = vec![];
+            let mut model_element_map = ElementMap::default();
             max_elevation = max_elevation.max(level.properties.elevation.0);
             min_elevation = min_elevation.min(level.properties.elevation.0);
             let mut floor_models_ele = XmlElement {
@@ -492,6 +494,7 @@ impl Site {
             // TODO(luca) We need this because there is no concept of ingestor or dispenser in
             // rmf_site yet. Remove when there is
             for model in level.models.values() {
+                let mut added = false;
                 if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string())
                 {
                     world.include.push(SdfWorldInclude {
@@ -500,6 +503,7 @@ impl Site {
                         pose: Some(model.pose.to_sdf(0.0)),
                         ..Default::default()
                     });
+                    added = true;
                 } else if model.source
                     == AssetSource::Search("OpenRobotics/TeleportDispenser".to_string())
                 {
@@ -509,6 +513,7 @@ impl Site {
                         pose: Some(model.pose.to_sdf(0.0)),
                         ..Default::default()
                     });
+                    added = true;
                 }
                 // Non static models are included separately and are not part of the static world
                 // TODO(luca) this will duplicate multiple instances of the model since it uses
@@ -521,6 +526,10 @@ impl Site {
                         r#static: Some(model.is_static.0),
                         ..Default::default()
                     });
+                    added = true;
+                }
+                if added {
+                    level_model_names.push(model.name.0.clone());
                 }
             }
             // Now add all the doors
@@ -535,7 +544,17 @@ impl Site {
                     true,
                     None,
                 )?);
+                level_model_names.push(door.name.0.clone());
+            }
+            for model_name in level_model_names.into_iter() {
+                let model_element = XmlElement {
+                    name: "model".into(),
+                    attributes: [("name".into(), model_name.into())].into(),
+                    ..Default::default()
+                };
+                model_element_map.push(model_element);
             }
+            floor_models_ele.data = ElementData::Nested(model_element_map);
             toggle_floors_plugin.elements.push(floor_models_ele);
         }
         for lift in self.lifts.values() {
@@ -682,12 +701,30 @@ impl Site {
                     )?;
                     // Add the pose of the lift to have world coordinates
                     world.model.push(dummy_shaft);
+                    // Add the shaft door to the level transparency plugin
+                    if let Some(ElementData::Nested(ref mut map)) = toggle_floors_plugin
+                        .elements
+                        .get_all_mut("floor")
+                        .and_then(|mut elems| {
+                            elems
+                                .find(|el| {
+                                    el.attributes.get("name") == Some(&level.properties.name.0)
+                                })
+                                .map(|e| &mut e.data)
+                        })
+                    {
+                        map.push(XmlElement {
+                            name: "model".into(),
+                            attributes: [("name".into(), shaft_door_name.clone())].into(),
+                            ..Default::default()
+                        });
+                    };
                     let level = levels.entry(*visit).or_default();
                     let element = XmlElement {
                         name: "door_pair".into(),
                         attributes: [
                             ("cabin_door".to_string(), cabin_door_name.clone()),
-                            ("shaft_door".to_string(), shaft_door_name.clone()),
+                            ("shaft_door".to_string(), shaft_door_name),
                         ]
                         .into(),
                         ..Default::default()

From 22ccab34d3a2e13b836bdaa6bbdbeff9d4305b2f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 11:14:36 +0800
Subject: [PATCH 49/71] Fix embedded asset path panic

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/assets.rs | 18 ++++++--
 rmf_site_editor/src/widgets/mod.rs | 71 +++++++++++++++++++++---------
 2 files changed, 65 insertions(+), 24 deletions(-)

diff --git a/rmf_site_editor/src/site/assets.rs b/rmf_site_editor/src/site/assets.rs
index 9197e9fc..6fde89fe 100644
--- a/rmf_site_editor/src/site/assets.rs
+++ b/rmf_site_editor/src/site/assets.rs
@@ -19,9 +19,21 @@ use crate::{shapes::*, site::*};
 use bevy::{asset::embedded_asset, math::Affine3A, prelude::*};
 
 pub(crate) fn add_site_icons(app: &mut App) {
-    embedded_asset!(app, "icons/battery.png");
-    embedded_asset!(app, "icons/parking.png");
-    embedded_asset!(app, "icons/stopwatch.png");
+    // Taken from https://github.com/bevyengine/bevy/issues/10377#issuecomment-1858797002
+    // TODO(luca) remove once we migrate to Bevy 0.13 that includes the fix
+    #[cfg(any(not(target_family = "windows"), target_env = "gnu"))]
+    {
+        embedded_asset!(app, "src/", "icons/battery.png");
+        embedded_asset!(app, "src/", "icons/parking.png");
+        embedded_asset!(app, "src/", "icons/stopwatch.png");
+    }
+
+    #[cfg(all(target_family = "windows", not(target_env = "gnu")))]
+    {
+        embedded_asset!(app, "src\\", "icons\\battery.png");
+        embedded_asset!(app, "src\\", "icons\\parking.png");
+        embedded_asset!(app, "src\\", "icons\\stopwatch.png");
+    }
 }
 
 #[derive(Resource)]
diff --git a/rmf_site_editor/src/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs
index 70dd3ea8..672097a5 100644
--- a/rmf_site_editor/src/widgets/mod.rs
+++ b/rmf_site_editor/src/widgets/mod.rs
@@ -98,27 +98,56 @@ pub struct PendingModel {
 pub struct StandardUiLayout;
 
 fn add_widgets_icons(app: &mut App) {
-    embedded_asset!(app, "icons/add.png");
-    embedded_asset!(app, "icons/alignment.png");
-    embedded_asset!(app, "icons/alpha.png");
-    embedded_asset!(app, "icons/confirm.png");
-    embedded_asset!(app, "icons/down.png");
-    embedded_asset!(app, "icons/edit.png");
-    embedded_asset!(app, "icons/empty.png");
-    embedded_asset!(app, "icons/exit.png");
-    embedded_asset!(app, "icons/global.png");
-    embedded_asset!(app, "icons/hidden.png");
-    embedded_asset!(app, "icons/hide.png");
-    embedded_asset!(app, "icons/merge.png");
-    embedded_asset!(app, "icons/opaque.png");
-    embedded_asset!(app, "icons/reject.png");
-    embedded_asset!(app, "icons/search.png");
-    embedded_asset!(app, "icons/select.png");
-    embedded_asset!(app, "icons/selected.png");
-    embedded_asset!(app, "icons/to_bottom.png");
-    embedded_asset!(app, "icons/to_top.png");
-    embedded_asset!(app, "icons/trash.png");
-    embedded_asset!(app, "icons/up.png");
+    // Taken from https://github.com/bevyengine/bevy/issues/10377#issuecomment-1858797002
+    // TODO(luca) remove once we migrate to Bevy 0.13 that includes the fix
+    #[cfg(any(not(target_family = "windows"), target_env = "gnu"))]
+    {
+        embedded_asset!(app, "src/", "icons/add.png");
+        embedded_asset!(app, "src/", "icons/alignment.png");
+        embedded_asset!(app, "src/", "icons/alpha.png");
+        embedded_asset!(app, "src/", "icons/confirm.png");
+        embedded_asset!(app, "src/", "icons/down.png");
+        embedded_asset!(app, "src/", "icons/edit.png");
+        embedded_asset!(app, "src/", "icons/empty.png");
+        embedded_asset!(app, "src/", "icons/exit.png");
+        embedded_asset!(app, "src/", "icons/global.png");
+        embedded_asset!(app, "src/", "icons/hidden.png");
+        embedded_asset!(app, "src/", "icons/hide.png");
+        embedded_asset!(app, "src/", "icons/merge.png");
+        embedded_asset!(app, "src/", "icons/opaque.png");
+        embedded_asset!(app, "src/", "icons/reject.png");
+        embedded_asset!(app, "src/", "icons/search.png");
+        embedded_asset!(app, "src/", "icons/select.png");
+        embedded_asset!(app, "src/", "icons/selected.png");
+        embedded_asset!(app, "src/", "icons/to_bottom.png");
+        embedded_asset!(app, "src/", "icons/to_top.png");
+        embedded_asset!(app, "src/", "icons/trash.png");
+        embedded_asset!(app, "src/", "icons/up.png");
+    }
+    #[cfg(all(target_family = "windows", not(target_env = "gnu")))]
+    {
+        embedded_asset!(app, "src\\", "icons\\add.png");
+        embedded_asset!(app, "src\\", "icons\\alignment.png");
+        embedded_asset!(app, "src\\", "icons\\alpha.png");
+        embedded_asset!(app, "src\\", "icons\\confirm.png");
+        embedded_asset!(app, "src\\", "icons\\down.png");
+        embedded_asset!(app, "src\\", "icons\\edit.png");
+        embedded_asset!(app, "src\\", "icons\\empty.png");
+        embedded_asset!(app, "src\\", "icons\\exit.png");
+        embedded_asset!(app, "src\\", "icons\\global.png");
+        embedded_asset!(app, "src\\", "icons\\hidden.png");
+        embedded_asset!(app, "src\\", "icons\\hide.png");
+        embedded_asset!(app, "src\\", "icons\\merge.png");
+        embedded_asset!(app, "src\\", "icons\\opaque.png");
+        embedded_asset!(app, "src\\", "icons\\reject.png");
+        embedded_asset!(app, "src\\", "icons\\search.png");
+        embedded_asset!(app, "src\\", "icons\\select.png");
+        embedded_asset!(app, "src\\", "icons\\selected.png");
+        embedded_asset!(app, "src\\", "icons\\to_bottom.png");
+        embedded_asset!(app, "src\\", "icons\\to_top.png");
+        embedded_asset!(app, "src\\", "icons\\trash.png");
+        embedded_asset!(app, "src\\", "icons\\up.png");
+    }
 }
 
 impl Plugin for StandardUiLayout {

From 80d69068c7f23850c89c0a7f4e07b87eaf67a39d Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 14:53:57 +0800
Subject: [PATCH 50/71] Move to for_each_mut API

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 28 +++++++++++-----------------
 1 file changed, 11 insertions(+), 17 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 4d58ce44..afa51b01 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -702,23 +702,17 @@ impl Site {
                     // Add the pose of the lift to have world coordinates
                     world.model.push(dummy_shaft);
                     // Add the shaft door to the level transparency plugin
-                    if let Some(ElementData::Nested(ref mut map)) = toggle_floors_plugin
-                        .elements
-                        .get_all_mut("floor")
-                        .and_then(|mut elems| {
-                            elems
-                                .find(|el| {
-                                    el.attributes.get("name") == Some(&level.properties.name.0)
-                                })
-                                .map(|e| &mut e.data)
-                        })
-                    {
-                        map.push(XmlElement {
-                            name: "model".into(),
-                            attributes: [("name".into(), shaft_door_name.clone())].into(),
-                            ..Default::default()
-                        });
-                    };
+                    toggle_floors_plugin.elements.for_each_mut("floor", |elem| {
+                        if elem.attributes.get("name") == Some(&level.properties.name.0) {
+                            if let ElementData::Nested(ref mut map) = elem.data {
+                                map.push(XmlElement {
+                                    name: "model".into(),
+                                    attributes: [("name".into(), shaft_door_name.clone())].into(),
+                                    ..Default::default()
+                                });
+                            }
+                        }
+                    });
                     let level = levels.entry(*visit).or_default();
                     let element = XmlElement {
                         name: "door_pair".into(),

From 8d97dacb1a4b3dfc3c4991e702dcdec6de42a9bc Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 15:12:01 +0800
Subject: [PATCH 51/71] Update sdformat_rs dependency

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml | 2 +-
 rmf_site_format/Cargo.toml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index 1ad55f9b..b1f92b45 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -46,7 +46,7 @@ rfd = "0.12"
 urdf-rs = "0.7"
 yaserde = "0.7"
 utm = "0.1.6"
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "48ebcad"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "9fc35f2"}
 gz-fuel = { git = "https://github.com/open-rmf/gz-fuel-rs", branch = "luca/ehttp" }
 pathdiff = "*"
 tera = "1.19.1"
diff --git a/rmf_site_format/Cargo.toml b/rmf_site_format/Cargo.toml
index d23431a5..0de63d22 100644
--- a/rmf_site_format/Cargo.toml
+++ b/rmf_site_format/Cargo.toml
@@ -18,7 +18,7 @@ uuid = { version = "1.1", features = ["v4", "serde"] }
 # add features=["bevy"] to a dependent Cargo.toml to get the bevy-related features
 # We depend on a bugfix released specifically in 0.7.3
 bevy = { version = "0.12", optional = true }
-sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "48ebcad"}
+sdformat_rs = { git = "https://github.com/open-rmf/sdf_rust_experimental", rev = "9fc35f2"}
 yaserde = "0.7"
 urdf-rs = "0.7.3"
 # Used for lazy initialization of static variable when they are non const

From f2933a401d37ed990c313ec6899c560793b2367c Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 15:27:45 +0800
Subject: [PATCH 52/71] Minor refactor for shared code

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 45 ++++++++++++--------------------------
 1 file changed, 14 insertions(+), 31 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index afa51b01..c431c356 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -558,6 +558,13 @@ impl Site {
             toggle_floors_plugin.elements.push(floor_models_ele);
         }
         for lift in self.lifts.values() {
+            let get_lift_anchor = |id: u32| -> Result<Anchor, SdfConversionError> {
+                lift
+                    .cabin_anchors
+                    .get(&id)
+                    .ok_or(SdfConversionError::BrokenAnchorReference(id))
+                    .cloned()
+            };
             // Cabin
             let LiftCabin::Rect(ref cabin) = lift.properties.cabin;
             let pose = lift
@@ -628,20 +635,8 @@ impl Site {
                     kind: door.kind.clone(),
                     marker: DoorMarker,
                 };
-                let left_anchor = lift
-                    .cabin_anchors
-                    .get(&door.reference_anchors.left())
-                    .ok_or(SdfConversionError::BrokenAnchorReference(
-                        door.reference_anchors.left(),
-                    ))?
-                    .clone();
-                let right_anchor = lift
-                    .cabin_anchors
-                    .get(&door.reference_anchors.right())
-                    .ok_or(SdfConversionError::BrokenAnchorReference(
-                        door.reference_anchors.right(),
-                    ))?
-                    .clone();
+                let left_anchor = get_lift_anchor(door.reference_anchors.left())?;
+                let right_anchor = get_lift_anchor(door.reference_anchors.right())?;
                 let x_offset = -face.u()
                     * (door_placement.thickness() / 2.0
                         + door_placement
@@ -672,27 +667,15 @@ impl Site {
                         lift_name,
                         face.label()
                     );
-                    let dummy_shaft = Door {
+                    let dummy_shaft_door = Door {
                         anchors: door.reference_anchors.clone(),
                         name: NameInSite(shaft_door_name.clone()),
                         kind: door.kind.clone(),
                         marker: DoorMarker,
                     };
-                    let left_anchor = lift
-                        .cabin_anchors
-                        .get(&door.reference_anchors.left())
-                        .ok_or(SdfConversionError::BrokenAnchorReference(
-                            door.reference_anchors.left(),
-                        ))?
-                        .clone();
-                    let right_anchor = lift
-                        .cabin_anchors
-                        .get(&door.reference_anchors.right())
-                        .ok_or(SdfConversionError::BrokenAnchorReference(
-                            door.reference_anchors.right(),
-                        ))?
-                        .clone();
-                    let dummy_shaft = dummy_shaft.to_sdf(
+                    let left_anchor = get_lift_anchor(door.reference_anchors.left())?;
+                    let right_anchor = get_lift_anchor(door.reference_anchors.right())?;
+                    let shaft_door = dummy_shaft_door.to_sdf(
                         left_anchor,
                         right_anchor,
                         Vec3::from(pose.trans) + Vec3::new(0.0, 0.0, level.properties.elevation.0),
@@ -700,7 +683,7 @@ impl Site {
                         Some(cabin_mesh_prefix.clone()),
                     )?;
                     // Add the pose of the lift to have world coordinates
-                    world.model.push(dummy_shaft);
+                    world.model.push(shaft_door);
                     // Add the shaft door to the level transparency plugin
                     toggle_floors_plugin.elements.for_each_mut("floor", |elem| {
                         if elem.attributes.get("name") == Some(&level.properties.name.0) {

From 264b36414f3c8e9d48500f2a45fc97f8e4f76e07 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 16:15:19 +0800
Subject: [PATCH 53/71] Further cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 635 ++++++++++++++++++-------------------
 1 file changed, 310 insertions(+), 325 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index c431c356..ab2261af 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -16,8 +16,7 @@
 */
 
 use crate::{
-    Anchor, Angle, AssetSource, Category, Door, DoorMarker, DoorType, LiftCabin, NameInSite, Pose,
-    Rotation, Site, Swing,
+    Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing,
 };
 use glam::Vec3;
 use once_cell::sync::Lazy;
@@ -105,325 +104,323 @@ fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink {
     }
 }
 
-impl Door<u32> {
-    pub fn to_sdf(
-        &self,
-        left_anchor: Anchor,
-        right_anchor: Anchor,
-        offset: Vec3,
-        ros_interface: bool,
-        name_override: Option<String>,
-    ) -> Result<SdfModel, SdfConversionError> {
-        let left_trans = left_anchor.translation_for_category(Category::Door);
-        let right_trans = right_anchor.translation_for_category(Category::Door);
-        let center = [
-            (left_trans[0] + right_trans[0]) / 2.0,
-            (left_trans[1] + right_trans[1]) / 2.0,
-        ];
-        let dx = left_trans[0] - right_trans[0];
-        let dy = left_trans[1] - right_trans[1];
-        let door_length = (dx * dx + dy * dy).sqrt();
-        let yaw = -dx.atan2(dy);
-        let prefix = name_override.clone().unwrap_or_default();
-        let labels = match self.kind {
-            DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
-                Vec::from(["body"])
+fn make_sdf_door(
+    left_anchor: Anchor,
+    right_anchor: Anchor,
+    offset: Vec3,
+    ros_interface: bool,
+    name_override: Option<&str>,
+    kind: &DoorType,
+    door_name: &str,
+) -> Result<SdfModel, SdfConversionError> {
+    let left_trans = left_anchor.translation_for_category(Category::Door);
+    let right_trans = right_anchor.translation_for_category(Category::Door);
+    let center = [
+        (left_trans[0] + right_trans[0]) / 2.0,
+        (left_trans[1] + right_trans[1]) / 2.0,
+    ];
+    let dx = left_trans[0] - right_trans[0];
+    let dy = left_trans[1] - right_trans[1];
+    let door_length = (dx * dx + dy * dy).sqrt();
+    let yaw = -dx.atan2(dy);
+    let prefix = name_override.unwrap_or_default().to_string();
+    let labels = match kind {
+        DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
+            Vec::from(["body"])
+        }
+        DoorType::DoubleSliding(_) | DoorType::DoubleSwing(_) => Vec::from(["right", "left"]),
+    };
+    let mut plugin = SdfPlugin {
+        name: "register_component".into(),
+        filename: "libregister_component.so".into(),
+        ..Default::default()
+    };
+    let mut component = XmlElement {
+        name: "component".into(),
+        ..Default::default()
+    };
+    let mut door_plugin_inner = XmlElement {
+        name: "door".into(),
+        ..Default::default()
+    };
+    component.attributes.insert("name".into(), "Door".into());
+    let mut component_data = ElementMap::default();
+    door_plugin_inner
+        .attributes
+        .insert("name".to_string(), door_name.to_string());
+    let mut door_model = SdfModel {
+        name: door_name.to_string(),
+        pose: Some(
+            Pose {
+                trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(),
+                rot: Rotation::Yaw(Angle::Rad(yaw)),
             }
-            DoorType::DoubleSliding(_) | DoorType::DoubleSwing(_) => Vec::from(["right", "left"]),
-        };
-        let mut plugin = SdfPlugin {
-            name: "register_component".into(),
-            filename: "libregister_component.so".into(),
-            ..Default::default()
-        };
-        let mut component = XmlElement {
-            name: "component".into(),
-            ..Default::default()
-        };
-        let mut door_plugin_inner = XmlElement {
-            name: "door".into(),
-            ..Default::default()
-        };
-        component.attributes.insert("name".into(), "Door".into());
-        let mut component_data = ElementMap::default();
-        door_plugin_inner
-            .attributes
-            .insert("name".to_string(), self.name.0.clone());
-        let mut door_model = SdfModel {
-            name: self.name.0.clone(),
-            pose: Some(
-                Pose {
-                    trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(),
-                    rot: Rotation::Yaw(Angle::Rad(yaw)),
-                }
-                .to_sdf(0.0),
-            ),
-            r#static: Some(false),
-            ..Default::default()
-        };
-        let link_name = if let Some(name_override) = name_override.as_ref() {
-            name_override
-        } else {
-            &self.name.0
-        };
-        for label in labels.iter() {
-            door_model.link.push(make_sdf_door_link(link_name, label));
+            .to_sdf(0.0),
+        ),
+        r#static: Some(false),
+        ..Default::default()
+    };
+    let link_name = if let Some(name_override) = name_override.as_ref() {
+        name_override
+    } else {
+        &door_name
+    };
+    for label in labels.iter() {
+        door_model.link.push(make_sdf_door_link(link_name, label));
+    }
+    let mut door_motion_params = vec![];
+    let joints = match kind {
+        DoorType::SingleSliding(door) => {
+            door_plugin_inner
+                .attributes
+                .insert("type".into(), "SlidingDoor".into());
+            door_plugin_inner
+                .attributes
+                .insert("left_joint_name".into(), "empty_joint".into());
+            door_plugin_inner
+                .attributes
+                .insert("right_joint_name".into(), prefix.clone() + "joint");
+            door_motion_params.push(("v_max_door", "0.2"));
+            door_motion_params.push(("a_max_door", "0.2"));
+            door_motion_params.push(("a_nom_door", "0.08"));
+            door_motion_params.push(("dx_min_door", "0.001"));
+            door_motion_params.push(("f_max_door", "100.0"));
+            let pose = Pose {
+                trans: [0.0, (door_length / 2.0) * door.towards.sign(), 1.25],
+                ..Default::default()
+            }
+            .to_sdf(0.0);
+            vec![SdfJoint {
+                name: prefix.clone() + "joint",
+                parent: "world".into(),
+                child: "body".into(),
+                r#type: "prismatic".into(),
+                pose: Some(pose),
+                axis: Some(SdfJointAxis {
+                    xyz: Vector3d::new(0.0, door.towards.sign().into(), 0.0),
+                    limit: SdfJointAxisLimit {
+                        lower: 0.0,
+                        upper: door_length as f64,
+                        ..Default::default()
+                    },
+                    ..Default::default()
+                }),
+                ..Default::default()
+            }]
         }
-        let mut door_motion_params = vec![];
-        let joints = match &self.kind {
-            DoorType::SingleSliding(door) => {
-                door_plugin_inner
-                    .attributes
-                    .insert("type".into(), "SlidingDoor".into());
-                door_plugin_inner
-                    .attributes
-                    .insert("left_joint_name".into(), "empty_joint".into());
-                door_plugin_inner
-                    .attributes
-                    .insert("right_joint_name".into(), prefix.clone() + "joint");
-                door_motion_params.push(("v_max_door", "0.2"));
-                door_motion_params.push(("a_max_door", "0.2"));
-                door_motion_params.push(("a_nom_door", "0.08"));
-                door_motion_params.push(("dx_min_door", "0.001"));
-                door_motion_params.push(("f_max_door", "100.0"));
-                let pose = Pose {
-                    trans: [0.0, (door_length / 2.0) * door.towards.sign(), 1.25],
+        DoorType::SingleSwing(door) => {
+            door_plugin_inner
+                .attributes
+                .insert("type".into(), "SwingDoor".into());
+            door_motion_params.push(("v_max_door", "0.5"));
+            door_motion_params.push(("a_max_door", "0.3"));
+            door_motion_params.push(("a_nom_door", "0.15"));
+            door_motion_params.push(("dx_min_door", "0.01"));
+            door_motion_params.push(("f_max_door", "500.0"));
+            let side = door.pivot_on.sign() as f64;
+            let (open, z) = match door.swing {
+                Swing::Forward(angle) => (angle.radians() as f64, side),
+                Swing::Backward(angle) => (angle.radians() as f64, -side),
+                // Only use the forward position for double doors
+                Swing::Both { forward, .. } => (forward.radians() as f64, side),
+            };
+            let lower = 0.0;
+            let upper = open.abs();
+            let pose = Pose {
+                trans: [0.0, (door_length / 2.0) * door.pivot_on.sign(), 1.25],
+                ..Default::default()
+            }
+            .to_sdf(0.0);
+            let (left_joint_name, right_joint_name) = ("empty_joint", prefix.clone() + "joint");
+            door_plugin_inner
+                .attributes
+                .insert("left_joint_name".into(), left_joint_name.into());
+            door_plugin_inner
+                .attributes
+                .insert("right_joint_name".into(), right_joint_name);
+            vec![SdfJoint {
+                name: prefix.clone() + "joint",
+                parent: "world".into(),
+                child: "body".into(),
+                r#type: "revolute".into(),
+                axis: Some(SdfJointAxis {
+                    xyz: Vector3d::new(0.0, 0.0, z),
+                    limit: SdfJointAxisLimit {
+                        lower,
+                        upper,
+                        ..Default::default()
+                    },
                     ..Default::default()
-                }
-                .to_sdf(0.0);
-                vec![SdfJoint {
-                    name: prefix.clone() + "joint",
+                }),
+                pose: Some(pose),
+                ..Default::default()
+            }]
+        }
+        DoorType::DoubleSliding(door) => {
+            door_plugin_inner
+                .attributes
+                .insert("type".into(), "DoubleSlidingDoor".into());
+            door_plugin_inner
+                .attributes
+                .insert("left_joint_name".into(), prefix.clone() + "left_joint");
+            door_plugin_inner
+                .attributes
+                .insert("right_joint_name".into(), prefix.clone() + "right_joint");
+            door_motion_params.push(("v_max_door", "0.2"));
+            door_motion_params.push(("a_max_door", "0.2"));
+            door_motion_params.push(("a_nom_door", "0.08"));
+            door_motion_params.push(("dx_min_door", "0.001"));
+            door_motion_params.push(("f_max_door", "100.0"));
+            let right_pose = Pose {
+                trans: [0.0, -door_length / 2.0, 1.25],
+                ..Default::default()
+            }
+            .to_sdf(0.0);
+            let left_pose = Pose {
+                trans: [0.0, door_length / 2.0, 1.25],
+                ..Default::default()
+            }
+            .to_sdf(0.0);
+            let left_length = (door.left_right_ratio / (1.0 + door.left_right_ratio)) * door_length;
+            let right_length = door_length - left_length;
+            vec![
+                SdfJoint {
+                    name: prefix.clone() + "right_joint",
                     parent: "world".into(),
-                    child: "body".into(),
+                    child: "right".into(),
                     r#type: "prismatic".into(),
-                    pose: Some(pose),
+                    pose: Some(right_pose),
                     axis: Some(SdfJointAxis {
-                        xyz: Vector3d::new(0.0, door.towards.sign().into(), 0.0),
+                        xyz: Vector3d::new(0.0, -1.0, 0.0),
                         limit: SdfJointAxisLimit {
                             lower: 0.0,
-                            upper: door_length as f64,
+                            upper: right_length as f64,
                             ..Default::default()
                         },
                         ..Default::default()
                     }),
                     ..Default::default()
-                }]
-            }
-            DoorType::SingleSwing(door) => {
-                door_plugin_inner
-                    .attributes
-                    .insert("type".into(), "SwingDoor".into());
-                door_motion_params.push(("v_max_door", "0.5"));
-                door_motion_params.push(("a_max_door", "0.3"));
-                door_motion_params.push(("a_nom_door", "0.15"));
-                door_motion_params.push(("dx_min_door", "0.01"));
-                door_motion_params.push(("f_max_door", "500.0"));
-                let side = door.pivot_on.sign() as f64;
-                let (open, z) = match door.swing {
-                    Swing::Forward(angle) => (angle.radians() as f64, side),
-                    Swing::Backward(angle) => (angle.radians() as f64, -side),
-                    // Only use the forward position for double doors
-                    Swing::Both { forward, .. } => (forward.radians() as f64, side),
-                };
-                let lower = 0.0;
-                let upper = open.abs();
-                let pose = Pose {
-                    trans: [0.0, (door_length / 2.0) * door.pivot_on.sign(), 1.25],
-                    ..Default::default()
-                }
-                .to_sdf(0.0);
-                let (left_joint_name, right_joint_name) = ("empty_joint", prefix.clone() + "joint");
-                door_plugin_inner
-                    .attributes
-                    .insert("left_joint_name".into(), left_joint_name.into());
-                door_plugin_inner
-                    .attributes
-                    .insert("right_joint_name".into(), right_joint_name);
-                vec![SdfJoint {
-                    name: prefix.clone() + "joint",
+                },
+                SdfJoint {
+                    name: prefix.clone() + "left_joint",
                     parent: "world".into(),
-                    child: "body".into(),
-                    r#type: "revolute".into(),
+                    child: "left".into(),
+                    r#type: "prismatic".into(),
+                    pose: Some(left_pose),
                     axis: Some(SdfJointAxis {
-                        xyz: Vector3d::new(0.0, 0.0, z),
+                        xyz: Vector3d::new(0.0, -1.0, 0.0),
                         limit: SdfJointAxisLimit {
-                            lower,
-                            upper,
+                            lower: -left_length as f64,
+                            upper: 0.0,
                             ..Default::default()
                         },
                         ..Default::default()
                     }),
-                    pose: Some(pose),
                     ..Default::default()
-                }]
+                },
+            ]
+        }
+        DoorType::DoubleSwing(door) => {
+            door_plugin_inner
+                .attributes
+                .insert("type".into(), "DoubleSwingDoor".into());
+            door_plugin_inner
+                .attributes
+                .insert("left_joint_name".into(), prefix.clone() + "left_joint");
+            door_plugin_inner
+                .attributes
+                .insert("right_joint_name".into(), prefix.clone() + "right_joint");
+            door_motion_params.push(("v_max_door", "0.5"));
+            door_motion_params.push(("a_max_door", "0.3"));
+            door_motion_params.push(("a_nom_door", "0.15"));
+            door_motion_params.push(("dx_min_door", "0.01"));
+            door_motion_params.push(("f_max_door", "500.0"));
+            let (open, z) = match door.swing {
+                Swing::Forward(angle) => (angle.radians() as f64, -1.0),
+                Swing::Backward(angle) => (angle.radians() as f64, 1.0),
+                // Only use the forward position for double doors
+                Swing::Both { forward, .. } => (forward.radians() as f64, -1.0),
+            };
+            let upper = open.abs();
+            let right_pose = Pose {
+                trans: [0.0, -door_length / 2.0, 1.25],
+                ..Default::default()
             }
-            DoorType::DoubleSliding(door) => {
-                door_plugin_inner
-                    .attributes
-                    .insert("type".into(), "DoubleSlidingDoor".into());
-                door_plugin_inner
-                    .attributes
-                    .insert("left_joint_name".into(), prefix.clone() + "left_joint");
-                door_plugin_inner
-                    .attributes
-                    .insert("right_joint_name".into(), prefix.clone() + "right_joint");
-                door_motion_params.push(("v_max_door", "0.2"));
-                door_motion_params.push(("a_max_door", "0.2"));
-                door_motion_params.push(("a_nom_door", "0.08"));
-                door_motion_params.push(("dx_min_door", "0.001"));
-                door_motion_params.push(("f_max_door", "100.0"));
-                let right_pose = Pose {
-                    trans: [0.0, -door_length / 2.0, 1.25],
-                    ..Default::default()
-                }
-                .to_sdf(0.0);
-                let left_pose = Pose {
-                    trans: [0.0, door_length / 2.0, 1.25],
-                    ..Default::default()
-                }
-                .to_sdf(0.0);
-                let left_length =
-                    (door.left_right_ratio / (1.0 + door.left_right_ratio)) * door_length;
-                let right_length = door_length - left_length;
-                vec![
-                    SdfJoint {
-                        name: prefix.clone() + "right_joint",
-                        parent: "world".into(),
-                        child: "right".into(),
-                        r#type: "prismatic".into(),
-                        pose: Some(right_pose),
-                        axis: Some(SdfJointAxis {
-                            xyz: Vector3d::new(0.0, -1.0, 0.0),
-                            limit: SdfJointAxisLimit {
-                                lower: 0.0,
-                                upper: right_length as f64,
-                                ..Default::default()
-                            },
-                            ..Default::default()
-                        }),
-                        ..Default::default()
-                    },
-                    SdfJoint {
-                        name: prefix.clone() + "left_joint",
-                        parent: "world".into(),
-                        child: "left".into(),
-                        r#type: "prismatic".into(),
-                        pose: Some(left_pose),
-                        axis: Some(SdfJointAxis {
-                            xyz: Vector3d::new(0.0, -1.0, 0.0),
-                            limit: SdfJointAxisLimit {
-                                lower: -left_length as f64,
-                                upper: 0.0,
-                                ..Default::default()
-                            },
-                            ..Default::default()
-                        }),
-                        ..Default::default()
-                    },
-                ]
+            .to_sdf(0.0);
+            let left_pose = Pose {
+                trans: [0.0, door_length / 2.0, 1.25],
+                ..Default::default()
             }
-            DoorType::DoubleSwing(door) => {
-                door_plugin_inner
-                    .attributes
-                    .insert("type".into(), "DoubleSwingDoor".into());
-                door_plugin_inner
-                    .attributes
-                    .insert("left_joint_name".into(), prefix.clone() + "left_joint");
-                door_plugin_inner
-                    .attributes
-                    .insert("right_joint_name".into(), prefix.clone() + "right_joint");
-                door_motion_params.push(("v_max_door", "0.5"));
-                door_motion_params.push(("a_max_door", "0.3"));
-                door_motion_params.push(("a_nom_door", "0.15"));
-                door_motion_params.push(("dx_min_door", "0.01"));
-                door_motion_params.push(("f_max_door", "500.0"));
-                let (open, z) = match door.swing {
-                    Swing::Forward(angle) => (angle.radians() as f64, -1.0),
-                    Swing::Backward(angle) => (angle.radians() as f64, 1.0),
-                    // Only use the forward position for double doors
-                    Swing::Both { forward, .. } => (forward.radians() as f64, -1.0),
-                };
-                let upper = open.abs();
-                let right_pose = Pose {
-                    trans: [0.0, -door_length / 2.0, 1.25],
-                    ..Default::default()
-                }
-                .to_sdf(0.0);
-                let left_pose = Pose {
-                    trans: [0.0, door_length / 2.0, 1.25],
-                    ..Default::default()
-                }
-                .to_sdf(0.0);
-                vec![
-                    SdfJoint {
-                        name: prefix.clone() + "right_joint",
-                        parent: "world".into(),
-                        child: "right".into(),
-                        r#type: "revolute".into(),
-                        axis: Some(SdfJointAxis {
-                            xyz: Vector3d::new(0.0, 0.0, z),
-                            limit: SdfJointAxisLimit {
-                                lower: 0.0,
-                                upper,
-                                ..Default::default()
-                            },
-                            ..Default::default()
-                        }),
-                        pose: Some(right_pose),
-                        ..Default::default()
-                    },
-                    SdfJoint {
-                        name: prefix.clone() + "left_joint",
-                        parent: "world".into(),
-                        child: "left".into(),
-                        r#type: "revolute".into(),
-                        axis: Some(SdfJointAxis {
-                            xyz: Vector3d::new(0.0, 0.0, z),
-                            limit: SdfJointAxisLimit {
-                                lower: -upper,
-                                upper: 0.0,
-                                ..Default::default()
-                            },
+            .to_sdf(0.0);
+            vec![
+                SdfJoint {
+                    name: prefix.clone() + "right_joint",
+                    parent: "world".into(),
+                    child: "right".into(),
+                    r#type: "revolute".into(),
+                    axis: Some(SdfJointAxis {
+                        xyz: Vector3d::new(0.0, 0.0, z),
+                        limit: SdfJointAxisLimit {
+                            lower: 0.0,
+                            upper,
                             ..Default::default()
-                        }),
-                        pose: Some(left_pose),
+                        },
                         ..Default::default()
-                    },
-                ]
-            }
-            DoorType::Model(_) => {
-                // Unimplemented! Use a fixed joint for now
-                let pose = Pose {
-                    trans: [0.0, door_length / 2.0, 1.25],
+                    }),
+                    pose: Some(right_pose),
                     ..Default::default()
-                }
-                .to_sdf(0.0);
-                vec![SdfJoint {
-                    name: prefix.clone() + "joint",
+                },
+                SdfJoint {
+                    name: prefix.clone() + "left_joint",
                     parent: "world".into(),
-                    child: "body".into(),
-                    r#type: "fixed".into(),
-                    pose: Some(pose),
+                    child: "left".into(),
+                    r#type: "revolute".into(),
+                    axis: Some(SdfJointAxis {
+                        xyz: Vector3d::new(0.0, 0.0, z),
+                        limit: SdfJointAxisLimit {
+                            lower: -upper,
+                            upper: 0.0,
+                            ..Default::default()
+                        },
+                        ..Default::default()
+                    }),
+                    pose: Some(left_pose),
                     ..Default::default()
-                }]
+                },
+            ]
+        }
+        DoorType::Model(_) => {
+            // Unimplemented! Use a fixed joint for now
+            let pose = Pose {
+                trans: [0.0, door_length / 2.0, 1.25],
+                ..Default::default()
             }
-        };
-        let b = ros_interface.to_string();
-        door_motion_params.push(("ros_interface", &b));
-        door_model.joint.extend(joints);
-        for (name, value) in door_motion_params.into_iter() {
-            component_data.push(XmlElement {
-                name: name.into(),
-                data: ElementData::String(value.to_string()),
+            .to_sdf(0.0);
+            vec![SdfJoint {
+                name: prefix.clone() + "joint",
+                parent: "world".into(),
+                child: "body".into(),
+                r#type: "fixed".into(),
+                pose: Some(pose),
                 ..Default::default()
-            });
+            }]
         }
-        component_data.push(door_plugin_inner);
-        component.data = ElementData::Nested(component_data);
-        plugin.elements.push(component);
-        door_model.plugin = vec![plugin];
-        Ok(door_model)
+    };
+    let b = ros_interface.to_string();
+    door_motion_params.push(("ros_interface", &b));
+    door_model.joint.extend(joints);
+    for (name, value) in door_motion_params.into_iter() {
+        component_data.push(XmlElement {
+            name: name.into(),
+            data: ElementData::String(value.to_string()),
+            ..Default::default()
+        });
     }
+    component_data.push(door_plugin_inner);
+    component.data = ElementData::Nested(component_data);
+    plugin.elements.push(component);
+    door_model.plugin = vec![plugin];
+    Ok(door_model)
 }
 
 impl Site {
@@ -433,6 +430,11 @@ impl Site {
                 .ok_or(SdfConversionError::BrokenAnchorReference(id))
                 .cloned()
         };
+        let get_level = |id: u32| -> Result<&Level, SdfConversionError> {
+            self.levels
+                .get(&id)
+                .ok_or(SdfConversionError::BrokenLevelReference(id))
+        };
         let mut root = WORLD_TEMPLATE.clone();
         let world = &mut root.world[0];
         let mut min_elevation = f32::MAX;
@@ -537,12 +539,14 @@ impl Site {
                 // TODO(luca) doors into toggle floors
                 let left_anchor = get_anchor(door.anchors.left())?;
                 let right_anchor = get_anchor(door.anchors.right())?;
-                world.model.push(door.to_sdf(
+                world.model.push(make_sdf_door(
                     left_anchor,
                     right_anchor,
                     Vec3::new(0.0, 0.0, level.properties.elevation.0),
                     true,
                     None,
+                    &door.kind,
+                    &door.name.0,
                 )?);
                 level_model_names.push(door.name.0.clone());
             }
@@ -559,8 +563,7 @@ impl Site {
         }
         for lift in self.lifts.values() {
             let get_lift_anchor = |id: u32| -> Result<Anchor, SdfConversionError> {
-                lift
-                    .cabin_anchors
+                lift.cabin_anchors
                     .get(&id)
                     .ok_or(SdfConversionError::BrokenAnchorReference(id))
                     .cloned()
@@ -628,13 +631,6 @@ impl Site {
                 let door = lift.cabin_doors.get(&door_placement.door).unwrap();
                 let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, face.label());
                 let cabin_mesh_prefix = format!("{}_{}", lift_name, face.label());
-                // Create a dummy cabin door first
-                let dummy_cabin = Door {
-                    anchors: door.reference_anchors.clone(),
-                    name: NameInSite(cabin_door_name.clone()),
-                    kind: door.kind.clone(),
-                    marker: DoorMarker,
-                };
                 let left_anchor = get_lift_anchor(door.reference_anchors.left())?;
                 let right_anchor = get_lift_anchor(door.reference_anchors.right())?;
                 let x_offset = -face.u()
@@ -642,45 +638,40 @@ impl Site {
                         + door_placement
                             .custom_gap
                             .unwrap_or_else(|| cabin.gap.unwrap_or(0.01)));
-                let mut cabin = dummy_cabin.to_sdf(
+                let mut cabin_door = make_sdf_door(
                     left_anchor,
                     right_anchor,
                     x_offset,
                     false,
-                    Some(cabin_mesh_prefix.clone()),
+                    Some(&cabin_mesh_prefix),
+                    &door.kind,
+                    &cabin_door_name,
                 )?;
-                for mut joint in cabin.joint.drain(..) {
+                for mut joint in cabin_door.joint.drain(..) {
                     // Move the joint to the lift and change its parenthood accordingly
                     joint.parent = "platform".into();
-                    joint.child = format!("{}::{}", cabin.name, joint.child);
+                    joint.child = format!("{}::{}", cabin_door.name, joint.child);
                     lift_joints.push(joint);
                 }
-                lift_models.push(cabin.into());
+                lift_models.push(cabin_door.into());
                 for visit in door.visits.0.iter() {
-                    let level = self
-                        .levels
-                        .get(visit)
-                        .ok_or(SdfConversionError::BrokenLevelReference(*visit))?;
+                    let level = get_level(*visit)?;
                     let shaft_door_name = format!(
                         "ShaftDoor_{}_{}_door_{}",
                         level.properties.name.0,
                         lift_name,
                         face.label()
                     );
-                    let dummy_shaft_door = Door {
-                        anchors: door.reference_anchors.clone(),
-                        name: NameInSite(shaft_door_name.clone()),
-                        kind: door.kind.clone(),
-                        marker: DoorMarker,
-                    };
                     let left_anchor = get_lift_anchor(door.reference_anchors.left())?;
                     let right_anchor = get_lift_anchor(door.reference_anchors.right())?;
-                    let shaft_door = dummy_shaft_door.to_sdf(
+                    let shaft_door = make_sdf_door(
                         left_anchor,
                         right_anchor,
                         Vec3::from(pose.trans) + Vec3::new(0.0, 0.0, level.properties.elevation.0),
                         false,
-                        Some(cabin_mesh_prefix.clone()),
+                        Some(&cabin_mesh_prefix),
+                        &door.kind,
+                        &shaft_door_name,
                     )?;
                     // Add the pose of the lift to have world coordinates
                     world.model.push(shaft_door);
@@ -710,10 +701,7 @@ impl Site {
                 }
             }
             for (key, door_pairs) in levels.into_iter() {
-                let level = self
-                    .levels
-                    .get(&key)
-                    .ok_or(SdfConversionError::BrokenLevelReference(key))?;
+                let level = get_level(key)?;
                 component_data.push(XmlElement {
                     name: "floor".into(),
                     attributes: [
@@ -860,21 +848,18 @@ mod tests {
     use crate::legacy::building_map::BuildingMap;
 
     #[test]
-    fn serde_roundtrip() {
-        let data = std::fs::read("../assets/demo_maps/hotel.building.yaml").unwrap();
+    fn serialize_sdf() {
+        let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
         let map = BuildingMap::from_bytes(&data).unwrap();
         let site = map.to_site().unwrap();
         // Convert to an sdf
         let sdf = site.to_sdf().unwrap();
-        dbg!(&sdf);
         let config = yaserde::ser::Config {
             perform_indent: true,
             write_document_declaration: true,
             ..Default::default()
         };
         let s = yaserde::ser::to_string_with_config(&sdf, &config).unwrap();
-        println!("{}", s);
         std::fs::write("test.sdf", s);
-        panic!();
     }
 }

From 012382ebc411eb07de4cbf39c0445b26111e7be1 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 16:35:12 +0800
Subject: [PATCH 54/71] Cleanup pose sdf conversion

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/sdf.rs | 32 ++++++++++++++++----------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index ab2261af..02b825d4 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -43,7 +43,7 @@ pub enum SdfConversionError {
 }
 
 impl Pose {
-    fn to_sdf(&self, elevation: f32) -> SdfPose {
+    fn to_sdf(&self) -> SdfPose {
         let p = &self.trans;
         let r = match self.rot {
             Rotation::Yaw(angle) => format!("0 0 {}", angle.radians()),
@@ -56,7 +56,7 @@ impl Pose {
             Rotation::Quat(quat) => format!("{} {} {} {}", quat[3], quat[0], quat[1], quat[2]),
         };
         SdfPose {
-            data: format!("{} {} {} {}", p[0], p[1], p[2] + elevation, r),
+            data: format!("{} {} {} {}", p[0], p[1], p[2], r),
             ..Default::default()
         }
     }
@@ -155,7 +155,7 @@ fn make_sdf_door(
                 trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(),
                 rot: Rotation::Yaw(Angle::Rad(yaw)),
             }
-            .to_sdf(0.0),
+            .to_sdf(),
         ),
         r#static: Some(false),
         ..Default::default()
@@ -189,7 +189,7 @@ fn make_sdf_door(
                 trans: [0.0, (door_length / 2.0) * door.towards.sign(), 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             vec![SdfJoint {
                 name: prefix.clone() + "joint",
                 parent: "world".into(),
@@ -230,7 +230,7 @@ fn make_sdf_door(
                 trans: [0.0, (door_length / 2.0) * door.pivot_on.sign(), 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             let (left_joint_name, right_joint_name) = ("empty_joint", prefix.clone() + "joint");
             door_plugin_inner
                 .attributes
@@ -275,12 +275,12 @@ fn make_sdf_door(
                 trans: [0.0, -door_length / 2.0, 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             let left_pose = Pose {
                 trans: [0.0, door_length / 2.0, 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             let left_length = (door.left_right_ratio / (1.0 + door.left_right_ratio)) * door_length;
             let right_length = door_length - left_length;
             vec![
@@ -346,12 +346,12 @@ fn make_sdf_door(
                 trans: [0.0, -door_length / 2.0, 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             let left_pose = Pose {
                 trans: [0.0, door_length / 2.0, 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             vec![
                 SdfJoint {
                     name: prefix.clone() + "right_joint",
@@ -395,7 +395,7 @@ fn make_sdf_door(
                 trans: [0.0, door_length / 2.0, 1.25],
                 ..Default::default()
             }
-            .to_sdf(0.0);
+            .to_sdf();
             vec![SdfJoint {
                 name: prefix.clone() + "joint",
                 parent: "world".into(),
@@ -502,7 +502,7 @@ impl Site {
                     world.include.push(SdfWorldInclude {
                         uri: "model://TeleportIngestor".to_string(),
                         name: Some(model.name.0.clone()),
-                        pose: Some(model.pose.to_sdf(0.0)),
+                        pose: Some(model.pose.to_sdf()),
                         ..Default::default()
                     });
                     added = true;
@@ -512,7 +512,7 @@ impl Site {
                     world.include.push(SdfWorldInclude {
                         uri: "model://TeleportDispenser".to_string(),
                         name: Some(model.name.0.clone()),
-                        pose: Some(model.pose.to_sdf(0.0)),
+                        pose: Some(model.pose.to_sdf()),
                         ..Default::default()
                     });
                     added = true;
@@ -524,7 +524,7 @@ impl Site {
                     world.include.push(SdfWorldInclude {
                         uri: format!("model://{}", model.name.0.clone()),
                         name: Some(model.name.0.clone()),
-                        pose: Some(model.pose.to_sdf(0.0)),
+                        pose: Some(model.pose.to_sdf()),
                         r#static: Some(model.is_static.0),
                         ..Default::default()
                     });
@@ -728,7 +728,7 @@ impl Site {
             world.model.push(SdfModel {
                 name: lift.properties.name.0.clone(),
                 r#static: Some(lift.properties.is_static.0),
-                pose: Some(pose.to_sdf(0.0)),
+                pose: Some(pose.to_sdf()),
                 link: vec![SdfLink {
                     name: "platform".into(),
                     collision: vec![SdfCollision {
@@ -796,7 +796,7 @@ impl Site {
             world.include.push(SdfWorldInclude {
                 uri: "model://".to_string() + robot_type,
                 name: Some(robot.name.0.clone()),
-                pose: Some(pose.to_sdf(0.0)),
+                pose: Some(pose.to_sdf()),
                 r#static: Some(robot.is_static.0),
                 ..Default::default()
             });
@@ -824,7 +824,7 @@ impl Site {
                             Angle::Rad(1.57),
                         ]);
                         pose.trans[0] = pose.trans[0] + 10.0;
-                        camera_pose.data = ElementData::String(pose.to_sdf(0.0).data);
+                        camera_pose.data = ElementData::String(pose.to_sdf().data);
                     }
                 }
             }

From 1cef31f31761e4b091818f62297ddf34bd72f23b Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 21 Mar 2024 16:54:39 +0800
Subject: [PATCH 55/71] Run at lower frequency to save CPU

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/lib.rs | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 2e559c5c..2a78b8b3 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -247,7 +247,9 @@ impl Plugin for SiteEditor {
         // TODO(luca) This schedule runner plugin runs forever.
         // We might need to have a custom one where we can inject exit conditions.
         if let Some(path) = &self.headless {
-            app.add_plugins(ScheduleRunnerPlugin::default());
+            // We really don't need a high update rate here since we are IO bound, set a low rate
+            // to save CPU.
+            app.add_plugins(ScheduleRunnerPlugin::run_loop(std::time::Duration::from_secs_f64(1.0 / 10.0)));
             app.insert_resource(HeadlessExport(path.clone()));
             app.add_systems(Last, headless_sdf_export);
         }

From 63fe98d0038558607ff9a50bd8999a76850e948a Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 2 Apr 2024 10:27:42 +0800
Subject: [PATCH 56/71] WIP remove floor transparency

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index da981a1c..44c5eff4 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -102,6 +102,10 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     let Some((mesh, material)) = get_mesh_and_material(res.mesh) else {
                         continue;
                     };
+                    // Remove transparency from floors
+                    let mut material = material.clone();
+                    material.alpha_mode = AlphaMode::Opaque;
+                    material.base_color.set_a(1.0);
                     collision_data.push(MeshData {
                         mesh,
                         material: None,
@@ -115,12 +119,15 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                 } else if let Ok((model, is_static, name)) = q_models.get(*child) {
                     let mut model_collisions = vec![];
                     let mut model_visuals = vec![];
-                    // TODO(luca) don't do full descendant iter here or we might add twice?
                     // Iterate through children and select all meshes
+                    let added_meshes = HashSet::new();
                     for model_child in DescendantIter::new(&q_children, model) {
                         if q_collisions.get(model_child).is_ok() {
                             // Now iterate through the children of the collision and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
+                                if added_meshes.contains(entity) {
+                                    continue;
+                                }
                                 let Some((mesh, _)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
@@ -142,10 +149,14 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                     material: None,
                                     pose: Some(pose),
                                 });
+                                added_meshes.insert(entity);
                             }
                         } else if q_visuals.get(model_child).is_ok() {
                             // Now iterate through the children of the visuals and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
+                                if added_meshes.contains(entity) {
+                                    continue;
+                                }
                                 let Some((mesh, material)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
@@ -167,6 +178,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                     material: Some(material),
                                     pose: Some(pose),
                                 });
+                                added_meshes.insert(entity);
                             }
                         }
                     }

From 4f3f40bc4b554435eda82627efd6934f1f2afe23 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 2 Apr 2024 12:22:16 +0800
Subject: [PATCH 57/71] Revert "WIP remove floor transparency"

This reverts commit 63fe98d0038558607ff9a50bd8999a76850e948a.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs | 14 +-------------
 1 file changed, 1 insertion(+), 13 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 44c5eff4..da981a1c 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -102,10 +102,6 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     let Some((mesh, material)) = get_mesh_and_material(res.mesh) else {
                         continue;
                     };
-                    // Remove transparency from floors
-                    let mut material = material.clone();
-                    material.alpha_mode = AlphaMode::Opaque;
-                    material.base_color.set_a(1.0);
                     collision_data.push(MeshData {
                         mesh,
                         material: None,
@@ -119,15 +115,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                 } else if let Ok((model, is_static, name)) = q_models.get(*child) {
                     let mut model_collisions = vec![];
                     let mut model_visuals = vec![];
+                    // TODO(luca) don't do full descendant iter here or we might add twice?
                     // Iterate through children and select all meshes
-                    let added_meshes = HashSet::new();
                     for model_child in DescendantIter::new(&q_children, model) {
                         if q_collisions.get(model_child).is_ok() {
                             // Now iterate through the children of the collision and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
-                                if added_meshes.contains(entity) {
-                                    continue;
-                                }
                                 let Some((mesh, _)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
@@ -149,14 +142,10 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                     material: None,
                                     pose: Some(pose),
                                 });
-                                added_meshes.insert(entity);
                             }
                         } else if q_visuals.get(model_child).is_ok() {
                             // Now iterate through the children of the visuals and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
-                                if added_meshes.contains(entity) {
-                                    continue;
-                                }
                                 let Some((mesh, material)) = get_mesh_and_material(entity) else {
                                     continue;
                                 };
@@ -178,7 +167,6 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                     material: Some(material),
                                     pose: Some(pose),
                                 });
-                                added_meshes.insert(entity);
                             }
                         }
                     }

From 16f0fffff8f69cddc770b98fd992679253ceb80b Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 2 Apr 2024 12:52:29 +0800
Subject: [PATCH 58/71] Minor API cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/Cargo.toml               |  2 +-
 rmf_site_editor/src/site/sdf_exporter.rs | 58 +++++++-----------------
 2 files changed, 17 insertions(+), 43 deletions(-)

diff --git a/rmf_site_editor/Cargo.toml b/rmf_site_editor/Cargo.toml
index b1f92b45..447851bd 100644
--- a/rmf_site_editor/Cargo.toml
+++ b/rmf_site_editor/Cargo.toml
@@ -21,7 +21,7 @@ bevy_mod_raycast = "0.16"
 bevy_mod_outline = "0.6"
 # PR merged after 0.10 but not released yet, bump to 0.10.1 once merged
 bevy_infinite_grid = { git = "https://github.com/ForesightMiningSoftwareCorporation/bevy_infinite_grid", rev = "86018dd" }
-bevy_gltf_export = { git = "https://github.com/luca-della-vedova/bevy_gltf_export", branch = "luca/bevy_0.12"}
+bevy_gltf_export = { git = "https://github.com/luca-della-vedova/bevy_gltf_export", branch = "luca/transform_api"}
 bevy_polyline = "0.8.1"
 bevy_stl = "0.12"
 bevy_obj = { version = "0.12.1", features = ["scene"] }
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index da981a1c..c859e716 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -1,6 +1,6 @@
 use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
-use bevy_gltf_export::{export_meshes, CompressGltfOptions, GltfPose, MeshData, MeshExportError};
+use bevy_gltf_export::{export_meshes, CompressGltfOptions, MeshData, MeshExportError};
 
 use std::path::Path;
 
@@ -79,8 +79,8 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
         let mut collision_data = Vec::new();
         let mut visual_data = Vec::new();
         if let Ok((level_name, elevation, children)) = q_levels.get(*site_child) {
-            let level_pose = GltfPose {
-                translation: [0.0, 0.0, **elevation],
+            let level_tf = Transform {
+                translation: Vec3::new(0.0, 0.0, **elevation),
                 ..Default::default()
             };
             for child in children.iter() {
@@ -91,12 +91,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     collision_data.push(MeshData {
                         mesh,
                         material: None,
-                        pose: Some(level_pose.clone()),
+                        transform: Some(level_tf.clone()),
                     });
                     visual_data.push(MeshData {
                         mesh,
                         material: Some(material),
-                        pose: Some(level_pose.clone()),
+                        transform: Some(level_tf.clone()),
                     });
                 } else if let Ok(res) = q_floors.get(*child) {
                     let Some((mesh, material)) = get_mesh_and_material(res.mesh) else {
@@ -105,12 +105,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     collision_data.push(MeshData {
                         mesh,
                         material: None,
-                        pose: Some(level_pose.clone()),
+                        transform: Some(level_tf.clone()),
                     });
                     visual_data.push(MeshData {
                         mesh,
                         material: Some(material),
-                        pose: Some(level_pose.clone()),
+                        transform: Some(level_tf.clone()),
                     });
                 } else if let Ok((model, is_static, name)) = q_models.get(*child) {
                     let mut model_collisions = vec![];
@@ -127,20 +127,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                 let Ok(tf) = q_global_tfs.get(entity) else {
                                     continue;
                                 };
-                                let tf = tf.compute_transform();
-                                let pose = GltfPose {
-                                    translation: [
-                                        tf.translation.x,
-                                        tf.translation.y,
-                                        tf.translation.z + **elevation,
-                                    ],
-                                    rotation: tf.rotation.to_array(),
-                                    scale: Some(tf.scale.to_array()),
-                                };
+                                let mut tf = tf.compute_transform();
+                                tf.translation.z = tf.translation.z + **elevation;
                                 model_collisions.push(MeshData {
                                     mesh,
                                     material: None,
-                                    pose: Some(pose),
+                                    transform: Some(tf),
                                 });
                             }
                         } else if q_visuals.get(model_child).is_ok() {
@@ -152,20 +144,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                 let Ok(tf) = q_global_tfs.get(entity) else {
                                     continue;
                                 };
-                                let tf = tf.compute_transform();
-                                let pose = GltfPose {
-                                    translation: [
-                                        tf.translation.x,
-                                        tf.translation.y,
-                                        tf.translation.z + **elevation,
-                                    ],
-                                    rotation: tf.rotation.to_array(),
-                                    scale: Some(tf.scale.to_array()),
-                                };
+                                let mut tf = tf.compute_transform();
+                                tf.translation.z = tf.translation.z + **elevation;
                                 model_visuals.push(MeshData {
                                     mesh,
                                     material: Some(material),
-                                    pose: Some(pose),
+                                    transform: Some(tf),
                                 });
                             }
                         }
@@ -208,16 +192,11 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                         let Some(door_name) = door_name else {
                             continue;
                         };
-                        let pose = GltfPose {
-                            translation: tf.translation.to_array(),
-                            rotation: tf.rotation.to_array(),
-                            scale: Some(tf.scale.to_array()),
-                        };
 
                         let data = MeshData {
                             mesh,
                             material: Some(material),
-                            pose: Some(pose.clone()),
+                            transform: Some(tf.clone()),
                         };
                         let filename =
                             format!("{}/{}_{}.glb", folder.display(), **door_name, segment_name);
@@ -264,7 +243,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                 lift_data.push(MeshData {
                     mesh,
                     material: Some(material),
-                    pose: None,
+                    transform: None,
                 });
             }
             let filename = format!("{}/{}.glb", folder.display(), **lift_name);
@@ -292,16 +271,11 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                         let Ok(tf) = q_tfs.get(*entity) else {
                             continue;
                         };
-                        let pose = GltfPose {
-                            translation: tf.translation.to_array(),
-                            rotation: tf.rotation.to_array(),
-                            scale: Some(tf.scale.to_array()),
-                        };
 
                         let data = MeshData {
                             mesh,
                             material: Some(material),
-                            pose: Some(pose.clone()),
+                            transform: Some(tf.clone()),
                         };
                         let filename = format!(
                             "{}/{}_{}_{}.glb",

From 0cd5d19353c4f571928f31b682a9941b683ab3e9 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Tue, 2 Apr 2024 15:31:13 +0800
Subject: [PATCH 59/71] Cleanup and refactor

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/keyboard.rs          |  2 +-
 rmf_site_editor/src/lib.rs               | 87 +++++-------------------
 rmf_site_editor/src/site/save.rs         |  5 +-
 rmf_site_editor/src/site/sdf_exporter.rs | 79 +++++++++++++++++++--
 4 files changed, 96 insertions(+), 77 deletions(-)

diff --git a/rmf_site_editor/src/keyboard.rs b/rmf_site_editor/src/keyboard.rs
index dcaf6bc5..27ea2034 100644
--- a/rmf_site_editor/src/keyboard.rs
+++ b/rmf_site_editor/src/keyboard.rs
@@ -20,7 +20,7 @@ use crate::{
     site::{AlignSiteDrawings, Delete},
     CreateNewWorkspace, CurrentWorkspace, LoadWorkspace, SaveWorkspace,
 };
-use bevy::{ecs::system::SystemParam, prelude::*, window::PrimaryWindow};
+use bevy::{prelude::*, window::PrimaryWindow};
 use bevy_egui::EguiContexts;
 
 #[derive(Debug, Clone, Copy, Resource)]
diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index 2a78b8b3..d42a3115 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -1,7 +1,4 @@
-use bevy::{
-    app::ScheduleRunnerPlugin, log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*,
-    render::renderer::RenderAdapterInfo,
-};
+use bevy::{app::ScheduleRunnerPlugin, log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*};
 use bevy_egui::EguiPlugin;
 use main_menu::MainMenuPlugin;
 // use warehouse_generator::WarehouseGeneratorPlugin;
@@ -78,9 +75,9 @@ pub struct CommandLineArgs {
     /// 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<String>,
-    /// Run in headless mode.
+    /// Run in headless mode and export the loaded site to the requested path.
     #[cfg_attr(not(target_arch = "wasm32"), arg(long))]
-    pub headless: Option<String>,
+    pub headless_export: Option<String>,
 }
 
 #[derive(Clone, Default, Eq, PartialEq, Debug, Hash, States)]
@@ -123,7 +120,7 @@ pub fn run_js() {
 
 pub fn run(command_line_args: Vec<String>) {
     let mut app = App::new();
-    let mut headless = None;
+    let mut headless_export = None;
 
     #[cfg(not(target_arch = "wasm32"))]
     {
@@ -134,17 +131,17 @@ pub fn run(command_line_args: Vec<String>) {
                 command_line_args.import.map(Into::into),
             ));
         }
-        headless = command_line_args.headless;
+        headless_export = command_line_args.headless_export;
     }
 
-    app.add_plugins(SiteEditor { headless });
+    app.add_plugins(SiteEditor { headless_export });
     app.run();
 }
 
 #[derive(Default)]
 pub struct SiteEditor {
-    /// Whether to run the site editor in headless mode.
-    pub headless: Option<String>,
+    /// Contains Some(path) if the site editor is running in headless mode exporting its site.
+    pub headless_export: Option<String>,
 }
 
 impl Plugin for SiteEditor {
@@ -153,7 +150,7 @@ impl Plugin for SiteEditor {
         let headless = {
             #[cfg(not(target_arch = "wasm32"))]
             {
-                self.headless.is_some()
+                self.headless_export.is_some()
             }
             #[cfg(target_arch = "wasm32")]
             {
@@ -221,10 +218,10 @@ impl Plugin for SiteEditor {
                 WorkcellEditorPlugin,
                 SitePlugin,
                 InteractionPlugin {
-                    headless: self.headless.is_some(),
+                    headless: self.headless_export.is_some(),
                 },
                 StandardUiLayout {
-                    headless: self.headless.is_some(),
+                    headless: self.headless_export.is_some(),
                 },
                 AnimationPlugin,
                 OccupancyPlugin,
@@ -244,63 +241,15 @@ impl Plugin for SiteEditor {
         app.world
             .remove_resource::<bevy::ecs::event::EventUpdateSignal>();
 
-        // TODO(luca) This schedule runner plugin runs forever.
-        // We might need to have a custom one where we can inject exit conditions.
-        if let Some(path) = &self.headless {
+        if let Some(path) = &self.headless_export {
             // We really don't need a high update rate here since we are IO bound, set a low rate
             // to save CPU.
-            app.add_plugins(ScheduleRunnerPlugin::run_loop(std::time::Duration::from_secs_f64(1.0 / 10.0)));
-            app.insert_resource(HeadlessExport(path.clone()));
-            app.add_systems(Last, headless_sdf_export);
-        }
-    }
-}
-
-#[derive(Debug, Resource, Default)]
-pub struct HeadlessExport(String);
-
-#[derive(Debug, Resource, Default)]
-pub struct Counter(u32, bool, bool);
-
-pub fn headless_sdf_export(
-    mut export: EventWriter<SaveWorkspace>,
-    mut exit: EventWriter<bevy::app::AppExit>,
-    missing_models: Query<
-        (),
-        (
-            With<site::TentativeModelFormat>,
-            Without<site::ModelSceneRoot>,
-        ),
-    >,
-    mut export_state: Local<Counter>,
-    path: Res<HeadlessExport>,
-    sites: Query<(Entity, &rmf_site_format::NameOfSite)>,
-) {
-    export_state.0 += 1;
-    if export_state.0 < 5 {
-        return;
-    }
-    if sites.is_empty() {
-        warn!("Site loading failed, aborting");
-        exit.send(bevy::app::AppExit);
-    }
-    if !missing_models.is_empty() {
-        // TODO(luca) implement a timeout logic?
-    } else {
-        if !export_state.1 {
-            export_state.0 = 0;
-            export_state.1 = true;
-        } else {
-            if !export_state.2 && export_state.0 > 5 {
-                export_state.2 = true;
-                for (e, name) in &sites {
-                    let path = std::path::PathBuf::from(path.0.clone());
-                    export.send(SaveWorkspace::new().to_sdf().to_path(&path));
-                    info!("Sent");
-                }
-            } else if export_state.2 && export_state.0 > 15 {
-                exit.send(bevy::app::AppExit);
-            }
+            // TODO(luca) this still seems to take quite some time, check where the bottleneck is.
+            app.add_plugins(ScheduleRunnerPlugin::run_loop(
+                std::time::Duration::from_secs_f64(1.0 / 10.0),
+            ));
+            app.insert_resource(site::HeadlessSdfExportState::new(path));
+            app.add_systems(Last, site::headless_sdf_export);
         }
     }
 }
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 1cd0811d..5f6b9e95 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1263,7 +1263,10 @@ pub fn save_site(world: &mut World) {
                 info!("Saving to {}", new_path.display());
                 let parent_folder = new_path.parent().unwrap();
                 if !parent_folder.exists() {
-                    std::fs::create_dir_all(new_path.parent().unwrap());
+                    if let Err(e) = std::fs::create_dir_all(new_path.parent().unwrap()) {
+                        error!("Unable to create folder {}: {e}", parent_folder.display());
+                        continue;
+                    }
                 }
                 let f = match std::fs::File::create(&new_path) {
                     Ok(f) => f,
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index c859e716..de7af539 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -1,14 +1,83 @@
 use bevy::ecs::system::SystemState;
 use bevy::prelude::*;
-use bevy_gltf_export::{export_meshes, CompressGltfOptions, MeshData, MeshExportError};
+use bevy_gltf_export::{export_meshes, CompressGltfOptions, MeshData};
 
 use std::path::Path;
 
+use crate::SaveWorkspace;
+
 use crate::site::{
-    ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, FloorSegments, LiftDoormat,
-    VisualMeshMarker,
+    ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, DrawingMarker, FloorSegments,
+    LiftDoormat, ModelSceneRoot, TentativeModelFormat, VisualMeshMarker,
+};
+use rmf_site_format::{
+    IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, NameOfSite, WallMarker,
 };
-use rmf_site_format::{IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, WallMarker};
+
+/// Manages a simple state machine where we:
+///   * Wait for a few iterations,
+///   * Make sure the world is loaded.
+///   * Send a save event.
+///   * Wait for a few iterations.
+///   * Exit.
+#[derive(Debug, Resource)]
+pub struct HeadlessSdfExportState {
+    iterations: u32,
+    world_loaded: bool,
+    save_requested: bool,
+    target_path: String,
+}
+
+impl HeadlessSdfExportState {
+    pub fn new(path: &str) -> Self {
+        Self {
+            iterations: 0,
+            world_loaded: false,
+            save_requested: false,
+            target_path: path.into(),
+        }
+    }
+}
+
+pub fn headless_sdf_export(
+    mut commands: Commands,
+    mut export: EventWriter<SaveWorkspace>,
+    mut exit: EventWriter<bevy::app::AppExit>,
+    missing_models: Query<(), (With<TentativeModelFormat>, Without<ModelSceneRoot>)>,
+    mut export_state: ResMut<HeadlessSdfExportState>,
+    sites: Query<(Entity, &NameOfSite)>,
+    drawings: Query<Entity, With<DrawingMarker>>,
+) {
+    export_state.iterations += 1;
+    if export_state.iterations < 5 {
+        return;
+    }
+    if sites.is_empty() {
+        warn!("Site loading failed, aborting");
+        exit.send(bevy::app::AppExit);
+    }
+    if !missing_models.is_empty() {
+        // Despawn all drawings, otherwise floors will become transparent.
+        for e in drawings.iter() {
+            commands.entity(e).despawn_recursive();
+        }
+        // TODO(luca) implement a timeout logic?
+    } else {
+        if !export_state.world_loaded {
+            export_state.iterations = 0;
+            export_state.world_loaded = true;
+        } else {
+            if !export_state.save_requested && export_state.iterations > 5 {
+                let path = std::path::PathBuf::from(export_state.target_path.clone());
+                export.send(SaveWorkspace::new().to_sdf().to_path(&path));
+                export_state.save_requested = true;
+                export_state.iterations = 0;
+            } else if export_state.save_requested && export_state.iterations > 5 {
+                exit.send(bevy::app::AppExit);
+            }
+        }
+    }
+}
 
 pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Result<(), String> {
     let mut state: SystemState<(
@@ -254,9 +323,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                 let Some(door) = door else {
                     continue;
                 };
-                println!("Found door with label {}", face.label());
                 if let Ok((_, segments)) = q_doors.get(door.door) {
-                    println!("Segments found");
                     // TODO(luca) this is duplicated with door generation, refactor
                     for (entity, segment_name) in segments
                         .body

From 1c1380ec064f50c9c4fe9e46ae20a1ae7b0d1ad6 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 3 Apr 2024 17:50:32 +0800
Subject: [PATCH 60/71] Add simple json saving / loading

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/save.rs | 45 ++++++++++++++++++--------------
 rmf_site_editor/src/workspace.rs | 27 ++++++++++++++++---
 rmf_site_format/src/sdf.rs       | 13 ++++-----
 rmf_site_format/src/site.rs      | 20 +++++++++++++-
 4 files changed, 76 insertions(+), 29 deletions(-)

diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 5f6b9e95..e232c0d1 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1215,7 +1215,9 @@ pub fn save_site(world: &mut World) {
                 if path_str.ends_with(".building.yaml") {
                     warn!("Detected old file format, converting to new format");
                     new_path = path_str.replace(".building.yaml", ".site.ron").into();
-                } else if !path_str.ends_with("site.ron") {
+                } else if path_str.ends_with(".site.json") {
+                    // Noop
+                } else if !path_str.ends_with(".site.ron") {
                     info!("Appending .site.ron to {}", new_path.display());
                     new_path = new_path.with_extension("site.ron");
                 }
@@ -1239,27 +1241,34 @@ pub fn save_site(world: &mut World) {
                     }
                 };
 
-                match site.to_writer(f) {
-                    Ok(()) => {
-                        info!("Save successful");
+                if new_path.extension().is_some_and(|e| e == "json") {
+                    match site.to_writer_json(f) {
+                        Ok(()) => {
+                            info!("Save successful");
+                        }
+                        Err(err) => {
+                            if let Some(old_default_path) = old_default_path {
+                                world.entity_mut(save_event.site).insert(old_default_path);
+                            }
+                            error!("Save failed: {err}");
+                        }
                     }
-                    Err(err) => {
-                        if let Some(old_default_path) = old_default_path {
-                            world.entity_mut(save_event.site).insert(old_default_path);
+                } else {
+                    match site.to_writer(f) {
+                        Ok(()) => {
+                            info!("Save successful");
+                        }
+                        Err(err) => {
+                            if let Some(old_default_path) = old_default_path {
+                                world.entity_mut(save_event.site).insert(old_default_path);
+                            }
+                            error!("Save failed: {err}");
                         }
-                        error!("Save failed: {err}");
                     }
                 }
             }
             ExportFormat::Sdf => {
                 // TODO(luca) reduce code duplication with default exporting
-                if path_str.ends_with(".building.yaml") {
-                    warn!("Detected old file format, converting to new format");
-                    new_path = path_str.replace(".building.yaml", ".world").into();
-                } else if !path_str.ends_with("site.ron") {
-                    info!("Appending .world to {}", new_path.display());
-                    new_path = new_path.with_extension("world");
-                }
                 info!("Saving to {}", new_path.display());
                 let parent_folder = new_path.parent().unwrap();
                 if !parent_folder.exists() {
@@ -1280,7 +1289,7 @@ pub fn save_site(world: &mut World) {
                 meshes_dir.push("meshes");
                 std::fs::create_dir(&meshes_dir).ok();
                 if let Err(e) = collect_site_meshes(world, save_event.site, &meshes_dir) {
-                    error!("Unable to collect site meshes {e}");
+                    error!("Unable to collect site meshes: {e}");
                 }
 
                 migrate_relative_paths(save_event.site, &new_path, world);
@@ -1296,9 +1305,7 @@ pub fn save_site(world: &mut World) {
                 let sdf = match site.to_sdf() {
                     Ok(sdf) => sdf,
                     Err(err) => {
-                        //error!("Unable to convert site to sdf: {err}");
-                        error!("Unable to convert site to sdf");
-                        dbg!(err);
+                        error!("Unable to convert site to sdf: {err}");
                         continue;
                     }
                 };
diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs
index 1bad93fd..f4e3531d 100644
--- a/rmf_site_editor/src/workspace.rs
+++ b/rmf_site_editor/src/workspace.rs
@@ -61,7 +61,8 @@ pub enum LoadWorkspace {
 #[derive(Clone)]
 pub enum WorkspaceData {
     LegacyBuilding(Vec<u8>),
-    Site(Vec<u8>),
+    RonSite(Vec<u8>),
+    JsonSite(Vec<u8>),
     Workcell(Vec<u8>),
     WorkcellUrdf(Vec<u8>),
 }
@@ -72,7 +73,9 @@ impl WorkspaceData {
         if filename.ends_with(".building.yaml") {
             Some(WorkspaceData::LegacyBuilding(data))
         } else if filename.ends_with("site.ron") {
-            Some(WorkspaceData::Site(data))
+            Some(WorkspaceData::RonSite(data))
+        } else if filename.ends_with("site.json") {
+            Some(WorkspaceData::JsonSite(data))
         } else if filename.ends_with("workcell.json") {
             Some(WorkspaceData::Workcell(data))
         } else if filename.ends_with("urdf") {
@@ -344,7 +347,7 @@ fn workspace_file_load_complete(
                     }
                 }
             }
-            WorkspaceData::Site(data) => {
+            WorkspaceData::RonSite(data) => {
                 info!("Opening site file");
                 match Site::from_bytes(&data) {
                     Ok(site) => {
@@ -362,6 +365,24 @@ fn workspace_file_load_complete(
                     }
                 }
             }
+            WorkspaceData::JsonSite(data) => {
+                info!("Opening site file");
+                match Site::from_json_bytes(&data) {
+                    Ok(site) => {
+                        // Switch state
+                        app_state.set(AppState::SiteEditor);
+                        load_site.send(LoadSite {
+                            site,
+                            focus: true,
+                            default_file,
+                        });
+                        interaction_state.set(InteractionState::Enable);
+                    }
+                    Err(err) => {
+                        error!("Failed loading site {:?}", err);
+                    }
+                }
+            }
             WorkspaceData::Workcell(data) => {
                 info!("Opening workcell file");
                 match Workcell::from_bytes(&data) {
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 02b825d4..265b77b5 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -22,23 +22,24 @@ use glam::Vec3;
 use once_cell::sync::Lazy;
 use sdformat_rs::*;
 use std::collections::BTreeMap;
+use thiserror::Error;
 
 static WORLD_TEMPLATE: Lazy<SdfRoot> = Lazy::new(|| {
     yaserde::de::from_str(include_str!("templates/gz_world.sdf"))
         .expect("Failed deserializing template")
 });
 
-#[derive(Debug)]
+#[derive(Debug, Error)]
 pub enum SdfConversionError {
-    /// An asset that can't be converted to an sdf world was found.
+    #[error("An asset that can't be converted to an sdf world was found")]
     UnsupportedAssetType,
-    /// Entity referenced a non existing anchor.
+    #[error("Entity [{0}] referenced a non existing anchor")]
     BrokenAnchorReference(u32),
-    /// Entity referenced a non existing level.
+    #[error("Entity [{0}] referenced a non existing level")]
     BrokenLevelReference(u32),
-    /// Parsing of a lift cabin failed
+    #[error("Parsing lift cabin for lift [{0}] failed")]
     LiftParsingError(String),
-    /// A lift had no initial level where it could be spawned
+    #[error("Lift [{0}] had no initial level where it could be spawned")]
     MissingInitialLevel(String),
 }
 
diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs
index 9c6e8c0b..0d0f9a53 100644
--- a/rmf_site_format/src/site.rs
+++ b/rmf_site_format/src/site.rs
@@ -154,6 +154,8 @@ fn default_style_config() -> Style {
 }
 
 impl Site {
+    // TODO(luca) the functions below assume that we only serialize to ron, now that we are adding
+    // json these should probably be renamed or made to accept an enum for the format?
     pub fn to_writer<W: io::Write>(&self, writer: W) -> ron::Result<()> {
         ron::ser::to_writer_pretty(writer, self, default_style_config())
     }
@@ -170,6 +172,14 @@ impl Site {
         ron::ser::to_string_pretty(self, style)
     }
 
+    pub fn to_writer_json<W: io::Write>(&self, writer: W) -> serde_json::Result<()> {
+        serde_json::to_writer_pretty(writer, self)
+    }
+
+    pub fn from_json_bytes(s: &[u8]) -> serde_json::Result<Self> {
+        serde_json::from_slice(s)
+    }
+
     pub fn from_reader<R: io::Read>(reader: R) -> ron::error::SpannedResult<Self> {
         // TODO(MXG): Validate the parsed data, e.g. make sure anchor pairs
         // belong to the same level.
@@ -204,10 +214,18 @@ mod tests {
     use crate::legacy::building_map::BuildingMap;
 
     #[test]
-    fn serde_roundtrip() {
+    fn ron_roundtrip() {
         let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
         let map = BuildingMap::from_bytes(&data).unwrap();
         let site_string = map.to_site().unwrap().to_string().unwrap();
         Site::from_str(&site_string).unwrap();
     }
+
+    #[test]
+    fn json_roundtrip() {
+        let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
+        let map = BuildingMap::from_bytes(&data).unwrap();
+        let site_string = map.to_site().unwrap().to_json().unwrap();
+        Site::from_json_str(&site_string).unwrap();
+    }
 }

From 640fe5c72f99087e478387a05c3b2b36420fd505 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 5 Apr 2024 10:06:21 +0800
Subject: [PATCH 61/71] Fix level alignment when levels have no measurements

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/alignment.rs | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/rmf_site_format/src/alignment.rs b/rmf_site_format/src/alignment.rs
index 63adab56..776451f8 100644
--- a/rmf_site_format/src/alignment.rs
+++ b/rmf_site_format/src/alignment.rs
@@ -42,7 +42,7 @@ pub struct FiducialVariables<T: RefTrait> {
     pub position: DVec2,
 }
 
-#[derive(Clone, Copy)]
+#[derive(Clone, Copy, Debug)]
 pub struct MeasurementVariables {
     pub in_pixels: f64,
     pub in_meters: f64,
@@ -135,6 +135,12 @@ pub fn align_legacy_building(building: &BuildingMap) -> HashMap<String, Alignmen
 
             initial_scale_numerator += m.in_meters / m.in_pixels;
         }
+        // If no measurements are available, default to a standard 5cm per pixel
+        let initial_scale = if !level.measurements.is_empty() {
+            initial_scale_numerator / level.measurements.len() as f64
+        } else {
+            0.05
+        };
         measurements.push(level_measurements);
 
         let mut level_fiducials = Vec::new();
@@ -148,12 +154,7 @@ pub fn align_legacy_building(building: &BuildingMap) -> HashMap<String, Alignmen
         }
         fiducials.push(level_fiducials);
 
-        u.extend([
-            0.0,
-            0.0,
-            0.0,
-            initial_scale_numerator / level.measurements.len() as f64,
-        ]);
+        u.extend([0.0, 0.0, 0.0, initial_scale]);
     }
 
     solve(&mut u, &fiducials, &measurements, false);

From 59d168fc54853ec8828ced7534c1035ad360a097 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 5 Apr 2024 13:20:29 +0800
Subject: [PATCH 62/71] Fix migrate relative paths computation

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/asset_source.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rmf_site_format/src/asset_source.rs b/rmf_site_format/src/asset_source.rs
index a02df55a..e9a6e77d 100644
--- a/rmf_site_format/src/asset_source.rs
+++ b/rmf_site_format/src/asset_source.rs
@@ -65,8 +65,8 @@ impl AssetSource {
             if Path::new(asset_path).is_relative() {
                 println!("Changing path for [{asset_path:?}]");
                 let new_path = diff_paths(
-                    &old_reference_path.with_file_name(asset_path.clone()),
-                    new_reference_path,
+                    old_reference_path.with_file_name(asset_path.clone()),
+                    new_reference_path.parent().ok_or(())?,
                 )
                 .ok_or(())?;
                 *asset_path = new_path.to_str().ok_or(())?.to_owned();

From 665811c8a5799d8431985c9477dcff294872b96c Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Thu, 23 May 2024 23:02:20 +0800
Subject: [PATCH 63/71] Fix test

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/site.rs | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs
index 0d0f9a53..15251281 100644
--- a/rmf_site_format/src/site.rs
+++ b/rmf_site_format/src/site.rs
@@ -190,6 +190,10 @@ impl Site {
         ron::de::from_str(s)
     }
 
+    pub fn to_json_str(&self) -> serde_json::Result<Vec<u8>> {
+        serde_json::to_vec_pretty(self)
+    }
+
     pub fn from_bytes<'a>(s: &'a [u8]) -> ron::error::SpannedResult<Self> {
         ron::de::from_bytes(s)
     }
@@ -225,7 +229,7 @@ mod tests {
     fn json_roundtrip() {
         let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
         let map = BuildingMap::from_bytes(&data).unwrap();
-        let site_string = map.to_site().unwrap().to_json().unwrap();
-        Site::from_json_str(&site_string).unwrap();
+        let site_string = map.to_site().unwrap().to_json_str().unwrap();
+        Site::from_json_bytes(&site_string).unwrap();
     }
 }

From 11bcfb069ea52b43bedf9a0e3aee301215fdf798 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 31 May 2024 14:45:35 +0200
Subject: [PATCH 64/71] Address simpler feedback

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 Cargo.lock                                    | 133 +-----------------
 rmf_site_editor/src/lib.rs                    |  46 +++---
 rmf_site_editor/src/site/door.rs              |   2 +-
 rmf_site_editor/src/site/file_menu.rs         |   8 +-
 rmf_site_editor/src/site/save.rs              |  31 ++--
 rmf_site_editor/src/site/sdf_exporter.rs      |  24 +++-
 .../src/widgets/view_nav_graphs.rs            |   4 +-
 rmf_site_editor/src/workspace.rs              |   4 +-
 rmf_site_format/src/camera_poses.rs           |   2 +-
 rmf_site_format/src/legacy/building_map.rs    |   5 +-
 rmf_site_format/src/site.rs                   |  28 ++--
 11 files changed, 95 insertions(+), 192 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index 966efc5a..a6e5fd76 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -735,11 +735,12 @@ dependencies = [
 [[package]]
 name = "bevy_gltf_export"
 version = "0.1.0"
-source = "git+https://github.com/luca-della-vedova/bevy_gltf_export?branch=luca/bevy_0.12#7fa783ec22c09cf18215aa3c45ce7f0856ccfa56"
+source = "git+https://github.com/luca-della-vedova/bevy_gltf_export?branch=luca/transform_api#a896bca4148648b44212d4a9e6be98dd64cc3d8f"
 dependencies = [
  "bevy_asset",
  "bevy_pbr",
  "bevy_render",
+ "bevy_transform",
  "gltf",
  "gltf-json",
  "image",
@@ -4406,7 +4407,7 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
 [[package]]
 name = "sdformat_rs"
 version = "0.1.0"
-source = "git+https://github.com/open-rmf/sdf_rust_experimental?rev=48ebcad#48ebcad0a8abb616fbe8fb4e1503f1db64e0e5df"
+source = "git+https://github.com/open-rmf/sdf_rust_experimental?rev=9fc35f2#9fc35f2b5f55afab37a4c1906c395fc22d069322"
 dependencies = [
  "convert_case",
  "nalgebra",
@@ -5914,131 +5915,3 @@ checksum = "73ab332fe2f6680068f3582b16a24f90ad7096d5d39b974d1c0aff0125116f02"
 dependencies = [
  "simd-adler32",
 ]
-
-[[patch.unused]]
-name = "action_msgs"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "actionlib_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "builtin_interfaces"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "composition_interfaces"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "diagnostic_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "example_interfaces"
-version = "0.10.2"
-
-[[patch.unused]]
-name = "examples_rclrs_message_demo"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "examples_rclrs_minimal_client_service"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "examples_rclrs_minimal_pub_sub"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "geometry_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "lifecycle_msgs"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "nav_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "rcl_interfaces"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "rclrs"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "rclrs_example_msgs"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "rclrs_tests"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "rmf_building_map_msgs"
-version = "1.4.0"
-
-[[patch.unused]]
-name = "rmf_site_format"
-version = "0.0.1"
-
-[[patch.unused]]
-name = "rosgraph_msgs"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "rosidl_runtime_rs"
-version = "0.4.1"
-
-[[patch.unused]]
-name = "sensor_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "service_msgs"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "shape_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "statistics_msgs"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "std_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "std_srvs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "stereo_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "test_msgs"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "trajectory_msgs"
-version = "5.0.0"
-
-[[patch.unused]]
-name = "type_description_interfaces"
-version = "1.6.0"
-
-[[patch.unused]]
-name = "unique_identifier_msgs"
-version = "2.3.2"
-
-[[patch.unused]]
-name = "visualization_msgs"
-version = "5.0.0"
diff --git a/rmf_site_editor/src/lib.rs b/rmf_site_editor/src/lib.rs
index d42a3115..5e97cd86 100644
--- a/rmf_site_editor/src/lib.rs
+++ b/rmf_site_editor/src/lib.rs
@@ -157,31 +157,29 @@ impl Plugin for SiteEditor {
                 false
             }
         };
-        {
-            plugins = if headless {
-                plugins
-                    .set(WindowPlugin {
-                        primary_window: None,
-                        exit_condition: bevy::window::ExitCondition::DontExit,
-                        close_when_requested: false,
-                    })
-                    .disable::<bevy::winit::WinitPlugin>()
-            } else {
-                plugins.set(WindowPlugin {
-                    primary_window: Some(Window {
-                        title: "RMF Site Editor".to_owned(),
-                        #[cfg(not(target_arch = "wasm32"))]
-                        resolution: (1600., 900.).into(),
-                        #[cfg(target_arch = "wasm32")]
-                        canvas: Some(String::from("#rmf_site_editor_canvas")),
-                        #[cfg(target_arch = "wasm32")]
-                        fit_canvas_to_parent: true,
-                        ..default()
-                    }),
-                    ..default()
+        plugins = if headless {
+            plugins
+                .set(WindowPlugin {
+                    primary_window: None,
+                    exit_condition: bevy::window::ExitCondition::DontExit,
+                    close_when_requested: false,
                 })
-            };
-        }
+                .disable::<bevy::winit::WinitPlugin>()
+        } else {
+            plugins.set(WindowPlugin {
+                primary_window: Some(Window {
+                    title: "RMF Site Editor".to_owned(),
+                    #[cfg(not(target_arch = "wasm32"))]
+                    resolution: (1600., 900.).into(),
+                    #[cfg(target_arch = "wasm32")]
+                    canvas: Some(String::from("#rmf_site_editor_canvas")),
+                    #[cfg(target_arch = "wasm32")]
+                    fit_canvas_to_parent: true,
+                    ..default()
+                }),
+                ..default()
+            })
+        };
         app.add_plugins((
             SiteAssetIoPlugin,
             plugins
diff --git a/rmf_site_editor/src/site/door.rs b/rmf_site_editor/src/site/door.rs
index 41457cc1..51e0f672 100644
--- a/rmf_site_editor/src/site/door.rs
+++ b/rmf_site_editor/src/site/door.rs
@@ -73,7 +73,7 @@ impl DoorBodyType {
         }
     }
 
-    pub fn labels(&self) -> Vec<&str> {
+    pub fn links(&self) -> Vec<&str> {
         match self {
             DoorBodyType::SingleSwing { .. }
             | DoorBodyType::SingleSliding { .. }
diff --git a/rmf_site_editor/src/site/file_menu.rs b/rmf_site_editor/src/site/file_menu.rs
index f067c284..c73516f8 100644
--- a/rmf_site_editor/src/site/file_menu.rs
+++ b/rmf_site_editor/src/site/file_menu.rs
@@ -26,6 +26,12 @@ pub struct ExportSdfMenu {
     export_sdf: Entity,
 }
 
+impl ExportSdfMenu {
+    pub fn get(&self) -> Entity {
+        self.export_sdf
+    }
+}
+
 impl FromWorld for ExportSdfMenu {
     fn from_world(world: &mut World) -> Self {
         let site_states = HashSet::from([
@@ -52,7 +58,7 @@ pub fn handle_export_sdf_menu_events(
     mut save_events: EventWriter<SaveWorkspace>,
 ) {
     for event in menu_events.read() {
-        if event.clicked() && event.source() == sdf_menu.export_sdf {
+        if event.clicked() && event.source() == sdf_menu.get() {
             save_events.send(SaveWorkspace {
                 destination: SaveWorkspaceDestination::Dialog,
                 format: ExportFormat::Sdf,
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index e232c0d1..8f01d444 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1254,7 +1254,7 @@ pub fn save_site(world: &mut World) {
                         }
                     }
                 } else {
-                    match site.to_writer(f) {
+                    match site.to_writer_ron(f) {
                         Ok(()) => {
                             info!("Save successful");
                         }
@@ -1270,9 +1270,12 @@ pub fn save_site(world: &mut World) {
             ExportFormat::Sdf => {
                 // TODO(luca) reduce code duplication with default exporting
                 info!("Saving to {}", new_path.display());
-                let parent_folder = new_path.parent().unwrap();
+                let Some(parent_folder) = new_path.parent() else {
+                    error!("Unable to save SDF. Please select a save path that has a parent directory.");
+                    continue;
+                };
                 if !parent_folder.exists() {
-                    if let Err(e) = std::fs::create_dir_all(new_path.parent().unwrap()) {
+                    if let Err(e) = std::fs::create_dir_all(parent_folder) {
                         error!("Unable to create folder {}: {e}", parent_folder.display());
                         continue;
                     }
@@ -1285,11 +1288,15 @@ pub fn save_site(world: &mut World) {
                     }
                 };
 
-                let mut meshes_dir = PathBuf::from(new_path.parent().unwrap());
+                let mut meshes_dir = PathBuf::from(parent_folder);
                 meshes_dir.push("meshes");
-                std::fs::create_dir(&meshes_dir).ok();
+                if let Err(e) = std::fs::create_dir_all(&meshes_dir) {
+                    error!("Unable to create folder {}: {e}", meshes_dir.display());
+                    continue;
+                }
                 if let Err(e) = collect_site_meshes(world, save_event.site, &meshes_dir) {
                     error!("Unable to collect site meshes: {e}");
+                    continue;
                 }
 
                 migrate_relative_paths(save_event.site, &new_path, world);
@@ -1314,10 +1321,16 @@ pub fn save_site(world: &mut World) {
                     write_document_declaration: true,
                     ..Default::default()
                 };
-                yaserde::ser::serialize_with_writer(&sdf, f, &config).unwrap();
-                let mut navgraph_dir = PathBuf::from(new_path.parent().unwrap());
+                if let Err(e) = yaserde::ser::serialize_with_writer(&sdf, f, &config) {
+                    error!("Failed serializing site to sdf: {e}");
+                    continue;
+                }
+                let mut navgraph_dir = PathBuf::from(parent_folder);
                 navgraph_dir.push("nav_graphs");
-                std::fs::create_dir(&navgraph_dir).ok();
+                if let Err(e) = std::fs::create_dir_all(&navgraph_dir) {
+                    error!("Unable to create folder {}: {e}", navgraph_dir.display());
+                    continue;
+                }
                 for (name, graph) in &graphs {
                     let mut graph_file = navgraph_dir.clone();
                     graph_file.push(name.to_owned() + ".yaml");
@@ -1402,7 +1415,7 @@ pub fn save_nav_graphs(world: &mut World) {
             }
         };
 
-        match site.to_writer(f) {
+        match site.to_writer_ron(f) {
             Ok(()) => {
                 info!("Save successful");
             }
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index de7af539..ee3845aa 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -11,7 +11,7 @@ use crate::site::{
     LiftDoormat, ModelSceneRoot, TentativeModelFormat, VisualMeshMarker,
 };
 use rmf_site_format::{
-    IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, NameOfSite, WallMarker,
+    IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, NameOfSite, SiteID, WallMarker,
 };
 
 /// Manages a simple state machine where we:
@@ -94,6 +94,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
         Query<((), With<LiftDoormat>)>,
         Query<&GlobalTransform>,
         Query<&Transform>,
+        Query<&SiteID>,
     )> = SystemState::new(world);
     let (
         q_children,
@@ -109,6 +110,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
         q_lift_door_mats,
         q_global_tfs,
         q_tfs,
+        q_site_ids,
     ) = state.get(world);
 
     let image_assets = world.resource::<Assets<Image>>();
@@ -131,11 +133,19 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
             return None;
         };
         let Some(mesh) = mesh_assets.get(mesh) else {
-            warn!("Mesh asset not found");
+            let site_id = q_site_ids.get(entity);
+            warn!(
+                "Mesh asset not found for entity {:?} with Site ID {:?} while exporting assets",
+                entity, site_id
+            );
             return None;
         };
         let Some(material) = material_assets.get(material) else {
-            warn!("Material asset not found");
+            let site_id = q_site_ids.get(entity);
+            warn!(
+                "Material asset not found for entity {:?} with Site ID {:?} while exporting assets",
+                entity, site_id
+            );
             return None;
         };
         Some((mesh, material))
@@ -187,7 +197,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     // TODO(luca) don't do full descendant iter here or we might add twice?
                     // Iterate through children and select all meshes
                     for model_child in DescendantIter::new(&q_children, model) {
-                        if q_collisions.get(model_child).is_ok() {
+                        if q_collisions.contains(model_child) {
                             // Now iterate through the children of the collision and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
                                 let Some((mesh, _)) = get_mesh_and_material(entity) else {
@@ -204,7 +214,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                     transform: Some(tf),
                                 });
                             }
-                        } else if q_visuals.get(model_child).is_ok() {
+                        } else if q_visuals.contains(model_child) {
                             // Now iterate through the children of the visuals and add them
                             for entity in DescendantIter::new(&q_children, model_child) {
                                 let Some((mesh, material)) = get_mesh_and_material(entity) else {
@@ -249,7 +259,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                         .body
                         .entities()
                         .iter()
-                        .zip(segments.body.labels().into_iter())
+                        .zip(segments.body.links().into_iter())
                     {
                         // Generate the visual and collisions here
                         let Some((mesh, material)) = get_mesh_and_material(*entity) else {
@@ -329,7 +339,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                         .body
                         .entities()
                         .iter()
-                        .zip(segments.body.labels().into_iter())
+                        .zip(segments.body.links().into_iter())
                     {
                         // Generate the visual and collisions here
                         let Some((mesh, material)) = get_mesh_and_material(*entity) else {
diff --git a/rmf_site_editor/src/widgets/view_nav_graphs.rs b/rmf_site_editor/src/widgets/view_nav_graphs.rs
index 7f4c76b6..6752717d 100644
--- a/rmf_site_editor/src/widgets/view_nav_graphs.rs
+++ b/rmf_site_editor/src/widgets/view_nav_graphs.rs
@@ -250,7 +250,9 @@ impl<'a, 'w1, 's1, 'w2, 's2> ViewNavGraphs<'a, 'w1, 's1, 'w2, 's2> {
                                         None => return None,
                                     };
 
-                                    match rmf_site_format::Site::from_bytes(&file.read().await) {
+                                    // TODO(luca) make this accept different file formats
+                                    match rmf_site_format::Site::from_bytes_ron(&file.read().await)
+                                    {
                                         Ok(from_site) => Some((
                                             file.path().to_owned(),
                                             ImportNavGraphs {
diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs
index f4e3531d..b18bc527 100644
--- a/rmf_site_editor/src/workspace.rs
+++ b/rmf_site_editor/src/workspace.rs
@@ -349,7 +349,7 @@ fn workspace_file_load_complete(
             }
             WorkspaceData::RonSite(data) => {
                 info!("Opening site file");
-                match Site::from_bytes(&data) {
+                match Site::from_bytes_ron(&data) {
                     Ok(site) => {
                         // Switch state
                         app_state.set(AppState::SiteEditor);
@@ -367,7 +367,7 @@ fn workspace_file_load_complete(
             }
             WorkspaceData::JsonSite(data) => {
                 info!("Opening site file");
-                match Site::from_json_bytes(&data) {
+                match Site::from_bytes_json(&data) {
                     Ok(site) => {
                         // Switch state
                         app_state.set(AppState::SiteEditor);
diff --git a/rmf_site_format/src/camera_poses.rs b/rmf_site_format/src/camera_poses.rs
index 185e0556..b41de0a5 100644
--- a/rmf_site_format/src/camera_poses.rs
+++ b/rmf_site_format/src/camera_poses.rs
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2022 Open Source Robotics Foundation
+ * 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.
diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs
index 6383fb75..3c255c5a 100644
--- a/rmf_site_format/src/legacy/building_map.rs
+++ b/rmf_site_format/src/legacy/building_map.rs
@@ -736,7 +736,10 @@ mod tests {
     fn site_conversion() {
         let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
         let map = BuildingMap::from_bytes(&data).unwrap();
-        println!("{}", map.to_site().unwrap().to_string().unwrap());
+        println!(
+            "{}",
+            String::from_utf8_lossy(&map.to_site().unwrap().to_bytes_json().unwrap())
+        );
     }
 
     #[test]
diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs
index 15251281..6311ed8a 100644
--- a/rmf_site_format/src/site.rs
+++ b/rmf_site_format/src/site.rs
@@ -154,21 +154,19 @@ fn default_style_config() -> Style {
 }
 
 impl Site {
-    // TODO(luca) the functions below assume that we only serialize to ron, now that we are adding
-    // json these should probably be renamed or made to accept an enum for the format?
-    pub fn to_writer<W: io::Write>(&self, writer: W) -> ron::Result<()> {
+    pub fn to_writer_ron<W: io::Write>(&self, writer: W) -> ron::Result<()> {
         ron::ser::to_writer_pretty(writer, self, default_style_config())
     }
 
-    pub fn to_writer_custom<W: io::Write>(&self, writer: W, style: Style) -> ron::Result<()> {
+    pub fn to_writer_custom_ron<W: io::Write>(&self, writer: W, style: Style) -> ron::Result<()> {
         ron::ser::to_writer_pretty(writer, self, style)
     }
 
-    pub fn to_string(&self) -> ron::Result<String> {
+    pub fn to_string_ron(&self) -> ron::Result<String> {
         ron::ser::to_string_pretty(self, default_style_config())
     }
 
-    pub fn to_string_custom(&self, style: Style) -> ron::Result<String> {
+    pub fn to_string_custom_ron(&self, style: Style) -> ron::Result<String> {
         ron::ser::to_string_pretty(self, style)
     }
 
@@ -176,25 +174,25 @@ impl Site {
         serde_json::to_writer_pretty(writer, self)
     }
 
-    pub fn from_json_bytes(s: &[u8]) -> serde_json::Result<Self> {
+    pub fn from_bytes_json(s: &[u8]) -> serde_json::Result<Self> {
         serde_json::from_slice(s)
     }
 
-    pub fn from_reader<R: io::Read>(reader: R) -> ron::error::SpannedResult<Self> {
+    pub fn from_reader_ron<R: io::Read>(reader: R) -> ron::error::SpannedResult<Self> {
         // TODO(MXG): Validate the parsed data, e.g. make sure anchor pairs
         // belong to the same level.
         ron::de::from_reader(reader)
     }
 
-    pub fn from_str<'a>(s: &'a str) -> ron::error::SpannedResult<Self> {
+    pub fn from_str_ron<'a>(s: &'a str) -> ron::error::SpannedResult<Self> {
         ron::de::from_str(s)
     }
 
-    pub fn to_json_str(&self) -> serde_json::Result<Vec<u8>> {
+    pub fn to_bytes_json(&self) -> serde_json::Result<Vec<u8>> {
         serde_json::to_vec_pretty(self)
     }
 
-    pub fn from_bytes<'a>(s: &'a [u8]) -> ron::error::SpannedResult<Self> {
+    pub fn from_bytes_ron<'a>(s: &'a [u8]) -> ron::error::SpannedResult<Self> {
         ron::de::from_bytes(s)
     }
 
@@ -221,15 +219,15 @@ mod tests {
     fn ron_roundtrip() {
         let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
         let map = BuildingMap::from_bytes(&data).unwrap();
-        let site_string = map.to_site().unwrap().to_string().unwrap();
-        Site::from_str(&site_string).unwrap();
+        let site_string = map.to_site().unwrap().to_string_ron().unwrap();
+        Site::from_str_ron(&site_string).unwrap();
     }
 
     #[test]
     fn json_roundtrip() {
         let data = std::fs::read("../assets/demo_maps/office.building.yaml").unwrap();
         let map = BuildingMap::from_bytes(&data).unwrap();
-        let site_string = map.to_site().unwrap().to_json_str().unwrap();
-        Site::from_json_bytes(&site_string).unwrap();
+        let site_string = map.to_site().unwrap().to_bytes_json().unwrap();
+        Site::from_bytes_json(&site_string).unwrap();
     }
 }

From 6426ece2a8f3f9fd1718b1d66d9b5f6bbe9ef3e3 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 31 May 2024 15:17:27 +0200
Subject: [PATCH 65/71] Only set camera pose on changed site

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/level.rs | 30 --------------------------
 rmf_site_editor/src/site/mod.rs   |  2 +-
 rmf_site_editor/src/site/site.rs  | 35 +++++++++++++++++++++++++++++--
 3 files changed, 34 insertions(+), 33 deletions(-)

diff --git a/rmf_site_editor/src/site/level.rs b/rmf_site_editor/src/site/level.rs
index fcaebac9..25610cea 100644
--- a/rmf_site_editor/src/site/level.rs
+++ b/rmf_site_editor/src/site/level.rs
@@ -66,33 +66,3 @@ pub fn assign_orphan_elements_to_level<T: Component>(
         commands.entity(current_level).add_child(orphan);
     }
 }
-
-pub fn set_camera_transform_on_level_change(
-    current_level: Res<CurrentLevel>,
-    mut camera_controls: ResMut<CameraControls>,
-    camera_poses: Query<&CameraPoses>,
-    mut transforms: Query<&mut Transform>,
-) {
-    if current_level.is_changed() {
-        let Some(level) = current_level.0 else {
-            return;
-        };
-
-        if let Ok(poses) = camera_poses.get(level) {
-            // TODO(luca) Add an actual default pose rather than first in map
-            let Some(pose) = poses.0.values().next() else {
-                return;
-            };
-            if let Ok(mut tf) = transforms.get_mut(camera_controls.perspective_camera_entities[0]) {
-                *tf = pose.transform();
-            }
-            let mut translation = pose.transform().translation;
-            // TODO(luca) these are the same value that are in rmf_site_format, should we change
-            // them?
-            translation.x = translation.x + 10.0;
-            translation.y = translation.y + 10.0;
-            translation.z = 0.0;
-            camera_controls.orbit_center = translation;
-        }
-    }
-}
diff --git a/rmf_site_editor/src/site/mod.rs b/rmf_site_editor/src/site/mod.rs
index f8e3ba84..904efcb9 100644
--- a/rmf_site_editor/src/site/mod.rs
+++ b/rmf_site_editor/src/site/mod.rs
@@ -341,7 +341,7 @@ impl Plugin for SitePlugin {
                 align_site_drawings,
                 clear_old_issues_on_new_validate_event,
                 export_lights,
-                set_camera_transform_on_level_change,
+                set_camera_transform_for_changed_site,
             )
                 .run_if(AppState::in_site_mode())
                 .in_set(SiteUpdateSet::BetweenVisibilityAndTransform),
diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs
index 15bf2a69..5a01be7d 100644
--- a/rmf_site_editor/src/site/site.rs
+++ b/rmf_site_editor/src/site/site.rs
@@ -15,9 +15,9 @@
  *
 */
 
-use crate::CurrentWorkspace;
+use crate::{interaction::CameraControls, CurrentWorkspace};
 use bevy::prelude::*;
-use rmf_site_format::{LevelElevation, LevelProperties, NameInSite, NameOfSite};
+use rmf_site_format::{CameraPoses, LevelElevation, LevelProperties, NameInSite, NameOfSite};
 
 /// Used as an event to command that a new site should be made the current one
 #[derive(Clone, Copy, Debug, Event)]
@@ -136,3 +136,34 @@ pub fn change_site(
         }
     }
 }
+
+pub fn set_camera_transform_for_changed_site(
+    current_workspace: Res<CurrentWorkspace>,
+    current_level: Res<CurrentLevel>,
+    mut camera_controls: ResMut<CameraControls>,
+    camera_poses: Query<&CameraPoses>,
+    mut transforms: Query<&mut Transform>,
+) {
+    if current_workspace.is_changed() {
+        let Some(level) = current_level.0 else {
+            return;
+        };
+
+        if let Ok(poses) = camera_poses.get(level) {
+            // TODO(luca) Add an actual default pose rather than first in map
+            let Some(pose) = poses.0.values().next() else {
+                return;
+            };
+            if let Ok(mut tf) = transforms.get_mut(camera_controls.perspective_camera_entities[0]) {
+                *tf = pose.transform();
+            }
+            let mut translation = pose.transform().translation;
+            // TODO(luca) these are the same value that are in rmf_site_format, should we change
+            // them?
+            translation.x = translation.x + 10.0;
+            translation.y = translation.y + 10.0;
+            translation.z = 0.0;
+            camera_controls.orbit_center = translation;
+        }
+    }
+}

From abb754406b78d8720c78f11d86950227496528cc Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 31 May 2024 16:59:37 +0200
Subject: [PATCH 66/71] Make each pose a UserCameraPose Bundle

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/load.rs           |  7 +++++++
 rmf_site_editor/src/site/save.rs           | 15 +++++++++++---
 rmf_site_editor/src/site/site.rs           | 19 +++++++++--------
 rmf_site_format/src/camera_poses.rs        | 24 ++++++++++++++++------
 rmf_site_format/src/legacy/building_map.rs | 23 ++++++++++++---------
 rmf_site_format/src/level.rs               |  6 +++---
 rmf_site_format/src/sdf.rs                 |  6 +++---
 7 files changed, 67 insertions(+), 33 deletions(-)

diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs
index cacd0f57..d64434eb 100644
--- a/rmf_site_editor/src/site/load.rs
+++ b/rmf_site_editor/src/site/load.rs
@@ -210,6 +210,13 @@ fn generate_site_entities(
                         .insert(SiteID(*physical_camera_id));
                     consider_id(*physical_camera_id);
                 }
+
+                for (camera_pose_id, camera_pose) in &level_data.user_camera_poses {
+                    level
+                        .spawn(camera_pose.clone())
+                        .insert(SiteID(*camera_pose_id));
+                    consider_id(*camera_pose_id);
+                }
             });
 
         // TODO(MXG): Log when a RecencyRanking fails to load correctly.
diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 8f01d444..0ab2ec9c 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -345,11 +345,11 @@ fn generate_levels(
                 &Children,
                 Option<&RecencyRanking<FloorMarker>>,
                 Option<&RecencyRanking<DrawingMarker>>,
-                &CameraPoses,
             ),
             Without<Pending>,
         >,
         Query<&SiteID>,
+        Query<(&Pose, &NameInSite, &SiteID), With<UserCameraPoseMarker>>,
     )> = SystemState::new(world);
 
     let (
@@ -367,6 +367,7 @@ fn generate_levels(
         q_walls,
         q_levels,
         q_site_ids,
+        q_user_camera_poses,
     ) = state.get(world);
 
     let get_anchor_id = |entity| {
@@ -411,7 +412,6 @@ fn generate_levels(
                 level_children,
                 floor_ranking,
                 drawing_ranking,
-                camera_poses,
             )) = q_levels.get(*c)
             {
                 let mut level = Level::new(
@@ -420,7 +420,6 @@ fn generate_levels(
                         elevation: elevation.clone(),
                         global_floor_visibility: floor_vis.clone(),
                         global_drawing_visibility: drawing_vis.clone(),
-                        camera_poses: camera_poses.clone(),
                     },
                     RankingsInLevel {
                         floors: floor_ranking
@@ -581,6 +580,16 @@ fn generate_levels(
                             },
                         );
                     }
+                    if let Ok((pose, name, id)) = q_user_camera_poses.get(*c) {
+                        level.user_camera_poses.insert(
+                            id.0,
+                            UserCameraPose {
+                                name: name.clone(),
+                                pose: pose.clone(),
+                                marker: UserCameraPoseMarker,
+                            },
+                        );
+                    }
                 }
                 levels.insert(level_id.0, level);
             }
diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs
index 5a01be7d..cc559edd 100644
--- a/rmf_site_editor/src/site/site.rs
+++ b/rmf_site_editor/src/site/site.rs
@@ -17,7 +17,9 @@
 
 use crate::{interaction::CameraControls, CurrentWorkspace};
 use bevy::prelude::*;
-use rmf_site_format::{CameraPoses, LevelElevation, LevelProperties, NameInSite, NameOfSite};
+use rmf_site_format::{
+    LevelElevation, LevelProperties, NameInSite, NameOfSite, Pose, UserCameraPoseMarker,
+};
 
 /// Used as an event to command that a new site should be made the current one
 #[derive(Clone, Copy, Debug, Event)]
@@ -123,7 +125,6 @@ pub fn change_site(
                                 elevation: LevelElevation(0.),
                                 global_floor_visibility: default(),
                                 global_drawing_visibility: default(),
-                                camera_poses: default(),
                             })
                             .set_parent(cmd.site)
                             .id();
@@ -141,7 +142,8 @@ pub fn set_camera_transform_for_changed_site(
     current_workspace: Res<CurrentWorkspace>,
     current_level: Res<CurrentLevel>,
     mut camera_controls: ResMut<CameraControls>,
-    camera_poses: Query<&CameraPoses>,
+    children: Query<&Children>,
+    user_camera_poses: Query<&Pose, With<UserCameraPoseMarker>>,
     mut transforms: Query<&mut Transform>,
 ) {
     if current_workspace.is_changed() {
@@ -149,11 +151,12 @@ pub fn set_camera_transform_for_changed_site(
             return;
         };
 
-        if let Ok(poses) = camera_poses.get(level) {
-            // TODO(luca) Add an actual default pose rather than first in map
-            let Some(pose) = poses.0.values().next() else {
-                return;
-            };
+        // TODO(luca) Add an actual default pose rather than first in query
+        if let Some(pose) = children
+            .get(level)
+            .ok()
+            .and_then(|children| children.iter().find_map(|c| user_camera_poses.get(*c).ok()))
+        {
             if let Ok(mut tf) = transforms.get_mut(camera_controls.perspective_camera_entities[0]) {
                 *tf = pose.transform();
             }
diff --git a/rmf_site_format/src/camera_poses.rs b/rmf_site_format/src/camera_poses.rs
index b41de0a5..0f74286a 100644
--- a/rmf_site_format/src/camera_poses.rs
+++ b/rmf_site_format/src/camera_poses.rs
@@ -15,19 +15,27 @@
  *
 */
 
-use crate::{Anchor, Category, Pose, Rotation};
+use crate::{Anchor, Category, NameInSite, Pose, Rotation};
 #[cfg(feature = "bevy")]
-use bevy::prelude::Component;
+use bevy::prelude::{Bundle, Component};
 use glam::{Affine3A, Quat, Vec3};
 use serde::{Deserialize, Serialize};
 use std::collections::HashMap;
 
+#[derive(Serialize, Deserialize, Default, Debug, Clone, PartialEq)]
+#[cfg_attr(feature = "bevy", derive(Bundle))]
+pub struct UserCameraPose {
+    pub pose: Pose,
+    pub name: NameInSite,
+    pub marker: UserCameraPoseMarker,
+}
+
 #[derive(Serialize, Deserialize, Default, Debug, Clone, PartialEq)]
 #[cfg_attr(feature = "bevy", derive(Component))]
-pub struct CameraPoses(pub HashMap<String, Pose>);
+pub struct UserCameraPoseMarker;
 
-impl CameraPoses {
-    pub fn from_anchors<'a, I>(anchors: I) -> Self
+impl UserCameraPose {
+    pub fn from_anchors<'a, I>(name: &str, anchors: I) -> Self
     where
         I: Iterator<Item = &'a Anchor>,
     {
@@ -56,6 +64,10 @@ impl CameraPoses {
             trans: (trans + offset).into(),
             rot: Rotation::Quat(rot),
         };
-        Self(HashMap::from([("default".to_string(), pose)]))
+        Self {
+            pose,
+            name: NameInSite(name.into()),
+            marker: Default::default(),
+        }
     }
 }
diff --git a/rmf_site_format/src/legacy/building_map.rs b/rmf_site_format/src/legacy/building_map.rs
index 3c255c5a..a69610eb 100644
--- a/rmf_site_format/src/legacy/building_map.rs
+++ b/rmf_site_format/src/legacy/building_map.rs
@@ -3,13 +3,12 @@ use super::{
 };
 use crate::{
     alignment::align_legacy_building, Affiliation, Anchor, Angle, AssetSource, AssociatedGraphs,
-    CameraPoses, Category, DisplayColor, Dock as SiteDock, Drawing as SiteDrawing,
-    DrawingProperties, Fiducial as SiteFiducial, FiducialGroup, FiducialMarker, Guided,
-    Lane as SiteLane, LaneMarker, Level as SiteLevel, LevelElevation,
-    LevelProperties as SiteLevelProperties, Motion, NameInSite, NameOfSite, NavGraph, Navigation,
-    OrientationConstraint, PixelsPerMeter, Pose, PreferredSemiTransparency, RankingsInLevel,
-    ReverseLane, Rotation, Site, SiteProperties, Texture as SiteTexture, TextureGroup,
-    DEFAULT_NAV_GRAPH_COLORS,
+    Category, DisplayColor, Dock as SiteDock, Drawing as SiteDrawing, DrawingProperties,
+    Fiducial as SiteFiducial, FiducialGroup, FiducialMarker, Guided, Lane as SiteLane, LaneMarker,
+    Level as SiteLevel, LevelElevation, LevelProperties as SiteLevelProperties, Motion, NameInSite,
+    NameOfSite, NavGraph, Navigation, OrientationConstraint, PixelsPerMeter, Pose,
+    PreferredSemiTransparency, RankingsInLevel, ReverseLane, Rotation, Site, SiteProperties,
+    Texture as SiteTexture, TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS,
 };
 use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot};
 use serde::{Deserialize, Serialize};
@@ -528,7 +527,11 @@ impl BuildingMap {
                 walls.insert(site_id.next().unwrap(), site_wall);
             }
 
-            let elevation = level.elevation as f32;
+            let mut user_camera_poses = BTreeMap::new();
+            user_camera_poses.insert(
+                site_id.next().unwrap(),
+                UserCameraPose::from_anchors("default", level_anchors.values()),
+            );
 
             level_name_to_id.insert(level_name.clone(), level_id);
             levels.insert(
@@ -536,10 +539,9 @@ impl BuildingMap {
                 SiteLevel {
                     properties: SiteLevelProperties {
                         name: NameInSite(level_name.clone()),
-                        elevation: LevelElevation(elevation),
+                        elevation: LevelElevation(level.elevation as f32),
                         global_floor_visibility: Default::default(),
                         global_drawing_visibility: Default::default(),
-                        camera_poses: CameraPoses::from_anchors(level_anchors.values()),
                     },
                     anchors: level_anchors,
                     doors,
@@ -550,6 +552,7 @@ impl BuildingMap {
                     physical_cameras,
                     walls,
                     rankings,
+                    user_camera_poses,
                 },
             );
 
diff --git a/rmf_site_format/src/level.rs b/rmf_site_format/src/level.rs
index f7f8a7c0..6981fc78 100644
--- a/rmf_site_format/src/level.rs
+++ b/rmf_site_format/src/level.rs
@@ -30,8 +30,6 @@ pub struct LevelProperties {
     pub global_floor_visibility: GlobalFloorVisibility,
     #[serde(default, skip_serializing_if = "is_default")]
     pub global_drawing_visibility: GlobalDrawingVisibility,
-    #[serde(default, skip_serializing_if = "is_default")]
-    pub camera_poses: CameraPoses,
 }
 
 impl Default for LevelProperties {
@@ -41,7 +39,6 @@ impl Default for LevelProperties {
             elevation: LevelElevation(0.0),
             global_floor_visibility: Default::default(),
             global_drawing_visibility: Default::default(),
-            camera_poses: Default::default(),
         }
     }
 }
@@ -71,6 +68,8 @@ pub struct Level {
     pub walls: BTreeMap<u32, Wall<u32>>,
     #[serde(default, skip_serializing_if = "RankingsInLevel::is_empty")]
     pub rankings: RankingsInLevel,
+    #[serde(default, skip_serializing_if = "BTreeMap::is_empty")]
+    pub user_camera_poses: BTreeMap<u32, UserCameraPose>,
 }
 
 impl Level {
@@ -86,6 +85,7 @@ impl Level {
             models: Default::default(),
             physical_cameras: Default::default(),
             walls: Default::default(),
+            user_camera_poses: Default::default(),
         }
     }
 }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 265b77b5..06c7fdb7 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -811,14 +811,14 @@ impl Site {
                 .find(|plugin| plugin.filename == "MinimalScene")
             {
                 if let Some(camera_pose) = minimal_scene.elements.get_mut("camera_pose") {
-                    if let Some(pose) = self
+                    if let Some(user_camera_pose) = self
                         .levels
                         .first_key_value()
-                        .and_then(|(_, level)| level.properties.camera_poses.0.values().next())
+                        .and_then(|(_, level)| level.user_camera_poses.values().next())
                     {
                         // TODO(luca) use level elevation here? It also seems that quaternion
                         // notation in Gazebo and Bevy is different, check
-                        let mut pose = pose.clone();
+                        let mut pose = user_camera_pose.pose.clone();
                         pose.rot = Rotation::EulerExtrinsicXYZ([
                             Angle::Rad(0.0),
                             Angle::Rad(0.6),

From 65d9f8a265f2722fe584e92089d5d5d572183f9e Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 31 May 2024 18:41:08 +0200
Subject: [PATCH 67/71] Make sure non static models use generated meshes

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/sdf_exporter.rs |  6 ++++--
 rmf_site_format/src/sdf.rs               | 27 ++++++++++++++++++++----
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index ee3845aa..5fa1da71 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -208,10 +208,12 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                 };
                                 let mut tf = tf.compute_transform();
                                 tf.translation.z = tf.translation.z + **elevation;
+                                // Non static meshes have their translation in the SDF element, not in the
+                                // gltf node
                                 model_collisions.push(MeshData {
                                     mesh,
                                     material: None,
-                                    transform: Some(tf),
+                                    transform: is_static.then_some(tf),
                                 });
                             }
                         } else if q_visuals.contains(model_child) {
@@ -228,7 +230,7 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                                 model_visuals.push(MeshData {
                                     mesh,
                                     material: Some(material),
-                                    transform: Some(tf),
+                                    transform: is_static.then_some(tf),
                                 });
                             }
                         }
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 06c7fdb7..2b0e9ff1 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -522,11 +522,30 @@ impl Site {
                 // TODO(luca) this will duplicate multiple instances of the model since it uses
                 // NameInSite instead of AssetSource for the URI, fix
                 else if !model.is_static.0 {
-                    world.include.push(SdfWorldInclude {
-                        uri: format!("model://{}", model.name.0.clone()),
-                        name: Some(model.name.0.clone()),
-                        pose: Some(model.pose.to_sdf()),
+                    world.model.push(SdfModel {
+                        name: model.name.0.clone(),
                         r#static: Some(model.is_static.0),
+                        pose: Some(model.pose.to_sdf()),
+                        link: vec![SdfLink {
+                            name: "link".into(),
+                            collision: vec![SdfCollision {
+                                name: "collision".into(),
+                                geometry: SdfGeometry::Mesh(SdfMeshShape {
+                                    uri: format!("meshes/{}_collision.glb", model.name.0),
+                                    ..Default::default()
+                                }),
+                                ..Default::default()
+                            }],
+                            visual: vec![SdfVisual {
+                                name: "visual".into(),
+                                geometry: SdfGeometry::Mesh(SdfMeshShape {
+                                    uri: format!("meshes/{}_visual.glb", model.name.0),
+                                    ..Default::default()
+                                }),
+                                ..Default::default()
+                            }],
+                            ..Default::default()
+                        }],
                         ..Default::default()
                     });
                     added = true;

From bc5089231c9422d8d19c42aa3b88fa8a2be94543 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <mxgrey@intrinsic.ai>
Date: Wed, 5 Jun 2024 16:39:19 +0000
Subject: [PATCH 68/71] Make SDF export more consistent with old building tools

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
---
 rmf_site_format/src/lift.rs | 15 +++++++++++++++
 rmf_site_format/src/sdf.rs  | 23 +++++++++--------------
 2 files changed, 24 insertions(+), 14 deletions(-)

diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs
index 397d4db1..ba423067 100644
--- a/rmf_site_format/src/lift.rs
+++ b/rmf_site_format/src/lift.rs
@@ -233,6 +233,21 @@ impl<T: RefTrait> LiftCabin<T> {
         };
         Ok(result)
     }
+
+    pub fn moment_of_inertia(&self, mass: f64) -> sdformat_rs::SdfInertialInertia {
+        match self {
+            Self::Rect(params) => {
+                sdformat_rs::SdfInertialInertia {
+                    ixx: mass/12.0 * (params.width.powi(2) + DEFAULT_CABIN_WALL_THICKNESS.powi(2)) as f64,
+                    iyy: mass/12.0 * (params.depth.powi(2) + DEFAULT_CABIN_WALL_THICKNESS.powi(2)) as f64,
+                    izz: mass/12.0 * (params.width.powi(2) + params.depth.powi(2)) as f64,
+                    ixy: 0.0,
+                    ixz: 0.0,
+                    iyz: 0.0,
+                }
+            }
+        }
+    }
 }
 
 #[derive(Clone, Debug)]
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 2b0e9ff1..6ade88bb 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -24,6 +24,8 @@ use sdformat_rs::*;
 use std::collections::BTreeMap;
 use thiserror::Error;
 
+const DEFAULT_CABIN_MASS: f64 = 1200.0;
+
 static WORLD_TEMPLATE: Lazy<SdfRoot> = Lazy::new(|| {
     yaserde::de::from_str(include_str!("templates/gz_world.sdf"))
         .expect("Failed deserializing template")
@@ -64,7 +66,6 @@ impl Pose {
 }
 
 fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink {
-    let door_mass = 50.0;
     SdfLink {
         name: link_name.to_string(),
         collision: vec![SdfCollision {
@@ -90,17 +91,6 @@ fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink {
             }),
             ..Default::default()
         }],
-        // TODO(luca) calculate inertia based on door properties
-        inertial: Some(SdfInertial {
-            mass: Some(door_mass),
-            inertia: Some(SdfInertialInertia {
-                ixx: 20.0,
-                iyy: 20.0,
-                izz: 5.0,
-                ..Default::default()
-            }),
-            ..Default::default()
-        }),
         ..Default::default()
     }
 }
@@ -634,8 +624,8 @@ impl Site {
                 axis: Some(SdfJointAxis {
                     xyz: Vector3d::new(0.0, 0.0, 1.0),
                     limit: SdfJointAxisLimit {
-                        lower: min_elevation.into(),
-                        upper: max_elevation.into(),
+                        lower: -std::f64::INFINITY,
+                        upper: std::f64::INFINITY,
                         ..Default::default()
                     },
                     ..Default::default()
@@ -774,6 +764,11 @@ impl Site {
                         }),
                         ..Default::default()
                     }],
+                    inertial: Some(SdfInertial {
+                        mass: Some(DEFAULT_CABIN_MASS),
+                        inertia: Some(lift.properties.cabin.moment_of_inertia(DEFAULT_CABIN_MASS)),
+                        ..Default::default()
+                    }),
                     ..Default::default()
                 }],
                 joint: lift_joints,

From 0e5e43b7899b80c015888334ee65d2dc3a691609 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 12 Jun 2024 16:15:02 +0800
Subject: [PATCH 69/71] Style

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/lift.rs | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/rmf_site_format/src/lift.rs b/rmf_site_format/src/lift.rs
index ba423067..0dd4bb28 100644
--- a/rmf_site_format/src/lift.rs
+++ b/rmf_site_format/src/lift.rs
@@ -236,16 +236,16 @@ impl<T: RefTrait> LiftCabin<T> {
 
     pub fn moment_of_inertia(&self, mass: f64) -> sdformat_rs::SdfInertialInertia {
         match self {
-            Self::Rect(params) => {
-                sdformat_rs::SdfInertialInertia {
-                    ixx: mass/12.0 * (params.width.powi(2) + DEFAULT_CABIN_WALL_THICKNESS.powi(2)) as f64,
-                    iyy: mass/12.0 * (params.depth.powi(2) + DEFAULT_CABIN_WALL_THICKNESS.powi(2)) as f64,
-                    izz: mass/12.0 * (params.width.powi(2) + params.depth.powi(2)) as f64,
-                    ixy: 0.0,
-                    ixz: 0.0,
-                    iyz: 0.0,
-                }
-            }
+            Self::Rect(params) => sdformat_rs::SdfInertialInertia {
+                ixx: mass / 12.0
+                    * (params.width.powi(2) + DEFAULT_CABIN_WALL_THICKNESS.powi(2)) as f64,
+                iyy: mass / 12.0
+                    * (params.depth.powi(2) + DEFAULT_CABIN_WALL_THICKNESS.powi(2)) as f64,
+                izz: mass / 12.0 * (params.width.powi(2) + params.depth.powi(2)) as f64,
+                ixy: 0.0,
+                ixz: 0.0,
+                iyz: 0.0,
+            },
         }
     }
 }

From 4f78ab37a5e03517da2c5511cdc6fbce65fa8f72 Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 14 Jun 2024 16:00:53 +0800
Subject: [PATCH 70/71] Remove unusued import

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_format/src/camera_poses.rs | 1 -
 1 file changed, 1 deletion(-)

diff --git a/rmf_site_format/src/camera_poses.rs b/rmf_site_format/src/camera_poses.rs
index 0f74286a..0c7605b2 100644
--- a/rmf_site_format/src/camera_poses.rs
+++ b/rmf_site_format/src/camera_poses.rs
@@ -20,7 +20,6 @@ use crate::{Anchor, Category, NameInSite, Pose, Rotation};
 use bevy::prelude::{Bundle, Component};
 use glam::{Affine3A, Quat, Vec3};
 use serde::{Deserialize, Serialize};
-use std::collections::HashMap;
 
 #[derive(Serialize, Deserialize, Default, Debug, Clone, PartialEq)]
 #[cfg_attr(feature = "bevy", derive(Bundle))]

From bb43fcbffe39a7e59a8018fa4fd93b84e1cc9dd3 Mon Sep 17 00:00:00 2001
From: Grey <grey@openrobotics.org>
Date: Fri, 14 Jun 2024 16:17:16 +0800
Subject: [PATCH 71/71] Use site ID to ensure unique file names (#219)

* Use site ID to ensure unique file names

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

* Fix style

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

* Fix door names for toggle floors plugin

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

---------

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Co-authored-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rmf_site_editor/src/site/save.rs         |  21 +++--
 rmf_site_editor/src/site/sdf_exporter.rs |  80 ++++++++++++----
 rmf_site_editor/src/workspace.rs         |   2 +
 rmf_site_format/src/sdf.rs               | 115 +++++++++++------------
 4 files changed, 133 insertions(+), 85 deletions(-)

diff --git a/rmf_site_editor/src/site/save.rs b/rmf_site_editor/src/site/save.rs
index 0ab2ec9c..06e5bf0f 100644
--- a/rmf_site_editor/src/site/save.rs
+++ b/rmf_site_editor/src/site/save.rs
@@ -1278,6 +1278,19 @@ pub fn save_site(world: &mut World) {
             }
             ExportFormat::Sdf => {
                 // TODO(luca) reduce code duplication with default exporting
+
+                // Make sure to generate the site before anything else, because
+                // generating the site will ensure that all items are assigned a
+                // SiteID, and the SDF export process will not work correctly if
+                // any are unassigned.
+                let site = match generate_site(world, save_event.site) {
+                    Ok(site) => site,
+                    Err(err) => {
+                        error!("Unable to compile site: {err}");
+                        continue;
+                    }
+                };
+
                 info!("Saving to {}", new_path.display());
                 let Some(parent_folder) = new_path.parent() else {
                     error!("Unable to save SDF. Please select a save path that has a parent directory.");
@@ -1309,14 +1322,6 @@ pub fn save_site(world: &mut World) {
                 }
 
                 migrate_relative_paths(save_event.site, &new_path, world);
-
-                let site = match generate_site(world, save_event.site) {
-                    Ok(site) => site,
-                    Err(err) => {
-                        error!("Unable to compile site: {err}");
-                        continue;
-                    }
-                };
                 let graphs = legacy::nav_graph::NavGraph::from_site(&site);
                 let sdf = match site.to_sdf() {
                     Ok(sdf) => sdf,
diff --git a/rmf_site_editor/src/site/sdf_exporter.rs b/rmf_site_editor/src/site/sdf_exporter.rs
index 5fa1da71..d3c14e74 100644
--- a/rmf_site_editor/src/site/sdf_exporter.rs
+++ b/rmf_site_editor/src/site/sdf_exporter.rs
@@ -53,7 +53,10 @@ pub fn headless_sdf_export(
         return;
     }
     if sites.is_empty() {
-        warn!("Site loading failed, aborting");
+        warn!(
+            "Unable to load site from file [{}] so we cannot export an SDF from it",
+            export_state.target_path,
+        );
         exit.send(bevy::app::AppExit);
     }
     if !missing_models.is_empty() {
@@ -128,6 +131,13 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
         std::fs::write(filename, bytes).map_err(|e| e.to_string())
     };
 
+    let get_site_id = |e: Entity| -> Result<u32, String> {
+        q_site_ids.get(e).map(|id| id.0).map_err(|_| {
+            let backtrace = std::backtrace::Backtrace::force_capture();
+            format!("Site ID was not available for entity {e:?}. Backtrace:\n{backtrace}")
+        })
+    };
+
     let get_mesh_and_material = |entity: Entity| -> Option<(&Mesh, &StandardMaterial)> {
         let Ok((mesh, material)) = q_pbr.get(entity) else {
             return None;
@@ -241,14 +251,22 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                         visual_data.extend(model_visuals);
                     } else {
                         // Create a new mesh for it
-                        let filename = format!("{}/{}_collision.glb", folder.display(), **name);
+                        let filename = format!(
+                            "{}/model_{}_collision.glb",
+                            folder.display(),
+                            get_site_id(*child)?,
+                        );
                         write_meshes_to_file(
                             model_collisions,
                             None,
                             CompressGltfOptions::skip_materials(),
                             filename,
                         )?;
-                        let filename = format!("{}/{}_visual.glb", folder.display(), **name);
+                        let filename = format!(
+                            "{}/model_{}_visual.glb",
+                            folder.display(),
+                            get_site_id(*child)?,
+                        );
                         write_meshes_to_file(
                             model_visuals,
                             Some(format!("{}_visual", **name)),
@@ -270,20 +288,22 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                         let Ok(tf) = q_tfs.get(*entity) else {
                             continue;
                         };
-                        let Some(door_name) = door_name else {
-                            continue;
-                        };
 
                         let data = MeshData {
                             mesh,
                             material: Some(material),
                             transform: Some(tf.clone()),
                         };
-                        let filename =
-                            format!("{}/{}_{}.glb", folder.display(), **door_name, segment_name);
+                        let filename = format!(
+                            "{}/door_{}_{}.glb",
+                            folder.display(),
+                            get_site_id(*child)?,
+                            segment_name,
+                        );
+                        let door_name = door_name.map(|n| n.0.as_str()).unwrap_or("");
                         write_meshes_to_file(
                             vec![data],
-                            None,
+                            Some(format!("door_{}_{}", door_name, segment_name)),
                             CompressGltfOptions::default(),
                             filename,
                         )?;
@@ -292,17 +312,25 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     continue;
                 };
             }
-            let filename = format!("{}/level_{}_collision.glb", folder.display(), **level_name);
+            let filename = format!(
+                "{}/level_{}_collision.glb",
+                folder.display(),
+                get_site_id(*site_child)?,
+            );
             write_meshes_to_file(
                 collision_data,
-                None,
+                Some(format!("level_{}_collision", **level_name)),
                 CompressGltfOptions::skip_materials(),
                 filename,
             )?;
-            let filename = format!("{}/level_{}_visual.glb", folder.display(), **level_name);
+            let filename = format!(
+                "{}/level_{}_visual.glb",
+                folder.display(),
+                get_site_id(*site_child)?,
+            );
             write_meshes_to_file(
                 visual_data,
-                Some(format!("level_{}_visuals", **level_name)),
+                Some(format!("level_{}_visual", **level_name)),
                 CompressGltfOptions::default(),
                 filename,
             )?;
@@ -327,8 +355,17 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                     transform: None,
                 });
             }
-            let filename = format!("{}/{}.glb", folder.display(), **lift_name);
-            write_meshes_to_file(lift_data, None, CompressGltfOptions::default(), filename)?;
+            let filename = format!(
+                "{}/lift_{}.glb",
+                folder.display(),
+                get_site_id(*site_child)?
+            );
+            write_meshes_to_file(
+                lift_data,
+                Some(format!("lift_{}", **lift_name)),
+                CompressGltfOptions::default(),
+                filename,
+            )?;
             // Now generate the lift doors
             let LiftCabin::Rect(cabin) = cabin;
             for (face, door) in cabin.doors().iter() {
@@ -357,15 +394,20 @@ pub fn collect_site_meshes(world: &mut World, site: Entity, folder: &Path) -> Re
                             transform: Some(tf.clone()),
                         };
                         let filename = format!(
-                            "{}/{}_{}_{}.glb",
+                            "{}/lift_{}_{}_{}.glb",
                             folder.display(),
-                            **lift_name,
+                            get_site_id(*site_child)?,
                             face.label(),
-                            segment_name
+                            segment_name,
                         );
                         write_meshes_to_file(
                             vec![data],
-                            None,
+                            Some(format!(
+                                "lift_{}_{}_{}",
+                                **lift_name,
+                                face.label(),
+                                segment_name
+                            )),
                             CompressGltfOptions::default(),
                             filename,
                         )?;
diff --git a/rmf_site_editor/src/workspace.rs b/rmf_site_editor/src/workspace.rs
index b18bc527..e1dfcd9d 100644
--- a/rmf_site_editor/src/workspace.rs
+++ b/rmf_site_editor/src/workspace.rs
@@ -299,6 +299,8 @@ pub fn dispatch_load_workspace_events(
                             .send(LoadWorkspaceFile(Some(path.clone()), data))
                             .expect("Failed sending load event");
                     }
+                } else {
+                    warn!("Unable to read file [{path:?}] so it cannot be loaded");
                 }
             }
             LoadWorkspace::Data(data) => {
diff --git a/rmf_site_format/src/sdf.rs b/rmf_site_format/src/sdf.rs
index 6ade88bb..3414f0e6 100644
--- a/rmf_site_format/src/sdf.rs
+++ b/rmf_site_format/src/sdf.rs
@@ -65,13 +65,13 @@ impl Pose {
     }
 }
 
-fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink {
+fn make_sdf_door_link(mesh_prefix: &str, link_name: &str) -> SdfLink {
     SdfLink {
         name: link_name.to_string(),
         collision: vec![SdfCollision {
-            name: format!("{}_collision", link_name),
+            name: format!("{link_name}_collision"),
             geometry: SdfGeometry::Mesh(SdfMeshShape {
-                uri: format!("meshes/{}_{}.glb", door_name, link_name),
+                uri: format!("meshes/{mesh_prefix}_{link_name}.glb"),
                 ..Default::default()
             }),
             surface: Some(SdfSurface {
@@ -84,9 +84,9 @@ fn make_sdf_door_link(door_name: &str, link_name: &str) -> SdfLink {
             ..Default::default()
         }],
         visual: vec![SdfVisual {
-            name: format!("{}_visual", link_name),
+            name: format!("{link_name}_visual"),
             geometry: SdfGeometry::Mesh(SdfMeshShape {
-                uri: format!("meshes/{}_{}.glb", door_name, link_name),
+                uri: format!("meshes/{mesh_prefix}_{link_name}.glb"),
                 ..Default::default()
             }),
             ..Default::default()
@@ -100,9 +100,9 @@ fn make_sdf_door(
     right_anchor: Anchor,
     offset: Vec3,
     ros_interface: bool,
-    name_override: Option<&str>,
     kind: &DoorType,
-    door_name: &str,
+    mesh_prefix: &str,
+    model_name: &str,
 ) -> Result<SdfModel, SdfConversionError> {
     let left_trans = left_anchor.translation_for_category(Category::Door);
     let right_trans = right_anchor.translation_for_category(Category::Door);
@@ -114,7 +114,6 @@ fn make_sdf_door(
     let dy = left_trans[1] - right_trans[1];
     let door_length = (dx * dx + dy * dy).sqrt();
     let yaw = -dx.atan2(dy);
-    let prefix = name_override.unwrap_or_default().to_string();
     let labels = match kind {
         DoorType::SingleSliding(_) | DoorType::SingleSwing(_) | DoorType::Model(_) => {
             Vec::from(["body"])
@@ -138,9 +137,9 @@ fn make_sdf_door(
     let mut component_data = ElementMap::default();
     door_plugin_inner
         .attributes
-        .insert("name".to_string(), door_name.to_string());
+        .insert("name".to_string(), model_name.to_owned());
     let mut door_model = SdfModel {
-        name: door_name.to_string(),
+        name: model_name.to_owned(),
         pose: Some(
             Pose {
                 trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(),
@@ -151,13 +150,8 @@ fn make_sdf_door(
         r#static: Some(false),
         ..Default::default()
     };
-    let link_name = if let Some(name_override) = name_override.as_ref() {
-        name_override
-    } else {
-        &door_name
-    };
     for label in labels.iter() {
-        door_model.link.push(make_sdf_door_link(link_name, label));
+        door_model.link.push(make_sdf_door_link(mesh_prefix, label));
     }
     let mut door_motion_params = vec![];
     let joints = match kind {
@@ -170,7 +164,7 @@ fn make_sdf_door(
                 .insert("left_joint_name".into(), "empty_joint".into());
             door_plugin_inner
                 .attributes
-                .insert("right_joint_name".into(), prefix.clone() + "joint");
+                .insert("right_joint_name".into(), model_name.to_owned() + "_joint");
             door_motion_params.push(("v_max_door", "0.2"));
             door_motion_params.push(("a_max_door", "0.2"));
             door_motion_params.push(("a_nom_door", "0.08"));
@@ -182,7 +176,7 @@ fn make_sdf_door(
             }
             .to_sdf();
             vec![SdfJoint {
-                name: prefix.clone() + "joint",
+                name: model_name.to_owned() + "_joint",
                 parent: "world".into(),
                 child: "body".into(),
                 r#type: "prismatic".into(),
@@ -222,7 +216,8 @@ fn make_sdf_door(
                 ..Default::default()
             }
             .to_sdf();
-            let (left_joint_name, right_joint_name) = ("empty_joint", prefix.clone() + "joint");
+            let (left_joint_name, right_joint_name) =
+                ("empty_joint", model_name.to_owned() + "_joint");
             door_plugin_inner
                 .attributes
                 .insert("left_joint_name".into(), left_joint_name.into());
@@ -230,7 +225,7 @@ fn make_sdf_door(
                 .attributes
                 .insert("right_joint_name".into(), right_joint_name);
             vec![SdfJoint {
-                name: prefix.clone() + "joint",
+                name: model_name.to_owned() + "_joint",
                 parent: "world".into(),
                 child: "body".into(),
                 r#type: "revolute".into(),
@@ -251,12 +246,14 @@ fn make_sdf_door(
             door_plugin_inner
                 .attributes
                 .insert("type".into(), "DoubleSlidingDoor".into());
-            door_plugin_inner
-                .attributes
-                .insert("left_joint_name".into(), prefix.clone() + "left_joint");
-            door_plugin_inner
-                .attributes
-                .insert("right_joint_name".into(), prefix.clone() + "right_joint");
+            door_plugin_inner.attributes.insert(
+                "left_joint_name".into(),
+                model_name.to_owned() + "_left_joint",
+            );
+            door_plugin_inner.attributes.insert(
+                "right_joint_name".into(),
+                model_name.to_owned() + "_right_joint",
+            );
             door_motion_params.push(("v_max_door", "0.2"));
             door_motion_params.push(("a_max_door", "0.2"));
             door_motion_params.push(("a_nom_door", "0.08"));
@@ -276,7 +273,7 @@ fn make_sdf_door(
             let right_length = door_length - left_length;
             vec![
                 SdfJoint {
-                    name: prefix.clone() + "right_joint",
+                    name: model_name.to_owned() + "_right_joint",
                     parent: "world".into(),
                     child: "right".into(),
                     r#type: "prismatic".into(),
@@ -293,7 +290,7 @@ fn make_sdf_door(
                     ..Default::default()
                 },
                 SdfJoint {
-                    name: prefix.clone() + "left_joint",
+                    name: model_name.to_owned() + "_left_joint",
                     parent: "world".into(),
                     child: "left".into(),
                     r#type: "prismatic".into(),
@@ -315,12 +312,14 @@ fn make_sdf_door(
             door_plugin_inner
                 .attributes
                 .insert("type".into(), "DoubleSwingDoor".into());
-            door_plugin_inner
-                .attributes
-                .insert("left_joint_name".into(), prefix.clone() + "left_joint");
-            door_plugin_inner
-                .attributes
-                .insert("right_joint_name".into(), prefix.clone() + "right_joint");
+            door_plugin_inner.attributes.insert(
+                "left_joint_name".into(),
+                model_name.to_owned() + "_left_joint",
+            );
+            door_plugin_inner.attributes.insert(
+                "right_joint_name".into(),
+                model_name.to_owned() + "_right_joint",
+            );
             door_motion_params.push(("v_max_door", "0.5"));
             door_motion_params.push(("a_max_door", "0.3"));
             door_motion_params.push(("a_nom_door", "0.15"));
@@ -345,7 +344,7 @@ fn make_sdf_door(
             .to_sdf();
             vec![
                 SdfJoint {
-                    name: prefix.clone() + "right_joint",
+                    name: model_name.to_owned() + "_right_joint",
                     parent: "world".into(),
                     child: "right".into(),
                     r#type: "revolute".into(),
@@ -362,7 +361,7 @@ fn make_sdf_door(
                     ..Default::default()
                 },
                 SdfJoint {
-                    name: prefix.clone() + "left_joint",
+                    name: model_name.to_owned() + "_left_joint",
                     parent: "world".into(),
                     child: "left".into(),
                     r#type: "revolute".into(),
@@ -388,7 +387,7 @@ fn make_sdf_door(
             }
             .to_sdf();
             vec![SdfJoint {
-                name: prefix.clone() + "joint",
+                name: model_name.to_owned() + "_joint",
                 parent: "world".into(),
                 child: "body".into(),
                 r#type: "fixed".into(),
@@ -435,7 +434,7 @@ impl Site {
             filename: "toggle_floors".into(),
             ..Default::default()
         };
-        for level in self.levels.values() {
+        for (level_id, level) in &self.levels {
             let mut level_model_names = vec![];
             let mut model_element_map = ElementMap::default();
             max_elevation = max_elevation.max(level.properties.elevation.0);
@@ -444,7 +443,7 @@ impl Site {
                 name: "floor".into(),
                 ..Default::default()
             };
-            let level_model_name = format!("level_{}", level.properties.name.0);
+            let level_model_name = &level.properties.name.0;
             floor_models_ele
                 .attributes
                 .insert("name".into(), level.properties.name.0.clone());
@@ -453,14 +452,14 @@ impl Site {
                 .insert("model_name".into(), level_model_name.clone());
             // Floors walls and static models are included in the level mesh
             world.model.push(SdfModel {
-                name: level_model_name,
+                name: level_model_name.clone(),
                 r#static: Some(true),
                 link: vec![SdfLink {
                     name: "link".into(),
                     collision: vec![SdfCollision {
                         name: "collision".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
-                            uri: format!("meshes/level_{}_collision.glb", level.properties.name.0),
+                            uri: format!("meshes/level_{}_collision.glb", level_id),
                             ..Default::default()
                         }),
                         surface: Some(SdfSurface {
@@ -475,7 +474,7 @@ impl Site {
                     visual: vec![SdfVisual {
                         name: "visual".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
-                            uri: format!("meshes/level_{}_visual.glb", level.properties.name.0),
+                            uri: format!("meshes/level_{}_visual.glb", level_id),
                             ..Default::default()
                         }),
                         ..Default::default()
@@ -486,7 +485,7 @@ impl Site {
             });
             // TODO(luca) We need this because there is no concept of ingestor or dispenser in
             // rmf_site yet. Remove when there is
-            for model in level.models.values() {
+            for (model_id, model) in &level.models {
                 let mut added = false;
                 if model.source == AssetSource::Search("OpenRobotics/TeleportIngestor".to_string())
                 {
@@ -521,7 +520,7 @@ impl Site {
                             collision: vec![SdfCollision {
                                 name: "collision".into(),
                                 geometry: SdfGeometry::Mesh(SdfMeshShape {
-                                    uri: format!("meshes/{}_collision.glb", model.name.0),
+                                    uri: format!("meshes/model_{}_collision.glb", model_id),
                                     ..Default::default()
                                 }),
                                 ..Default::default()
@@ -529,7 +528,7 @@ impl Site {
                             visual: vec![SdfVisual {
                                 name: "visual".into(),
                                 geometry: SdfGeometry::Mesh(SdfMeshShape {
-                                    uri: format!("meshes/{}_visual.glb", model.name.0),
+                                    uri: format!("meshes/model_{}_visual.glb", model_id),
                                     ..Default::default()
                                 }),
                                 ..Default::default()
@@ -545,20 +544,20 @@ impl Site {
                 }
             }
             // Now add all the doors
-            for door in level.doors.values() {
-                // TODO(luca) doors into toggle floors
+            for (door_id, door) in &level.doors {
                 let left_anchor = get_anchor(door.anchors.left())?;
                 let right_anchor = get_anchor(door.anchors.right())?;
-                world.model.push(make_sdf_door(
+                let door_model = make_sdf_door(
                     left_anchor,
                     right_anchor,
                     Vec3::new(0.0, 0.0, level.properties.elevation.0),
                     true,
-                    None,
                     &door.kind,
-                    &door.name.0,
-                )?);
-                level_model_names.push(door.name.0.clone());
+                    format!("door_{door_id}").as_str(),
+                    door.name.0.as_str(),
+                )?;
+                level_model_names.push(door_model.name.clone());
+                world.model.push(door_model);
             }
             for model_name in level_model_names.into_iter() {
                 let model_element = XmlElement {
@@ -571,7 +570,7 @@ impl Site {
             floor_models_ele.data = ElementData::Nested(model_element_map);
             toggle_floors_plugin.elements.push(floor_models_ele);
         }
-        for lift in self.lifts.values() {
+        for (lift_id, lift) in &self.lifts {
             let get_lift_anchor = |id: u32| -> Result<Anchor, SdfConversionError> {
                 lift.cabin_anchors
                     .get(&id)
@@ -640,7 +639,7 @@ impl Site {
                 // TODO(luca) remove unwrap
                 let door = lift.cabin_doors.get(&door_placement.door).unwrap();
                 let cabin_door_name = format!("CabinDoor_{}_door_{}", lift_name, face.label());
-                let cabin_mesh_prefix = format!("{}_{}", lift_name, face.label());
+                let cabin_mesh_prefix = format!("lift_{}_{}", lift_id, face.label());
                 let left_anchor = get_lift_anchor(door.reference_anchors.left())?;
                 let right_anchor = get_lift_anchor(door.reference_anchors.right())?;
                 let x_offset = -face.u()
@@ -653,8 +652,8 @@ impl Site {
                     right_anchor,
                     x_offset,
                     false,
-                    Some(&cabin_mesh_prefix),
                     &door.kind,
+                    &cabin_mesh_prefix,
                     &cabin_door_name,
                 )?;
                 for mut joint in cabin_door.joint.drain(..) {
@@ -679,8 +678,8 @@ impl Site {
                         right_anchor,
                         Vec3::from(pose.trans) + Vec3::new(0.0, 0.0, level.properties.elevation.0),
                         false,
-                        Some(&cabin_mesh_prefix),
                         &door.kind,
+                        &cabin_mesh_prefix,
                         &shaft_door_name,
                     )?;
                     // Add the pose of the lift to have world coordinates
@@ -744,7 +743,7 @@ impl Site {
                     collision: vec![SdfCollision {
                         name: "collision".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
-                            uri: format!("meshes/{}.glb", lift.properties.name.0),
+                            uri: format!("meshes/lift_{}.glb", lift_id),
                             ..Default::default()
                         }),
                         surface: Some(SdfSurface {
@@ -759,7 +758,7 @@ impl Site {
                     visual: vec![SdfVisual {
                         name: "visual".into(),
                         geometry: SdfGeometry::Mesh(SdfMeshShape {
-                            uri: format!("meshes/{}.glb", lift.properties.name.0),
+                            uri: format!("meshes/lift_{}.glb", lift_id),
                             ..Default::default()
                         }),
                         ..Default::default()