From bed026dd205ac6b329e7d2c02cca62becf96cb05 Mon Sep 17 00:00:00 2001 From: Michael Pivato Date: Fri, 15 Mar 2024 21:17:13 +1030 Subject: [PATCH] Clean up rust implementation, add lidar implementation in rust --- car-rs/Cargo.lock | 360 ++++++++++++++---- car-rs/Cargo.toml | 15 +- car-rs/build.rs | 3 +- car-rs/src/grpcserver.rs | 66 +++- car-rs/src/lib.rs | 15 +- car-rs/src/lidar.rs | 324 ++++++++++++++++ car-rs/src/main.rs | 31 +- .../main/proto/car/slam/SlamController.proto | 20 +- 8 files changed, 729 insertions(+), 105 deletions(-) create mode 100644 car-rs/src/lidar.rs diff --git a/car-rs/Cargo.lock b/car-rs/Cargo.lock index ca8ea39..a793660 100644 --- a/car-rs/Cargo.lock +++ b/car-rs/Cargo.lock @@ -2,27 +2,6 @@ # It is not intended for manual editing. version = 3 -[[package]] -name = "CoreFoundation-sys" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d0e9889e6db118d49d88d84728d0e964d973a5680befb5f85f55141beea5c20b" -dependencies = [ - "libc", - "mach 0.1.2", -] - -[[package]] -name = "IOKit-sys" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "99696c398cbaf669d2368076bdb3d627fb0ce51a26899d7c61228c5c0af3bf4a" -dependencies = [ - "CoreFoundation-sys", - "libc", - "mach 0.1.2", -] - [[package]] name = "aho-corasick" version = "0.7.18" @@ -78,13 +57,13 @@ checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" [[package]] name = "axum" -version = "0.5.13" +version = "0.6.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6b9496f0c1d1afb7a2af4338bbe1d969cddfead41d87a9fb3aaa6d0bbc7af648" +checksum = "678c5130a507ae3a7c797f9a17393c14849300b8440eac47cdb90a5bdcb3a543" dependencies = [ "async-trait", "axum-core", - "bitflags", + "bitflags 1.3.2", "bytes", "futures-util", "http", @@ -96,9 +75,9 @@ dependencies = [ "mime", "percent-encoding", "pin-project-lite", + "rustversion", "serde", "sync_wrapper", - "tokio", "tower", "tower-http", "tower-layer", @@ -107,9 +86,9 @@ dependencies = [ [[package]] name = "axum-core" -version = "0.2.7" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e4f44a0e6200e9d11a1cdc989e4b358f6e3d354fbf48478f345a17f4e43f8635" +checksum = "1cae3e661676ffbacb30f1a824089a8c9150e71017f7e1e38f2aa32009188d34" dependencies = [ "async-trait", "bytes", @@ -117,6 +96,9 @@ dependencies = [ "http", "http-body", "mime", + "rustversion", + "tower-layer", + "tower-service", ] [[package]] @@ -131,6 +113,12 @@ version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" +[[package]] +name = "bitflags" +version = "2.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf" + [[package]] name = "bytes" version = "1.2.1" @@ -141,12 +129,14 @@ checksum = "ec8a7b6a70fde80372154c65702f00a0f56f3e1c36abbc6c440484be248856db" name = "car-rs" version = "0.1.0" dependencies = [ + "clap", "futures-core", "futures-util", "prost", "rppal", "serialport", "tokio", + "tokio-stream", "tonic", "tonic-build", ] @@ -157,6 +147,49 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" +[[package]] +name = "clap" +version = "4.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "42dfd32784433290c51d92c438bb72ea5063797fc3cc9a21a8c4346bebbb2098" +dependencies = [ + "bitflags 2.4.2", + "clap_derive", + "clap_lex", + "is-terminal", + "once_cell", + "strsim", + "termcolor", +] + +[[package]] +name = "clap_derive" +version = "4.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fddf67631444a3a3e3e5ac51c36a5e01335302de677bd78759eaa90ab1f46644" +dependencies = [ + "heck", + "proc-macro-error", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "clap_lex" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "033f6b7a4acb1f358c742aaca805c939ee73b4c6209ae4318ec7aca81c42e646" +dependencies = [ + "os_str_bytes", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06ea2b9bc92be3c2baa9334a323ebca2d6f074ff852cd1d7b11064035cd3868f" + [[package]] name = "either" version = "1.7.0" @@ -287,6 +320,12 @@ dependencies = [ "libc", ] +[[package]] +name = "hermit-abi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d231dfb89cfffdbc30e7fc41579ed6066ad03abda9e567ccafae602b97ec5024" + [[package]] name = "http" version = "0.2.8" @@ -382,6 +421,27 @@ dependencies = [ "cfg-if", ] +[[package]] +name = "io-kit-sys" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4769cb30e5dcf1710fc6730d3e94f78c47723a014a567de385e113c737394640" +dependencies = [ + "core-foundation-sys", + "mach2", +] + +[[package]] +name = "is-terminal" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f23ff5ef2b80d608d61efee834934d862cd92461afc0560dedf493e4c033738b" +dependencies = [ + "hermit-abi 0.3.9", + "libc", + "windows-sys 0.52.0", +] + [[package]] name = "itertools" version = "0.10.3" @@ -405,9 +465,9 @@ checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" [[package]] name = "libc" -version = "0.2.127" +version = "0.2.153" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "505e71a4706fa491e9b1b55f51b95d4037d0821ee40131190475f692b35b009b" +checksum = "9c198f91728a82281a64e1f4f9eeb25d82cb32a5de251c6bd1b5154d63a8e7bd" [[package]] name = "libudev" @@ -449,28 +509,19 @@ dependencies = [ ] [[package]] -name = "mach" -version = "0.1.2" +name = "mach2" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2fd13ee2dd61cc82833ba05ade5a30bb3d63f7ced605ef827063c63078302de9" -dependencies = [ - "libc", -] - -[[package]] -name = "mach" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b823e83b2affd8f40a9ee8c29dbc56404c1e34cd2710921f2801e2cf29527afa" +checksum = "19b955cdeb2a02b9117f121ce63aa52d08ade45de53e48fe6a38b39c10f6f709" dependencies = [ "libc", ] [[package]] name = "matchit" -version = "0.5.0" +version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "73cbba799671b762df5a175adf59ce145165747bb891505c43d09aefbbf38beb" +checksum = "b87248edafb776e59e6ee64a79086f65890d3510f2c656c000bf2a7e8a0aea40" [[package]] name = "memchr" @@ -493,7 +544,7 @@ dependencies = [ "libc", "log", "wasi", - "windows-sys", + "windows-sys 0.36.1", ] [[package]] @@ -504,11 +555,11 @@ checksum = "e5ce46fe64a9d73be07dcbe690a38ce1b293be448fd8ce1e6c1b8062c9f72c6a" [[package]] name = "nix" -version = "0.24.2" +version = "0.26.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "195cdbc1741b8134346d515b3a56a1c94b0912758009cfd53f99ea0f57b065fc" +checksum = "598beaf3cc6fdd9a5dfb1630c2800c7acd31df7aaf0f565796fba2b53ca1af1b" dependencies = [ - "bitflags", + "bitflags 1.3.2", "cfg-if", "libc", ] @@ -519,7 +570,7 @@ version = "1.13.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "19e64526ebdee182341572e50e9ad03965aa510cd94427a4549448f285e957a1" dependencies = [ - "hermit-abi", + "hermit-abi 0.1.19", "libc", ] @@ -529,6 +580,12 @@ version = "1.13.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "18a6dbe30758c9f83eb00cbea4ac95966305f5a7772f3f42ebfc7fc7eddbd8e1" +[[package]] +name = "os_str_bytes" +version = "6.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2355d85b9a3786f481747ced0e0ff2ba35213a1f9bd406ed906554d7af805a1" + [[package]] name = "parking_lot" version = "0.12.1" @@ -549,7 +606,7 @@ dependencies = [ "libc", "redox_syscall", "smallvec", - "windows-sys", + "windows-sys 0.36.1", ] [[package]] @@ -622,6 +679,30 @@ dependencies = [ "syn", ] +[[package]] +name = "proc-macro-error" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" +dependencies = [ + "proc-macro-error-attr", + "proc-macro2", + "quote", + "syn", + "version_check", +] + +[[package]] +name = "proc-macro-error-attr" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" +dependencies = [ + "proc-macro2", + "quote", + "version_check", +] + [[package]] name = "proc-macro2" version = "1.0.43" @@ -643,9 +724,9 @@ dependencies = [ [[package]] name = "prost-build" -version = "0.11.1" +version = "0.11.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7f835c582e6bd972ba8347313300219fed5bfa52caf175298d860b61ff6069bb" +checksum = "276470f7f281b0ed53d2ae42dd52b4a8d08853a3c70e7fe95882acbb98a6ae94" dependencies = [ "bytes", "heck", @@ -654,9 +735,11 @@ dependencies = [ "log", "multimap", "petgraph", + "prettyplease", "prost", "prost-types", "regex", + "syn", "tempfile", "which", ] @@ -729,7 +812,7 @@ version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fb5a58c1855b4b6819d59012155603f0b22ad30cad752600aadfcb695265519a" dependencies = [ - "bitflags", + "bitflags 1.3.2", ] [[package]] @@ -768,6 +851,12 @@ dependencies = [ "libc", ] +[[package]] +name = "rustversion" +version = "1.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5583e89e108996506031660fe09baa5011b9dd0341b89029313006d1fb508d70" + [[package]] name = "scopeguard" version = "1.1.0" @@ -782,18 +871,20 @@ checksum = "e590c437916fb6b221e1d00df6e3294f3fccd70ca7e92541c475d6ed6ef5fee2" [[package]] name = "serialport" -version = "4.2.0" +version = "4.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aab92efb5cf60ad310548bc3f16fa6b0d950019cb7ed8ff41968c3d03721cf12" +checksum = "8f5a15d0be940df84846264b09b51b10b931fb2f275becb80934e3568a016828" dependencies = [ - "CoreFoundation-sys", - "IOKit-sys", - "bitflags", + "bitflags 2.4.2", "cfg-if", + "core-foundation-sys", + "io-kit-sys", "libudev", - "mach 0.3.2", + "mach2", "nix", "regex", + "scopeguard", + "unescaper", "winapi", ] @@ -831,6 +922,12 @@ dependencies = [ "winapi", ] +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + [[package]] name = "syn" version = "1.0.99" @@ -862,6 +959,35 @@ dependencies = [ "winapi", ] +[[package]] +name = "termcolor" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06794f8f6c5c898b3275aebefa6b8a1cb24cd2c6c79397ab15774837a0bc5755" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "thiserror" +version = "1.0.39" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a5ab016db510546d856297882807df8da66a16fb8c4101cb8b30054b0d5b2d9c" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.39" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5420d42e90af0c38c3290abcca25b9b3bdf379fc9f55c528f53a269d9c9a267e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + [[package]] name = "tokio" version = "1.20.1" @@ -906,9 +1032,9 @@ dependencies = [ [[package]] name = "tokio-stream" -version = "0.1.9" +version = "0.1.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "df54d54117d6fdc4e4fea40fe1e4e566b3505700e148a6827e59b34b0d2600d9" +checksum = "d660770404473ccd7bc9f8b28494a811bc18542b915c0855c51e8f419d5223ce" dependencies = [ "futures-core", "pin-project-lite", @@ -931,9 +1057,9 @@ dependencies = [ [[package]] name = "tonic" -version = "0.8.0" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "498f271adc46acce75d66f639e4d35b31b2394c295c82496727dafa16d465dd2" +checksum = "8f219fad3b929bef19b1f86fbc0358d35daed8f2cac972037ac0dc10bbb8d5fb" dependencies = [ "async-stream", "async-trait", @@ -963,9 +1089,9 @@ dependencies = [ [[package]] name = "tonic-build" -version = "0.8.0" +version = "0.8.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2fbcd2800e34e743b9ae795867d5f77b535d3a3be69fd731e39145719752df8c" +checksum = "5bf5e9b9c0f7e0a7c027dcfaba7b2c60816c7049171f679d99ee2ff65d0de8c4" dependencies = [ "prettyplease", "proc-macro2", @@ -1000,7 +1126,7 @@ version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3c530c8675c1dbf98facee631536fa116b5fb6382d7dd6dc1b118d970eafe3ba" dependencies = [ - "bitflags", + "bitflags 1.3.2", "bytes", "futures-core", "futures-util", @@ -1015,9 +1141,9 @@ dependencies = [ [[package]] name = "tower-layer" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "343bc9466d3fe6b0f960ef45960509f84480bf4fd96f92901afe7ff3df9d3a62" +checksum = "c20c8dbed6283a09604c3e69b4b7eeb54e298b8a600d4d5ecb5ad39de609f1d0" [[package]] name = "tower-service" @@ -1074,12 +1200,27 @@ version = "0.2.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "59547bce71d9c38b83d9c0e92b6066c4253371f15005def0c30d9657f50c7642" +[[package]] +name = "unescaper" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0adf6ad32eb5b3cadff915f7b770faaac8f7ff0476633aa29eb0d9584d889d34" +dependencies = [ + "thiserror", +] + [[package]] name = "unicode-ident" version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c4f5b37a154999a8f3f98cc23a628d850e154479cd94decf3414696e12e31aaf" +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + [[package]] name = "want" version = "0.3.0" @@ -1123,6 +1264,15 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" +[[package]] +name = "winapi-util" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f29e6f9198ba0d26b4c9f07dbe6f9ed633e1f3d5b8b414090084349e46a52596" +dependencies = [ + "winapi", +] + [[package]] name = "winapi-x86_64-pc-windows-gnu" version = "0.4.0" @@ -1135,39 +1285,105 @@ version = "0.36.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ea04155a16a59f9eab786fe12a4a450e75cdb175f9e0d80da1e17db09f55b8d2" dependencies = [ - "windows_aarch64_msvc", - "windows_i686_gnu", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_msvc", + "windows_aarch64_msvc 0.36.1", + "windows_i686_gnu 0.36.1", + "windows_i686_msvc 0.36.1", + "windows_x86_64_gnu 0.36.1", + "windows_x86_64_msvc 0.36.1", ] +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-targets" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7dd37b7e5ab9018759f893a1952c9420d060016fc19a472b4bb20d1bdd694d1b" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc 0.52.4", + "windows_i686_gnu 0.52.4", + "windows_i686_msvc 0.52.4", + "windows_x86_64_gnu 0.52.4", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc 0.52.4", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bcf46cf4c365c6f2d1cc93ce535f2c8b244591df96ceee75d8e83deb70a9cac9" + [[package]] name = "windows_aarch64_msvc" version = "0.36.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9bb8c3fd39ade2d67e9874ac4f3db21f0d710bee00fe7cab16949ec184eeaa47" +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da9f259dd3bcf6990b55bffd094c4f7235817ba4ceebde8e6d11cd0c5633b675" + [[package]] name = "windows_i686_gnu" version = "0.36.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "180e6ccf01daf4c426b846dfc66db1fc518f074baa793aa7d9b9aaeffad6a3b6" +[[package]] +name = "windows_i686_gnu" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b474d8268f99e0995f25b9f095bc7434632601028cf86590aea5c8a5cb7801d3" + [[package]] name = "windows_i686_msvc" version = "0.36.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e2e7917148b2812d1eeafaeb22a97e4813dfa60a3f8f78ebe204bcc88f12f024" +[[package]] +name = "windows_i686_msvc" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1515e9a29e5bed743cb4415a9ecf5dfca648ce85ee42e15873c3cd8610ff8e02" + [[package]] name = "windows_x86_64_gnu" version = "0.36.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4dcd171b8776c41b97521e5da127a2d86ad280114807d0b2ab1e462bc764d9e1" +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5eee091590e89cc02ad514ffe3ead9eb6b660aedca2183455434b93546371a03" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77ca79f2451b49fa9e2af39f0747fe999fcda4f5e241b2898624dca97a1f2177" + [[package]] name = "windows_x86_64_msvc" version = "0.36.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c811ca4a8c853ef420abd8592ba53ddbbac90410fab6903b3e79972a631f7680" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32b752e52a2da0ddfbdbcc6fceadfeede4c939ed16d13e648833a61dfb611ed8" diff --git a/car-rs/Cargo.toml b/car-rs/Cargo.toml index b421f4e..87b5ccb 100644 --- a/car-rs/Cargo.toml +++ b/car-rs/Cargo.toml @@ -11,16 +11,23 @@ rppal = { version = "0.13.1", optional = true } futures-core = "0.3" futures-util = "0.3" tokio = { version = "1", features = ["full"] } +tokio-stream = "0.1.11" prost = "0.11" # https://github.com/hyperium/tonic -tonic = "0.8.0" +tonic = "0.8.3" # https://docs.rs/serialport/4.0.1/serialport/index.html -serialport = "4.0.1" +serialport = "4.3.0" + +clap = { version = "4.1.8", features = ["derive"] } [build-dependencies] -tonic-build = "0.8.0" +tonic-build = "0.8.3" [features] -rppal = ["dep:rppal"] \ No newline at end of file +rppal = ["dep:rppal"] + +# How to get dependencies for my own projects, so I don't need to upload to crates.io +# or create my own rust package repo. +# https://doc.rust-lang.org/cargo/reference/specifying-dependencies.html#specifying-dependencies-from-git-repositories \ No newline at end of file diff --git a/car-rs/build.rs b/car-rs/build.rs index b47d655..5313ae4 100644 --- a/car-rs/build.rs +++ b/car-rs/build.rs @@ -1,6 +1,7 @@ fn main() -> Result<(), Box> { + let protos = ["slam/SlamController.proto", "control/motorService.proto"]; tonic_build::configure().compile( - &["../protobuf/src/main/proto/car/control/motorService.proto"], + &protos.map(|proto| "../protobuf/src/main/proto/car/".to_owned() + proto), &["../protobuf/src/main/proto"], )?; Ok(()) diff --git a/car-rs/src/grpcserver.rs b/car-rs/src/grpcserver.rs index 1c46876..754ff6c 100644 --- a/car-rs/src/grpcserver.rs +++ b/car-rs/src/grpcserver.rs @@ -2,17 +2,28 @@ pub mod motor_control_service { tonic::include_proto!("motor_control"); } +pub mod slam_controller_service { + tonic::include_proto!("slam_control"); +} + use std::{sync::Mutex, time::Duration}; use car_rs::{Servo, Vehicle}; use futures_util::StreamExt; use motor_control_service::car_control_server::CarControl; use tokio::time; +use tokio_stream::wrappers::ReceiverStream; use tonic::{Request, Response, Status, Streaming}; -use self::motor_control_service::{ - RecordingReqeust, RecordingResponse, SaveRequest, SaveResponse, SteeringRequest, - SteeringResponse, ThrottleRequest, ThrottleResponse, Vehicle2DRequest, Vehicle2DResponse, +use self::{ + motor_control_service::{ + RecordingReqeust, RecordingResponse, SaveRequest, SaveResponse, SteeringRequest, + SteeringResponse, ThrottleRequest, ThrottleResponse, Vehicle2DRequest, Vehicle2DResponse, + }, + slam_controller_service::{ + slam_control_server::SlamControl, SlamDetails, SlamLocation, SlamScan, + StartMapStreamingResponse, StopStreamingRequest, StopStreamingResponse, + }, }; #[derive(Debug)] @@ -20,8 +31,6 @@ pub struct MotorControlService where T: Vehicle, { - // TODO: Any better way than mutex? need it over refcell to implement send, and need a smart pointer - // for interior mutability (since the generated protobuf functions aren't mut) vehicle: Mutex, } @@ -62,10 +71,16 @@ impl CarControl for MotorControlService { request: Request>, ) -> Result, Status> { let mut stream = request.into_inner(); + + // If we don't a request for 3 seconds, timeout and stop the vehicle while let Ok(Some(Ok(req))) = time::timeout(Duration::from_secs(3), stream.next()).await { let mut vehicle = self.vehicle.lock().unwrap(); - vehicle.set_throttle(req.throttle.unwrap().throttle as f64); - vehicle.set_steering(req.steering.unwrap().steering as f64); + if let Some(throttle) = req.throttle { + vehicle.set_throttle(throttle.throttle as f64); + } + if let Some(steering) = req.steering { + vehicle.set_steering(steering.steering as f64); + } } self.vehicle.lock().unwrap().set_throttle(0.); @@ -87,3 +102,40 @@ impl CarControl for MotorControlService { unimplemented!() } } + +#[derive(Debug)] +pub struct SlamControlService {} + +#[tonic::async_trait] +impl SlamControl for SlamControlService { + async fn start_map_streaming( + &self, + request: Request, + ) -> Result, Status> { + todo!() + } + + type map_streamStream = ReceiverStream>; + + async fn map_stream( + &self, + request: Request, + ) -> Result, Status> { + let scan = SlamScan { + map: vec![], + location: Some(SlamLocation { + theta: 1., + x: 1., + y: 1., + }), + }; + todo!() + } + + async fn stop_streaming( + &self, + request: Request, + ) -> Result, Status> { + todo!() + } +} diff --git a/car-rs/src/lib.rs b/car-rs/src/lib.rs index 80da653..f5d38eb 100644 --- a/car-rs/src/lib.rs +++ b/car-rs/src/lib.rs @@ -2,6 +2,8 @@ use std::sync::{Arc, Mutex}; use serialport::SerialPort; +mod lidar; + // TODO: Should be returning results in these traits pub trait Servo { fn get_value(&self) -> f64; @@ -91,14 +93,14 @@ pub mod rppal { } pub struct Esp32SerialPwmServo { - serial_port: Arc>, + serial_port: T, value: f64, channel: u8, pin: u8, } impl Esp32SerialPwmServo { - pub fn new(serial_port: Arc>, channel: u8, pin: u8) -> Esp32SerialPwmServo { + pub fn new(serial_port: T, channel: u8, pin: u8) -> Esp32SerialPwmServo { let mut servo = Esp32SerialPwmServo { serial_port, value: 0., @@ -112,11 +114,7 @@ impl Esp32SerialPwmServo { impl Esp32SerialPwmServo { fn init_pwm(&mut self) { - let bytes_written = self - .serial_port - .lock() - .unwrap() - .write(&[0, 1, self.channel, self.pin]); + let bytes_written = self.serial_port.write(&[0, 1, self.channel, self.pin]); // TODO: Better error handling (even anyhow would be better) match bytes_written { Ok(size) => println!("{}", size), @@ -132,7 +130,6 @@ impl Servo for Esp32SerialPwmServo { fn set_value(&mut self, value: f64) { let mut temp_value = value; - // TODO: Panic when out of bounds? if temp_value < -1. { temp_value = -1.; } else if temp_value > 1. { @@ -141,8 +138,6 @@ impl Servo for Esp32SerialPwmServo { self.value = temp_value; let bytes_written = self .serial_port - .lock() - .unwrap() .write(&[self.channel, ((value + 1.) / 2. * 255.) as u8]); // TODO: Better error handling match bytes_written { diff --git a/car-rs/src/lidar.rs b/car-rs/src/lidar.rs new file mode 100644 index 0000000..991a533 --- /dev/null +++ b/car-rs/src/lidar.rs @@ -0,0 +1,324 @@ +use core::fmt; +use serialport::ClearBuffer; +use serialport::SerialPort; +use std::io; +use std::io::Error; +use std::string::FromUtf8Error; + +const SYNC: u8 = 0xA5; +const SYNC2: u8 = 0x5A; + +const GET_INFO: u8 = 0x50; +const GET_HEALTH: u8 = 0x52; + +const STOP: u8 = 0x25; +const RESET: u8 = 0x40; + +const SCAN: u8 = 0x20; +#[allow(dead_code)] +const FORCE_SCAN: u8 = 0x21; + +const DESCRIPTOR_LEN: usize = 7; +const INFO_LEN: u8 = 20; +const HEALTH_LEN: u8 = 3; + +const INFO_TYPE: u8 = 4; +const HEALTH_TYPE: u8 = 6; +const SCAN_TYPE: u8 = 129; + +const SET_PWM: u8 = 0xF0; +const MAX_MOTOR_PWM: usize = 1023; +#[allow(dead_code)] +const DEFAULT_MOTOR_PWM: usize = 660; + +const SCAN_SIZE: u8 = 5; + +pub enum HealthStatus { + Good, + Warning, + Error, +} + +impl HealthStatus { + pub fn from_raw(raw: u8) -> HealthStatus { + match raw { + 0 => HealthStatus::Good, + 1 => HealthStatus::Warning, + _ => HealthStatus::Error, + } + } +} + +pub enum RPLidarError { + FailedToStart, + IncorrectInfoFormat, + IncorrectHealthFormat, + ScanError(String), + IncorrectDescriptorFormat, + IOError(Error), +} + +impl From for RPLidarError { + fn from(err: Error) -> Self { + RPLidarError::IOError(err) + } +} + +impl From for RPLidarError { + fn from(_: FromUtf8Error) -> Self { + RPLidarError::IncorrectDescriptorFormat + } +} + +impl fmt::Debug for RPLidarError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + Self::FailedToStart => write!(f, "FailedToStart"), + Self::IncorrectInfoFormat => write!(f, "IncorrectInfoFormat"), + Self::IncorrectHealthFormat => write!(f, "IncorrectHealthFormat"), + Self::ScanError(arg0) => f.debug_tuple("ScanError").field(arg0).finish(), + Self::IncorrectDescriptorFormat => write!(f, "IncorrectDescriptorFormat"), + Self::IOError(arg0) => f.debug_tuple("IOError").field(arg0).finish(), + } + } +} + +pub struct LidarScan { + pub new_scan: bool, + pub quality: u8, + pub angle: f64, + pub distance: f64, +} + +fn process_scan(raw: [u8; SCAN_SIZE as usize]) -> Result { + let new_scan = raw[0] & 0b1; + let inversed_new_scan = (raw[0] >> 1) & 0b1; + let quality = raw[0] >> 2; + if new_scan == inversed_new_scan { + return Err(RPLidarError::ScanError(String::from( + "New scan flags mismatch", + ))); + } + + if (raw[1] & 0b1) != 1 { + return Err(RPLidarError::ScanError(String::from( + "Check bit not equal to 1", + ))); + } + + let angle = ((((raw[1] as isize) >> 1) + (raw[2] as isize) << 7) as f64) / 64.; + let distance = (((raw[3] as isize) + ((raw[4] as isize) << 8)) as f64) / 4.; + + Ok(LidarScan { + new_scan: new_scan == 1, + quality, + angle, + distance, + }) +} + +pub struct Lidar { + motor_running: bool, + serial_port: T, + is_scanning: bool, + measurements_per_batch: usize, + max_buffer_measurements: u32, +} + +impl Lidar { + pub fn new(serial_port: T) -> Lidar { + Lidar { + motor_running: false, + serial_port, + is_scanning: false, + measurements_per_batch: 200, + max_buffer_measurements: 500, + } + } + + pub fn connect(&mut self) { + self.serial_port + .set_baud_rate(115200) + .expect("Failed to set baudrate"); + } + + pub fn start_motor(&mut self) { + self.serial_port + .write_data_terminal_ready(true) + .expect("Failed to write dtr"); + // self.set_pwm(DEFAULT_MOTOR_PWM); + self.motor_running = true; + } + + pub fn start_scanning(&mut self) -> Result { + self.start_motor(); + let (status, _) = self.get_health()?; + + match status { + HealthStatus::Error => Err(RPLidarError::IncorrectHealthFormat), + _ => { + self.send_command(vec![SCAN]); + let (data_size, is_single, data_type) = self.read_descriptor()?; + if data_size != SCAN_SIZE || is_single || data_type != SCAN_TYPE { + return Err(RPLidarError::IncorrectDescriptorFormat); + } + self.is_scanning = true; + Ok(data_size) + } + } + } + + pub fn stop_motor(&mut self) { + self.set_pwm(0); + self.serial_port + .write_data_terminal_ready(false) + .expect("Failed to write dtr"); + self.motor_running = false; + } + + pub fn get_info(&mut self) -> Result<(u8, (u8, u8), u8, String), RPLidarError> { + self.send_command(vec![GET_INFO]); + + let (data_size, is_single, data_type) = self.read_descriptor()?; + + if data_size != INFO_LEN || !is_single || data_type != INFO_TYPE { + return Err(RPLidarError::IncorrectInfoFormat); + } + let mut buf = [0; INFO_LEN as usize]; + self.serial_port.read(&mut buf)?; + let raw = buf; + let serial_number = String::from_utf8(Vec::from(&raw[4..]))?; + return Ok((raw[0], (raw[2], raw[1]), raw[3], serial_number)); + } + + pub fn get_health(&mut self) -> Result<(HealthStatus, usize), RPLidarError> { + self.send_command(vec![GET_HEALTH]); + + let (data_size, is_single, data_type) = self.read_descriptor()?; + + if data_size != HEALTH_LEN || !is_single || data_type != HEALTH_TYPE { + return Err(RPLidarError::IncorrectHealthFormat); + } + + let mut buf = [0; HEALTH_LEN as usize]; + self.serial_port.read(&mut buf)?; + let raw = buf; + let status = HealthStatus::from_raw(raw[0]); + let error_code = (raw[1] as usize) << 8 + raw[2]; + return Ok((status, error_code)); + } + + pub fn clear_input(&mut self) { + self.serial_port + .clear(ClearBuffer::Input) + .expect("Failed to clear input buffer"); + } + + pub fn stop(&mut self) { + self.send_command(vec![STOP]); + self.is_scanning = false; + } + + pub fn reset(&mut self) { + self.send_command(vec![RESET]); + } + + pub fn receive_measurement_and_clear_buffer( + &mut self, + max_buffer_measurements: u32, + ) -> Result<[u8; SCAN_SIZE as usize], RPLidarError> { + if !self.is_scanning { + return Err(RPLidarError::ScanError(String::from( + "Haven't started scanning", + ))); + } + let mut buf = [0; SCAN_SIZE as usize]; + self.serial_port.read(&mut buf)?; + let raw = buf; + if max_buffer_measurements > 0 { + if self + .serial_port + .bytes_to_read() + .expect("Failed to get bytes to read") + > max_buffer_measurements * SCAN_SIZE as u32 + { + println!("Too many measurements in the input buffer. Clearing Buffer"); + self.serial_port + .clear(ClearBuffer::Input) + .expect("Failed to clear input buffer."); + } + } + Ok(raw) + } + + fn send_command(&mut self, command: Vec) { + let mut vec = vec![SYNC]; + vec.extend(command); + self.serial_port + .write(&vec) + .expect("Failed to send command"); + } + + fn send_payload_command(&mut self, cmd: u8, payload: Vec) { + let size: u8 = payload + .len() + .try_into() + .expect("Failed to convert payload length"); + let mut req = vec![SYNC]; + req.push(cmd); + req.push(size); + req.extend(payload); + let checksum = self.calc_checksum(&req); + req.push(checksum); + self.serial_port + .write(&req) + .expect("Failed to send payload"); + } + + fn calc_checksum(&self, data: &Vec) -> u8 { + data.iter() + .copied() + .reduce(|accum, next| accum ^ next) + .expect("Failed to calculate checksum") + .to_owned() + } + + fn read_descriptor(&mut self) -> Result<(u8, bool, u8), RPLidarError> { + let mut descriptor: [u8; DESCRIPTOR_LEN] = [0; DESCRIPTOR_LEN]; + let ret = self.serial_port.read(&mut descriptor)?; + if ret != DESCRIPTOR_LEN || (descriptor[0] != SYNC && descriptor[1] != SYNC2) { + eprintln!("Failed to read enough or something"); + Err(RPLidarError::IncorrectDescriptorFormat) + } else { + let is_single = descriptor[DESCRIPTOR_LEN - 2] == 0; + Ok((descriptor[2], is_single, descriptor[DESCRIPTOR_LEN - 1])) + } + } + + fn set_pwm(&mut self, pwm: usize) { + assert!(pwm <= MAX_MOTOR_PWM); + self.send_payload_command(SET_PWM, Vec::from(pwm.to_ne_bytes())); + } +} + +// Probably want to have separate iterator types like we do in swift for measurements vs scans +impl Iterator for Lidar { + type Item = Vec; + + fn next(&mut self) -> Option { + if self.is_scanning { + let mut all_scans: Vec = Vec::new(); + while all_scans.len() < self.measurements_per_batch { + if let Ok(raw) = + self.receive_measurement_and_clear_buffer(self.max_buffer_measurements) + { + if let Ok(scan) = process_scan(raw) { + all_scans.push(scan); + } + } + } + return Some(all_scans); + } + None + } +} diff --git a/car-rs/src/main.rs b/car-rs/src/main.rs index b66f42a..cf8265f 100644 --- a/car-rs/src/main.rs +++ b/car-rs/src/main.rs @@ -1,6 +1,5 @@ -use std::sync::{Arc, Mutex}; - use car_rs::{Esp32SerialPwmServo, ServoVehicle}; +use clap::Parser; use grpcserver::{ motor_control_service::car_control_server::CarControlServer, MotorControlService, }; @@ -9,16 +8,34 @@ use tonic::transport::Server; mod grpcserver; +#[derive(Parser, Debug)] +#[command(version, about, long_about = None)] +struct Args { + #[arg(short, long)] + serial_port: String, + + #[arg(short, long, default_value_t = 10000)] + web_port: u32, + + #[arg(short, long, default_value_t = 32400)] + baud_rate: u32, +} + #[tokio::main] async fn main() -> Result<(), Box> { - let addr = "[::1]:10000".parse().unwrap(); + let args = Args::parse(); + let addr = format!("[::1]:{}", &args.web_port).parse().unwrap(); - let serial_port = serialport::new("", 32400) + let mut steering_port = serialport::new(&args.serial_port, args.baud_rate) .open_native() .expect("Could not open serial port"); - let serial_servo = Arc::new(Mutex::new(serial_port)); - let steering_servo = Esp32SerialPwmServo::new(serial_servo.clone(), 1, 12); - let throttle_servo = Esp32SerialPwmServo::new(serial_servo.clone(), 2, 18); + steering_port.set_exclusive(false)?; + let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate) + .open_native() + .expect("Could not open serial port"); + throttle_port.set_exclusive(false)?; + let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12); + let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18); let motor_control = MotorControlService::new(ServoVehicle::new(steering_servo, throttle_servo)); diff --git a/protobuf/src/main/proto/car/slam/SlamController.proto b/protobuf/src/main/proto/car/slam/SlamController.proto index 6888096..8acdca9 100644 --- a/protobuf/src/main/proto/car/slam/SlamController.proto +++ b/protobuf/src/main/proto/car/slam/SlamController.proto @@ -1,11 +1,11 @@ syntax = "proto3"; +package SlamControl; + option java_multiple_files = true; option java_package = "org.vato.carcontroller"; option java_outer_classname = "SlamControllerProto"; -import "google/protobuf/empty.proto"; - message SlamDetails { int32 map_size_pixels = 1; int32 map_size_meters = 2; @@ -26,10 +26,22 @@ message SlamScan{ SlamLocation location = 2; } +message StartMapStreamingResponse { + +} + +message StopStreamingRequest { + +} + +message StopStreamingResponse { + +} + service SlamControl { - rpc start_map_streaming(SlamDetails) returns (google.protobuf.Empty) {} + rpc start_map_streaming(SlamDetails) returns (StartMapStreamingResponse) {} rpc map_stream(SlamDetails) returns (stream SlamScan) {} - rpc stop_streaming(google.protobuf.Empty) returns (google.protobuf.Empty) {} + rpc stop_streaming(StopStreamingRequest) returns (StopStreamingResponse) {} } \ No newline at end of file