diff --git a/Cargo.lock b/Cargo.lock index 4338079e..a6e5fd76 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -20,9 +20,9 @@ checksum = "c71b1793ee61086797f5c80b6efa2b8ffa6d5dd703f118545808a7f2e27f7046" [[package]] name = "accesskit" -version = "0.12.2" +version = "0.12.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6cb10ed32c63247e4e39a8f42e8e30fb9442fbf7878c8e4a9849e7e381619bea" +checksum = "74a4b14f3d99c1255dcba8f45621ab1a2e7540a0009652d33989005a4d0bfc6b" [[package]] name = "accesskit_consumer" @@ -121,7 +121,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "37fe60779335388a88c01ac6c3be40304d1e349de3ada3b15f7808bb90fa9dce" dependencies = [ "alsa-sys", - "bitflags 2.4.2", + "bitflags 2.5.0", "libc", ] @@ -230,9 +230,9 @@ dependencies = [ [[package]] name = "anyhow" -version = "1.0.80" +version = "1.0.81" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5ad32ce52e4161730f7098c077cd2ed6229b5804ccf99e5366be1ab72a98b4e1" +checksum = "0952808a6c2afd1aa8947271f3a60f1a6763c7b912d210184c5149b5cf147247" [[package]] name = "approx" @@ -327,7 +327,7 @@ dependencies = [ "async-task", "concurrent-queue", "fastrand 2.0.1", - "futures-lite 2.2.0", + "futures-lite 2.3.0", "slab", ] @@ -526,7 +526,7 @@ dependencies = [ "bevy_macro_utils", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -579,7 +579,7 @@ dependencies = [ "bevy_render", "bevy_transform", "bevy_utils", - "bitflags 2.4.2", + "bitflags 2.5.0", "radsort", "serde", ] @@ -592,7 +592,7 @@ checksum = "f484318350462c58ba3942a45a656c1fd6b6e484a6b6b7abc3a787ad1a51e500" dependencies = [ "bevy_macro_utils", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -640,7 +640,7 @@ dependencies = [ "bevy_macro_utils", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -732,6 +732,23 @@ dependencies = [ "thiserror", ] +[[package]] +name = "bevy_gltf_export" +version = "0.1.0" +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", + "serde", + "serde_json", + "thiserror", +] + [[package]] name = "bevy_hierarchy" version = "0.12.1" @@ -833,7 +850,7 @@ dependencies = [ "proc-macro2", "quote", "rustc-hash", - "syn 2.0.52", + "syn 2.0.53", "toml_edit 0.20.7", ] @@ -925,7 +942,7 @@ dependencies = [ "bevy_transform", "bevy_utils", "bevy_window", - "bitflags 2.4.2", + "bitflags 2.5.0", "bytemuck", "fixedbitset", "naga_oil", @@ -941,7 +958,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dc6824e97e332fafcbfe31d169b2b9073b9ba568b2a9f59cfd3ac4697982b4bf" dependencies = [ "bevy", - "bitflags 2.4.2", + "bitflags 2.5.0", "naga", ] @@ -979,7 +996,7 @@ dependencies = [ "bevy_macro_utils", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", "uuid", ] @@ -1007,7 +1024,7 @@ dependencies = [ "bevy_transform", "bevy_utils", "bevy_window", - "bitflags 2.4.2", + "bitflags 2.5.0", "bytemuck", "codespan-reporting", "downcast-rs", @@ -1038,7 +1055,7 @@ dependencies = [ "bevy_macro_utils", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -1079,7 +1096,7 @@ dependencies = [ "bevy_render", "bevy_transform", "bevy_utils", - "bitflags 2.4.2", + "bitflags 2.5.0", "bytemuck", "fixedbitset", "guillotiere", @@ -1096,7 +1113,7 @@ checksum = "b503a4a2a4e15456ae19e7f467b7f3447ccdcf9bbb10de9f4f3c1b2e88e12cdd" dependencies = [ "anyhow", "bevy", - "futures-lite 2.2.0", + "futures-lite 2.3.0", "stl_io", "thiserror", ] @@ -1221,7 +1238,7 @@ checksum = "7aafecc952b6b8eb1a93c12590bd867d25df2f4ae1033a01dfdfc3c35ebccfff" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -1271,7 +1288,7 @@ version = "0.69.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a00dc851838a2120612785d195287475a3ac45514741da670b735818822129a0" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "cexpr", "clang-sys", "itertools 0.12.1", @@ -1282,7 +1299,7 @@ dependencies = [ "regex", "rustc-hash", "shlex", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -1300,6 +1317,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" @@ -1314,18 +1337,18 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.4.2" +version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf" +checksum = "cf4b9d6a944f767f8e5e0db018570623c85f3d925ac718db4e06d0187adb21c1" dependencies = [ "serde", ] [[package]] name = "blake3" -version = "1.5.0" +version = "1.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0231f06152bf547e9c2b5194f247cd97aacf6dcd8b15d8e5ec0663f64580da87" +checksum = "30cca6d3674597c30ddf2c587bf8d9d65c9a84d2326d941cc79c9842dfe0ef52" dependencies = [ "arrayref", "arrayvec", @@ -1379,7 +1402,7 @@ dependencies = [ "async-task", "fastrand 2.0.1", "futures-io", - "futures-lite 2.2.0", + "futures-lite 2.3.0", "piper", "tracing", ] @@ -1396,28 +1419,28 @@ dependencies = [ [[package]] name = "bumpalo" -version = "3.15.3" +version = "3.15.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8ea184aa71bb362a1157c896979544cc23974e08fd265f29ea96b59f0b4a555b" +checksum = "7ff69b9dd49fd426c69a0db9fc04dd934cdb6645ff000864d98f7e2af8830eaa" [[package]] name = "bytemuck" -version = "1.14.3" +version = "1.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a2ef034f05691a48569bd920a96c81b9d91bbad1ab5ac7c4616c1f6ef36cb79f" +checksum = "5d6d68c57235a3a081186990eca2867354726650f42f7516ca50c28d6281fd15" dependencies = [ "bytemuck_derive", ] [[package]] name = "bytemuck_derive" -version = "1.5.0" +version = "1.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "965ab7eb5f8f97d2a083c799f3a1b994fc397b2fe2da5d1da1626ce15a39f2b1" +checksum = "4da9a32f3fed317401fa3c862968128267c3106685286e15d5aaa3d7389c2f60" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -1444,9 +1467,9 @@ dependencies = [ [[package]] name = "cc" -version = "1.0.89" +version = "1.0.90" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a0ba8f7aaa012f30d5b2861462f6708eccd49c3c39863fe083a308035f63d723" +checksum = "8cd6604a82acf3039f1144f54b8eb34e91ffba622051189e71b781822d5ee1f5" dependencies = [ "jobserver", "libc", @@ -1491,9 +1514,9 @@ checksum = "fd16c4719339c4530435d38e511904438d07cce7950afa3718a84ac36c10e89e" [[package]] name = "chrono" -version = "0.4.34" +version = "0.4.35" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5bc015644b92d5890fab7489e49d21f879d5c990186827d42ec511919404f38b" +checksum = "8eaf5903dcbc0a39312feb77df2ff4c76387d591b9fc7b04a238dcf8bb62639a" dependencies = [ "android-tzdata", "iana-time-zone", @@ -1536,9 +1559,9 @@ dependencies = [ [[package]] name = "clap" -version = "4.5.1" +version = "4.5.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c918d541ef2913577a0f9566e9ce27cb35b6df072075769e0b26cb5a554520da" +checksum = "949626d00e063efc93b6dca932419ceb5432f99769911c0b995f7e884c778813" dependencies = [ "clap_builder", "clap_derive", @@ -1546,9 +1569,9 @@ dependencies = [ [[package]] name = "clap_builder" -version = "4.5.1" +version = "4.5.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9f3e7391dad68afb0c2ede1bf619f579a3dc9c2ec67f089baa397123a2f3d1eb" +checksum = "ae129e2e766ae0ec03484e609954119f123cc1fe650337e155d03b022f24f7b4" dependencies = [ "anstream", "anstyle", @@ -1558,14 +1581,14 @@ dependencies = [ [[package]] name = "clap_derive" -version = "4.5.0" +version = "4.5.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "307bc0538d5f0f83b8248db3087aa92fe504e4691294d0c96c0eabc33f47ba47" +checksum = "90239a040c80f5e14809ca132ddc4176ab33d5e17e49691793296e3fcb34d72f" dependencies = [ - "heck 0.4.1", + "heck 0.5.0", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -1576,9 +1599,9 @@ checksum = "98cc8fbded0c607b7ba9dd60cd98df59af97e84d24e49c8557331cfc26d301ce" [[package]] name = "clipboard-win" -version = "5.2.0" +version = "5.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "12f9a0700e0127ba15d1d52dd742097f821cd9c65939303a44d970465040a297" +checksum = "d517d4b86184dbb111d3556a10f1c8a04da7428d2987bf1081602bf11c3aa9ee" dependencies = [ "error-code", ] @@ -1660,9 +1683,9 @@ checksum = "f7144d30dcf0fafbce74250a3963025d8d52177934239851c917d29f1df280c2" [[package]] name = "constgebra" -version = "0.1.3" +version = "0.1.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "edd23e864550e6dafc1e41ac78ce4f1ccddc8672b40c403524a04ff3f0518420" +checksum = "e1aaf9b65849a68662ac6c0810c8893a765c960b907dd7cfab9c4a50bf764fbc" dependencies = [ "const_soft_float", ] @@ -1830,6 +1853,12 @@ version = "0.8.19" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "248e3bacc7dc6baa3b21e405ee045c3047101a49145e7e9eca583ab4c2ca5345" +[[package]] +name = "crunchy" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7" + [[package]] name = "crypto-common" version = "0.1.6" @@ -1846,7 +1875,7 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e16e44ab292b1dddfdaf7be62cfd8877df52f2f3fde5858d95bab606be259f20" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "libloading 0.8.3", "winapi", ] @@ -2010,7 +2039,7 @@ checksum = "3fe2568f851fd6144a45fa91cfed8fe5ca8fc0b56ba6797bfc1ed2771b90e37c" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -2116,6 +2145,22 @@ dependencies = [ "pin-project-lite", ] +[[package]] +name = "exr" +version = "1.72.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "887d93f60543e9a9362ef8a21beedd0a833c5d9610e18c67abe15a5963dcb1a4" +dependencies = [ + "bit_field", + "flume", + "half", + "lebe", + "miniz_oxide", + "rayon-core", + "smallvec", + "zune-inflate", +] + [[package]] name = "fastrand" version = "1.9.0" @@ -2177,6 +2222,15 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8" +[[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" @@ -2210,7 +2264,7 @@ checksum = "1a5c6c585bc94aaf2c7b51dd4c2ba22680844aba4c687be581871a6f518c5742" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -2263,9 +2317,9 @@ dependencies = [ [[package]] name = "futures-lite" -version = "2.2.0" +version = "2.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "445ba825b27408685aaecefd65178908c36c6e96aaf6d8599419d46e624192ba" +checksum = "52527eb5074e35e9339c6b4e8d12600c7128b68fb25dcb9fa9dec18f7c25f3a5" dependencies = [ "fastrand 2.0.1", "futures-core", @@ -2375,11 +2429,21 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "gif" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3fb2d69b19215e18bb912fa30f7ce15846e301408695e44e0ef719f1da9e19f2" +dependencies = [ + "color_quant", + "weezl", +] + [[package]] name = "gilrs" -version = "0.10.4" +version = "0.10.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d8b2e57a9cb946b5d04ae8638c5f554abb5a9f82c4c950fd5b1fee6d119592fb" +checksum = "499067aa54af19f88732dc418f61f23d5912de1518665bb0eca034ca0d07574c" dependencies = [ "fnv", "gilrs-core", @@ -2390,9 +2454,9 @@ dependencies = [ [[package]] name = "gilrs-core" -version = "0.5.10" +version = "0.5.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0af1827b7dd2f36d740ae804c1b3ea0d64c12533fb61ff91883005143a0e8c5a" +checksum = "85c132270a155f2548e67d66e731075c336c39098afc555752f3df8f882c720e" dependencies = [ "core-foundation", "inotify", @@ -2406,7 +2470,7 @@ dependencies = [ "vec_map", "wasm-bindgen", "web-sys", - "windows 0.52.0", + "windows 0.54.0", ] [[package]] @@ -2511,7 +2575,7 @@ dependencies = [ "inflections", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -2554,7 +2618,7 @@ version = "0.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fbcd2dba93594b227a1f57ee09b8b9da8892c34d55aa332e034a228d0fe6a171" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "gpu-alloc-types", ] @@ -2564,7 +2628,7 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "98ff03b468aa837d70984d55f5d3f846f6ec31fe34bbb97c4f85219caeee1ca4" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", ] [[package]] @@ -2586,7 +2650,7 @@ version = "0.2.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cc11df1ace8e7e564511f53af41f3e42ddc95b56fd07b3f4445d2a6048bc682c" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "gpu-descriptor-types", "hashbrown 0.14.3", ] @@ -2597,7 +2661,7 @@ version = "0.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6bf0b36e6f090b7e1d8a4b49c0cb81c1f8376f72198c65dd3ad9ff3556b8b78c" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", ] [[package]] @@ -2641,12 +2705,22 @@ source = "git+https://github.com/open-rmf/gz-fuel-rs?branch=luca/ehttp#44b3fc616 dependencies = [ "dirs", "ehttp", - "futures-lite 2.2.0", + "futures-lite 2.3.0", "itertools 0.12.1", "serde", "serde_json", ] +[[package]] +name = "half" +version = "2.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5eceaaeec696539ddaf7b333340f1af35a5aa87ae3e4f3ead0532f72affab2e" +dependencies = [ + "cfg-if", + "crunchy", +] + [[package]] name = "hash32" version = "0.2.1" @@ -2712,9 +2786,9 @@ dependencies = [ [[package]] name = "heck" -version = "0.4.1" +version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" [[package]] name = "hexasphere" @@ -2808,9 +2882,12 @@ dependencies = [ "bytemuck", "byteorder", "color_quant", + "exr", + "gif", "jpeg-decoder", "num-traits", "png", + "qoi", "tiff", ] @@ -2880,9 +2957,9 @@ checksum = "d3b7357d2bbc5ee92f8e899ab645233e43d21407573cceb37fed8bc3dede2c02" [[package]] name = "io-kit-sys" -version = "0.4.0" +version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4769cb30e5dcf1710fc6730d3e94f78c47723a014a567de385e113c737394640" +checksum = "617ee6cf8e3f66f3b4ea67a4058564628cde41901316e19f559e14c7c72c5e7b" dependencies = [ "core-foundation-sys", "mach2", @@ -2948,6 +3025,9 @@ name = "jpeg-decoder" version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f5d4a7da358eff58addd2877a45865158f0d78c911d43a5784ceb7bbf52833b0" +dependencies = [ + "rayon", +] [[package]] name = "js-sys" @@ -2990,6 +3070,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" @@ -3039,7 +3125,7 @@ version = "0.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "85c833ca1e66078851dba29046874e38f08b2c883700aa29a03ddd3b23814ee8" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "libc", "redox_syscall 0.4.1", ] @@ -3050,7 +3136,7 @@ version = "0.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3af92c55d7d839293953fcd0fda5ecfe93297cfde6ffbdec13b41d99c0ba6607" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "libc", "redox_syscall 0.4.1", ] @@ -3148,7 +3234,7 @@ version = "0.26.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "623b5e6cefd76e58f774bd3cc0c6f5c7615c58c03a97815245a25c3c9bdee318" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "block", "core-graphics-types", "foreign-types 0.5.0", @@ -3157,15 +3243,6 @@ dependencies = [ "paste", ] -[[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" @@ -3201,7 +3278,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c1ceaaa4eedaece7e4ec08c55c640ba03dbb73fb812a6570a59bcf1930d0f70e" dependencies = [ "bit-set", - "bitflags 2.4.2", + "bitflags 2.5.0", "codespan-reporting", "hexf-parse", "indexmap 1.9.3", @@ -3282,7 +3359,7 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2076a31b7010b17a38c01907c45b945e8f11495ee4dd588309718901b1f7a5b7" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "jni-sys", "log", "ndk-sys 0.5.0+25.2.9519653", @@ -3316,12 +3393,13 @@ dependencies = [ [[package]] name = "nix" -version = "0.27.1" +version = "0.28.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2eb04e9c688eff1c89d72b407f168cf79bb9e867a9d3323ed6c01519eb9cc053" +checksum = "ab2156c4fce2f8df6c499cc1c763e4394b7482525bf2a9701c9d79d215f519e4" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "cfg-if", + "cfg_aliases", "libc", ] @@ -3394,7 +3472,7 @@ checksum = "ed3955f1a9c7c0c15e092f9c887db08b1fc683305fdf6eb6684f22555355e202" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -3475,7 +3553,7 @@ dependencies = [ "proc-macro-crate 1.3.1", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -3487,7 +3565,7 @@ dependencies = [ "proc-macro-crate 3.1.0", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -3751,7 +3829,7 @@ dependencies = [ "pest_meta", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -3885,9 +3963,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.78" +version = "1.0.79" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e2422ad645d89c99f8f3e6b88a9fdeca7fabeac836b1002371c4367c8f984aae" +checksum = "e835ff2298f5721608eb1a980ecaee1aef2c132bf95ecc026a11b7bf3c01c02e" dependencies = [ "unicode-ident", ] @@ -3898,6 +3976,15 @@ version = "1.0.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "43d84d1d7a6ac92673717f9f6d1518374ef257669c24ebc5ac25d5033828be58" +[[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.35" @@ -3961,6 +4048,26 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" +[[package]] +name = "rayon" +version = "1.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e4963ed1bc86e4f3ee217022bd855b297cef07fb9eac5dfa1f788b220b49b3bd" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1465873a3dfdaa8ae7cb14b4383657caab0b3e8a0aa9ae8e04b044854c8dfce2" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + [[package]] name = "rectangle-pack" version = "0.4.2" @@ -4096,6 +4203,7 @@ version = "0.0.1" dependencies = [ "bevy", "bevy_egui", + "bevy_gltf_export", "bevy_infinite_grid", "bevy_mod_outline", "bevy_mod_raycast", @@ -4128,6 +4236,7 @@ dependencies = [ "urdf-rs", "utm", "wasm-bindgen", + "yaserde", ] [[package]] @@ -4137,14 +4246,17 @@ dependencies = [ "bevy", "float_eq", "glam", + "once_cell", "pathdiff", "ron", + "sdformat_rs", "serde", "serde_json", "serde_yaml", "thiserror", "urdf-rs", "uuid", + "yaserde", ] [[package]] @@ -4170,7 +4282,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b91f7eff05f748767f183df4320a63d6936e9c6107d97c9e6bdd9784f4289c94" dependencies = [ "base64 0.21.7", - "bitflags 2.4.2", + "bitflags 2.5.0", "serde", "serde_derive", ] @@ -4213,7 +4325,7 @@ version = "0.38.31" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6ea3e1a662af26cd7a3ba09c0297a31af215563ecf42817c98df621387f4e949" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "errno", "libc", "linux-raw-sys", @@ -4262,23 +4374,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", - "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.17" @@ -4312,10 +4407,9 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" [[package]] name = "sdformat_rs" version = "0.1.0" -source = "git+https://github.com/open-rmf/sdf_rust_experimental?rev=a5daef0#a5daef0c03380e55cfdaba507c89883a87127a05" +source = "git+https://github.com/open-rmf/sdf_rust_experimental?rev=9fc35f2#9fc35f2b5f55afab37a4c1906c395fc22d069322" dependencies = [ "convert_case", - "minidom", "nalgebra", "xmltree", "yaserde", @@ -4345,7 +4439,7 @@ checksum = "7eb0b34b42edc17f6b7cac84a52a1c5f0e1bb2227e997ca9011ea3dd34e8610b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -4468,17 +4562,6 @@ 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.1" @@ -4555,9 +4638,9 @@ checksum = "81cdd64d312baedb58e21336b31bc043b77e01cc99033ce76ef539f78e965ebc" [[package]] name = "svg_fmt" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8fb1df15f412ee2e9dfc1c504260fa695c1c3f10fe9f4a6ee2d2184d7d6450e2" +checksum = "f83ba502a3265efb76efb89b0a2f7782ad6f2675015d4ce37e4b547dda42b499" [[package]] name = "syn" @@ -4572,9 +4655,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.52" +version = "2.0.53" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b699d15b36d1f02c3e7c69f8ffef53de37aefae075d8488d4ba1a7788d574a07" +checksum = "7383cd0e49fff4b6b90ca5670bfd3e9d6a733b3f90c686605aa7eec8c4996032" dependencies = [ "proc-macro2", "quote", @@ -4597,12 +4680,12 @@ dependencies = [ [[package]] name = "system-deps" -version = "6.2.0" +version = "6.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2a2d580ff6a20c55dfb86be5f9c238f67835d0e81cbdea8bf5680e0897320331" +checksum = "a3e535eb8dded36d55ec13eddacd30dec501792ff23a0b1682c38601b8cf2349" dependencies = [ "cfg-expr", - "heck 0.4.1", + "heck 0.5.0", "pkg-config", "toml", "version-compare", @@ -4659,9 +4742,9 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.57" +version = "1.0.58" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e45bcbe8ed29775f228095caf2cd67af7a4ccf756ebff23a306bf3e8b47b24b" +checksum = "03468839009160513471e86a034bb2c5c0e4baae3b43f79ffc55c4a5427b3297" dependencies = [ "thiserror-impl", ] @@ -4683,18 +4766,18 @@ checksum = "e4c60d69f36615a077cc7663b9cb8e42275722d23e58a7fa3d2c7f2915d09d04" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] name = "thiserror-impl" -version = "1.0.57" +version = "1.0.58" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a953cb265bef375dae3de6663da4d3804eee9682ea80d8e2542529b73c531c81" +checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -4745,14 +4828,14 @@ dependencies = [ [[package]] name = "toml" -version = "0.8.10" +version = "0.8.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a9aad4a3066010876e8dcf5a8a06e70a558751117a145c6ce2b82c2e2054290" +checksum = "e9dd1545e8208b4a5af1aa9bbd0b4cf7e9ea08fabc5d0a5c67fcaafa17433aa3" dependencies = [ "serde", "serde_spanned", "toml_datetime", - "toml_edit 0.22.6", + "toml_edit 0.22.8", ] [[package]] @@ -4799,9 +4882,9 @@ dependencies = [ [[package]] name = "toml_edit" -version = "0.22.6" +version = "0.22.8" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2c1b5fd4128cc8d3e0cb74d4ed9a9cc7c7284becd4df68f5f940e1ad123606f6" +checksum = "c12219811e0c1ba077867254e5ad62ee2c9c190b0d957110750ac0cda1ae96cd" dependencies = [ "indexmap 2.2.5", "serde", @@ -4829,7 +4912,7 @@ checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -5071,9 +5154,9 @@ checksum = "95b09e3b3a0abd1ccb77673a6b7b8875d9d1c80626154add451cf18392dc4c3c" [[package]] name = "uuid" -version = "1.7.0" +version = "1.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f00cc9702ca12d3c81455259621e676d0f7251cec66a21e98fe2e9a37db93b2a" +checksum = "a183cf7feeba97b4dd1c0d46788634f6221d87fa961b305bed08c851829efcc0" dependencies = [ "getrandom", "serde", @@ -5093,9 +5176,9 @@ checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191" [[package]] name = "version-compare" -version = "0.1.1" +version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "579a42fc0b8e0c63b76519a339be31bed574929511fa53c1a3acae26eb258f29" +checksum = "852e951cb7832cb45cb1169900d19760cfa39b82bc0ea9c0e5a14ae88411c98b" [[package]] name = "version_check" @@ -5146,7 +5229,7 @@ dependencies = [ "once_cell", "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", "wasm-bindgen-shared", ] @@ -5180,7 +5263,7 @@ checksum = "e94f17b526d0a461a191c78ea52bbce64071ed5c04c9ffe424dcb38f74171bb7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", "wasm-bindgen-backend", "wasm-bindgen-shared", ] @@ -5276,7 +5359,7 @@ checksum = "0f8a44dd301a30ceeed3c27d8c0090433d3da04d7b2a4042738095a424d12ae7" dependencies = [ "arrayvec", "bit-vec", - "bitflags 2.4.2", + "bitflags 2.5.0", "codespan-reporting", "log", "naga", @@ -5301,7 +5384,7 @@ dependencies = [ "arrayvec", "ash", "bit-set", - "bitflags 2.4.2", + "bitflags 2.5.0", "block", "core-graphics-types", "d3d12", @@ -5338,7 +5421,7 @@ version = "0.17.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ee64d7398d0c2f9ca48922c902ef69c42d000c759f3db41e355f4a570b052b67" dependencies = [ - "bitflags 2.4.2", + "bitflags 2.5.0", "js-sys", "web-sys", ] @@ -5410,16 +5493,6 @@ dependencies = [ "windows-targets 0.48.5", ] -[[package]] -name = "windows" -version = "0.52.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e48a53791691ab099e5e2ad123536d0fff50652600abaf43bbf952894110d0be" -dependencies = [ - "windows-core 0.52.0", - "windows-targets 0.52.4", -] - [[package]] name = "windows" version = "0.54.0" @@ -5825,7 +5898,7 @@ checksum = "9ce1b18ccd8e73a9321186f97e46f9f04b778851177567b1975109d26a08d2a6" dependencies = [ "proc-macro2", "quote", - "syn 2.0.52", + "syn 2.0.53", ] [[package]] @@ -5833,3 +5906,12 @@ name = "zeroize" version = "1.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "525b4ec142c6b68a2d10f01f7bbf6755599ca3f81ea53b8431b7dd348f5fdb2d" + +[[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 ce8112d4..447851bd 100644 --- a/rmf_site_editor/Cargo.toml +++ b/rmf_site_editor/Cargo.toml @@ -21,6 +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/transform_api"} bevy_polyline = "0.8.1" bevy_stl = "0.12" bevy_obj = { version = "0.12.1", features = ["scene"] } @@ -43,8 +44,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 = "a5daef0"} +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_editor/examples/extending_menu.rs b/rmf_site_editor/examples/extending_menu.rs index 6f3f6442..82510145 100644 --- a/rmf_site_editor/examples/extending_menu.rs +++ b/rmf_site_editor/examples/extending_menu.rs @@ -84,5 +84,7 @@ impl Plugin for MyMenuPlugin { /// Lets embed site editor in our application with our own plugin fn main() { - App::new().add_plugins((SiteEditor, MyMenuPlugin)).run(); + App::new() + .add_plugins((SiteEditor::default(), MyMenuPlugin)) + .run(); } diff --git a/rmf_site_editor/src/interaction/mod.rs b/rmf_site_editor/src/interaction/mod.rs index ed8aa3ad..d583b15a 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::::visible(true), CategoryVisibilityPlugin::::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 27c88166..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::prelude::*; +use bevy::{prelude::*, window::PrimaryWindow}; use bevy_egui::EguiContexts; #[derive(Debug, Clone, Copy, Resource)] @@ -55,8 +55,15 @@ fn handle_keyboard_input( mut debug_mode: ResMut, mut align_site: EventWriter, current_workspace: Res, + primary_windows: Query>, ) { - 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 ffc25c81..5e97cd86 100644 --- a/rmf_site_editor/src/lib.rs +++ b/rmf_site_editor/src/lib.rs @@ -1,4 +1,4 @@ -use bevy::{log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*}; +use bevy::{app::ScheduleRunnerPlugin, log::LogPlugin, pbr::DirectionalLightShadowMap, prelude::*}; use bevy_egui::EguiPlugin; use main_menu::MainMenuPlugin; // use warehouse_generator::WarehouseGeneratorPlugin; @@ -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; @@ -75,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, + /// Run in headless mode and export the loaded site to the requested path. + #[cfg_attr(not(target_arch = "wasm32"), arg(long))] + pub headless_export: Option, } #[derive(Clone, Default, Eq, PartialEq, Debug, Hash, States)] @@ -117,6 +120,7 @@ pub fn run_js() { pub fn run(command_line_args: Vec) { let mut app = App::new(); + let mut headless_export = None; #[cfg(not(target_arch = "wasm32"))] { @@ -127,34 +131,59 @@ pub fn run(command_line_args: Vec) { command_line_args.import.map(Into::into), )); } + headless_export = command_line_args.headless_export; } - app.add_plugins(SiteEditor); + app.add_plugins(SiteEditor { headless_export }); app.run(); } -pub struct SiteEditor; +#[derive(Default)] +pub struct SiteEditor { + /// Contains Some(path) if the site editor is running in headless mode exporting its site. + pub headless_export: Option, +} impl Plugin for SiteEditor { fn build(&self, app: &mut App) { + let mut plugins = DefaultPlugins.build(); + let headless = { + #[cfg(not(target_arch = "wasm32"))] + { + self.headless_export.is_some() + } + #[cfg(target_arch = "wasm32")] + { + false + } + }; + plugins = if headless { + plugins + .set(WindowPlugin { + primary_window: None, + exit_condition: bevy::window::ExitCondition::DontExit, + close_when_requested: false, + }) + .disable::() + } 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, - DefaultPlugins - .build() + plugins .disable::() - .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() - }) .set(ImagePlugin { default_sampler: SamplerDescriptor { address_mode_u: AddressMode::Repeat, @@ -174,6 +203,7 @@ impl Plugin for SiteEditor { ..default() }), )); + app.insert_resource(DirectionalLightShadowMap { size: 2048 }) .add_state::() .add_plugins(( @@ -185,8 +215,12 @@ impl Plugin for SiteEditor { MainMenuPlugin, WorkcellEditorPlugin, SitePlugin, - InteractionPlugin, - StandardUiLayout, + InteractionPlugin { + headless: self.headless_export.is_some(), + }, + StandardUiLayout { + headless: self.headless_export.is_some(), + }, AnimationPlugin, OccupancyPlugin, WorkspacePlugin, @@ -197,10 +231,23 @@ impl Plugin for SiteEditor { IssuePlugin, OSMViewPlugin, SiteWireframePlugin, + SiteFileMenuPlugin, )); + // Ref https://github.com/bevyengine/bevy/issues/10877. The default behavior causes issues // with events being accumulated when not read (i.e. scrolling mouse wheel on a UI widget). app.world .remove_resource::(); + + 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. + // 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/main_menu.rs b/rmf_site_editor/src/main_menu.rs index 1dd55a32..bf2417fc 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, mut _app_state: ResMut>, autoload: Option>, + primary_windows: Query>, ) { 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/site/door.rs b/rmf_site_editor/src/site/door.rs index e6a9895b..51e0f672 100644 --- a/rmf_site_editor/src/site/door.rs +++ b/rmf_site_editor/src/site/door.rs @@ -72,6 +72,19 @@ impl DoorBodyType { | DoorBodyType::DoubleSliding { left, right } => vec![*left, *right], } } + + pub fn links(&self) -> Vec<&str> { + match self { + DoorBodyType::SingleSwing { .. } + | DoorBodyType::SingleSliding { .. } + | DoorBodyType::Model { .. } => { + vec!["body"] + } + DoorBodyType::DoubleSwing { .. } | DoorBodyType::DoubleSliding { .. } => { + vec!["left", "right"] + } + } + } } #[derive(Debug, Clone, Copy, Component)] 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..c73516f8 --- /dev/null +++ b/rmf_site_editor/src/site/file_menu.rs @@ -0,0 +1,80 @@ +/* + * 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 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([ + AppState::SiteEditor, + AppState::SiteVisualizer, + AppState::SiteDrawingEditor, + ]); + let file_header = world.resource::().get(); + let export_sdf = world + .spawn(( + MenuItem::Text("Export Sdf".to_string()), + MenuVisualizationStates(site_states), + )) + .set_parent(file_header) + .id(); + + ExportSdfMenu { export_sdf } + } +} + +pub fn handle_export_sdf_menu_events( + mut menu_events: EventReader, + sdf_menu: Res, + mut save_events: EventWriter, +) { + for event in menu_events.read() { + if event.clicked() && event.source() == sdf_menu.get() { + 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::().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 e985b7a9..6ff46226 100644 --- a/rmf_site_editor/src/site/floor.rs +++ b/rmf_site_editor/src/site/floor.rs @@ -28,7 +28,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/level.rs b/rmf_site_editor/src/site/level.rs index fda3ac9a..25610cea 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( diff --git a/rmf_site_editor/src/site/load.rs b/rmf_site_editor/src/site/load.rs index cbe298f3..7bb2f73d 100644 --- a/rmf_site_editor/src/site/load.rs +++ b/rmf_site_editor/src/site/load.rs @@ -221,6 +221,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/mod.rs b/rmf_site_editor/src/site/mod.rs index 3faa751a..904efcb9 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::*; @@ -335,6 +341,7 @@ impl Plugin for SitePlugin { align_site_drawings, clear_old_issues_on_new_validate_event, export_lights, + set_camera_transform_for_changed_site, ) .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 5d574ff3..458d8319 100644 --- a/rmf_site_editor/src/site/save.rs +++ b/rmf_site_editor/src/site/save.rs @@ -25,13 +25,14 @@ use std::{ }; 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)] @@ -363,6 +364,7 @@ fn generate_levels( Without, >, Query<&SiteID>, + Query<(&Pose, &NameInSite, &SiteID), With>, )> = SystemState::new(world); let ( @@ -380,6 +382,7 @@ fn generate_levels( q_walls, q_levels, q_site_ids, + q_user_camera_poses, ) = state.get(world); let get_anchor_id = |entity| { @@ -592,6 +595,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); } @@ -1221,42 +1234,154 @@ 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.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"); + } + 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::(save_event.site).cloned(); - migrate_relative_paths(save_event.site, &new_path, world); + let old_default_path = world.get::(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"); + 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}"); + } + } + } else { + match site.to_writer_ron(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 + + // 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."); + continue; + }; + if !parent_folder.exists() { + if let Err(e) = std::fs::create_dir_all(parent_folder) { + error!("Unable to create folder {}: {e}", parent_folder.display()); + continue; + } } - error!("Save failed: {err}"); + let f = match std::fs::File::create(&new_path) { + Ok(f) => f, + Err(err) => { + error!("Unable to save file {}: {err}", new_path.display()); + continue; + } + }; + + let mut meshes_dir = PathBuf::from(parent_folder); + meshes_dir.push("meshes"); + 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); + let graphs = legacy::nav_graph::NavGraph::from_site(&site); + let sdf = match site.to_sdf() { + Ok(sdf) => sdf, + Err(err) => { + error!("Unable to convert site to sdf: {err}"); + continue; + } + }; + let config = yaserde::ser::Config { + perform_indent: true, + write_document_declaration: true, + ..Default::default() + }; + 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"); + 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"); + info!( + "Saving legacy nav graph to {}", + graph_file.to_str().unwrap_or("") + ); + 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."); + continue; } } } @@ -1319,7 +1444,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 new file mode 100644 index 00000000..d3c14e74 --- /dev/null +++ b/rmf_site_editor/src/site/sdf_exporter.rs @@ -0,0 +1,420 @@ +use bevy::ecs::system::SystemState; +use bevy::prelude::*; +use bevy_gltf_export::{export_meshes, CompressGltfOptions, MeshData}; + +use std::path::Path; + +use crate::SaveWorkspace; + +use crate::site::{ + ChildLiftCabinGroup, CollisionMeshMarker, DoorSegments, DrawingMarker, FloorSegments, + LiftDoormat, ModelSceneRoot, TentativeModelFormat, VisualMeshMarker, +}; +use rmf_site_format::{ + IsStatic, LevelElevation, LiftCabin, ModelMarker, NameInSite, NameOfSite, SiteID, 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, + mut exit: EventWriter, + missing_models: Query<(), (With, Without)>, + mut export_state: ResMut, + sites: Query<(Entity, &NameOfSite)>, + drawings: Query>, +) { + export_state.iterations += 1; + if export_state.iterations < 5 { + return; + } + if sites.is_empty() { + 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() { + // 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<( + Query<&Children>, + Query<(&NameInSite, &LevelElevation, &Children)>, + Query>, + Query<&FloorSegments>, + Query<(Option<&NameInSite>, &DoorSegments)>, + Query<(Entity, &IsStatic, &NameInSite), With>, + Query<(), With>, + Query<(), With>, + Query<(&Handle, &Handle)>, + Query<(&NameInSite, &LiftCabin, &ChildLiftCabinGroup)>, + Query<((), With)>, + Query<&GlobalTransform>, + Query<&Transform>, + Query<&SiteID>, + )> = SystemState::new(world); + let ( + q_children, + q_levels, + q_walls, + q_floors, + q_doors, + q_models, + q_collisions, + q_visuals, + q_pbr, + q_lift_cabins, + q_lift_door_mats, + q_global_tfs, + q_tfs, + q_site_ids, + ) = state.get(world); + + let image_assets = world.resource::>(); + let mesh_assets = world.resource::>(); + let material_assets = world.resource::>(); + let write_meshes_to_file = |meshes: Vec, + name: Option, + options: CompressGltfOptions, + filename: String| + -> Result<(), String> { + let image_getter = |id: &Handle| image_assets.get(id).cloned(); + 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_site_id = |e: Entity| -> Result { + 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; + }; + let Some(mesh) = mesh_assets.get(mesh) else { + 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 { + 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)) + }; + + let Ok(site_children) = q_children.get(site) else { + return Ok(()); + }; + for site_child in site_children.iter() { + 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_tf = Transform { + translation: Vec3::new(0.0, 0.0, **elevation), + ..Default::default() + }; + for child in children.iter() { + 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: None, + transform: Some(level_tf.clone()), + }); + visual_data.push(MeshData { + mesh, + material: Some(material), + 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 { + continue; + }; + collision_data.push(MeshData { + mesh, + material: None, + transform: Some(level_tf.clone()), + }); + visual_data.push(MeshData { + mesh, + material: Some(material), + transform: Some(level_tf.clone()), + }); + } 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) { + 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 { + continue; + }; + let Ok(tf) = q_global_tfs.get(entity) else { + continue; + }; + 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: is_static.then_some(tf), + }); + } + } 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 { + continue; + }; + let Ok(tf) = q_global_tfs.get(entity) else { + continue; + }; + let mut tf = tf.compute_transform(); + tf.translation.z = tf.translation.z + **elevation; + model_visuals.push(MeshData { + mesh, + material: Some(material), + transform: is_static.then_some(tf), + }); + } + } + } + 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!( + "{}/model_{}_collision.glb", + folder.display(), + get_site_id(*child)?, + ); + write_meshes_to_file( + model_collisions, + None, + CompressGltfOptions::skip_materials(), + filename, + )?; + let filename = format!( + "{}/model_{}_visual.glb", + folder.display(), + get_site_id(*child)?, + ); + 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 + .entities() + .iter() + .zip(segments.body.links().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 data = MeshData { + mesh, + material: Some(material), + transform: Some(tf.clone()), + }; + 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], + Some(format!("door_{}_{}", door_name, segment_name)), + CompressGltfOptions::default(), + filename, + )?; + } + } else { + continue; + }; + } + let filename = format!( + "{}/level_{}_collision.glb", + folder.display(), + get_site_id(*site_child)?, + ); + write_meshes_to_file( + collision_data, + Some(format!("level_{}_collision", **level_name)), + CompressGltfOptions::skip_materials(), + filename, + )?; + let filename = format!( + "{}/level_{}_visual.glb", + folder.display(), + get_site_id(*site_child)?, + ); + write_meshes_to_file( + visual_data, + Some(format!("level_{}_visual", **level_name)), + CompressGltfOptions::default(), + filename, + )?; + } + // 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 + let mut lift_data = vec![]; + for entity in DescendantIter::new(&q_children, **cabin_children) { + 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: Some(material), + transform: None, + }); + } + 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() { + let Some(door) = door else { + continue; + }; + if let Ok((_, segments)) = q_doors.get(door.door) { + // TODO(luca) this is duplicated with door generation, refactor + for (entity, segment_name) in segments + .body + .entities() + .iter() + .zip(segments.body.links().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 data = MeshData { + mesh, + material: Some(material), + transform: Some(tf.clone()), + }; + let filename = format!( + "{}/lift_{}_{}_{}.glb", + folder.display(), + get_site_id(*site_child)?, + face.label(), + segment_name, + ); + write_meshes_to_file( + vec![data], + Some(format!( + "lift_{}_{}_{}", + **lift_name, + face.label(), + segment_name + )), + CompressGltfOptions::default(), + filename, + )?; + } + } + } + } + } + Ok(()) +} diff --git a/rmf_site_editor/src/site/site.rs b/rmf_site_editor/src/site/site.rs index ce9586e7..cc559edd 100644 --- a/rmf_site_editor/src/site/site.rs +++ b/rmf_site_editor/src/site/site.rs @@ -15,9 +15,11 @@ * */ -use crate::CurrentWorkspace; +use crate::{interaction::CameraControls, CurrentWorkspace}; use bevy::prelude::*; -use rmf_site_format::{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)] @@ -135,3 +137,36 @@ pub fn change_site( } } } + +pub fn set_camera_transform_for_changed_site( + current_workspace: Res, + current_level: Res, + mut camera_controls: ResMut, + children: Query<&Children>, + user_camera_poses: Query<&Pose, With>, + mut transforms: Query<&mut Transform>, +) { + if current_workspace.is_changed() { + let Some(level) = current_level.0 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(); + } + 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/widgets/mod.rs b/rmf_site_editor/src/widgets/mod.rs index 672097a5..df662e6f 100644 --- a/rmf_site_editor/src/widgets/mod.rs +++ b/rmf_site_editor/src/widgets/mod.rs @@ -95,7 +95,9 @@ pub struct PendingModel { } #[derive(Default)] -pub struct StandardUiLayout; +pub struct StandardUiLayout { + pub headless: bool, +} fn add_widgets_icons(app: &mut App) { // Taken from https://github.com/bevyengine/bevy/issues/10377#issuecomment-1858797002 @@ -165,32 +167,34 @@ impl Plugin for StandardUiLayout { .init_resource::() .add_plugins(MenuPluginManager) .init_resource::() - .init_resource::() - .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::(); + 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/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/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>, mut egui_context: EguiContexts, mut save_events: EventWriter, + primary_windows: Query>, ) { - 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/workcell/save.rs b/rmf_site_editor/src/workcell/save.rs index d3f880f6..33e15d49 100644 --- a/rmf_site_editor/src/workcell/save.rs +++ b/rmf_site_editor/src/workcell/save.rs @@ -336,6 +336,9 @@ pub fn save_workcell(world: &mut World) { } }; } + 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 abd8316a..d03b5e6f 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), - Site(Vec), + RonSite(Vec), + JsonSite(Vec), Workcell(Vec), WorkcellUrdf(Vec), LoadSite(LoadSite), @@ -73,7 +74,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") { @@ -146,6 +149,11 @@ impl SaveWorkspace { self.format = ExportFormat::Urdf; self } + + pub fn to_sdf(mut self) -> Self { + self.format = ExportFormat::Sdf; + self + } } #[derive(Default, Debug, Clone)] @@ -161,6 +169,7 @@ pub enum ExportFormat { #[default] Default, Urdf, + Sdf, } /// Event used in channels to communicate the file handle that was chosen by the user. @@ -315,6 +324,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) => { @@ -363,9 +374,27 @@ fn workspace_file_load_complete( } } } - WorkspaceData::Site(data) => { + 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); + load_site.send(LoadSite { + site, + focus: true, + default_file, + }); + interaction_state.set(InteractionState::Enable); + } + Err(err) => { + error!("Failed loading site {:?}", err); + } + } + } + WorkspaceData::JsonSite(data) => { + info!("Opening site file"); + match Site::from_bytes_json(&data) { Ok(site) => { // Switch state app_state.set(AppState::SiteEditor); @@ -519,6 +548,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 775309fd..0de63d22 100644 --- a/rmf_site_format/Cargo.toml +++ b/rmf_site_format/Cargo.toml @@ -18,7 +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.12", optional = true } +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 +once_cell = "1" pathdiff = "*" [dev-dependencies] diff --git a/rmf_site_format/src/alignment.rs b/rmf_site_format/src/alignment.rs index 776451f8..64977fbd 100644 --- a/rmf_site_format/src/alignment.rs +++ b/rmf_site_format/src/alignment.rs @@ -549,25 +549,28 @@ fn traverse_locations( } 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/camera_poses.rs b/rmf_site_format/src/camera_poses.rs new file mode 100644 index 00000000..0c7605b2 --- /dev/null +++ b/rmf_site_format/src/camera_poses.rs @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +use crate::{Anchor, Category, NameInSite, Pose, Rotation}; +#[cfg(feature = "bevy")] +use bevy::prelude::{Bundle, Component}; +use glam::{Affine3A, Quat, Vec3}; +use serde::{Deserialize, Serialize}; + +#[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 UserCameraPoseMarker; + +impl UserCameraPose { + pub fn from_anchors<'a, I>(name: &str, anchors: I) -> Self + where + I: Iterator, + { + // 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 { + 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 b8c1f903..375477e7 100644 --- a/rmf_site_format/src/legacy/building_map.rs +++ b/rmf_site_format/src/legacy/building_map.rs @@ -8,7 +8,7 @@ use crate::{ 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, + Texture as SiteTexture, TextureGroup, UserCameraPose, DEFAULT_NAV_GRAPH_COLORS, }; use glam::{DAffine2, DMat3, DQuat, DVec2, DVec3, EulerRot}; use serde::{Deserialize, Serialize}; @@ -33,6 +33,8 @@ pub struct BuildingMap { pub name: String, #[serde(default)] pub coordinate_system: CoordinateSystem, + #[serde(default)] + pub reference_level_name: Option, pub levels: BTreeMap, // TODO(MXG): Consider parsing legacy crowdsim data and converting it to // a format that will have future support. @@ -51,6 +53,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 { + let mut anchors: HashSet = 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); @@ -70,8 +94,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); } @@ -153,7 +196,7 @@ impl BuildingMap { let mut wall_texture_map: HashMap = HashMap::new(); let mut lift_cabin_anchors: BTreeMap> = 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 = BTreeMap::new(); let mut cartesian_fiducials: HashMap> = HashMap::new(); @@ -314,19 +357,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 } @@ -349,13 +386,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)), @@ -492,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( @@ -500,7 +539,7 @@ 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(), }, @@ -513,6 +552,7 @@ impl BuildingMap { physical_cameras, walls, rankings, + user_camera_poses, }, ); @@ -599,7 +639,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(), }, @@ -703,7 +743,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/level.rs b/rmf_site_format/src/level.rs index b4486773..6981fc78 100644 --- a/rmf_site_format/src/level.rs +++ b/rmf_site_format/src/level.rs @@ -68,6 +68,8 @@ pub struct Level { pub walls: BTreeMap>, #[serde(default, skip_serializing_if = "RankingsInLevel::is_empty")] pub rankings: RankingsInLevel, + #[serde(default, skip_serializing_if = "BTreeMap::is_empty")] + pub user_camera_poses: BTreeMap, } impl Level { @@ -83,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/lib.rs b/rmf_site_format/src/lib.rs index 336b96b0..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::*; @@ -98,6 +101,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/lift.rs b/rmf_site_format/src/lift.rs index 397d4db1..0dd4bb28 100644 --- a/rmf_site_format/src/lift.rs +++ b/rmf_site_format/src/lift.rs @@ -233,6 +233,21 @@ impl LiftCabin { }; 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 new file mode 100644 index 00000000..3414f0e6 --- /dev/null +++ b/rmf_site_format/src/sdf.rs @@ -0,0 +1,879 @@ +/* + * 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::{ + Anchor, Angle, AssetSource, Category, DoorType, Level, LiftCabin, Pose, Rotation, Site, Swing, +}; +use glam::Vec3; +use once_cell::sync::Lazy; +use sdformat_rs::*; +use std::collections::BTreeMap; +use thiserror::Error; + +const DEFAULT_CABIN_MASS: f64 = 1200.0; + +static WORLD_TEMPLATE: Lazy = Lazy::new(|| { + yaserde::de::from_str(include_str!("templates/gz_world.sdf")) + .expect("Failed deserializing template") +}); + +#[derive(Debug, Error)] +pub enum SdfConversionError { + #[error("An asset that can't be converted to an sdf world was found")] + UnsupportedAssetType, + #[error("Entity [{0}] referenced a non existing anchor")] + BrokenAnchorReference(u32), + #[error("Entity [{0}] referenced a non existing level")] + BrokenLevelReference(u32), + #[error("Parsing lift cabin for lift [{0}] failed")] + LiftParsingError(String), + #[error("Lift [{0}] had no initial level where it could be spawned")] + MissingInitialLevel(String), +} + +impl Pose { + fn to_sdf(&self) -> 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[3], quat[0], quat[1], quat[2]), + }; + SdfPose { + data: format!("{} {} {} {}", p[0], p[1], p[2], r), + ..Default::default() + } + } +} + +fn make_sdf_door_link(mesh_prefix: &str, link_name: &str) -> SdfLink { + SdfLink { + name: link_name.to_string(), + collision: vec![SdfCollision { + name: format!("{link_name}_collision"), + geometry: SdfGeometry::Mesh(SdfMeshShape { + uri: format!("meshes/{mesh_prefix}_{link_name}.glb"), + ..Default::default() + }), + surface: Some(SdfSurface { + contact: Some(SdfSurfaceContact { + collide_bitmask: Some("0x02".into()), + ..Default::default() + }), + ..Default::default() + }), + ..Default::default() + }], + visual: vec![SdfVisual { + name: format!("{link_name}_visual"), + geometry: SdfGeometry::Mesh(SdfMeshShape { + uri: format!("meshes/{mesh_prefix}_{link_name}.glb"), + ..Default::default() + }), + ..Default::default() + }], + ..Default::default() + } +} + +fn make_sdf_door( + left_anchor: Anchor, + right_anchor: Anchor, + offset: Vec3, + ros_interface: bool, + kind: &DoorType, + mesh_prefix: &str, + model_name: &str, +) -> Result { + 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 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(), model_name.to_owned()); + let mut door_model = SdfModel { + name: model_name.to_owned(), + pose: Some( + Pose { + trans: (Vec3::from([center[0], center[1], 0.0]) + offset).to_array(), + rot: Rotation::Yaw(Angle::Rad(yaw)), + } + .to_sdf(), + ), + r#static: Some(false), + ..Default::default() + }; + for label in labels.iter() { + door_model.link.push(make_sdf_door_link(mesh_prefix, 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(), 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")); + 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(); + vec![SdfJoint { + name: model_name.to_owned() + "_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() + }] + } + 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(); + 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()); + door_plugin_inner + .attributes + .insert("right_joint_name".into(), right_joint_name); + vec![SdfJoint { + name: model_name.to_owned() + "_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() + }), + 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(), + 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")); + 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(); + let left_pose = Pose { + trans: [0.0, door_length / 2.0, 1.25], + ..Default::default() + } + .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![ + SdfJoint { + name: model_name.to_owned() + "_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: model_name.to_owned() + "_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() + }, + ] + } + DoorType::DoubleSwing(door) => { + door_plugin_inner + .attributes + .insert("type".into(), "DoubleSwingDoor".into()); + 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")); + 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(); + let left_pose = Pose { + trans: [0.0, door_length / 2.0, 1.25], + ..Default::default() + } + .to_sdf(); + vec![ + SdfJoint { + name: model_name.to_owned() + "_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: model_name.to_owned() + "_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() + }, + ..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(); + vec![SdfJoint { + name: model_name.to_owned() + "_joint", + parent: "world".into(), + child: "body".into(), + r#type: "fixed".into(), + pose: Some(pose), + ..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()), + ..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 { + pub fn to_sdf(&self) -> Result { + let get_anchor = |id: u32| -> Result { + self.get_anchor(id) + .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; + let mut max_elevation = f32::MIN; + let mut toggle_floors_plugin = SdfPlugin { + name: "toggle_floors".into(), + filename: "toggle_floors".into(), + ..Default::default() + }; + 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); + min_elevation = min_elevation.min(level.properties.elevation.0); + let mut floor_models_ele = XmlElement { + name: "floor".into(), + ..Default::default() + }; + let level_model_name = &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 + world.model.push(SdfModel { + 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_id), + ..Default::default() + }), + surface: Some(SdfSurface { + contact: Some(SdfSurfaceContact { + collide_bitmask: Some("0x01".into()), + ..Default::default() + }), + ..Default::default() + }), + ..Default::default() + }], + visual: vec![SdfVisual { + name: "visual".into(), + geometry: SdfGeometry::Mesh(SdfMeshShape { + uri: format!("meshes/level_{}_visual.glb", level_id), + ..Default::default() + }), + ..Default::default() + }], + ..Default::default() + }], + ..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_id, model) in &level.models { + let mut added = false; + 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()), + ..Default::default() + }); + added = true; + } 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()), + ..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 + // NameInSite instead of AssetSource for the URI, fix + else if !model.is_static.0 { + 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/model_{}_collision.glb", model_id), + ..Default::default() + }), + ..Default::default() + }], + visual: vec![SdfVisual { + name: "visual".into(), + geometry: SdfGeometry::Mesh(SdfMeshShape { + uri: format!("meshes/model_{}_visual.glb", model_id), + ..Default::default() + }), + ..Default::default() + }], + ..Default::default() + }], + ..Default::default() + }); + added = true; + } + if added { + level_model_names.push(model.name.0.clone()); + } + } + // Now add all the doors + for (door_id, door) in &level.doors { + let left_anchor = get_anchor(door.anchors.left())?; + let right_anchor = get_anchor(door.anchors.right())?; + let door_model = make_sdf_door( + left_anchor, + right_anchor, + Vec3::new(0.0, 0.0, level.properties.elevation.0), + true, + &door.kind, + 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 { + 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_id, lift) in &self.lifts { + let get_lift_anchor = |id: u32| -> Result { + lift.cabin_anchors + .get(&id) + .ok_or(SdfConversionError::BrokenAnchorReference(id)) + .cloned() + }; + // Cabin + let LiftCabin::Rect(ref cabin) = lift.properties.cabin; + 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(), + ..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![]; + let lift_name = &lift.properties.name.0; + elements.push(("lift_name", lift_name.clone())); + let initial_floor = lift + .properties + .initial_level + .0 + .and_then(|id| self.levels.get(&id)) + .map(|level| level.properties.name.0.clone()) + .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())); + 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())); + elements.push(("cabin_joint_name", "cabin_joint".to_string())); + let mut levels: BTreeMap = BTreeMap::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), + limit: SdfJointAxisLimit { + lower: -std::f64::INFINITY, + upper: std::f64::INFINITY, + ..Default::default() + }, + ..Default::default() + }), + ..Default::default() + }]; + 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_placement.door).unwrap(); + let cabin_door_name = format!("CabinDoor_{}_door_{}", 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() + * (door_placement.thickness() / 2.0 + + door_placement + .custom_gap + .unwrap_or_else(|| cabin.gap.unwrap_or(0.01))); + let mut cabin_door = make_sdf_door( + left_anchor, + right_anchor, + x_offset, + false, + &door.kind, + &cabin_mesh_prefix, + &cabin_door_name, + )?; + 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_door.name, joint.child); + lift_joints.push(joint); + } + lift_models.push(cabin_door.into()); + for visit in door.visits.0.iter() { + let level = get_level(*visit)?; + let shaft_door_name = format!( + "ShaftDoor_{}_{}_door_{}", + level.properties.name.0, + lift_name, + face.label() + ); + let left_anchor = get_lift_anchor(door.reference_anchors.left())?; + let right_anchor = get_lift_anchor(door.reference_anchors.right())?; + 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, + &door.kind, + &cabin_mesh_prefix, + &shaft_door_name, + )?; + // Add the pose of the lift to have world coordinates + 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) { + 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(), + attributes: [ + ("cabin_door".to_string(), cabin_door_name.clone()), + ("shaft_door".to_string(), shaft_door_name), + ] + .into(), + ..Default::default() + }; + level.push(element); + } + } + for (key, door_pairs) in levels.into_iter() { + let level = get_level(key)?; + component_data.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() + }); + } + component.data = ElementData::Nested(component_data); + plugin.elements.push(component); + world.model.push(SdfModel { + name: lift.properties.name.0.clone(), + r#static: Some(lift.properties.is_static.0), + pose: Some(pose.to_sdf()), + link: vec![SdfLink { + name: "platform".into(), + collision: vec![SdfCollision { + name: "collision".into(), + geometry: SdfGeometry::Mesh(SdfMeshShape { + uri: format!("meshes/lift_{}.glb", lift_id), + ..Default::default() + }), + surface: Some(SdfSurface { + contact: Some(SdfSurfaceContact { + collide_bitmask: Some("0x04".into()), + ..Default::default() + }), + ..Default::default() + }), + ..Default::default() + }], + visual: vec![SdfVisual { + name: "visual".into(), + geometry: SdfGeometry::Mesh(SdfMeshShape { + uri: format!("meshes/lift_{}.glb", lift_id), + ..Default::default() + }), + ..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, + model: lift_models, + 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. + } + + // Spawn the robots now + // TODO(luca) use robot properties and export this like other meshes + // instead of including a tag + 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.include.push(SdfWorldInclude { + uri: "model://".to_string() + robot_type, + name: Some(robot.name.0.clone()), + pose: Some(pose.to_sdf()), + r#static: Some(robot.is_static.0), + ..Default::default() + }); + } + 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(user_camera_pose) = self + .levels + .first_key_value() + .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 = user_camera_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().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 + // 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) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::legacy::building_map::BuildingMap; + + #[test] + 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(); + 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(); + std::fs::write("test.sdf", s); + } +} diff --git a/rmf_site_format/src/site.rs b/rmf_site_format/src/site.rs index a9fed5d3..5fdb36eb 100644 --- a/rmf_site_format/src/site.rs +++ b/rmf_site_format/src/site.rs @@ -154,33 +154,45 @@ fn default_style_config() -> Style { } impl Site { - pub fn to_writer(&self, writer: W) -> ron::Result<()> { + pub fn to_writer_ron(&self, writer: W) -> ron::Result<()> { ron::ser::to_writer_pretty(writer, self, default_style_config()) } - pub fn to_writer_custom(&self, writer: W, style: Style) -> ron::Result<()> { + pub fn to_writer_custom_ron(&self, writer: W, style: Style) -> ron::Result<()> { ron::ser::to_writer_pretty(writer, self, style) } - pub fn to_string(&self) -> ron::Result { + pub fn to_string_ron(&self) -> ron::Result { ron::ser::to_string_pretty(self, default_style_config()) } - pub fn to_string_custom(&self, style: Style) -> ron::Result { + pub fn to_string_custom_ron(&self, style: Style) -> ron::Result { ron::ser::to_string_pretty(self, style) } - pub fn from_reader(reader: R) -> ron::error::SpannedResult { + pub fn to_writer_json(&self, writer: W) -> serde_json::Result<()> { + serde_json::to_writer_pretty(writer, self) + } + + pub fn from_bytes_json(s: &[u8]) -> serde_json::Result { + serde_json::from_slice(s) + } + + pub fn from_reader_ron(reader: R) -> ron::error::SpannedResult { // 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 { + pub fn from_str_ron<'a>(s: &'a str) -> ron::error::SpannedResult { ron::de::from_str(s) } - pub fn from_bytes<'a>(s: &'a [u8]) -> ron::error::SpannedResult { + pub fn to_bytes_json(&self) -> serde_json::Result> { + serde_json::to_vec_pretty(self) + } + + pub fn from_bytes_ron<'a>(s: &'a [u8]) -> ron::error::SpannedResult { ron::de::from_bytes(s) } @@ -222,10 +234,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_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_string().unwrap(); - Site::from_str(&site_string).unwrap(); + let site_string = map.to_site().unwrap().to_bytes_json().unwrap(); + Site::from_bytes_json(&site_string).unwrap(); } } 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 @@ + + + + + 0.01 + 1.0 + + + + + + + + + + + + + + 1 1 1 + 0.8 0.8 0.8 + false + + + + + + 1000 + 845 +