diff --git a/.envrc b/.envrc new file mode 100644 index 00000000000..4ceec9afec7 --- /dev/null +++ b/.envrc @@ -0,0 +1 @@ +use flake ./#noFHS diff --git a/.gitignore b/.gitignore index 3875fd529aa..cd0706b67c0 100644 --- a/.gitignore +++ b/.gitignore @@ -21,3 +21,7 @@ logging/ # Ignore perf output flamegraph.svg perf.data* + +# Nix +result +.direnv diff --git a/Cargo.lock b/Cargo.lock index 69b25b6871e..edda827adaf 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1,6 +1,6 @@ # This file is automatically @generated by Cargo. # It is not intended for manual editing. -version = 3 +version = 4 [[package]] name = "addr2line" @@ -37,6 +37,12 @@ dependencies = [ "memchr", ] +[[package]] +name = "allocator-api2" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" + [[package]] name = "android-tzdata" version = "0.1.1" @@ -58,18 +64,80 @@ version = "0.1.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4b46cbb362ab8752921c97e041f5e366ee6297bd428a31275b9fcf1e380f7299" +[[package]] +name = "anstream" +version = "0.6.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "301af1932e46185686725e0fad2f8f2aa7da69dd70bf6ecc44d6b703844a3933" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + [[package]] name = "anstyle" version = "1.0.10" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "55cc3b69f167a1ef2e161439aa98aed94e6028e5f9a59be9a6ffb47aef1651f9" +[[package]] +name = "anstyle-parse" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e7644824f0aa2c7b9384579234ef10eb7efb6a0deb83f9630a49594dd9c15c2" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c8bdeb6047d8983be085bab0ba1472e6dc604e7041dbf6fcd5e71523014fae9" +dependencies = [ + "windows-sys 0.59.0", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "403f75924867bb1033c59fbf0797484329750cfbe3c4325cd33127941fabc882" +dependencies = [ + "anstyle", + "once_cell_polyfill", + "windows-sys 0.59.0", +] + [[package]] name = "anyhow" version = "1.0.95" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "34ac096ce696dc2fcabef30516bb13c0a68a11d30131d3df6f04711467681b04" +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "arbitrary" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dde20b3d026af13f561bdd0f15edf01fc734f0dafcedbaf42bba506a9517f223" +dependencies = [ + "derive_arbitrary", +] + [[package]] name = "assert_approx_eq" version = "1.1.0" @@ -148,6 +216,28 @@ dependencies = [ "generic-array", ] +[[package]] +name = "bluerobotics-ping" +version = "0.3.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "13ab52e9d304ad393420b54faac80c2cafd9b0d8a0ade35e9e0a098f74a7d51e" +dependencies = [ + "arbitrary", + "bytes", + "clap", + "convert_case", + "futures", + "proc-macro2", + "quote", + "serde", + "serde_bytes", + "serde_json", + "tokio", + "tokio-serial", + "tokio-util", + "tracing", +] + [[package]] name = "bumpalo" version = "3.17.0" @@ -162,9 +252,9 @@ checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "bytes" -version = "1.10.0" +version = "1.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f61dac84819c6588b558454b194026eb1f09c293b9036ae9b159e74e73ab6cf9" +checksum = "d71b6127be86fdcfddb610f7182ac57211d4b18a3e9c82eb2d17662f2227ad6a" [[package]] name = "bzip2" @@ -289,21 +379,36 @@ dependencies = [ [[package]] name = "clap" -version = "4.5.28" +version = "4.5.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3e77c3243bd94243c03672cb5154667347c457ca271254724f9f393aee1c05ff" +checksum = "40b6887a1d8685cebccf115538db5c0efe625ccac9696ad45c409d96566e910f" dependencies = [ "clap_builder", + "clap_derive", ] [[package]] name = "clap_builder" -version = "4.5.27" +version = "4.5.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b26884eb4b57140e4d2d93652abfa49498b938b3c9179f9fc487b0acc3edad7" +checksum = "e0c66c08ce9f0c698cbce5c0279d0bb6ac936d8674174fe48f736533b964f59e" dependencies = [ + "anstream", "anstyle", "clap_lex", + "strsim", +] + +[[package]] +name = "clap_derive" +version = "4.5.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2c7947ae4cc3d851207c1adb5b5e260ff0cca11446b1d6d1423788e442257ce" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn 2.0.98", ] [[package]] @@ -312,6 +417,12 @@ version = "0.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f46ad14479a25103f283c0f10005961cf086d8dc42205bb44c46ac563475dca6" +[[package]] +name = "colorchoice" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75" + [[package]] name = "concurrent-queue" version = "2.5.0" @@ -327,6 +438,15 @@ version = "0.1.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "245097e9a4535ee1e3e3931fcfcd55a796a44c643e8596ff6566d68f09b87bbc" +[[package]] +name = "convert_case" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baaaa0ecca5b51987b9423ccdc971514dd8b0bb7b4060b983d3664dad3f1f89f" +dependencies = [ + "unicode-segmentation", +] + [[package]] name = "core-foundation" version = "0.9.4" @@ -492,6 +612,17 @@ dependencies = [ "syn 2.0.98", ] +[[package]] +name = "derive_arbitrary" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30542c1ad912e0e3d22a1935c290e12e8a29d704a420177a31faad4a601a0800" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.98", +] + [[package]] name = "digest" version = "0.10.7" @@ -535,6 +666,16 @@ version = "1.0.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "92773504d58c093f6de2459af4af33faa518c13451eb8f2b5698ed3d36e7c813" +[[package]] +name = "earcutr" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "79127ed59a85d7687c409e9978547cffb7dc79675355ed22da6b66fd5f6ead01" +dependencies = [ + "itertools 0.11.0", + "num-traits", +] + [[package]] name = "either" version = "1.13.0" @@ -615,12 +756,24 @@ dependencies = [ "miniz_oxide", ] +[[package]] +name = "float_next_after" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8bf7cc16383c4b8d58b9905a8509f02926ce3058053c056376248d958c9df1e8" + [[package]] name = "fnv" version = "1.0.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" +[[package]] +name = "foldhash" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9c4f5dac5e15c24eb999c26181a6ca40b39fe946cbe4c263c7209467bc83af2" + [[package]] name = "foreign-types" version = "0.3.2" @@ -744,6 +897,46 @@ dependencies = [ "version_check", ] +[[package]] +name = "geo" +version = "0.30.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4416397671d8997e9a3e7ad99714f4f00a22e9eaa9b966a5985d2194fc9e02e1" +dependencies = [ + "earcutr", + "float_next_after", + "geo-types", + "geographiclib-rs", + "i_overlay", + "log", + "num-traits", + "robust", + "rstar", + "spade", +] + +[[package]] +name = "geo-types" +version = "0.7.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62ddb1950450d67efee2bbc5e429c68d052a822de3aad010d28b351fbb705224" +dependencies = [ + "approx", + "num-traits", + "rayon", + "rstar", + "serde", +] + +[[package]] +name = "geographiclib-rs" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f611040a2bb37eaa29a78a128d1e92a378a03e0b6e66ae27398d42b1ba9a7841" +dependencies = [ + "libm", +] + [[package]] name = "getrandom" version = "0.2.15" @@ -824,11 +1017,51 @@ dependencies = [ "crunchy", ] +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.15.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash", +] + +[[package]] +name = "hdbscan" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d4317842ec54a54ca44aa9bd7f599f5b723c07eaeb4a98875909724500c9cf1a" +dependencies = [ + "kdtree", + "num-traits", +] + +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" [[package]] name = "hermit-abi" @@ -957,6 +1190,50 @@ dependencies = [ "tracing", ] +[[package]] +name = "i_float" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85df3a416829bb955fdc2416c7b73680c8dcea8d731f2c7aa23e1042fe1b8343" +dependencies = [ + "serde", +] + +[[package]] +name = "i_key_sort" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "347c253b4748a1a28baf94c9ce133b6b166f08573157e05afe718812bc599fcd" + +[[package]] +name = "i_overlay" +version = "2.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0542dfef184afdd42174a03dcc0625b6147fb73e1b974b1a08a2a42ac35cee49" +dependencies = [ + "i_float", + "i_key_sort", + "i_shape", + "i_tree", + "rayon", +] + +[[package]] +name = "i_shape" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a38f5a42678726718ff924f6d4a0e79b129776aeed298f71de4ceedbd091bce" +dependencies = [ + "i_float", + "serde", +] + +[[package]] +name = "i_tree" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "155181bc97d770181cf9477da51218a19ee92a8e5be642e796661aee2b601139" + [[package]] name = "iana-time-zone" version = "0.1.61" @@ -1187,6 +1464,12 @@ dependencies = [ "windows-sys 0.59.0", ] +[[package]] +name = "is_terminal_polyfill" +version = "1.70.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7943c866cc5cd64cbc25b2e01621d07fa8eb2a1a23160ee81ce38704e97b8ecf" + [[package]] name = "itertools" version = "0.10.5" @@ -1196,6 +1479,15 @@ dependencies = [ "either", ] +[[package]] +name = "itertools" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1c173a5686ce8bfa551b3563d0c2170bf24ca44da99c7ca4bfdab5418c3fe57" +dependencies = [ + "either", +] + [[package]] name = "itertools" version = "0.13.0" @@ -1230,12 +1522,28 @@ dependencies = [ "wasm-bindgen", ] +[[package]] +name = "kdtree" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0f0a0e9f770b65bac9aad00f97a67ab5c5319effed07f6da385da3c2115e47ba" +dependencies = [ + "num-traits", + "thiserror 1.0.69", +] + [[package]] name = "libc" version = "0.2.169" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b5aba8db14291edd000dfcc4d620c7ebfb122c613afb886ca8803fa4e128a20a" +[[package]] +name = "libm" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" + [[package]] name = "libredox" version = "0.1.3" @@ -1389,6 +1697,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", + "libm", ] [[package]] @@ -1406,6 +1715,12 @@ version = "1.20.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "945462a4b81e43c4e3ba96bd7b49d834c6f61198356aa858733bc4acf3cbe62e" +[[package]] +name = "once_cell_polyfill" +version = "1.70.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4895175b425cb1f87721b59f0f286c2092bd4af812243672510e1ac53e2e0ad" + [[package]] name = "oorandom" version = "11.1.4" @@ -1657,9 +1972,9 @@ dependencies = [ [[package]] name = "proc-macro2" -version = "1.0.93" +version = "1.0.95" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60946a68e5f9d28b0dc1c21bb8a97ee7d018a8b322fa57838ba31cc878e22d99" +checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778" dependencies = [ "unicode-ident", ] @@ -1851,6 +2166,23 @@ dependencies = [ "windows-sys 0.52.0", ] +[[package]] +name = "robust" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e27ee8bb91ca0adcf0ecb116293afa12d393f9c2b9b9cd54d33e8078fe19839" + +[[package]] +name = "rstar" +version = "0.12.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "421400d13ccfd26dfa5858199c30a5d76f9c54e0dba7575273025b43c5175dbb" +dependencies = [ + "heapless", + "num-traits", + "smallvec", +] + [[package]] name = "rustc-demangle" version = "0.1.24" @@ -1976,18 +2308,27 @@ checksum = "f79dfe2d285b0488816f30e700a7438c5a73d816b5b7d3ac72fbc48b0d185e03" [[package]] name = "serde" -version = "1.0.217" +version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "02fc4265df13d6fa1d00ecff087228cc0a2b5f3c0e87e258d8b94a156e984c70" +checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6" dependencies = [ "serde_derive", ] +[[package]] +name = "serde_bytes" +version = "0.11.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8437fd221bde2d4ca316d61b90e337e9e702b3820b87d63caa9ba6c02bd06d96" +dependencies = [ + "serde", +] + [[package]] name = "serde_derive" -version = "1.0.217" +version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5a9bf7cf98d04a2b28aead066b7496853d4779c9cc183c440dbac457641e19a0" +checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ "proc-macro2", "quote", @@ -1996,9 +2337,9 @@ dependencies = [ [[package]] name = "serde_json" -version = "1.0.138" +version = "1.0.140" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d434192e7da787e94a6ea7e9670b26a036d0ca41e0b7efb2676dd32bae872949" +checksum = "20068b6e96dc6c9bd23e01df8827e6c7e1f2fddd43c21810382803c136b99373" dependencies = [ "itoa", "memchr", @@ -2107,6 +2448,18 @@ dependencies = [ "windows-sys 0.52.0", ] +[[package]] +name = "spade" +version = "2.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a14e31a007e9f85c32784b04f89e6e194bb252a4d41b4a8ccd9e77245d901c8c" +dependencies = [ + "hashbrown", + "num-traits", + "robust", + "smallvec", +] + [[package]] name = "spin" version = "0.9.8" @@ -2119,6 +2472,12 @@ version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + [[package]] name = "subtle" version = "2.6.1" @@ -2132,6 +2491,7 @@ dependencies = [ "anyhow", "assert_approx_eq", "async-channel", + "bluerobotics-ping", "bytes", "cc", "chrono", @@ -2140,7 +2500,10 @@ dependencies = [ "derive-getters", "flate2", "futures", + "geo", + "geo-types", "graphviz-rust", + "hdbscan", "itertools 0.13.0", "nonzero", "num-traits", @@ -2151,10 +2514,12 @@ dependencies = [ "rayon", "reqwest", "serde", + "serde_json", "syn 2.0.98", "tar", "tokio", "tokio-serial", + "tokio-util", "toml", "uuid", "zip-extract", @@ -2310,9 +2675,9 @@ dependencies = [ [[package]] name = "tokio" -version = "1.43.0" +version = "1.45.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d61fa4ffa3de412bfea335c6ecff681de2b609ba3c77ef3e00e521813a9ed9e" +checksum = "75ef51a33ef1da925cea3e4eb122833cb377c61439ca401b770f54902b806779" dependencies = [ "backtrace", "bytes", @@ -2373,9 +2738,9 @@ dependencies = [ [[package]] name = "tokio-util" -version = "0.7.13" +version = "0.7.15" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d7fcaa8d55a2bdd6b83ace262b016eca0d79ee02818c5c1bcdf0305114081078" +checksum = "66a539a9ad6d5d281510d5bd368c973d636c02dbf8a67300bfb6b950696ad7df" dependencies = [ "bytes", "futures-core", @@ -2452,9 +2817,21 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "784e0ac535deb450455cbfa28a6f0df145ea1bb7ae51b821cf5e7927fdcfbdd0" dependencies = [ "pin-project-lite", + "tracing-attributes", "tracing-core", ] +[[package]] +name = "tracing-attributes" +version = "0.1.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "81383ab64e72a7a8b8e13130c49e3dab29def6d0c7d76a03087b3cf71c5c6903" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.98", +] + [[package]] name = "tracing-core" version = "0.1.33" @@ -2497,6 +2874,12 @@ version = "1.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a210d160f08b701c8721ba1c726c11662f877ea6b7094007e1ca9a1041945034" +[[package]] +name = "unicode-segmentation" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f6ccf251212114b54433ec949fd6a7841275f9ada20dddd2f29e9ceea4501493" + [[package]] name = "untrusted" version = "0.9.0" @@ -2526,6 +2909,12 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b6c140620e7ffbb22c2dee59cafe6084a59b5ffc27a8859a5f0d494b5d52b6be" +[[package]] +name = "utf8parse" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" + [[package]] name = "uuid" version = "1.13.1" diff --git a/Cargo.toml b/Cargo.toml index c094a979575..9f089a1d1d8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -23,7 +23,7 @@ required-features = ["graphing"] [features] default = [] logging = [] -annotated_streams = [] +annotated_streams = ["logging"] #unblocked_logging = ["logging"] cuda = ["dep:cc"] cuda_f16 = ["cuda"] @@ -32,6 +32,9 @@ networked_testing = [] [dependencies] opencv = { version = "0.94.2", default-features = false, features = ["dnn", "imgcodecs", "imgproc", "videoio", "cudaimgproc", "cudafilters"] } # Vision processing +geo = { version = "0.30.0"} +geo-types = "0.7.16" +hdbscan = "0.10.1" # opencv = "0.94.2" tokio-serial = "5.4.1" # Async serial comms tokio = { version = "1.38.0", features = ["full"] } # Async runtime @@ -51,6 +54,9 @@ reqwest = { version = "0.12.3", optional = true } # Downloading godot sim async-channel = "2.3.1" # Blocking -> Async thread message passing crossbeam = "0.8.4" # Blocking thread message passing nonzero = "0.2.0" +bluerobotics-ping = "0.3.5" +serde_json = "1.0.140" +tokio-util = { version = "0.7.15" } [build-dependencies] quote = { version = "1.0.36", optional = true } diff --git a/flake.lock b/flake.lock new file mode 100644 index 00000000000..ff876250449 --- /dev/null +++ b/flake.lock @@ -0,0 +1,100 @@ +{ + "nodes": { + "fenix": { + "inputs": { + "nixpkgs": [ + "nixpkgs" + ], + "rust-analyzer-src": "rust-analyzer-src" + }, + "locked": { + "lastModified": 1753252982, + "narHash": "sha256-brrpvP+4GRXLHjvnDr1j1/yA4117hzs6t9IT60JuSI8=", + "owner": "nix-community", + "repo": "fenix", + "rev": "8546562a84feb5370ce57493277b6f2c3cbdc432", + "type": "github" + }, + "original": { + "owner": "nix-community", + "repo": "fenix", + "type": "github" + } + }, + "flake-parts": { + "inputs": { + "nixpkgs-lib": "nixpkgs-lib" + }, + "locked": { + "lastModified": 1753121425, + "narHash": "sha256-TVcTNvOeWWk1DXljFxVRp+E0tzG1LhrVjOGGoMHuXio=", + "owner": "hercules-ci", + "repo": "flake-parts", + "rev": "644e0fc48951a860279da645ba77fe4a6e814c5e", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "flake-parts", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1753151930, + "narHash": "sha256-XSQy6wRKHhRe//iVY5lS/ZpI/Jn6crWI8fQzl647wCg=", + "owner": "NixOS", + "repo": "nixpkgs", + "rev": "83e677f31c84212343f4cc553bab85c2efcad60a", + "type": "github" + }, + "original": { + "owner": "NixOS", + "ref": "nixpkgs-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "nixpkgs-lib": { + "locked": { + "lastModified": 1751159883, + "narHash": "sha256-urW/Ylk9FIfvXfliA1ywh75yszAbiTEVgpPeinFyVZo=", + "owner": "nix-community", + "repo": "nixpkgs.lib", + "rev": "14a40a1d7fb9afa4739275ac642ed7301a9ba1ab", + "type": "github" + }, + "original": { + "owner": "nix-community", + "repo": "nixpkgs.lib", + "type": "github" + } + }, + "root": { + "inputs": { + "fenix": "fenix", + "flake-parts": "flake-parts", + "nixpkgs": "nixpkgs" + } + }, + "rust-analyzer-src": { + "flake": false, + "locked": { + "lastModified": 1753204114, + "narHash": "sha256-xH8EIod+Hwog4P9OwX9hdtk6Nqr54M0tzMI71yGNOYI=", + "owner": "rust-lang", + "repo": "rust-analyzer", + "rev": "b40fce3ccdc5f94453c6aca4da8b64174a03a5ad", + "type": "github" + }, + "original": { + "owner": "rust-lang", + "ref": "nightly", + "repo": "rust-analyzer", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/flake.nix b/flake.nix new file mode 100644 index 00000000000..aed4b1a3ecf --- /dev/null +++ b/flake.nix @@ -0,0 +1,83 @@ +{ + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable"; + fenix = { + url = "github:nix-community/fenix"; + inputs.nixpkgs.follows = "nixpkgs"; + }; + flake-parts.url = "github:hercules-ci/flake-parts"; + }; + + outputs = inputs: + inputs.flake-parts.lib.mkFlake {inherit inputs;} { + systems = ["x86_64-linux"]; + perSystem = { + pkgs, + config, + ... + }: { + devShells = let + rustToolchain = with inputs.fenix.packages.${pkgs.system}; + combine ( + with stable; [ + clippy + rustc + cargo + rustfmt + rust-src + targets.aarch64-unknown-linux-gnu.stable.rust-std + ] + ); + rustPackages = with pkgs; [ + rustToolchain + openssl + pkg-config + cargo-deny + cargo-edit + cargo-watch + cargo-expand + cargo-info + rust-analyzer + bacon + ]; + in { + default = + (pkgs.buildFHSEnv { + name = "sw8s-rust-fhs"; + targetPkgs = pkgs: + with pkgs.llvmPackages_19; + [ + libclang + libllvm + bintools + clang + ] + ++ rustPackages; + extraOutputsToInstall = [ + "dev" + "out" + ]; + profile = '' + export JETSON_DEVSHELL_MODE=1 + ''; + }).env; + noFHS = pkgs.mkShell { + nativeBuildInputs = [pkgs.pkg-config]; + buildInputs = with pkgs; + [ + llvmPackages_19.clang + opencv + ] + ++ rustPackages; + LIBCLANG_PATH = "${pkgs.llvmPackages_19.libclang.lib}/lib"; + }; + }; + formatter = pkgs.nixfmt-tree; + packages.image = pkgs.dockerTools.streamNixShellImage { + name = "test"; + tag = "latest"; + drv = config.devShells.fhs; + }; + }; + }; +} diff --git a/jetson/config.toml b/jetson/config.toml new file mode 100644 index 00000000000..e563d7b9887 --- /dev/null +++ b/jetson/config.toml @@ -0,0 +1,2 @@ +sysroot_url = "https://github.com/Cowboylaserkittenjetshark/make-sysroot/releases/download/v1.0.0/sysroot-jetson.tar.xz" +fetch_sysroot = true diff --git a/jetson/src/main.rs b/jetson/src/main.rs index aa2b940429a..a92d5a2910c 100644 --- a/jetson/src/main.rs +++ b/jetson/src/main.rs @@ -1,6 +1,6 @@ /// Adapted from a script written by Marcus Behel use std::{ - env::{args, current_dir, set_var}, + env::{args, current_dir, set_var, var}, fmt::Write, fs::read_to_string, process::{exit, Command}, @@ -29,8 +29,9 @@ async fn main() -> Result<()> { let config_str = read_to_string("config.toml").context("Could not read config file config.toml")?; let config: Config = toml::from_str(config_str.as_str()).context("Failed to parse config")?; + let devshell_mode = var("JETSON_DEVSHELL_MODE").is_ok(); - tools_check()?; + tools_check(devshell_mode)?; let mut system_args = args().skip(1).collect::>(); if system_args.is_empty() { @@ -38,7 +39,7 @@ async fn main() -> Result<()> { "build".to_string(), "--release".to_string(), "--features".to_string(), - "logging,annotated_streams".to_string(), + "logging".to_string(), ]; } @@ -56,19 +57,23 @@ async fn main() -> Result<()> { let multibar_clone = multibar.clone(); // Jetson Nano architecture - let toolchain_install = spawn_blocking(move || { - // Prevent progress bars from overlapping with toolchain output - multibar.set_draw_target(ProgressDrawTarget::hidden()); + let toolchain_install = if devshell_mode { + None + } else { + Some(spawn_blocking(move || { + // Prevent progress bars from overlapping with toolchain output + multibar.set_draw_target(ProgressDrawTarget::hidden()); - Command::new("rustup") - .args(["target", "add", "aarch64-unknown-linux-gnu"]) - .spawn() - .unwrap() - .wait() - .unwrap(); + Command::new("rustup") + .args(["target", "add", "aarch64-unknown-linux-gnu"]) + .spawn() + .unwrap() + .wait() + .unwrap(); - multibar.set_draw_target(ProgressDrawTarget::stdout()); - }); + multibar.set_draw_target(ProgressDrawTarget::stdout()); + })) + }; let sysroot_clone = sysroot.clone(); let config_clone = config.clone(); @@ -205,13 +210,20 @@ async fn main() -> Result<()> { + sysroot .join("./usr/include/opencv4/opencv2") .to_str() - .unwrap(), + .unwrap() + + (if devshell_mode { + ",/usr/lib/clang/19/include" + } else { + "" + }), ); // Wait for Jetson Nano toolchain - toolchain_install - .await - .context("failure while waiting for Jetson Nano toolchain install")?; + if let Some(handle) = toolchain_install { + handle + .await + .context("failure while waiting for Jetson Nano toolchain install")?; + } Command::new("cargo") .current_dir(parent_dir.clone()) @@ -235,10 +247,14 @@ async fn main() -> Result<()> { } /// Checks that all required programs are installed -fn tools_check() -> Result<()> { - ["rustup", "cargo", "clang", "lld"] - .into_iter() - .try_for_each(program_check) +fn tools_check(devshell_mode: bool) -> Result<()> { + let mut tools = vec!["cargo", "clang"]; + if !devshell_mode { + tools.push("rustup"); + tools.push("ld"); + } + + tools.into_iter().try_for_each(program_check) } /// Checks that all programs are installed diff --git a/night_config.toml b/night_config.toml new file mode 100644 index 00000000000..6b0b6af8d07 --- /dev/null +++ b/night_config.toml @@ -0,0 +1,99 @@ +control_board_path = "/dev/ttyACM0" +control_board_backup_path = "/dev/ttyACM4" +meb_path = "/dev/ttyACM2" +front_cam_path = "/dev/video0" +bottom_cam_path = "/dev/video1" +color_profile = "Night Testing" +shark = "Left" +saw_fish = "Right" + +[missions.gate] +depth = -1.0 +speed = 1.0 +true_count = 4 +false_count = 1 +side = "Right" + +[missions.path_align] +depth = -1.0 +speed = -0.0 +forward_speed = 0.2 +detections = 8 + +[missions.slalom] +depth = -0.75 +speed = 0.4 +start_detections = 5 +end_detections = 10 +side = "Right" +centered_threshold = 0.0 +dumb_strafe_secs = 1 +init_duration = 4.0 +strafe_duration = 2.0 +traversal_duration = 6.0 +yaw_adjustment = 20.0 +yaw_speed = -0.2 +area_bounds = { start = 630.0, end = 11000.0 } +correction_factor = -0.4 + + +[missions.coinflip] +depth = -1.15 +angle_correction = 0.2 +true_count = 2 + +[missions.octagon] + +[missions.bin] +depth = -1.0 +speed = 0.3 + +[sonar] +serial_port = "/dev/ttyUSB0" +serial_baud_rate = 115200 +bootloader = "Skip" +[sonar.auto_transmit] +mode = 1 +gain_setting = "Low" +transmit_duration = 80 +sample_period = 500 +transmit_frequency = 750 +number_of_samples = 1024 +start_angle = 0 +stop_angle = 399 +num_steps = 1 +delay = 0 + +[color_profiles."Night Testing".red] +start = { y = 107, u = 92, v = 100 } +end = { y = 167, u = 160, v = 128 } + +[color_profiles."Night Testing".orange] +start = { y = 73, u = 87, v = 137 } +end = { y = 189, u = 136, v = 222 } + +[color_profiles."Night Testing".purple] +start = { y = 0, u = 131, v = 117 } +end = { y = 139, u = 255, v = 255 } + +[color_profiles."Night Testing".yellow] +start = { y = 0, u = 0, v = 111 } +end = { y = 144, u = 114, v = 255 } + +[color_profiles."B2 Day".red] +start = { y = 77, u = 101, v = 113 } +end = { y = 200, u = 170, v = 210 } + +[color_profiles."B2 Day".orange] +start = { y = 111, u = 55, v = 134 } +end = { y = 255, u = 112, v = 194 } + +[color_profiles."B2 Day".purple] +start = { y = 0, u = 131, v = 117 } +end = { y = 139, u = 255, v = 255 } + +[color_profiles."B2 Day".yellow] +start = { y = 0, u = 0, v = 111 } +end = { y = 144, u = 114, v = 255 } + + diff --git a/playstreams.sh b/playstreams.sh deleted file mode 100755 index a9dc9f5e169..00000000000 --- a/playstreams.sh +++ /dev/null @@ -1,2 +0,0 @@ -mpv rtsp://192.168.2.5:8554/front.mp4 --title=Front --no-cache --untimed --no-demuxer-thread --vd-lavc-threads=1 --profile=low-latency --no-correct-pts --osc=no & -mpv rtsp://192.168.2.5:8554/bottom.mp4 --title=Front --no-cache --untimed --no-demuxer-thread --vd-lavc-threads=1 --profile=low-latency --no-correct-pts --osc=no & diff --git a/run.sh b/run.sh new file mode 100755 index 00000000000..cd7b024b49d --- /dev/null +++ b/run.sh @@ -0,0 +1,18 @@ +cd ~/deploy/ +while true; do + case $1 in + 1) + ln -f -s config_1.toml config.toml + ./sw8s_rust arm gate_run_reckon spin slalom_left slalom_left slalom_left static_align + ;; + 2) + ln -f -s config_2.toml config.toml + ./sw8s_rust arm gate_run_reckon spin slalom_left slalom_right slalom_right static_align + ;; + 3) + ln -f -s config_3.toml config.toml + ./sw8s_rust arm gate_run_reckon spin + ;; + *) echo "NO" + esac +done diff --git a/src/comms/auv_control_board/util.rs b/src/comms/auv_control_board/util.rs index 47e785d66c4..1e4fc32e48d 100644 --- a/src/comms/auv_control_board/util.rs +++ b/src/comms/auv_control_board/util.rs @@ -1,7 +1,6 @@ +// Implementing use std::{error::Error, fmt::Display}; -/// Implementing - pub const START_BYTE: u8 = 253; pub const END_BYTE: u8 = 254; pub const ESCAPE_BYTE: u8 = 255; @@ -50,7 +49,7 @@ pub enum AcknowledgeErr { impl Display for AcknowledgeErr { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{:?}", self) + write!(f, "{self:?}") } } diff --git a/src/comms/control_board/mod.rs b/src/comms/control_board/mod.rs index d3e3b5dd515..d7cd4eee085 100644 --- a/src/comms/control_board/mod.rs +++ b/src/comms/control_board/mod.rs @@ -1,15 +1,10 @@ use core::fmt::Debug; -use std::{ - ops::Deref, - sync::{Arc, OnceLock}, - time::Duration, -}; +use std::{ops::Deref, sync::Arc, time::Duration}; use anyhow::{anyhow, bail, Result}; use tokio::{ io::{self, AsyncRead, AsyncWrite, AsyncWriteExt, WriteHalf}, net::TcpStream, - spawn, sync::Mutex, time::{sleep, timeout}, }; @@ -32,29 +27,6 @@ pub enum SensorStatuses { AllGood, } -static STAB_2_DRIFT: OnceLock>> = OnceLock::new(); -fn stab_2_drift() -> f32 { - let drift_val = STAB_2_DRIFT.get_or_init(|| { - let drift_val = Arc::new(std::sync::Mutex::new(0.0)); - - let drift_val_clone = drift_val.clone(); - spawn(async move { - sleep(Duration::from_secs(5)).await; - loop { - { - let mut drift_val_inner = drift_val_clone.lock().unwrap(); - // *drift_val_inner += 0.015; - } - sleep(Duration::from_secs(1)).await - } - }); - - drift_val - }); - - *drift_val.lock().unwrap() -} - pub static LAST_YAW: std::sync::Mutex> = std::sync::Mutex::new(None); #[derive(Debug)] @@ -157,7 +129,7 @@ impl ControlBoard { .await?; self.stability_assist_pid_tune('Y', 2.0, 0.0, 0.0, 0.1, false) .await?; - self.stability_assist_pid_tune('Z', 10.0, 0.0, 0.0, 1.0, false) + self.stability_assist_pid_tune('Z', 4.0, 0.0, 0.0, 1.0, false) .await?; self.stability_assist_pid_tune('D', 1.5, 0.0, 0.0, 1.0, false) .await @@ -336,19 +308,9 @@ impl ControlBoard { let mut message = Vec::with_capacity(32 * 8); message.extend(SASSIST_2); - [ - x, - y, - target_pitch, - target_roll, - ( - target_yaw - // + stab_2_drift() - ), - target_depth, - ] - .iter() - .for_each(|val| message.extend(val.to_le_bytes())); + [x, y, target_pitch, target_roll, target_yaw, target_depth] + .iter() + .for_each(|val| message.extend(val.to_le_bytes())); *LAST_YAW.lock().unwrap() = Some(target_yaw); self.write_out_basic(message).await diff --git a/src/comms/control_board/response.rs b/src/comms/control_board/response.rs index 2764c3f29ad..6264836b23d 100644 --- a/src/comms/control_board/response.rs +++ b/src/comms/control_board/response.rs @@ -155,7 +155,7 @@ impl ResponseMap { given_crc.to_ne_bytes(), calculated_crc.to_ne_bytes(), payload, - payload.iter().map(|byte| format!("{:02x}", byte).to_string()).reduce(|acc, x| acc + &x).unwrap_or("".to_string()) + payload.iter().map(|byte| format!("{byte:02x}").to_string()).reduce(|acc, x| acc + &x).unwrap_or("".to_string()) )); } }).await diff --git a/src/comms/meb/response.rs b/src/comms/meb/response.rs index 99cde522130..3c99a487d8d 100644 --- a/src/comms/meb/response.rs +++ b/src/comms/meb/response.rs @@ -175,7 +175,7 @@ impl Statuses { given_crc.to_ne_bytes(), calculated_crc.to_ne_bytes(), payload, - payload.iter().map(|byte| format!("{:02x}", byte).to_string()).reduce(|acc, x| acc + &x).unwrap_or("".to_string()) + payload.iter().map(|byte| format!("{byte:02x}").to_string()).reduce(|acc, x| acc + &x).unwrap_or("".to_string()) )); } }).await; diff --git a/src/config/bin.rs b/src/config/bin.rs new file mode 100644 index 00000000000..5be127003a1 --- /dev/null +++ b/src/config/bin.rs @@ -0,0 +1,16 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Config { + pub depth: f32, + pub speed: f32, +} + +impl Default for Config { + fn default() -> Self { + Self { + depth: -1.25, + speed: 0.2, + } + } +} diff --git a/src/config/coinflip.rs b/src/config/coinflip.rs new file mode 100644 index 00000000000..809ce91d8f0 --- /dev/null +++ b/src/config/coinflip.rs @@ -0,0 +1,18 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Config { + pub depth: f32, + pub angle_correction: f32, + pub true_count: u32, +} + +impl Default for Config { + fn default() -> Self { + Self { + depth: -1.25, + angle_correction: 15.0, + true_count: 4, + } + } +} diff --git a/src/config/gate.rs b/src/config/gate.rs index cb9f5404a8f..6065eddf3cc 100644 --- a/src/config/gate.rs +++ b/src/config/gate.rs @@ -1,3 +1,4 @@ +use super::Side; use serde::{Deserialize, Serialize}; #[derive(Debug, Serialize, Deserialize)] @@ -6,6 +7,14 @@ pub struct Config { pub speed: f32, pub true_count: u32, pub false_count: u32, + pub side: Side, + pub yaw_speed: f32, + pub strafe_speed: f32, + pub init_duration: f32, + pub strafe_duration: f32, + pub traversal_duration: f32, + pub yaw_adjustment: f32, + pub correction_factor: f32, } impl Default for Config { @@ -15,6 +24,14 @@ impl Default for Config { speed: 0.2, true_count: 4, false_count: 1, + side: Side::default(), + yaw_speed: 0.2, + strafe_speed: 0.2, + correction_factor: 0.2, + init_duration: 3.0, + strafe_duration: 2.0, + traversal_duration: 8.0, + yaw_adjustment: 20.0, } } } diff --git a/src/config/mod.rs b/src/config/mod.rs index 04274b74cba..624217c6f4d 100644 --- a/src/config/mod.rs +++ b/src/config/mod.rs @@ -1,15 +1,20 @@ +pub mod bin; +pub mod coinflip; pub mod gate; +pub mod octagon; pub mod path_align; +pub mod slalom; +pub mod sonar; -use std::{ - fs::{read_to_string, write}, - ops::{Deref, DerefMut}, - path::PathBuf, -}; +use std::fs::read_to_string; +use crate::vision::Yuv; use anyhow::Result; -use crossbeam::epoch::CompareAndSetOrdering; use serde::{Deserialize, Serialize}; +use std::collections::HashMap; +use std::ops::RangeInclusive; + +pub const SHUTDOWN_TIMEOUT: u64 = 5; // Default values const CONFIG_FILE: &str = "config.toml"; @@ -26,7 +31,12 @@ pub struct Config { pub meb_path: String, pub front_cam_path: String, pub bottom_cam_path: String, + pub sonar: sonar::Config, pub missions: Missions, + pub color_profile: String, + pub color_profiles: HashMap, + pub shark: Side, + pub saw_fish: Side, } impl Config { @@ -36,6 +46,12 @@ impl Config { } } +impl Config { + pub fn get_color_profile(&self) -> Option<&ColorProfile> { + self.color_profiles.get(&self.color_profile) + } +} + impl Default for Config { fn default() -> Self { Self { @@ -44,7 +60,12 @@ impl Default for Config { meb_path: MEB_PATH.to_string(), front_cam_path: FRONT_CAM.to_string(), bottom_cam_path: BOTTOM_CAM.to_string(), + sonar: sonar::Config::default(), missions: Missions::default(), + color_profile: "".to_string(), + color_profiles: HashMap::new(), + shark: Side::default(), + saw_fish: Side::default(), } } } @@ -53,4 +74,29 @@ impl Default for Config { pub struct Missions { pub gate: gate::Config, pub path_align: path_align::Config, + pub slalom: slalom::Config, + pub bin: bin::Config, + pub octagon: octagon::Config, + pub coinflip: coinflip::Config, +} + +#[derive(Debug, Serialize, Deserialize)] +pub struct ColorProfile { + pub red: RangeInclusive, + pub orange: RangeInclusive, + pub yellow: RangeInclusive, + pub purple: RangeInclusive, + pub black: RangeInclusive, +} + +#[derive(Debug, Serialize, Deserialize, PartialEq, Eq, Hash, Clone)] +pub enum Side { + Right, + Left, +} + +impl Default for Side { + fn default() -> Self { + Self::Right + } } diff --git a/src/config/octagon.rs b/src/config/octagon.rs new file mode 100644 index 00000000000..5844c1b6804 --- /dev/null +++ b/src/config/octagon.rs @@ -0,0 +1,10 @@ +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Config {} + +impl Default for Config { + fn default() -> Self { + Self {} + } +} diff --git a/src/config/path_align.rs b/src/config/path_align.rs index 0375be3d07e..72bab3c43f5 100644 --- a/src/config/path_align.rs +++ b/src/config/path_align.rs @@ -4,7 +4,12 @@ use serde::{Deserialize, Serialize}; pub struct Config { pub depth: f32, pub speed: f32, + pub forward_speed: f32, + pub strafe_speed: f32, pub detections: u8, + pub yaw_angle: f32, + pub forward_duration: u64, + pub yaw_wait: u64, } impl Default for Config { @@ -12,7 +17,12 @@ impl Default for Config { Self { depth: -1.25, speed: 0.3, + forward_speed: 0.3, + strafe_speed: 0.3, detections: 10, + yaw_angle: 15.0, + forward_duration: 3, + yaw_wait: 3, } } } diff --git a/src/config/slalom.rs b/src/config/slalom.rs new file mode 100644 index 00000000000..97404e49164 --- /dev/null +++ b/src/config/slalom.rs @@ -0,0 +1,43 @@ +use std::ops::RangeInclusive; + +use super::Side; +use serde::{Deserialize, Serialize}; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Config { + pub depth: f32, + pub speed: f32, + pub start_detections: u8, + pub end_detections: u8, + pub side: Side, + pub centered_threshold: f32, + pub dumb_strafe_secs: u64, + pub init_duration: f32, + pub strafe_duration: f32, + pub traversal_duration: f32, + pub yaw_adjustment: f32, + pub yaw_speed: f32, + pub area_bounds: RangeInclusive, + pub correction_factor: f32, +} + +impl Default for Config { + fn default() -> Self { + Self { + depth: -1.25, + speed: 0.3, + start_detections: 10, + end_detections: 10, + side: Side::Left, + centered_threshold: 0.0, + dumb_strafe_secs: 2, + init_duration: 1.0, + strafe_duration: 2.0, + traversal_duration: 6.0, + yaw_adjustment: 15.0, + yaw_speed: 0.2, + area_bounds: 1000.0..=11000.0, + correction_factor: 0.4, + } + } +} diff --git a/src/config/sonar.rs b/src/config/sonar.rs new file mode 100644 index 00000000000..66e39cba66a --- /dev/null +++ b/src/config/sonar.rs @@ -0,0 +1,77 @@ +use serde::{Deserialize, Serialize}; +use std::path::PathBuf; + +#[derive(Debug, Serialize, Deserialize)] +pub struct Config { + pub serial_port: PathBuf, + pub serial_baud_rate: u32, + pub bootloader: Bootloader, + pub auto_transmit: AutoTransmit, +} + +impl Default for Config { + fn default() -> Self { + Self { + serial_port: "/dev/ttyACM4".into(), + serial_baud_rate: 115200, + bootloader: Bootloader::default(), + auto_transmit: AutoTransmit::default(), + } + } +} + +#[derive(Debug, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy)] +pub enum Bootloader { + Skip = 0, + Run = 1, +} + +impl Default for Bootloader { + fn default() -> Self { + Self::Skip + } +} + +#[derive(Debug, Serialize, Deserialize, Clone, Copy)] +pub struct AutoTransmit { + pub mode: u8, + pub gain_setting: Gain, + pub transmit_duration: u16, + pub sample_period: u16, + pub transmit_frequency: u16, + pub number_of_samples: u16, + pub start_angle: u16, + pub stop_angle: u16, + pub num_steps: u8, + pub delay: u8, +} + +impl Default for AutoTransmit { + fn default() -> Self { + Self { + mode: 1, + gain_setting: Gain::default(), + transmit_duration: 500, + sample_period: 20000, + transmit_frequency: 700, + number_of_samples: 1000, + start_angle: 0, + stop_angle: 399, + num_steps: 1, + delay: 0, + } + } +} + +#[derive(Debug, Serialize, Deserialize, PartialEq, Eq, Hash, Clone, Copy)] +pub enum Gain { + Low = 0, + Normal = 1, + High = 2, +} + +impl Default for Gain { + fn default() -> Self { + Self::Normal + } +} diff --git a/src/main.rs b/src/main.rs index c4704169ccc..40d3bbfea3e 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,33 +1,38 @@ use anyhow::{bail, Result}; -use crossbeam::epoch::Pointable; use std::env::temp_dir; use std::env; use std::process::exit; +use std::time::Duration; use sw8s_rust_lib::{ comms::{ control_board::{ControlBoard, SensorStatuses}, meb::MainElectronicsBoard, }, - config::Config, + config::{Config, SHUTDOWN_TIMEOUT}, logln, missions::{ action::ActionExec, action_context::FullActionContext, align_buoy::{buoy_align, buoy_align_shot}, basic::descend_and_go_forward, + bin::bin, circle_buoy::{ buoy_circle_sequence, buoy_circle_sequence_blind, buoy_circle_sequence_model, }, - coinflip::coinflip, + coinflip::{coinflip, coinflip_procedural}, example::{initial_descent, pid_test}, - fancy_octagon::fancy_octagon, fire_torpedo::{FireLeftTorpedo, FireRightTorpedo}, - gate::{gate_run_coinflip, gate_run_complex, gate_run_naive, gate_run_testing}, + gate::{ + gate_run_complex, gate_run_cv_procedural, gate_run_dead_reckon, gate_run_naive, + gate_run_procedural, gate_run_testing, + }, meb::WaitArm, octagon::octagon, - path_align::path_align_procedural, + path_align::{path_align_procedural, static_align_procedural}, reset_torpedo::ResetTorpedo, + slalom::slalom, + sonar::sonar, spin::spin, vision::PIPELINE_KILL, }, @@ -40,13 +45,12 @@ use tokio::{ signal, sync::{ mpsc::{self, UnboundedSender}, - OnceCell, RwLock, + OnceCell, RwLock, Semaphore, }, time::{sleep, timeout}, }; use tokio_serial::SerialStream; -pub mod config; -use std::time::Duration; +use tokio_util::sync::CancellationToken; static CONFIG_CELL: OnceCell = OnceCell::const_new(); async fn config() -> &'static Config { @@ -147,13 +151,29 @@ async fn static_context() -> &'static FullActionContext<'static, WriteHalf>() { - run_mission(&arg).await.unwrap(); + let _guard = SHUTDOWN_GUARD.acquire().await.unwrap(); + run_mission(&arg, mission_ct.clone()).await.unwrap(); } // Send shutdown signal @@ -183,15 +204,22 @@ async fn main() { } /// Graceful shutdown, see -async fn shutdown_handler() -> UnboundedSender { +async fn shutdown_handler() -> (UnboundedSender, CancellationToken) { let (shutdown_tx, mut shutdown_rx) = mpsc::unbounded_channel::(); + let mission_ct = CancellationToken::new(); + let mission_ct_clone = mission_ct.clone(); tokio::spawn(async move { // Wait for shutdown signal - let exit_status = tokio::select! {_ = signal::ctrl_c() => { - logln!("CTRL-C RECV"); - 1 }, Some(x) = shutdown_rx.recv() => { - logln!("SHUTDOWN SIGNAL RECV"); - x }}; + let exit_status = tokio::select! { + _ = signal::ctrl_c() => { + logln!("CTRL-C RECV"); + 1 + }, + Some(x) = shutdown_rx.recv() => { + logln!("SHUTDOWN SIGNAL RECV"); + x + } + }; let status = control_board().await.sensor_status_query().await; @@ -216,21 +244,38 @@ async fn shutdown_handler() -> UnboundedSender { // Reset Torpedo ResetTorpedo::new(static_context().await).execute().await; - // If shutdown is unexpected, immediately exit nonzero + // If shutdown is unexpected, cancel running missions and exit nonzero if exit_status != 0 { + // Cancel running missions + mission_ct_clone.cancel(); + // Wait for running mission to exit + if timeout( + Duration::from_secs(SHUTDOWN_TIMEOUT), + SHUTDOWN_GUARD.acquire(), + ) + .await + .is_err() + { + logln!("Missions did not exit within {SHUTDOWN_TIMEOUT} seconds") + } exit(exit_status) }; }); - shutdown_tx + (shutdown_tx, mission_ct) } -async fn run_mission(mission: &str) -> Result<()> { +async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> { + /// Wrapper for missions that do not directly use the cancellation token + macro_rules! ctwrap { + ($fut:expr) => {{ + let _ = cancel.run_until_cancelled($fut).await; + Ok(()) + }}; + } + let config = config().await; let res = match mission.to_lowercase().as_str() { - "arm" => { - WaitArm::new(static_context().await).execute().await; - Ok(()) - } + "arm" => ctwrap!(WaitArm::new(static_context().await).execute()), "empty" => { let control_board = control_board().await; control_board @@ -319,69 +364,70 @@ async fn run_mission(mission: &str) -> Result<()> { logln!("Finished travel"); Ok(()) } - "descend" | "forward" => { - let _ = descend_and_go_forward(&FullActionContext::new( + "descend" | "forward" => ctwrap!(descend_and_go_forward(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), + "gate_run_naive" => ctwrap!(gate_run_naive(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), + "gate_run_complex" => ctwrap!(gate_run_complex(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), + "gate_run_coinflip" => ctwrap!(gate_run_cv_procedural( + &FullActionContext::new( control_board().await, meb().await, front_cam().await, bottom_cam().await, gate_target().await, - )) - .execute() - .await; - Ok(()) - } - "gate_run_naive" => { - let _ = gate_run_naive(&FullActionContext::new( + ), + &config.missions.gate, + &config.get_color_profile().unwrap(), + )), + "gate_run_yolo" => ctwrap!(gate_run_procedural( + &FullActionContext::new( control_board().await, meb().await, front_cam().await, bottom_cam().await, gate_target().await, - )) - .execute() - .await; - Ok(()) - } - "gate_run_complex" => { - let _ = gate_run_complex(&FullActionContext::new( + ), + &config.missions.gate + )), + "gate_run_reckon" => ctwrap!(gate_run_dead_reckon( + &FullActionContext::new( control_board().await, meb().await, front_cam().await, bottom_cam().await, gate_target().await, - )) - .execute() - .await; - Ok(()) - } - "gate_run_coinflip" => { - let _ = gate_run_coinflip( - &FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - ), - &config.missions.gate, - ) - .execute() - .await; - Ok(()) - } - "gate_run_testing" => { - let _ = gate_run_testing(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - )) - .execute() - .await; - Ok(()) - } + ), + &config.missions.gate, + &config.get_color_profile().unwrap(), + )), + "gate_run_testing" => ctwrap!(gate_run_testing(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), "start_cam" => { // This has not been tested logln!("Opening camera"); @@ -390,98 +436,61 @@ async fn run_mission(mission: &str) -> Result<()> { logln!("Opened camera"); Ok(()) } - "path_align" => { - let _ = path_align_procedural( - &FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - ), - &config.missions.path_align, - ) - .await; - Ok(()) - } - /* - "buoy_circle" => { - bail!("TODO"); - let _ = gate_run(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await,bottom_cam().await, - gate_target().await, - )) - .execute() - .await; - Ok(()) - } - */ - "example" => { - let _ = initial_descent(&FullActionContext::new( + "path_align" => ctwrap!(path_align_procedural( + &FullActionContext::new( control_board().await, meb().await, front_cam().await, bottom_cam().await, gate_target().await, - )) - .execute() - .await; - Ok(()) - } - "pid_test" => { - let _ = pid_test(&FullActionContext::new( - control_board().await, - meb().await, - front_cam().await, - bottom_cam().await, - gate_target().await, - )) - .execute() - .await; - Ok(()) - } - "octagon" => { - let _ = octagon(static_context().await).execute().await; - Ok(()) - } - "fancy_octagon" => { - let _ = fancy_octagon(static_context().await).execute().await; - Ok(()) - } - "buoy_circle" => { - let _ = buoy_circle_sequence(&FullActionContext::new( + ), + &config.missions.path_align, + &config.get_color_profile().unwrap(), + )), + "static_align" => ctwrap!(static_align_procedural( + &FullActionContext::new( control_board().await, meb().await, front_cam().await, bottom_cam().await, gate_target().await, - )) - .execute() - .await; - Ok(()) - } - "buoy_model" => { - let _ = buoy_circle_sequence_model(static_context().await) - .execute() - .await; - Ok(()) - } - "buoy_blind" => { - let _ = buoy_circle_sequence_blind(static_context().await) - .execute() - .await; - Ok(()) - } - "buoy_align" => { - let _ = buoy_align(static_context().await).execute().await; - Ok(()) - } - "spin" => { - let _ = spin(static_context().await).execute().await; - Ok(()) - } + ), + &config.missions.path_align, + )), + "example" => ctwrap!(initial_descent(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), + "pid_test" => ctwrap!(pid_test(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), + "octagon" => ctwrap!(octagon( + static_context().await, + &config.missions.octagon, + &config.get_color_profile().unwrap() + ) + .execute()), + "buoy_circle" => ctwrap!(buoy_circle_sequence(&FullActionContext::new( + control_board().await, + meb().await, + front_cam().await, + bottom_cam().await, + gate_target().await, + )) + .execute()), + "buoy_model" => ctwrap!(buoy_circle_sequence_model(static_context().await).execute()), + "buoy_blind" => ctwrap!(buoy_circle_sequence_blind(static_context().await).execute()), + "buoy_align" => ctwrap!(buoy_align(static_context().await).execute()), + "spin" => ctwrap!(spin(static_context().await).execute()), "torpedo" | "fire_torpedo" => { let _ = buoy_align_shot(static_context().await).execute().await; Ok(()) @@ -494,8 +503,10 @@ async fn run_mission(mission: &str) -> Result<()> { Ok(()) } "coinflip" => { - let _ = coinflip(static_context().await).execute().await; - Ok(()) + ctwrap!(coinflip_procedural( + static_context().await, + &config.missions.coinflip + )) } // Just stall out forever "forever" | "infinite" => loop { @@ -511,6 +522,23 @@ async fn run_mission(mission: &str) -> Result<()> { .unwrap(); Ok(()) } + "slalom_left" => ctwrap!(slalom( + static_context().await, + &config.missions.slalom, + false, + &config.get_color_profile().unwrap() + )), + "slalom_right" => ctwrap!(slalom( + static_context().await, + &config.missions.slalom, + true, + &config.get_color_profile().unwrap() + )), + "sonar" => { + let _ = sonar(static_context().await, &config.sonar, cancel).await; + Ok(()) + } + "bin" => ctwrap!(bin(static_context().await, &config.missions.bin)), x => bail!("Invalid argument: [{x}]"), }; diff --git a/src/missions/action.rs b/src/missions/action.rs index 59ca7a26a01..a4b52520ae8 100644 --- a/src/missions/action.rs +++ b/src/missions/action.rs @@ -102,17 +102,15 @@ impl Action for ActionConditional { let mut combined_str = true_str.body + &false_str.body + &condition_str.body; for tail_id in condition_str.tail_ids { - combined_str.push_str(&format!("\"{}\" [shape = diamond];\n", tail_id)); + combined_str.push_str(&format!("\"{tail_id}\" [shape = diamond];\n")); for head_id in &true_str.head_ids { combined_str.push_str(&format!( - "\"{}\" -> \"{}\" [label = \"True\"];\n", - tail_id, head_id, + "\"{tail_id}\" -> \"{head_id}\" [label = \"True\"];\n" )); } for head_id in &false_str.head_ids { combined_str.push_str(&format!( - "\"{}\" -> \"{}\" [label = \"False\"];\n", - tail_id, head_id, + "\"{tail_id}\" -> \"{head_id}\" [label = \"False\"];\n", )); } } @@ -176,25 +174,22 @@ impl Action for ActionDataConditional \"{}\" [color = purple, fontcolor = purple, label = \"True (Pass Data)\"];\n", - tail_id, head_id, + "\"{tail_id}\" -> \"{head_id}\" [color = purple, fontcolor = purple, label = \"True (Pass Data)\"];\n" )); } for head_id in &false_str.head_ids { combined_str.push_str(&format!( - "\"{}\" -> \"{}\" [label = \"False\"];\n", - tail_id, head_id, + "\"{tail_id}\" -> \"{head_id}\" [label = \"False\"];\n" )); } } for condition_head_id in &condition_str.head_ids { for head_id in &false_str.head_ids { combined_str.push_str(&format!( - "\"{}\" -> \"{}\" [color = purple, fontcolor = purple, label = \"Pass Data\"];\n", - condition_head_id, head_id, + "\"{condition_head_id}\" -> \"{head_id}\" [color = purple, fontcolor = purple, label = \"Pass Data\"];\n" )); } } @@ -300,11 +295,11 @@ impl Action for RaceAction { [&first_str.head_ids, &second_str.head_ids] .into_iter() .flatten() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", race_id, id))); + .for_each(|id| body_str.push_str(&format!("\"{race_id}\" -> \"{id}\";\n"))); [&first_str.tail_ids, &second_str.tail_ids] .into_iter() .flatten() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", id, resolve_id))); + .for_each(|id| body_str.push_str(&format!("\"{id}\" -> \"{resolve_id}\";\n"))); body_str.push_str("}\n"); DotString { @@ -366,21 +361,19 @@ impl Action for DualAction { } } else { let mut body_str = format!( - "subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = blue;\n\"{}\" [label = \"Dual\", shape = box, fontcolor = blue, style = dashed];\n", - Uuid::new_v4(), - dual_head - ) + &format!("{}\" [label = \"Collect\", shape = box, fontcolor = blue, style = dashed];\n", dual_tail) + + "subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = blue;\n\"{dual_head}\" [label = \"Dual\", shape = box, fontcolor = blue, style = dashed];\n", + Uuid::new_v4()) + &format!("{dual_tail}\" [label = \"Collect\", shape = box, fontcolor = blue, style = dashed];\n") + &first_str.body + &second_str.body; vec![first_str.head_ids, second_str.head_ids] .into_iter() .flatten() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", dual_head, id))); + .for_each(|id| body_str.push_str(&format!("\"{dual_head}\" -> \"{id}\";\n"))); vec![first_str.tail_ids, second_str.tail_ids] .into_iter() .flatten() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", id, dual_tail))); + .for_each(|id| body_str.push_str(&format!("\"{id}\" -> \"{dual_tail}\";\n"))); body_str.push_str("}\n"); DotString { @@ -434,10 +427,7 @@ impl Action for ActionChain { let mut body_str = first_str.body + &second_str.body; for tail in &first_str.tail_ids { for head in &second_str.head_ids { - body_str.push_str(&format!( - "\"{}\" -> \"{}\" [color = purple, fontcolor = purple, label = \"Pass Data\"];\n", - tail, head - )) + body_str.push_str(&format!("\"{tail}\" -> \"{head}\" [color = purple, fontcolor = purple, label = \"Pass Data\"];\n")) } } @@ -497,7 +487,7 @@ impl Action for ActionSequence { let mut body_str = first_str.body + &second_str.body; for tail in &first_str.tail_ids { for head in &second_str.head_ids { - body_str.push_str(&format!("\"{}\" -> \"{}\" {};\n", tail, head, label)) + body_str.push_str(&format!("\"{tail}\" -> \"{head}\" {label};\n")) } } @@ -578,25 +568,19 @@ impl Action for ActionParallel { color = "darkgreen"; } - let mut body_str = format!( - "subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = {};\n\"{}\" [label = \"{}\", shape = box, fontcolor = {}, style = dashed];\n", - Uuid::new_v4(), - color, - par_head, - name, - color, - ) + &format!("{}\" [label = \"Collect\", shape = box, fontcolor = {}, style = dashed];\n", par_tail, color) + - &first_str.body - + &second_str.body; + let mut body_str = format!("subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = {color};\n\"{par_head}\" [label = \"{name}\", shape = box, fontcolor = {color}, style = dashed];\n", Uuid::new_v4()) + + &format!("{par_tail}\" [label = \"Collect\", shape = box, fontcolor = {color}, style = dashed];\n") + + &first_str.body + + &second_str.body; vec![first_str.head_ids, second_str.head_ids] .into_iter() .flatten() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", par_head, id))); + .for_each(|id| body_str.push_str(&format!("\"{par_head}\" -> \"{id}\";\n"))); vec![first_str.tail_ids, second_str.tail_ids] .into_iter() .flatten() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", id, par_tail))); + .for_each(|id| body_str.push_str(&format!("\"{id}\" -> \"{par_tail}\";\n"))); body_str.push_str("}\n"); DotString { @@ -693,17 +677,15 @@ impl Action for ActionConcurrent { vec![first_str.head_ids, second_str.head_ids] .into_iter() .flatten() - .for_each(|id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", concurrent_head, id)) - }); + .for_each(|id| body_str.push_str(&format!("\"{concurrent_head}\" -> \"{id}\";\n"))); let tail_ids = if parent != "TupleSecond" { - body_str.push_str(&(format!("\"{}\" [label = \"Converge\", shape = box, fontcolor = {}, style = dashed];\n", concurrent_tail, color))); + body_str.push_str(&(format!("\"{concurrent_tail}\" [label = \"Converge\", shape = box, fontcolor = {color}, style = dashed];\n"))); vec![first_str.tail_ids, second_str.tail_ids.clone()] .into_iter() .flatten() .for_each(|id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", id, concurrent_tail)) + body_str.push_str(&format!("\"{id}\" -> \"{concurrent_tail}\";\n")) }); vec![concurrent_tail] } else { @@ -783,30 +765,21 @@ impl Action for ActionConcurrentSplit { color = "darkgreen"; } - let mut body_str = format!( - "subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = {};\n\"{}\" [label = \"{}\", shape = box, fontcolor = {}, style = dashed];\n", - Uuid::new_v4(), - color, - concurrent_head, - name, - color, - ); + let mut body_str = format!("subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = {color};\n\"{concurrent_head}\" [label = \"{name}\", shape = box, fontcolor = {color}, style = dashed];\n", Uuid::new_v4()); body_str.push_str(&(first_str.body + &second_str.body)); vec![first_str.head_ids, second_str.head_ids] .into_iter() .flatten() - .for_each(|id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", concurrent_head, id)) - }); + .for_each(|id| body_str.push_str(&format!("\"{concurrent_head}\" -> \"{id}\";\n"))); let tail_ids = if parent != "TupleSecond" { - body_str.push_str(&(format!("\"{}\" [label = \"Converge\", shape = box, fontcolor = {}, style = dashed];\n", concurrent_tail, color))); + body_str.push_str(&(format!("\"{concurrent_tail}\" [label = \"Converge\", shape = box, fontcolor = {color}, style = dashed];\n"))); vec![first_str.tail_ids, second_str.tail_ids.clone()] .into_iter() .flatten() .for_each(|id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", id, concurrent_tail)) + body_str.push_str(&format!("\"{id}\" -> \"{concurrent_tail}\";\n")) }); vec![concurrent_tail] } else { @@ -866,11 +839,10 @@ impl Action for ActionUntil { let mut body_str = action_str.body; for head in &action_str.head_ids { - body_str.push_str(&format!("\"{}\" [shape = diamond];\n", head)); + body_str.push_str(&format!("\"{head}\" [shape = diamond];\n")); for tail in &action_str.tail_ids { body_str.push_str(&format!( - "\"{}\":sw -> \"{}\":nw [label = \"Fail Within Count\"];\n", - tail, head + "\"{tail}\":sw -> \"{head}\":nw [label = \"Fail Within Count\"];\n" )) } } @@ -916,11 +888,8 @@ impl Action for ActionWhile { let mut body_str = action_str.body; for head in &action_str.head_ids { for tail in &action_str.tail_ids { - body_str.push_str(&format!("\"{}\" [shape = diamond];\n", tail)); - body_str.push_str(&format!( - "\"{}\" -> \"{}\" [label = \"True\"];\n", - tail, head - )) + body_str.push_str(&format!("\"{tail}\" [shape = diamond];\n")); + body_str.push_str(&format!("\"{tail}\" -> \"{head}\" [label = \"True\"];\n")) } } @@ -1091,30 +1060,21 @@ impl Action for ActionSelect { color = "darkgreen"; } - let mut body_str = format!( - "subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = {};\n\"{}\" [label = \"{}\", shape = box, fontcolor = {}, style = dashed];\n", - Uuid::new_v4(), - color, - concurrent_head, - name, - color, - ); + let mut body_str = format!("subgraph \"cluster_{}\" {{\nstyle = dashed;\ncolor = {color};\n\"{concurrent_head}\" [label = \"{name}\", shape = box, fontcolor = {color}, style = dashed];\n", Uuid::new_v4()); body_str.push_str(&(first_str.body + &second_str.body)); vec![first_str.head_ids, second_str.head_ids] .into_iter() .flatten() - .for_each(|id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", concurrent_head, id)) - }); + .for_each(|id| body_str.push_str(&format!("\"{concurrent_head}\" -> \"{id}\";\n"))); let tail_ids = if parent != "TupleSecond" { - body_str.push_str(&(format!("\"{}\" [label = \"Converge\", shape = box, fontcolor = {}, style = dashed];\n", concurrent_tail, color))); + body_str.push_str(&(format!("\"{concurrent_tail}\" [label = \"Converge\", shape = box, fontcolor = {color}, style = dashed];\n"))); vec![first_str.tail_ids, second_str.tail_ids.clone()] .into_iter() .flatten() .for_each(|id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", id, concurrent_tail)) + body_str.push_str(&format!("\"{id}\" -> \"{concurrent_tail}\";\n")) }); vec![concurrent_tail] } else { diff --git a/src/missions/action_context.rs b/src/missions/action_context.rs index 3498b12e19a..2af13b5fb4c 100644 --- a/src/missions/action_context.rs +++ b/src/missions/action_context.rs @@ -1,5 +1,6 @@ use core::fmt::Debug; use opencv::core::Mat; +#[cfg(feature = "annotated_streams")] use opencv::mod_prelude::ToInputArray; use tokio::io::{AsyncWriteExt, WriteHalf}; use tokio::sync::RwLock; @@ -137,7 +138,7 @@ impl FrontCamIO for EmptyActionContext { todo!() } #[cfg(feature = "annotated_streams")] - async fn annotate_front_camera(&self, image: &impl ToInputArray) { + async fn annotate_front_camera(&self, _image: &impl ToInputArray) { todo!(); } async fn get_desired_buoy_gate(&self) -> Target { @@ -153,7 +154,7 @@ impl BottomCamIO for EmptyActionContext { todo!() } #[cfg(feature = "annotated_streams")] - async fn annotate_bottom_camera(&self, image: &impl ToInputArray) { + async fn annotate_bottom_camera(&self, _image: &impl ToInputArray) { todo!(); } } diff --git a/src/missions/align_buoy.rs b/src/missions/align_buoy.rs index 82e95598cd8..8a5277b0820 100644 --- a/src/missions/align_buoy.rs +++ b/src/missions/align_buoy.rs @@ -30,7 +30,7 @@ use crate::{ use super::{ action::ActionExec, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, + action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, }; pub fn buoy_align< @@ -42,7 +42,7 @@ pub fn buoy_align< + Unpin, >( context: &'static Con, -) -> impl ActionExec<()> + '_ { +) -> impl ActionExec<()> + 'static { const Y_SPEED: f32 = 0.2; const Y_SPEED_FAST: f32 = 0.5; const DEPTH: f32 = -1.5; @@ -153,7 +153,7 @@ pub fn buoy_align_shot< + Unpin, >( context: &'static Con, -) -> impl ActionExec<()> + '_ { +) -> impl ActionExec<()> + 'static { const Y_SPEED: f32 = 0.2; const DEPTH: f32 = -0.9; const TRUE_COUNT: u32 = 2; diff --git a/src/missions/basic.rs b/src/missions/basic.rs index cb438f73638..5849347c56f 100644 --- a/src/missions/basic.rs +++ b/src/missions/basic.rs @@ -54,7 +54,7 @@ where // time in seconds that each action will wait until before continuing onto the next action. let dive_duration = 2.0; - let forward_duration = 2.0; + let forward_duration = 0.0; ActionSequence::new( WaitArm::new(context), ActionSequence::new( diff --git a/src/missions/bin.rs b/src/missions/bin.rs new file mode 100644 index 00000000000..da99b82f3d2 --- /dev/null +++ b/src/missions/bin.rs @@ -0,0 +1,38 @@ +use tokio::io::WriteHalf; +use tokio_serial::SerialStream; + +use super::action_context::{BottomCamIO, GetControlBoard, GetMainElectronicsBoard}; +use crate::{ + config::bin::Config, + missions::{action::ActionExec, vision::VisionNormBottom}, + vision::{ + bin::Bin, + nn_cv2::OnnxModel, + }, +}; + +pub async fn bin< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, +>( + context: &Con, + config: &Config, +) { + #[cfg(feature = "logging")] + logln!("Starting bin"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + let mut vision = VisionNormBottom::, f64>::new(context, Bin::default()); + + loop { + #[cfg(feature = "logging")] + logln!("DOING BIN DETECTION"); + if let Ok(detections) = vision.execute().await { + unimplemented!(); + } + } + + #[cfg(feature = "logging")] + logln!("Finished bin"); +} diff --git a/src/missions/circle_buoy.rs b/src/missions/circle_buoy.rs index 0a7c7d8a4e0..75549a9b860 100644 --- a/src/missions/circle_buoy.rs +++ b/src/missions/circle_buoy.rs @@ -21,7 +21,7 @@ use crate::{ use super::{ action::{ActionExec, ActionSequence}, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, + action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, basic::DelayAction, movement::ZeroMovement, }; @@ -99,7 +99,7 @@ pub fn buoy_circle_sequence_model< + Unpin, >( context: &'static Con, -) -> impl ActionExec<()> + '_ { +) -> impl ActionExec<()> + 'static { const BUOY_X_SPEED: f32 = -0.0; const BUOY_Y_SPEED: f32 = 0.0; const DEPTH: f32 = -1.0; @@ -148,7 +148,7 @@ pub fn buoy_circle_sequence_blind< + Unpin, >( context: &'static Con, -) -> impl ActionExec<()> + '_ { +) -> impl ActionExec<()> + 'static { const BUOY_X_SPEED: f32 = 0.4; const BUOY_Y_SPEED: f32 = 0.15; const BUOY_YAW_SPEED: f32 = 12.0; diff --git a/src/missions/coinflip.rs b/src/missions/coinflip.rs index 5fe485b4c8b..3a24c25a121 100644 --- a/src/missions/coinflip.rs +++ b/src/missions/coinflip.rs @@ -1,8 +1,11 @@ +use itertools::Itertools; use tokio::io::WriteHalf; +use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; use crate::{ act_nest, + config::coinflip::Config, missions::{ extra::AlwaysTrue, meb::WaitArm, @@ -28,6 +31,86 @@ use super::{ vision::{DetectTarget, VisionNorm}, }; +pub async fn coinflip_procedural< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, +>( + context: &Con, + config: &Config, +) { + #[cfg(feature = "logging")] + logln!("Starting path align"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + let mut vision = + VisionNorm::, f64>::new(context, GatePoles::default()); + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + let DEPTH = config.depth; + + let _ = cb + .stability_1_speed_set(0.0, 0.0, config.angle_correction, 0.0, 0.0, DEPTH) + .await; + + let mut true_count = 0; + let max_true_count = config.true_count; + let mut target_yaw = initial_yaw; + let angle_correction = config.angle_correction; + + loop { + #[allow(unused_variables)] + let detections = vision.execute().await.unwrap_or_else(|e| { + #[cfg(feature = "logging")] + logln!("Getting path detection resulted in error: `{e}`\n\tUsing empty detection vec"); + vec![] + }); + + // let gate = detections + // .iter() + // .filter(|d| matches!(d.class().identifier, Target::Gate)) + // .collect_vec(); + let shark = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::Shark)) + .collect_vec(); + let sawfish = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::Sawfish)) + .collect_vec(); + + let leftPole = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::LeftPole)) + .collect_vec(); + + let rightPole = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::RightPole)) + .collect_vec(); + + if (shark.len() > 0 || sawfish.len() > 0 || !leftPole.is_empty()) { + if true_count > max_true_count { + let _ = cb + .stability_1_speed_set(0.0, 0.0, 0.0, 0.0, 0.0, DEPTH) + .await; + break; + } else { + true_count += 1; + } + } else { + true_count = 0; + } + } +} + pub fn coinflip< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( @@ -68,10 +151,14 @@ pub fn coinflip< ), act_nest!( wrap_action(ActionConcurrent::new, FirstValid::new), - DetectTarget::, Offset2D>::new(Target::Blue), + DetectTarget::, Offset2D>::new(Target::Gate), DetectTarget::, Offset2D>::new(Target::Middle), - DetectTarget::, Offset2D>::new(Target::Red), - DetectTarget::, Offset2D>::new(Target::Pole), + DetectTarget::, Offset2D>::new(Target::LeftPole), + DetectTarget::, Offset2D>::new( + Target::RightPole + ), + DetectTarget::, Offset2D>::new(Target::Shark), + DetectTarget::, Offset2D>::new(Target::Sawfish), ), CountTrue::new(TRUE_COUNT), ), diff --git a/src/missions/example.rs b/src/missions/example.rs index 0e1f332bce0..766c7d0ee35 100644 --- a/src/missions/example.rs +++ b/src/missions/example.rs @@ -13,7 +13,7 @@ use super::{ comms::StartBno055, extra::{AlwaysTrue, OutputType, UnwrapAction}, meb::WaitArm, - movement::{Descend, Stability2Movement, Stability2Pos, ZeroMovement}, + movement::{Descend, Stability2Movement, Stability2Pos}, }; /// Example function for Action system @@ -41,8 +41,6 @@ pub fn pid_test< >( context: &Con, ) -> impl ActionExec<()> + '_ { - const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.6; act_nest!( diff --git a/src/missions/extra.rs b/src/missions/extra.rs index d855e2d6b7b..fc0ce9afc65 100644 --- a/src/missions/extra.rs +++ b/src/missions/extra.rs @@ -411,20 +411,20 @@ impl Action for InOrderFail { "In Order", ); - body_str.push_str(&(format!("\"{}\" [label = \"All Resolved\", shape = diamond, fontcolor = black, style = dashed];\n", order_tail))); + body_str.push_str(&(format!("\"{order_tail}\" [label = \"All Resolved\", shape = diamond, fontcolor = black, style = dashed];\n"))); body_str.push_str(&(first_str.body + &second_str.body)); first_str .head_ids .iter() - .for_each(|id| body_str.push_str(&format!("\"{}\" -> \"{}\";\n", order_head, id))); + .for_each(|id| body_str.push_str(&format!("\"{order_head}\" -> \"{id}\";\n"))); first_str.tail_ids.iter().for_each(|tail_id| { second_str.head_ids.iter().for_each(|head_id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", tail_id, head_id)) + body_str.push_str(&format!("\"{tail_id}\" -> \"{head_id}\";\n")) }) }); second_str.tail_ids.iter().for_each(|tail_id| { - body_str.push_str(&format!("\"{}\" -> \"{}\";\n", tail_id, order_tail)) + body_str.push_str(&format!("\"{tail_id}\" -> \"{order_tail}\";\n")) }); body_str.push_str("}\n"); diff --git a/src/missions/fancy_octagon.rs b/src/missions/fancy_octagon.rs deleted file mode 100644 index 9f350485034..00000000000 --- a/src/missions/fancy_octagon.rs +++ /dev/null @@ -1,205 +0,0 @@ -use opencv::core::Size; -use tokio::io::WriteHalf; -use tokio_serial::SerialStream; - -use crate::{ - act_nest, - missions::{ - action::{ - ActionChain, ActionConcurrent, ActionDataConditional, ActionSequence, ActionWhile, - RaceAction, TupleSecond, - }, - basic::DelayAction, - extra::{ - AlwaysBetterFalse, AlwaysBetterTrue, AlwaysTrue, CountFalse, CountTrue, OutputType, - Terminal, ToVec, - }, - movement::{ - AdjustType, ClampX, ConstYaw, LinearYawFromX, NoAdjust, OffsetToPose, SetX, - Stability2Adjust, Stability2Movement, Stability2Pos, StripY, ZeroMovement, - }, - vision::{DetectTarget, ExtractPosition, MidPoint, Norm, Vision}, - }, - vision::{ - path::{Path, Yuv}, - Offset2D, - }, - POOL_YAW_SIGN, -}; - -use super::{ - action::ActionExec, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, -}; - -pub fn octagon_path_model() -> Path { - Path::new( - (Yuv { - y: 64, - u: 127, - v: 127, - })..=(Yuv { - y: 180, - u: 224, - v: 224, - }), - 5.0..=200.0, - 4, - Size::from((400, 300)), - 3, - ) -} - -pub fn fancy_octagon< - Con: Send - + Sync - + GetControlBoard> - + GetMainElectronicsBoard - + FrontCamIO - + Unpin, ->( - context: &'static Con, -) -> impl ActionExec<()> + '_ { - const FULL_SPEED_Y: f32 = 0.7; - const FULL_SPEED_X: f32 = 0.1; - const FULL_SPEED_PITCH: f32 = -45.0 / 4.0; - const DEPTH: f32 = -0.75; - - const INIT_X: f32 = 0.0; - const INIT_Y: f32 = 0.0; - const INIT_TIME: f32 = 3.0; - - const BLIND_TIME: f32 = 3.0; - - const X_CLAMP: f32 = 0.3; - - const FALSE_COUNT: u32 = 3; - const ADJUST_COUNT: u32 = 2; - - const OCTAGON_SPIN: f32 = 80.0 * POOL_YAW_SIGN; - - const MISSION_END_TIME: f32 = INIT_TIME + BLIND_TIME + 13.0; - - const ALIGN_YAW_SPEED: f32 = 5.0 * POOL_YAW_SIGN; - - act_nest!( - ActionSequence::new, - ActionWhile::new(act_nest!( - ActionSequence::new, - act_nest!( - ActionChain::new, - NoAdjust::::new(), - ConstYaw::::new(AdjustType::Adjust(OCTAGON_SPIN)), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.0, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(1.0), - ActionChain::::new(AlwaysTrue::default(), CountTrue::new(ADJUST_COUNT)), - ),), - DelayAction::new(2.0), - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new(INIT_X, INIT_Y, 0.0, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - DelayAction::new(INIT_TIME), - act_nest!( - ActionChain::new, - Stability2Movement::new( - context, - Stability2Pos::new( - FULL_SPEED_X, - FULL_SPEED_Y, - FULL_SPEED_PITCH, - 0.0, - None, - DEPTH - ) - ), - OutputType::<()>::new(), - ), - DelayAction::new(MISSION_END_TIME), - ActionWhile::new(ActionSequence::new( - act_nest!( - ActionChain::new, - ConstYaw::::new(AdjustType::Adjust(ALIGN_YAW_SPEED)), - Stability2Movement::new( - context, - Stability2Pos::new(0.0, 0.0, FULL_SPEED_PITCH, 0.0, None, DEPTH) - ), - OutputType::<()>::new(), - ), - act_nest!( - ActionChain::new, - Vision::::new(context, octagon_path_model()), - DetectTarget::new(true), - CountTrue::new(3), - ), - ),), - ActionWhile::new(act_nest!( - ActionChain::new, - Vision::::new(context, octagon_path_model()), - ActionDataConditional::new( - DetectTarget::new(true), - ActionSequence::new( - act_nest!( - ActionChain::new, - Norm::new(Path::default()), - ExtractPosition::new(), - MidPoint::new(), - OffsetToPose::>::default(), - LinearYawFromX::::new(7.0), - ClampX::::new(X_CLAMP), - StripY::::new(), - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new( - FULL_SPEED_X, - FULL_SPEED_Y, - FULL_SPEED_PITCH, - 0.0, - None, - DEPTH - ) - ), - OutputType::<()>::new(), - ), - ), - AlwaysBetterTrue::new(), - ), - ActionSequence::new( - act_nest!( - ActionSequence::new, - Terminal::new(), - SetX::::new(AdjustType::Replace(FULL_SPEED_X)), - StripY::::new(), - ActionChain::new( - Stability2Movement::new( - context, - Stability2Pos::new( - FULL_SPEED_X, - FULL_SPEED_Y, - FULL_SPEED_PITCH, - 0.0, - None, - DEPTH - ) - ), - OutputType::<()>::new(), - ), - ), - AlwaysBetterFalse::new(), - ), - ), - CountFalse::new(FALSE_COUNT) - ),), - ZeroMovement::new(context, DEPTH), - OutputType::<()>::new() - ) -} diff --git a/src/missions/gate.rs b/src/missions/gate.rs index a765bcc5b04..403b3488852 100644 --- a/src/missions/gate.rs +++ b/src/missions/gate.rs @@ -1,9 +1,11 @@ +use itertools::Itertools; use tokio::io::WriteHalf; +use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; use crate::{ act_nest, - config::gate::Config, + config::{gate::Config, ColorProfile, Side}, missions::{ action::{ActionConcurrentSplit, ActionDataConditional}, basic::descend_depth_and_go_forward, @@ -14,6 +16,7 @@ use crate::{ vision::{MidPoint, OffsetClass}, }, vision::{ + gate_cv::GateCV, gate_poles::{GatePoles, Target}, nn_cv2::{OnnxModel, YoloClass}, Offset2D, @@ -36,6 +39,472 @@ use super::{ vision::{DetectTarget, ExtractPosition, VisionNorm, VisionNormOffset}, }; +pub async fn gate_run_dead_reckon< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, +>( + context: &Con, + config: &Config, + color_profile: &ColorProfile, +) { + #[cfg(feature = "logging")] + logln!("Starting Procedural Gate"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + // let _ = cb + // .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + // .await; + + // sleep(Duration::from_secs_f32(config.init_duration)).await; + // let mut mult = 1.0; + // if let Side::Left = config.side { + // mult = -1.0; + // } else { + // mult = 1.0; + // } + // let _ = cb + // .stability_2_speed_set( + // config.strafe_speed * mult, + // 0.0, + // 0.0, + // 0.0, + // initial_yaw, + // config.depth, + // ) + // .await; + + // sleep(Duration::from_secs_f32(config.strafe_duration)).await; + + let _ = cb + .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, initial_yaw, config.depth) + .await; + + sleep(Duration::from_secs_f32(config.traversal_duration)).await; + + let _ = cb + .stability_1_speed_set(0.0, 0.0, 0.0, 0.0, 0.0, config.depth) + .await; +} + +pub async fn gate_run_cv_procedural< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, +>( + context: &Con, + config: &Config, + color_profile: &ColorProfile, +) { + #[cfg(feature = "logging")] + logln!("Starting Procedural Gate"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + // let mut vision = VisionNorm::, f64>::new(context, GateCV::default()); + let mut vision = + VisionNorm::::new(context, GateCV::from_color_profile(color_profile)); + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; + + const TOLERANCE: f32 = 0.3; + + let mut gate_state = GateState::Align; + let mut yaw_target = 0.0; + let mut true_count = 0; + let mut false_count = 0; + + loop { + #[allow(unused_variables)] + let detections = vision.execute().await.unwrap_or_else(|e| { + #[cfg(feature = "logging")] + logln!("Getting path detection resulted in error: `{e}`\n\tUsing empty detection vec"); + vec![] + }); + + let leftPole = detections.iter().filter(|d| *d.class()).collect_vec(); + let leftPole_avg_x = leftPole + .iter() + .map(|d| *d.position().x() as f32) + .sum::(); + + let rightPole = detections.iter().filter(|d| !*d.class()).collect_vec(); + let rightPole_avg_x = rightPole + .iter() + .map(|d| *d.position().x() as f32) + .sum::(); + + match gate_state { + GateState::Align => match config.side { + Side::Left => { + if leftPole.len() > 0 { + false_count = 0; + let mut correction; + if leftPole_avg_x < 0.2 { + true_count += 1; + if true_count >= config.true_count { + #[cfg(feature = "logging")] + logln!("ALIGNED"); + if let Some(current_angle) = cb.responses().get_angles().await { + let current_yaw = *current_angle.yaw(); + yaw_target = current_yaw; + } + gate_state = GateState::Approach; + } else { + #[cfg(feature = "logging")] + logln!("true_count: {true_count}/4"); + } + } else { + correction = dbg!(config.correction_factor * leftPole_avg_x); + let _ = cb + .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth) + .await; + } + } else { + #[cfg(feature = "logging")] + logln!("SEARCHING"); + + let _ = cb + .stability_1_speed_set( + 0.0, + 0.0, + -config.yaw_speed, + 0.0, + 0.0, + config.depth, + ) + .await; + + false_count += 1; + #[cfg(feature = "logging")] + logln!("NO DETECTIONS"); + if false_count >= 100 { + #[cfg(feature = "logging")] + logln!("KILLED NO DET"); + break; + } + } + } + + Side::Right => { + if rightPole.len() > 0 { + false_count = 0; + let mut correction; + if rightPole_avg_x < 0.2 { + true_count += 1; + if true_count >= config.true_count { + #[cfg(feature = "logging")] + logln!("ALIGNED"); + if let Some(current_angle) = cb.responses().get_angles().await { + let current_yaw = *current_angle.yaw(); + yaw_target = current_yaw; + } + gate_state = GateState::Approach; + } else { + #[cfg(feature = "logging")] + logln!("true_count: {true_count}/4"); + } + } else { + correction = dbg!(config.correction_factor * rightPole_avg_x); + let _ = cb + .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth) + .await; + } + } else { + #[cfg(feature = "logging")] + logln!("SEARCHING"); + + let _ = cb + .stability_1_speed_set( + 0.0, + 0.0, + config.yaw_speed, + 0.0, + 0.0, + config.depth, + ) + .await; + + false_count += 1; + #[cfg(feature = "logging")] + logln!("NO DETECTIONS"); + if false_count >= 100 { + #[cfg(feature = "logging")] + logln!("KILLED NO DET"); + break; + } + } + } + }, + GateState::Approach => { + #[cfg(feature = "logging")] + logln!("APPROACH"); + + let strafe_direction = if let Side::Left = config.side { + -1.0 + } else { + 1.0 + }; + + let _ = cb + .stability_2_speed_set( + config.strafe_speed * strafe_direction, + 0.0, + 0.0, + 0.0, + yaw_target, + config.depth, + ) + .await; + + sleep(Duration::from_secs(config.strafe_duration as u64)).await; + + yaw_target = (yaw_target + + (if let Side::Left = config.side { + config.yaw_adjustment + } else { + -config.yaw_adjustment + })); + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, yaw_target, config.depth) + .await; + + sleep(Duration::from_secs(config.init_duration as u64)).await; + + let _ = cb + .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, yaw_target, config.depth) + .await; + + sleep(Duration::from_secs(config.traversal_duration as u64)).await; + + break; + } + } + } +} + +pub async fn gate_run_procedural< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, +>( + context: &Con, + config: &Config, +) { + #[cfg(feature = "logging")] + logln!("Starting Procedural Gate"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + let mut vision = + VisionNorm::, f64>::new(context, GatePoles::default()); + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; + + const TOLERANCE: f32 = 0.3; + + let mut true_count = 0; + + loop { + #[allow(unused_variables)] + let detections = vision.execute().await.unwrap_or_else(|e| { + #[cfg(feature = "logging")] + logln!("Getting path detection resulted in error: `{e}`\n\tUsing empty detection vec"); + vec![] + }); + + let rightPole = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::RightPole)) + .collect_vec(); + + /* let middle = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::Middle)) + .collect_vec(); */ + + let shark = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::Sawfish)) + .collect_vec(); + + let sawfish = detections + .iter() + .filter(|d| matches!(d.class().identifier, Target::Shark)) + .collect_vec(); + + let mut traversal_timer = DelayAction::new(8.0); // forward duration in second + + match config.side { + Side::Left => { + if !shark.is_empty() { + // Center on average x of blue + let avg_x = shark.iter().map(|d| *d.position().x() as f32).sum::() + / shark.len() as f32; + + #[cfg(feature = "logging")] + logln!("SHARK AVG X: {}", avg_x); + + if avg_x.abs() > TOLERANCE { + let correction = 0.4 * avg_x; + let fwd = 0.0; + + let _ = cb + .stability_2_speed_set( + correction, + fwd, + 0.0, + 0.0, + initial_yaw, + config.depth, + ) + .await; + } else { + let fwd = config.speed; + let correction = 0.05; + true_count += 1; + + if true_count >= config.true_count { + let _ = cb + .stability_2_speed_set( + correction, + fwd, + 0.0, + 0.0, + initial_yaw, + config.depth, + ) + .await; + // let _ = cb + // .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth) + // .await; + + traversal_timer.execute().await; + break; + } + } + } else { + // Fallback search behavior + #[cfg(feature = "logging")] + logln!("LEFT: Missing Features, Fallback"); + + let correction = -0.2; + let fwd = 0.05; + + let _ = cb + .stability_2_speed_set(correction, fwd, 0.0, 0.0, initial_yaw, config.depth) + .await; + // let _ = cb + // .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth) + // .await; + + DelayAction::new(1.0).execute().await; + } + } + + Side::Right => { + if !sawfish.is_empty() { + // Center on average x of blue + let avg_x = (sawfish + .iter() + .map(|d| *d.position().x() as f32) + .sum::() + / sawfish.len() as f32); + + #[cfg(feature = "logging")] + logln!("SAWFISH AVG X: {}", avg_x); + + if avg_x.abs() > TOLERANCE { + let correction = 0.4 * avg_x; + let fwd = 0.05; + + let _ = cb + .stability_2_speed_set( + correction, + fwd, + 0.0, + 0.0, + initial_yaw, + config.depth, + ) + .await; + // let _ = cb + // .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth) + // .await; + } else { + let fwd = config.speed; + let correction = 0.05; + true_count += 1; + + if true_count >= config.true_count { + let _ = cb + .stability_2_speed_set( + correction, + fwd, + 0.0, + 0.0, + initial_yaw, + config.depth, + ) + .await; + // let _ = cb + // .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth) + // .await; + + traversal_timer.execute().await; + break; + } + } + } else { + // Fallback search behavior + #[cfg(feature = "logging")] + logln!("RIGHT: Missing Features, Fallback"); + + let correction = 0.2; + let fwd = 0.05; + + let _ = cb + .stability_2_speed_set(correction, fwd, 0.0, 0.0, initial_yaw, config.depth) + .await; + // let _ = cb + // .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth) + // .await; + } + } + } + } +} + pub fn gate_run_naive< Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, >( @@ -75,8 +544,6 @@ pub fn gate_run_complex< >( context: &Con, ) -> impl ActionExec> + '_ { - const TIMEOUT: f32 = 30.0; - let depth: f32 = -1.40; act_nest!( @@ -110,8 +577,6 @@ pub fn gate_run_coinflip< context: &'a Con, config: &Config, ) -> impl ActionExec> + 'a { - const TIMEOUT: f32 = 30.0; - let depth = config.depth; act_nest!( @@ -269,3 +734,8 @@ pub fn gate_run_testing< let depth: f32 = -1.0; adjust_logic(context, depth, CountTrue::new(3)) } + +enum GateState { + Align, + Approach, +} diff --git a/src/missions/mod.rs b/src/missions/mod.rs index 1bd232562c0..e8b1096e425 100644 --- a/src/missions/mod.rs +++ b/src/missions/mod.rs @@ -2,13 +2,13 @@ pub mod action; pub mod action_context; pub mod align_buoy; pub mod basic; +pub mod bin; pub mod buoy_hit; pub mod circle_buoy; pub mod coinflip; pub mod comms; pub mod example; pub mod extra; -pub mod fancy_octagon; pub mod fire_torpedo; pub mod gate; pub mod graph; @@ -17,5 +17,7 @@ pub mod movement; pub mod octagon; pub mod path_align; pub mod reset_torpedo; +pub mod slalom; +pub mod sonar; pub mod spin; pub mod vision; diff --git a/src/missions/movement.rs b/src/missions/movement.rs index e30485062ae..10c143de5b3 100644 --- a/src/missions/movement.rs +++ b/src/missions/movement.rs @@ -1,7 +1,6 @@ use crate::comms::control_board::ControlBoard; use crate::comms::control_board::LAST_YAW; use crate::logln; -use crate::vision::Angle2D; use crate::vision::DrawRect2d; use crate::vision::Offset2D; use crate::vision::RelPos; @@ -2496,7 +2495,7 @@ impl NoAdjust { } impl ActionMod for NoAdjust { - fn modify(&mut self, input: &T) {} + fn modify(&mut self, _input: &T) {} } impl ActionExec for NoAdjust { diff --git a/src/missions/octagon.rs b/src/missions/octagon.rs index 3409a7fee93..047eb5a3000 100644 --- a/src/missions/octagon.rs +++ b/src/missions/octagon.rs @@ -1,9 +1,9 @@ -use opencv::core::Size; use tokio::io::WriteHalf; use tokio_serial::SerialStream; use crate::{ act_nest, + config::{octagon::Config, ColorProfile}, missions::{ action::{ ActionChain, ActionConcurrent, ActionDataConditional, ActionSequence, ActionWhile, @@ -20,13 +20,13 @@ use crate::{ }, vision::{DetectTarget, ExtractPosition, MidPoint, Norm, Vision}, }, - vision::{octagon::Octagon, path::Yuv, Offset2D}, + vision::{octagon::Octagon, Offset2D}, POOL_YAW_SIGN, }; use super::{ action::ActionExec, - action_context::{GetControlBoard, FrontCamIO, GetMainElectronicsBoard}, + action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}, }; pub fn octagon_path_model() -> Octagon { @@ -42,7 +42,9 @@ pub fn octagon< + Unpin, >( context: &'static Con, -) -> impl ActionExec<()> + '_ { + config: &Config, + color_profile: &ColorProfile, +) -> impl ActionExec<()> + 'static { const FULL_SPEED_Y: f32 = 0.7; const FULL_SPEED_X: f32 = 0.0; const FULL_SPEED_PITCH: f32 = -45.0 / 4.0; diff --git a/src/missions/path_align.rs b/src/missions/path_align.rs index 0527fea25a9..b520f47d121 100644 --- a/src/missions/path_align.rs +++ b/src/missions/path_align.rs @@ -3,7 +3,8 @@ use tokio::time::{sleep, Duration}; use tokio_serial::SerialStream; use crate::config::path_align::Config; -use crate::{act_nest, missions::vision::VisionNormBottomAngle, vision::path_cv::PathCV}; +use crate::config::ColorProfile; +use crate::{missions::vision::VisionNormBottomAngle, vision::path_cv::PathCV}; use super::{ action::ActionExec, @@ -15,26 +16,45 @@ pub async fn path_align_procedural< >( context: &Con, config: &Config, + color_profile: &ColorProfile, ) { #[cfg(feature = "logging")] logln!("Starting path align"); let cb = context.get_control_board(); - cb.bno055_periodic_read(true).await; - let mut vision_norm_bottom = - VisionNormBottomAngle::::new(context, PathCV::default()); + let _ = cb.bno055_periodic_read(true).await; + let mut vision_norm_bottom = VisionNormBottomAngle::::new( + context, + PathCV::from_color_profile(color_profile), + ); let initial_yaw = loop { if let Some(initial_angle) = cb.responses().get_angles().await { - break *initial_angle.yaw() as f32; + break *initial_angle.yaw(); } else { #[cfg(feature = "logging")] logln!("Failed to get initial angle"); } }; + // let _ = cb + // .stability_1_speed_set(config.speed, 0.1, 0.0, 0.0, 0.0, config.depth) + // .await; let _ = cb - .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, initial_yaw, config.depth) + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; + + sleep(Duration::from_secs(3)).await; + + let _ = cb + .stability_2_speed_set( + config.speed, + config.forward_speed, + 0.0, + 0.0, + initial_yaw, + config.depth, + ) .await; let mut last_set_yaw = initial_yaw; @@ -52,9 +72,10 @@ pub async fn path_align_procedural< } if let Some(current_angle) = cb.responses().get_angles().await { - let current_yaw = *current_angle.yaw() as f32; + let current_yaw = *current_angle.yaw(); // For the opencv impl of path detection, the returned vector is guaranteed to contain 1 item + #[allow(unused_variables)] let detections = vision_norm_bottom.execute().await.unwrap_or_else(|e| { #[cfg(feature = "logging")] logln!( @@ -73,7 +94,7 @@ pub async fn path_align_procedural< if let Some(position) = positions.next() { x = *position.x() as f32; - y = (*position.y() as f32) * -1.0; + y = -(*position.y() as f32); yaw = current_yaw + (*position.angle() * -1.0) as f32; last_set_yaw = yaw; @@ -83,6 +104,7 @@ pub async fn path_align_procedural< continue; } + #[allow(unused_variables)] if let Err(e) = cb .stability_2_speed_set(x, y, 0.0, 0.0, last_set_yaw, config.depth) .await @@ -98,7 +120,55 @@ pub async fn path_align_procedural< #[cfg(feature = "logging")] logln!("Positive detection count: {consec_detections}"); } - cb.stability_2_speed_set(0.0, 1.0, 0.0, 0.0, last_set_yaw, config.depth) + let _ = cb + .stability_2_speed_set(0.0, 1.0, 0.0, 0.0, last_set_yaw, config.depth) .await; sleep(Duration::from_secs(1)).await; } + +pub async fn static_align_procedural< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + BottomCamIO, +>( + context: &Con, + config: &Config, +) { + #[cfg(feature = "logging")] + logln!("Starting static align"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + let target_yaw = initial_yaw + config.yaw_angle; + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; + + sleep(Duration::from_secs(config.yaw_wait)).await; + + let _ = cb + .stability_2_speed_set( + config.strafe_speed, + config.forward_speed, + 0.0, + 0.0, + target_yaw, + config.depth, + ) + .await; + + sleep(Duration::from_secs(config.forward_duration)).await; + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, target_yaw, config.depth) + .await; +} diff --git a/src/missions/slalom.rs b/src/missions/slalom.rs new file mode 100644 index 00000000000..220732691e9 --- /dev/null +++ b/src/missions/slalom.rs @@ -0,0 +1,214 @@ +use hdbscan::{Center, Hdbscan}; +use itertools::Itertools; +use std::f64::consts::PI; + +use tokio::{ + io::WriteHalf, + select, + time::{sleep, Duration}, +}; +use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; +use tokio_util::sync::CancellationToken; + +use bluerobotics_ping::{ + device::{Ping360, PingDevice}, + ping360::AutoDeviceDataStruct, +}; + +use super::action_context::{FrontCamIO, GetControlBoard, GetMainElectronicsBoard}; +use crate::{ + config::{slalom::Config, sonar::Config as SonarConfig, ColorProfile, Side::*}, + missions::{ + action::ActionExec, + basic::DelayAction, + vision::{VisionNorm, VisionNormAngle}, + }, +}; + +// TODO: Consider filtering detections by angle (poles will always be upright) +pub async fn slalom< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard + FrontCamIO, +>( + context: &Con, + config: &Config, + flip: bool, + color_profile: &ColorProfile, +) { + use crate::vision::slalom::Slalom; + #[cfg(feature = "logging")] + logln!("Starting slalom"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + let mut vision = VisionNormAngle::::new( + context, + Slalom::from_color_profile(color_profile, config.area_bounds.clone()), + ); + + let initial_yaw = loop { + if let Some(initial_angle) = cb.responses().get_angles().await { + break *initial_angle.yaw(); + } else { + #[cfg(feature = "logging")] + logln!("Failed to get initial angle"); + } + }; + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth) + .await; + + sleep(Duration::from_secs(3)).await; + + let mut yaw_target = 0.0; + let mut true_count = 0; + let mut false_count = 0; + let mut init_timer = DelayAction::new(config.init_duration); + let mut traversal_timer = DelayAction::new(config.traversal_duration); // forward duration in second + let mut strafe_timer = DelayAction::new(config.strafe_duration); + + enum SlalomState { + Align, + Approach, + } + + let mut slalom_state = SlalomState::Align; + + // let _ = cb + // .stability_2_speed_set(0.05, config.speed, 0.0, 0.0, initial_yaw, config.depth) + // .await; + // init_timer.execute().await; + + #[cfg(feature = "logging")] + logln!("Starting slalom detection"); + + // Default left, right if flipped + 'detections: loop { + #[allow(unused_variables)] + let detections = vision.execute().await.unwrap_or_else(|e| { + #[cfg(feature = "logging")] + logln!( + "Getting slalom detection resulted in error: `{e}`\n\tUsing empty detection vec" + ); + vec![] + }); + + let mut positions = detections + .into_iter() + .filter_map(|d| d.class().then_some(d.position().clone())); + + match slalom_state { + SlalomState::Align => { + #[cfg(feature = "logging")] + logln!("ALIGN"); + + if let Some(position) = positions.next() { + false_count = 0; + let x = *position.x() as f32; + dbg!(&x); + let mut correction = 0.0; + if x.abs() < 0.2 { + true_count += 1; + if true_count >= 4 { + correction = 0.0; + #[cfg(feature = "logging")] + logln!("ALIGNED"); + slalom_state = SlalomState::Approach; + if let Some(current_angle) = cb.responses().get_angles().await { + let current_yaw = *current_angle.yaw(); + yaw_target = current_yaw; + } + } else { + #[cfg(feature = "logging")] + logln!("true_count: {true_count}/4"); + } + } else { + correction = dbg!(config.correction_factor * x); + let _ = cb + .stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth) + .await; + } + } else { + #[cfg(feature = "logging")] + logln!("SEARCHING"); + + let _ = cb + .stability_1_speed_set( + 0.0, + 0.0, + if flip { + config.yaw_speed + } else { + -config.yaw_speed + }, + 0.0, + 0.0, + config.depth, + ) + .await; + + false_count += 1; + #[cfg(feature = "logging")] + logln!("NO DETECTIONS"); + if false_count >= 500 { + #[cfg(feature = "logging")] + logln!("KILLED NO DET"); + break 'detections; + } + } + } + + SlalomState::Approach => { + #[cfg(feature = "logging")] + logln!("APPROACH"); + + let strafe_direction = if let Left = config.side { -1.0 } else { 1.0 }; + + let _ = cb + .stability_2_speed_set( + config.speed * strafe_direction, + 0.0, + 0.0, + 0.0, + yaw_target, + config.depth, + ) + .await; + + sleep(Duration::from_secs(config.strafe_duration as u64)).await; + + yaw_target = (yaw_target + + (if let Left = config.side { + config.yaw_adjustment + } else { + -config.yaw_adjustment + })); + + let _ = cb + .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, yaw_target, config.depth) + .await; + + sleep(Duration::from_secs(config.init_duration as u64)).await; + + // init_timer.execute().await; + + // let _ = cb + // .stability_1_speed_set(0.0, config.speed, 0.0, 0.0, 0.0, config.depth) + // .await; + + // + let _ = cb + .stability_2_speed_set(0.0, config.speed, 0.0, 0.0, yaw_target, config.depth) + .await; + + // traversal_timer.execute().await; + sleep(Duration::from_secs(config.traversal_duration as u64)).await; + + break 'detections; + } + } + + // The current implementation is guaranteed to return exactly 1 item + } +} diff --git a/src/missions/sonar.rs b/src/missions/sonar.rs new file mode 100644 index 00000000000..75468c7bc6d --- /dev/null +++ b/src/missions/sonar.rs @@ -0,0 +1,173 @@ +use bluerobotics_ping::{ + common::{DeviceInformationStruct, ProtocolVersionStruct}, + device::{Ping360, PingDevice}, + ping360::AutoDeviceDataStruct, +}; +use serde::{Deserialize, Serialize}; +use std::{ + fs::{self, OpenOptions}, + io::BufWriter, + path::PathBuf, + time::SystemTime, +}; +use tokio::{io::WriteHalf, select}; +use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; +use tokio_util::sync::CancellationToken; + +use super::action_context::{GetControlBoard, GetMainElectronicsBoard}; +use crate::config::sonar::Config; + +pub async fn sonar< + Con: Send + Sync + GetControlBoard> + GetMainElectronicsBoard, +>( + context: &Con, + cfg: &Config, + cancel: CancellationToken, +) { + #[cfg(feature = "logging")] + logln!("Starting sonar"); + + let cb = context.get_control_board(); + let _ = cb.bno055_periodic_read(true).await; + + #[cfg(feature = "logging")] + logln!("Initializing sonar with: {:?}", cfg.serial_port); + let port = loop { + match tokio_serial::new(cfg.serial_port.to_string_lossy(), cfg.serial_baud_rate) + .open_native_async() + { + Ok(port) => break port, + #[allow(unused_variables)] + Err(e) => { + #[cfg(feature = "logging")] + logln!("Error opening serial port: {}", e); + } + } + }; + + #[allow(unused_variables)] + port.clear(tokio_serial::ClearBuffer::All) + .unwrap_or_else(|e| { + #[cfg(feature = "logging")] + logln!("Failed to clear sonar serial port: {}", e); + }); + + let ping360 = Ping360::new(port); + + // #[cfg(feature = "logging")] + // logln!("Reseting sonar unit"); + // loop { + // if let Err(e) = ping360.reset(cfg.bootloader as u8, 0).await { + // #[cfg(feature = "logging")] + // logln!("Failed to reset sonar unit: {e:#?}"); + // } else { + // break; + // } + // } + + #[cfg(feature = "logging")] + logln!("Reseting MOTOR sonar unit"); + #[allow(unused_variables)] + while let Err(e) = ping360.motor_off().await { + #[cfg(feature = "logging")] + logln!("Failed to reset sonar unit: {e:#?}"); + } + + let (protocol_version, device_information) = + tokio::try_join!(ping360.protocol_version(), ping360.device_information()) + .expect("Failed to get device data!"); + + // let _ = cb + // .stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, -1.25) + // .await; + + #[cfg(feature = "logging")] + logln!("Opening log file"); + let time = SystemTime::now() + .duration_since(SystemTime::UNIX_EPOCH) + .unwrap() + .as_secs(); + let directory = "./logging/sonar/"; + let filename = format!("{time}.log"); + let path = PathBuf::from(directory).join(filename); + let mut open_options = OpenOptions::new(); + open_options.append(true).create(true); + + let file = open_options + .open(path.as_path()) + .unwrap_or_else(|e| match path.parent() { + Some(parent) => { + fs::create_dir_all(parent) + .map_err(|e| format!("Failed to create log file parent directory: {e}")) + .unwrap(); + open_options + .open(path) + .map_err(|e| format!("Failed to open log file: {e}")) + .unwrap() + } + None => { + panic!("Failed to open log file: {e}"); + } + }); + + let file = BufWriter::new(file); + + #[cfg(feature = "logging")] + logln!("Starting sonar auto transmit"); + let at = cfg.auto_transmit; + #[allow(unused_variables)] + while let Err(e) = ping360 + .auto_transmit( + at.mode, + at.gain_setting as u8, + at.transmit_duration, + at.sample_period, + at.transmit_frequency, + at.number_of_samples, + at.start_angle, + at.stop_angle, + at.num_steps, + at.delay, + ) + .await + { + #[cfg(feature = "logging")] + logln!("Failed to start sonar auto transmit: {e:#?}"); + } + + let mut data: Vec = Vec::new(); + + #[cfg(feature = "logging")] + logln!("Recording data"); + loop { + select! { + _ = cancel.cancelled() => { break; }, + r = ping360.auto_device_data() => { + if let Ok(d) = r { + let angle_clone = d.angle; + data.push(d); + #[cfg(feature = "logging")] + logln!("Got data {}", angle_clone); + if angle_clone >= at.stop_angle { + break; + } + } + } + } + } + + let log = SonarLogFile { + protocol_version, + device_information, + data, + }; + + serde_json::to_writer_pretty(file, &log).expect("Failed to write sonar log file"); +} + +#[derive(Serialize, Deserialize)] +struct SonarLogFile { + protocol_version: ProtocolVersionStruct, + device_information: DeviceInformationStruct, + data: Vec, +} diff --git a/src/missions/spin.rs b/src/missions/spin.rs index 7932b1ee71a..cd69c477005 100644 --- a/src/missions/spin.rs +++ b/src/missions/spin.rs @@ -25,9 +25,9 @@ pub fn spin< context: &Con, ) -> impl ActionExec<()> + '_ { const GATE_DEPTH: f32 = -1.75; - const DEPTH: f32 = -1.75; + const DEPTH: f32 = -1.15; const Z_TARGET: f32 = 0.0; - const FORWARD_SPEED: f32 = 1.0; + const FORWARD_SPEED: f32 = 0.0; const SPIN_SPEED: f32 = 1.0; act_nest!( @@ -39,7 +39,7 @@ pub fn spin< ), OutputType::<()>::new(), ), - DelayAction::new(5.0), + DelayAction::new(2.0), ActionWhile::new(TupleSecond::new(ActionConcurrent::new( act_nest!( ActionSequence::new, @@ -55,6 +55,7 @@ pub fn spin< SpinCounter::new(4, context) ))), ZeroMovement::new(context, DEPTH), + DelayAction::new(3.0), OutputType::<()>::new(), ) } diff --git a/src/missions/vision.rs b/src/missions/vision.rs index eb146ed3fd8..54b3eeaabed 100644 --- a/src/missions/vision.rs +++ b/src/missions/vision.rs @@ -8,7 +8,6 @@ use super::action::{Action, ActionExec, ActionMod}; use super::action_context::BottomCamIO; use super::graph::DotString; use crate::logln; -use crate::vision::nn_cv2::VisionModel; use crate::vision::{ Angle2D, Draw, DrawRect2d, Offset2D, RelPos, RelPosAngle, VisualDetection, VisualDetector, }; @@ -268,6 +267,84 @@ where } } +/// Runs a vision routine to obtain object positions +/// +/// The relative positions are normalized to [-1, 1] on both axes. +/// The values are returned with an angle. +#[derive(Debug)] +pub struct VisionNormAngle<'a, T, U, V> { + context: &'a T, + model: U, + _num: PhantomData, +} + +impl<'a, T, U, V> VisionNormAngle<'a, T, U, V> { + pub const fn new(context: &'a T, model: U) -> Self { + Self { + context, + model, + _num: PhantomData, + } + } +} + +impl Action for VisionNormAngle<'_, T, U, V> {} + +impl< + T: FrontCamIO + Send + Sync, + V: Num + Float + FromPrimitive + Send + Sync, + U: VisualDetector + Send + Sync, + > ActionExec>>>> + for VisionNormAngle<'_, T, U, V> +where + U::Position: RelPosAngle + Debug + for<'a> Mul<&'a Mat, Output = U::Position>, + VisualDetection: Draw, + U::ClassEnum: Send + Sync + Debug, +{ + async fn execute(&mut self) -> Result>>> { + #[cfg(feature = "logging")] + { + logln!("Running detection..."); + } + + #[allow(unused_mut)] + let mut mat = self.context.get_front_camera_mat().await.clone(); + let detections = self.model.detect(&mat); + #[cfg(feature = "logging")] + logln!("Detect attempt: {:#?}", detections); + let detections = detections?; + #[cfg(feature = "logging")] + { + detections.iter().for_each(|x| { + let x = VisualDetection::new( + x.class().clone(), + self.model.normalize(x.position()) * &mat, + ); + x.draw(&mut mat).unwrap() + }); + create_dir_all("/tmp/detect").unwrap(); + imwrite( + &("/tmp/detect/".to_string() + &Uuid::new_v4().to_string() + ".jpeg"), + &mat, + &Vector::default(), + ) + .unwrap(); + #[cfg(feature = "annotated_streams")] + self.context.annotate_front_camera(&mat).await; + } + + Ok(detections + .into_iter() + .map(|detect| { + VisualDetection::new( + detect.class().clone(), + self.model.normalize(detect.position()).offset_angle(), + ) + }) + .collect()) + } +} + /// Runs a vision routine to obtain object positions /// /// The relative positions are normalized to [-1, 1] on both axes. diff --git a/src/video_source/appsink.rs b/src/video_source/appsink.rs index 50005f3936a..85b28d9bddb 100644 --- a/src/video_source/appsink.rs +++ b/src/video_source/appsink.rs @@ -1,21 +1,28 @@ use anyhow::{anyhow, Result}; -use opencv::core::Size; -use opencv::mod_prelude::ToInputArray; -use opencv::prelude::Mat; -use opencv::videoio::VideoCaptureAPIs; -use opencv::videoio::{ - VideoCapture, VideoWriter, CAP_GSTREAMER, CAP_PROP_FPS, CAP_PROP_FRAME_HEIGHT, - CAP_PROP_FRAME_WIDTH, +use opencv::{ + prelude::Mat, + videoio::{VideoCapture, VideoCaptureAPIs, VideoCaptureTrait}, }; -use opencv::videoio::{VideoCaptureTrait, VideoCaptureTraitConst, VideoWriterTrait}; -use std::fs::create_dir_all; -use std::path::Path; -use std::sync; -use std::sync::Arc; -use std::thread::spawn; +use std::{fs::create_dir_all, path::Path, sync::Arc, thread::spawn}; use tokio::sync::Mutex; -use crate::logln; +#[cfg(feature = "logging")] +use { + crate::logln, + opencv::videoio::{VideoCaptureTraitConst, CAP_GSTREAMER}, +}; +#[cfg(feature = "annotated_streams")] +use { + opencv::{ + core::Size, + mod_prelude::ToInputArray, + videoio::{ + VideoWriter, VideoWriterTrait, CAP_PROP_FPS, CAP_PROP_FRAME_HEIGHT, + CAP_PROP_FRAME_WIDTH, + }, + }, + std::sync, +}; use super::MatSource; @@ -41,10 +48,11 @@ impl Camera { let rtsp_string = "h264. ! queue ! h264parse config_interval=-1 ! video/x-h264,stream-format=byte-stream,alignment=au ! rtspclientsink location=rtsp://127.0.0.1:8554/".to_string() + camera_name + ".mp4 "; + // unsharp luma-radius-2.0 luma-amount=2.5 chroma-radius=2.0 chroma-amount=2.5 ! let capture_string = pipeline_head(camera_path, camera_dimensions.0, camera_dimensions.1, 30) + " ! jpegdec ! tee name=raw " - + "raw. ! queue ! videoconvert ! appsink " + + "raw. ! queue ! videoconvert ! videobalance brightness=0.0 ! appsink " + "raw. ! queue ! videoconvert ! " + &h264_enc_pipeline(2048000) + " ! tee name=h264 " @@ -58,11 +66,33 @@ impl Camera { + ".mp4\" "; #[cfg(feature = "annotated_streams")] + let rtsp_string = "h264. ! queue ! h264parse config_interval=-1 ! video/x-h264,stream-format=byte-stream,alignment=au ! rtspclientsink location=rtsp://127.0.0.1:8554/".to_string() + + camera_name + "_annotated.mp4 "; + #[cfg(feature = "annotated_streams")] let output_string = "appsrc ! videoconvert ! ".to_string() + &h264_enc_pipeline(2048000) - + " ! mpegtsmux ! rtspclientsink location=rtspt://127.0.0.1:8554/" + // + " ! h264parse config_interval=-1 ! video/x-h264,stream-format=byte-stream,alignment=au" + + " ! h264parse config_interval=-1 ! video/x-h264,stream-format=byte-stream,alignment=au !" + + " rtspclientsink location=rtsp://127.0.0.1:8554/" + camera_name + "_annotated.mp4 "; + #[cfg(feature = "annotated_streams")] + dbg!(&output_string); + // pipeline_head(camera_path, camera_dimensions.0, camera_dimensions.1, 30) + // "appsrc ! image/jpeg, width=480, height=640, framerate=30/1".to_string() + // + " ! jpegdec ! tee name=raw " + // + "raw. ! queue ! videoconvert ! videobalance brightness=0.0 ! appsink " + // + "raw. ! queue ! videoconvert ! " + // + &h264_enc_pipeline(2048000) + // + " ! tee name=h264 " + // + if rtsp { &rtsp_string } else { "" } + // + "h264. ! queue ! mpegtsmux ! filesink location=\"" + // + filesink + // .to_str() + // .ok_or(anyhow!("filesink_dir is not a string"))? + // + "/" + // + camera_name + // + "_annotated.mp4\" "; let frame: Arc>> = Arc::default(); let frame_copy = frame.clone(); @@ -121,10 +151,11 @@ impl Camera { Camera::new(camera_path, camera_name, filesink_dir, (640, 480), true) } + #[cfg(feature = "annotated_streams")] pub fn push_annotated_frame(&self, image: &impl ToInputArray) { let writer = self.output.clone(); let mut writer = writer.lock().unwrap(); - writer.write(image); + let _ = writer.write(image); } } diff --git a/src/vision/bin.rs b/src/vision/bin.rs new file mode 100644 index 00000000000..44b71cb9a77 --- /dev/null +++ b/src/vision/bin.rs @@ -0,0 +1,95 @@ +use anyhow::Result; +use derive_getters::Getters; +use opencv::{core::Size, prelude::Mat}; + +use crate::load_onnx; + +use super::{ + nn_cv2::{OnnxModel, VisionModel, YoloClass, YoloDetection}, + yolo_model::YoloProcessor, +}; + +use core::hash::Hash; +use std::{error::Error, fmt::Display}; + +#[derive(Debug, PartialEq, Eq, Hash, Clone)] +pub enum Target { + Bin, + ReefShark, + SawFish, +} + +impl From> for Target { + fn from(value: YoloClass) -> Self { + value.identifier + } +} + +#[derive(Debug)] +pub struct TargetError { + x: i32, +} + +impl Display for TargetError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{} is outside known classIDs [0, 1]", self.x) + } +} + +impl Error for TargetError {} + +impl TryFrom for Target { + type Error = TargetError; + fn try_from(value: i32) -> std::result::Result { + match value { + 0 => Ok(Self::Bin), + 1 => Ok(Self::ReefShark), + 2 => Ok(Self::SawFish), + x => Err(TargetError { x }), + } + } +} + +impl Display for Target { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{self:?}") + } +} + +#[derive(Debug, Clone, Getters)] +pub struct Bin { + model: T, + threshold: f64, +} + +impl Bin { + pub fn new(model_name: &str, model_size: i32, threshold: f64) -> Result { + let model = OnnxModel::from_file(model_name, model_size, 2)?; + + Ok(Self { model, threshold }) + } + + pub fn load_640(threshold: f64) -> Self { + let model = load_onnx!("models/bin_640.onnx", 640, 3); + + Self { model, threshold } + } +} + +impl Default for Bin { + fn default() -> Self { + Self::load_640(0.60) + } +} + +impl YoloProcessor for Bin { + type Target = Target; + + fn detect_yolo_v5(&mut self, image: &Mat) -> Vec { + self.model.detect_yolo_v5(image, self.threshold) + } + + fn model_size(&self) -> Size { + self.model.size() + } +} diff --git a/src/vision/buoy.rs b/src/vision/buoy.rs index 37326cc1c24..e146eb306cd 100644 --- a/src/vision/buoy.rs +++ b/src/vision/buoy.rs @@ -58,7 +58,7 @@ impl Target { impl Display for Target { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{:?}", self) + write!(f, "{self:?}") } } diff --git a/src/vision/buoy_model.rs b/src/vision/buoy_model.rs index d1f746422a5..f994e006911 100644 --- a/src/vision/buoy_model.rs +++ b/src/vision/buoy_model.rs @@ -48,7 +48,7 @@ impl TryFrom for Target { impl Display for Target { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{:?}", self) + write!(f, "{self:?}") } } diff --git a/src/vision/gate.rs b/src/vision/gate.rs index 2f49e16f04a..dd22fca5a05 100644 --- a/src/vision/gate.rs +++ b/src/vision/gate.rs @@ -45,7 +45,7 @@ impl TryFrom for Target { impl Display for Target { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{:?}", self) + write!(f, "{self:?}") } } diff --git a/src/vision/gate_cv.rs b/src/vision/gate_cv.rs new file mode 100644 index 00000000000..31ed93df615 --- /dev/null +++ b/src/vision/gate_cv.rs @@ -0,0 +1,189 @@ +use crate::config::{ColorProfile, Side}; + +use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv}; +use opencv::{ + core::{in_range, Point, Scalar, Size, Vector}, + imgproc::{ + box_points, contour_area_def, cvt_color_def, find_contours_def, min_area_rect, + CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, RETR_EXTERNAL, + }, + prelude::{Mat, MatTraitConst, MatTraitConstManual}, +}; +use std::ops::RangeInclusive; + +#[derive(Debug)] +pub struct GateCV { + color_bounds_red: RangeInclusive, + color_bounds_black: RangeInclusive, + size: Size, + image: MatWrapper, +} + +impl GateCV { + pub fn new( + color_bounds_red: RangeInclusive, + color_bounds_black: RangeInclusive, + size: Size, + ) -> Self { + Self { + color_bounds_red, + color_bounds_black, + size, + image: Mat::default().into(), + } + } + + pub fn from_color_profile(color_profile: &ColorProfile) -> Self { + Self::new( + color_profile.red.clone(), + color_profile.black.clone(), + Size::from((400, 300)), + ) + } +} + +// TODO: Change these to match slalom, not path +impl Default for GateCV { + fn default() -> Self { + Self::new( + (Yuv { + y: 20, + u: 100, + v: 160, + })..=(Yuv { + y: 220, + u: 135, + v: 255, + }), + (Yuv { + y: 20, + u: 100, + v: 160, + })..=(Yuv { + y: 220, + u: 135, + v: 255, + }), + Size::from((400, 300)), + ) + } +} + +impl VisualDetector for GateCV { + type ClassEnum = bool; + type Position = PosVector; + + fn detect( + &mut self, + input_image: &Mat, + ) -> anyhow::Result>> { + self.image = resize(input_image, &self.size)?.into(); + let mut yuv_image = Mat::default(); + + cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?; + + let red_start = self.color_bounds_red.start(); + let red_end = self.color_bounds_red.end(); + let black_start = self.color_bounds_black.start(); + let black_end = self.color_bounds_black.end(); + let lower_red = Scalar::new( + red_start.y as f64, + red_start.u as f64, + red_start.v as f64, + 0., + ); + let upper_red = Scalar::new(red_end.y as f64, red_end.u as f64, red_end.v as f64, 0.); + + let lower_black = Scalar::new( + black_start.y as f64, + black_start.u as f64, + black_start.v as f64, + 0., + ); + let upper_black = Scalar::new( + black_end.y as f64, + black_end.u as f64, + black_end.v as f64, + 0., + ); + + let mut red_mask = Mat::default(); + let _ = in_range(&yuv_image, &lower_red, &upper_red, &mut red_mask); + + let mut black_mask = Mat::default(); + let _ = in_range(&yuv_image, &lower_black, &upper_black, &mut black_mask); + + let mut contours_red = Vector::>::new(); + find_contours_def( + &red_mask, + &mut contours_red, + RETR_EXTERNAL, + CHAIN_APPROX_SIMPLE, + )?; + let mut contours_black = Vector::>::new(); + find_contours_def( + &black_mask, + &mut contours_black, + RETR_EXTERNAL, + CHAIN_APPROX_SIMPLE, + )?; + + let max_contour_red = contours_red.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); + let max_contour_black = contours_black.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); + + if let Some(contour_red) = max_contour_red { + if let Some(contour_black) = max_contour_black { + let red_rect = min_area_rect(&contour_red).unwrap(); + let black_rect = min_area_rect(&contour_black).unwrap(); + let center_red = red_rect.center; + let center_black = black_rect.center; + + let red_angle = red_rect.angle; + let black_angle = black_rect.angle; + if (red_angle - black_angle).abs() > 20.0 { + let red_x = center_red.x; + let black_x = center_black.x; + let red_y = center_red.y; + let black_y = center_black.y; + if (red_x - black_x).abs() < 50.0 { + let side; + if black_y > red_y { + // Right + side = false; + } else { + // Left + side = true; + } + let pole_x = (red_x + black_x) / 2.0; + let pole_y = (red_y + black_y) / 2.0; + return Ok(vec![VisualDetection { + class: side, + position: PosVector::new(pole_x as f64, pole_y as f64, 0.0, 0.0), + }]); + } + } + } + } + Ok(vec![]) + } + + fn normalize(&mut self, pos: &Self::Position) -> Self::Position { + let img_size = self.image.size().unwrap(); + Self::Position::new( + ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, + ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + 0., + *pos.angle(), + ) + } +} diff --git a/src/vision/gate_poles.rs b/src/vision/gate_poles.rs index a839f2e2b67..1b9774dd8b1 100644 --- a/src/vision/gate_poles.rs +++ b/src/vision/gate_poles.rs @@ -1,6 +1,18 @@ use anyhow::Result; use derive_getters::Getters; -use opencv::{core::Size, prelude::Mat}; +use itertools::MergeJoinBy; +use opencv::core::{multiply, multiply_def, MatTraitManual, BORDER_CONSTANT, CV_8U}; +use opencv::imgproc::{ + dilate_def, get_structuring_element, morphology_default_border_value, MORPH_RECT, +}; +use opencv::{ + core::{in_range, merge, split, Point, Scalar, Size, Vector}, + imgproc::{ + self, contour_area_def, cvt_color_def, dilate, find_contours_def, min_area_rect, + CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, LINE_8, RETR_EXTERNAL, + }, + prelude::{Mat, MatTraitConst, MatTraitConstManual}, +}; use crate::load_onnx; @@ -15,6 +27,10 @@ use std::{error::Error, fmt::Display}; #[derive(Debug, PartialEq, Eq, Hash, Clone)] pub enum Target { Red, + LeftPole, + RightPole, + Shark, + Sawfish, Pole, Blue, Gate, @@ -44,11 +60,19 @@ impl TryFrom for Target { type Error = TargetError; fn try_from(value: i32) -> std::result::Result { match value { - 0 => Ok(Self::Red), - 1 => Ok(Self::Pole), - 2 => Ok(Self::Blue), - 3 => Ok(Self::Gate), - 4 => Ok(Self::Middle), + // 0 => Ok(Self::Red), + // 1 => Ok(Self::Pole), + // 2 => Ok(Self::Blue), + // 3 => Ok(Self::Gate), + // 4 => Ok(Self::Middle), + 0 => Ok(Self::Gate), + 1 => Ok(Self::Middle), + 2 => Ok(Self::Shark), + 3 => Ok(Self::Sawfish), + // 4 => Ok(Self::Pole), + // 5 => Ok(Self::Pole), + 5 => Ok(Self::LeftPole), + 4 => Ok(Self::RightPole), x => Err(TargetError { x }), } } @@ -56,7 +80,7 @@ impl TryFrom for Target { impl Display for Target { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{:?}", self) + write!(f, "{self:?}") } } @@ -74,7 +98,7 @@ impl GatePoles { } pub fn load_640(threshold: f64) -> Self { - let model = load_onnx!("models/gate_new_640.onnx", 640, 5); + let model = load_onnx!("models/new_gate.onnx", 640, 6); Self { model, threshold } } @@ -82,7 +106,7 @@ impl GatePoles { impl Default for GatePoles { fn default() -> Self { - Self::load_640(0.5) + Self::load_640(0.75) } } @@ -90,7 +114,40 @@ impl YoloProcessor for GatePoles { type Target = Target; fn detect_yolo_v5(&mut self, image: &Mat) -> Vec { - self.model.detect_yolo_v5(image, self.threshold) + let mut channels = Vector::::new(); + let _ = split(image, &mut channels).unwrap(); + let b = channels.get(0).unwrap(); + let g = channels.get(1).unwrap(); + let r = channels.get(2).unwrap(); + let mut mult_b = Mat::default(); + let _ = multiply(&b, &1.0, &mut mult_b, 1.0, -1).unwrap(); + let values = Vector::::from_iter(vec![b, g, r]); + let mut output_img = Mat::default(); + let _ = merge(&values, &mut output_img).unwrap(); + let mut dilated = Mat::default(); + // let kernel = Vector::>::from_iter(vec![ + // Vector::::from_iter(vec![1, 1, 1]), + // Vector::::from_iter(vec![1, 1, 1]), + // Vector::::from_iter(vec![1, 1, 1]), + // ]); + // let kernel = + // get_structuring_element(MORPH_RECT, Size::new(3, 3), Point::new(-1, -1)).unwrap(); + let kernel = Mat::ones_size(Size::new(3, 3), CV_8U).unwrap(); + let _ = dilate( + &output_img, + &mut dilated, + &kernel, + Point::new(-1, -1), + 1, + BORDER_CONSTANT, + morphology_default_border_value().unwrap(), + ) + .unwrap(); + + dbg!(image.dims()); + dbg!(dilated.dims()); + + self.model.detect_yolo_v5(&dilated, self.threshold) } fn model_size(&self) -> Size { diff --git a/src/vision/mod.rs b/src/vision/mod.rs index c74b4de5ef5..616c7dabee7 100644 --- a/src/vision/mod.rs +++ b/src/vision/mod.rs @@ -3,10 +3,11 @@ use derive_getters::Getters; use itertools::Itertools; use num_traits::{zero, FromPrimitive, Num}; use opencv::{ - core::{MatTraitConst, Point, Rect, Rect2d, Scalar, Vector}, + core::{MatTraitConst, Point, Rect2d, Scalar, VecN, Vector}, imgproc::{self, LINE_8}, prelude::Mat, }; +use serde::{Deserialize, Serialize}; use std::{ fmt::Debug, hash::Hash, @@ -14,9 +15,11 @@ use std::{ ops::{Add, Deref, DerefMut, Div, Mul}, }; +pub mod bin; pub mod buoy; pub mod buoy_model; pub mod gate; +pub mod gate_cv; pub mod gate_poles; pub mod image_prep; pub mod nn_cv2; @@ -24,6 +27,8 @@ pub mod octagon; pub mod path; pub mod path_cv; pub mod pca; +pub mod slalom; +pub mod slalom_yolo; pub mod yolo_model; pub trait Draw { @@ -287,16 +292,20 @@ impl RelPos for DrawRect2d { impl Draw for DrawRect2d { fn draw(&self, canvas: &mut Mat) -> Result<()> { - imgproc::rectangle( - canvas, - self.inner - .to() - .ok_or(anyhow!("f64 outside bounds of i32"))?, - Scalar::from((0.0, 0.0, 255.0)), - 2, - LINE_8, - 0, - )?; + // imgproc::rectangle( + // canvas, + // self.inner + // .to() + // .ok_or(anyhow!("f64 outside bounds of i32"))?, + // Scalar::from((0.0, 0.0, 255.0)), + // 2, + // LINE_8, + // 0, + // )?; + + let center = Point::new((self.x as i32), (self.y as i32)); + + imgproc::circle_def(canvas, center, 5, Scalar::from((0.0, 0.0, 255.0)))?; Ok(()) } } @@ -410,3 +419,66 @@ impl From> for VecMatWrapper { unsafe impl Send for VecMatWrapper {} unsafe impl Sync for VecMatWrapper {} + +#[derive(Debug, PartialEq, Serialize, Deserialize, Clone, Copy)] +pub struct Yuv { + pub y: u8, + pub u: u8, + pub v: u8, +} + +impl From<&VecN> for Yuv { + fn from(value: &VecN) -> Self { + Self { + y: value[0], + u: value[1], + v: value[2], + } + } +} + +impl From<&Yuv> for VecN { + fn from(val: &Yuv) -> Self { + VecN::from_array([val.y, val.u, val.v]) + } +} + +#[derive(Debug, Clone, Getters, PartialEq)] +pub struct PosVector { + x: f64, + y: f64, + z: f64, + angle: f64, +} + +impl PosVector { + fn new(x: f64, y: f64, z: f64, angle: f64) -> Self { + Self { x, y, z, angle } + } +} + +impl RelPosAngle for PosVector { + type Number = f64; + + fn offset_angle(&self) -> Angle2D { + Angle2D { + x: self.x, + y: self.y, + angle: self.angle, + } + } +} + +impl Mul<&Mat> for PosVector { + type Output = Self; + + fn mul(self, rhs: &Mat) -> Self::Output { + let size = rhs.size().unwrap(); + Self { + x: (self.x + 0.5) * (size.width as f64), + y: (self.y + 0.5) * (size.height as f64), + z: 0., + angle: self.angle, + } + } +} diff --git a/src/vision/models/2025Gate.onnx b/src/vision/models/2025Gate.onnx new file mode 100644 index 00000000000..82dacd73f94 Binary files /dev/null and b/src/vision/models/2025Gate.onnx differ diff --git a/src/vision/models/2025Slalom.onnx b/src/vision/models/2025Slalom.onnx new file mode 100644 index 00000000000..a7b7f4fd50d Binary files /dev/null and b/src/vision/models/2025Slalom.onnx differ diff --git a/src/vision/models/bin_640.onnx b/src/vision/models/bin_640.onnx new file mode 100644 index 00000000000..e7df6557deb Binary files /dev/null and b/src/vision/models/bin_640.onnx differ diff --git a/src/vision/models/bins_320.onnx b/src/vision/models/bins_320.onnx deleted file mode 100644 index 2a181898d4b..00000000000 Binary files a/src/vision/models/bins_320.onnx and /dev/null differ diff --git a/src/vision/models/bins_640.onnx b/src/vision/models/gate_6_17_25.onnx similarity index 51% rename from src/vision/models/bins_640.onnx rename to src/vision/models/gate_6_17_25.onnx index a29fcf1ab5d..82e625bf63e 100644 Binary files a/src/vision/models/bins_640.onnx and b/src/vision/models/gate_6_17_25.onnx differ diff --git a/src/vision/models/new_gate.onnx b/src/vision/models/new_gate.onnx new file mode 100644 index 00000000000..9274d6c0227 Binary files /dev/null and b/src/vision/models/new_gate.onnx differ diff --git a/src/vision/models/slalom_640.onnx b/src/vision/models/slalom_640.onnx new file mode 100644 index 00000000000..36f07758a02 Binary files /dev/null and b/src/vision/models/slalom_640.onnx differ diff --git a/src/vision/nn_cv2.rs b/src/vision/nn_cv2.rs index f8f2c03026e..317eb878bd6 100644 --- a/src/vision/nn_cv2.rs +++ b/src/vision/nn_cv2.rs @@ -1,8 +1,7 @@ use anyhow::Result; use derive_getters::Getters; -use itertools::Itertools; use opencv::{ - core::{MatTraitConstManual, Rect2d, Scalar, Size, VecN, Vector, CV_32F}, + core::{Rect2d, Scalar, Size, VecN, Vector, CV_32F}, dnn::{blob_from_image, read_net_from_onnx, read_net_from_onnx_buffer, Net}, prelude::{Mat, MatTraitConst, NetTrait, NetTraitConst}, }; diff --git a/src/vision/octagon.rs b/src/vision/octagon.rs index b2d66b3ee39..327db219318 100644 --- a/src/vision/octagon.rs +++ b/src/vision/octagon.rs @@ -1,26 +1,13 @@ -use std::{fs::create_dir_all, ops::RangeInclusive}; - -use chrono::Offset; -use itertools::Itertools; +use super::{image_prep::resize, MatWrapper, Offset2D, VisualDetection, VisualDetector}; use opencv::{ - core::{ - in_range, MatTrait, Point, Point2f, Point2i, Rect, Scalar, Size, VecN, Vector, - BORDER_DEFAULT, CV_32F, - }, - imgcodecs::imwrite, - imgproc::{ - bounding_rect, contour_area, cvt_color, filter_2d, find_contours, find_contours_def, - CHAIN_APPROX_SIMPLE, COLOR_RGB2YUV, COLOR_YUV2RGB, RETR_TREE, - }, - prelude::{Mat, MatTraitConst, MatTraitConstManual}, + core::{in_range, Point, Size, VecN, Vector}, + imgproc::{find_contours, CHAIN_APPROX_SIMPLE, RETR_TREE}, + prelude::{Mat, MatTraitConst}, }; -use uuid::Uuid; - -use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; +use std::ops::RangeInclusive; -use super::{ - image_prep::resize, pca::PosVector, MatWrapper, Offset2D, VisualDetection, VisualDetector, -}; +#[cfg(feature = "logging")] +use {opencv::imgcodecs::imwrite, std::fs::create_dir_all, uuid::Uuid}; #[derive(Debug, PartialEq, Clone, Copy)] pub struct Rgb { @@ -135,7 +122,7 @@ impl VisualDetector for Octagon { .unwrap(); } - println!("MASK: {:#?}", mask); + println!("MASK: {mask:#?}"); let mut contours_out: Vector> = Vector::new(); find_contours( diff --git a/src/vision/path.rs b/src/vision/path.rs index bcc285f539d..6990324d120 100644 --- a/src/vision/path.rs +++ b/src/vision/path.rs @@ -1,20 +1,22 @@ -use std::{fs::create_dir_all, ops::RangeInclusive}; - +use super::{ + image_prep::{kmeans, resize}, + pca::PosVector, + MatWrapper, VisualDetection, VisualDetector, +}; +use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; use itertools::Itertools; use opencv::{ - core::{in_range, AlgorithmHint, Size, VecN, Vector}, - imgcodecs::imwrite, + core::{in_range, AlgorithmHint, Size, VecN}, imgproc::{cvt_color, COLOR_RGB2YUV, COLOR_YUV2RGB}, prelude::{Mat, MatTraitConst, MatTraitConstManual}, }; -use uuid::Uuid; +use std::ops::RangeInclusive; -use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; - -use super::{ - image_prep::{kmeans, resize}, - pca::PosVector, - MatWrapper, VisualDetection, VisualDetector, +#[cfg(feature = "logging")] +use { + opencv::{core::Vector, imgcodecs::imwrite}, + std::fs::create_dir_all, + uuid::Uuid, }; static FORWARD: (f64, f64) = (0.0, -1.0); diff --git a/src/vision/path_cv.rs b/src/vision/path_cv.rs index 386083ed2a0..e16334a66c1 100644 --- a/src/vision/path_cv.rs +++ b/src/vision/path_cv.rs @@ -1,70 +1,14 @@ -use std::{ - fs::create_dir_all, - ops::{Mul, RangeInclusive}, -}; - -use derive_getters::Getters; -use itertools::Itertools; +use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv}; +use crate::{config::ColorProfile, vision::Draw}; use opencv::{ - core::{in_range, Point, Scalar, Size, VecN, Vector}, - imgcodecs::imwrite, + core::{in_range, Point, Scalar, Size, Vector}, imgproc::{ - self, box_points, circle, contour_area_def, cvt_color_def, find_contours_def, - min_area_rect, CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, COLOR_YUV2BGR, LINE_8, RETR_EXTERNAL, + self, contour_area_def, cvt_color_def, find_contours_def, min_area_rect, + CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, LINE_8, RETR_EXTERNAL, }, prelude::{Mat, MatTraitConst, MatTraitConstManual}, }; -use uuid::Uuid; - -use crate::vision::image_prep::{binary_pca, cvt_binary_to_points}; -use crate::vision::{Angle2D, Draw, Offset2D, RelPosAngle}; - -use super::{ - image_prep::{kmeans, resize}, - MatWrapper, VisualDetection, VisualDetector, -}; - -static FORWARD: (f64, f64) = (0.0, -1.0); - -#[derive(Debug, Clone, Getters, PartialEq)] -pub struct PosVector { - x: f64, - y: f64, - z: f64, - angle: f64, -} - -impl PosVector { - fn new(x: f64, y: f64, z: f64, angle: f64) -> Self { - Self { x, y, z, angle } - } -} - -impl RelPosAngle for PosVector { - type Number = f64; - - fn offset_angle(&self) -> Angle2D { - Angle2D { - x: self.x, - y: self.y, - angle: self.angle, - } - } -} - -impl Mul<&Mat> for PosVector { - type Output = Self; - - fn mul(self, rhs: &Mat) -> Self::Output { - let size = rhs.size().unwrap(); - Self { - x: (self.x + 0.5) * (size.width as f64), - y: (self.y + 0.5) * (size.height as f64), - z: 0., - angle: self.angle, - } - } -} +use std::ops::RangeInclusive; impl Draw for VisualDetection { fn draw(&self, canvas: &mut Mat) -> anyhow::Result<()> { @@ -75,7 +19,7 @@ impl Draw for VisualDetection { Scalar::from((0.0, 0.0, 255.0)) }; - let angle_rad = (*self.position.angle() as f32) * (3.14152965 / 180.0); + let angle_rad = (*self.position.angle() as f32) * (3.1415296 / 180.0); let b = (angle_rad.cos() * 640.0) / 2.0; let a = (angle_rad.sin() * 480.0) / 2.0; @@ -115,47 +59,10 @@ impl Draw for VisualDetection { } } -#[derive(Debug, PartialEq)] -pub struct Yuv { - pub y: u8, - pub u: u8, - pub v: u8, -} - -impl From<&VecN> for Yuv { - fn from(value: &VecN) -> Self { - Self { - y: value[0], - u: value[1], - v: value[2], - } - } -} - -impl From<&Yuv> for VecN { - fn from(val: &Yuv) -> Self { - VecN::from_array([val.y, val.u, val.v]) - } -} - -impl Yuv { - fn in_range(&self, range: &RangeInclusive) -> bool { - self.y >= range.start().y - && self.u >= range.start().u - && self.v >= range.start().v - && self.y <= range.end().y - && self.u <= range.end().u - && self.v <= range.end().v - } -} - #[derive(Debug)] pub struct PathCV { color_bounds: RangeInclusive, - width_bounds: RangeInclusive, - num_regions: i32, size: Size, - attempts: i32, image: MatWrapper, } @@ -166,22 +73,17 @@ impl PathCV { } impl PathCV { - pub fn new( - color_bounds: RangeInclusive, - width_bounds: RangeInclusive, - num_regions: i32, - size: Size, - attempts: i32, - ) -> Self { + pub fn new(color_bounds: RangeInclusive, size: Size) -> Self { Self { color_bounds, - width_bounds, - num_regions, size, - attempts, image: Mat::default().into(), } } + + pub fn from_color_profile(color_profile: &ColorProfile) -> Self { + Self::new(dbg!(color_profile.orange.clone()), Size::from((400, 300))) + } } impl Default for PathCV { @@ -192,21 +94,11 @@ impl Default for PathCV { u: 127, v: 255, }), - 20.0..=800.0, - 4, Size::from((400, 300)), - 3, ) } } -fn compute_angle(v1: (f64, f64), v2: (f64, f64)) -> f64 { - let dot = (v1.0 * v2.0) + (v1.1 * v2.1); - let norm = |vec: (f64, f64)| ((vec.0 * vec.0) + (vec.1 * vec.1)).sqrt(); - let norm_combined = norm(v1) * norm(v2); - (dot / norm_combined).acos() -} - impl VisualDetector for PathCV { type ClassEnum = bool; type Position = PosVector; @@ -236,7 +128,7 @@ impl VisualDetector for PathCV { ); let mut mask = Mat::default(); - in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); + let _ = in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); let mut contours = Vector::>::new(); find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; @@ -252,18 +144,15 @@ impl VisualDetector for PathCV { let area = contour_area_def(&contour)?; if area > 5000.0 { let rect = min_area_rect(&contour)?; - let mut angle = rect.angle as f64; - let width = rect.size.width; - let height = rect.size.height; - let mut boxRect = Mat::default(); - imgproc::box_points(rect, &mut boxRect)?; + let mut box_rect = Mat::default(); + imgproc::box_points(rect, &mut box_rect)?; - let boxVec: Vec> = boxRect.to_vec_2d()?; + let box_vec: Vec> = box_rect.to_vec_2d()?; - let zero = boxVec[0].clone(); - let one = boxVec[1].clone(); - let two = boxVec[2].clone(); + let zero = box_vec[0].clone(); + let one = box_vec[1].clone(); + let two = box_vec[2].clone(); let edge1 = (one[0] - zero[0], one[1] - zero[1]); let edge2 = (two[0] - one[0], two[1] - one[1]); @@ -273,14 +162,14 @@ impl VisualDetector for PathCV { let edge2mag = (edge2.0.powf(2.0) + edge2.1.powf(2.0)).sqrt(); let longest_edge = if edge2mag > edge1mag { edge2 } else { edge1 }; - let mut angle = (longest_edge.0 / longest_edge.1).atan().to_degrees() * -1.0; + let mut angle = -(longest_edge.0 / longest_edge.1).atan().to_degrees(); angle = ((angle + 180.0) % 360.0) - 180.0; if angle < -90.0 { angle += 180.0; } - println!("{:?}", angle); + println!("{angle:?}"); let center_adjusted_x = rect.center.x as f64; let center_adjusted_y = rect.center.y as f64; @@ -348,7 +237,7 @@ impl VisualDetector for PathCV { ); let mut mask = Mat::default(); - in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); + let _ = in_range(&yuv_image, &lower_orange, &upper_orange, &mut mask); let mut contours = Vector::>::new(); find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; @@ -364,18 +253,15 @@ impl VisualDetector for PathCV { let area = contour_area_def(&contour)?; if area > 5000.0 { let rect = min_area_rect(&contour)?; - let mut angle = rect.angle as f64; - let width = rect.size.width; - let height = rect.size.height; - let mut boxRect = Mat::default(); - imgproc::box_points(rect, &mut boxRect)?; + let mut box_rect = Mat::default(); + imgproc::box_points(rect, &mut box_rect)?; - let boxVec: Vec> = boxRect.to_vec_2d()?; + let box_vec: Vec> = box_rect.to_vec_2d()?; - let zero = boxVec[0].clone(); - let one = boxVec[1].clone(); - let two = boxVec[2].clone(); + let zero = box_vec[0].clone(); + let one = box_vec[1].clone(); + let two = box_vec[2].clone(); let edge1 = (one[0] - zero[0], one[1] - zero[1]); let edge2 = (two[0] - one[0], two[1] - one[1]); @@ -385,14 +271,14 @@ impl VisualDetector for PathCV { let edge2mag = (edge2.0.powf(2.0) + edge2.1.powf(2.0)).sqrt(); let longest_edge = if edge2mag > edge1mag { edge2 } else { edge1 }; - let mut angle = (longest_edge.0 / longest_edge.1).atan().to_degrees() * -1.0; + let mut angle = -(longest_edge.0 / longest_edge.1).atan().to_degrees(); angle = ((angle + 180.0) % 360.0) - 180.0; if angle < -90.0 { angle += 180.0; } - println!("{:?}", angle); + println!("{angle:?}"); let center_adjusted_x = rect.center.x as f64; let center_adjusted_y = rect.center.y as f64; diff --git a/src/vision/pca.rs b/src/vision/pca.rs index 46499e436bb..5b9a2e3f502 100644 --- a/src/vision/pca.rs +++ b/src/vision/pca.rs @@ -1,8 +1,6 @@ use std::{fmt::Debug, ops::Mul}; -use anyhow::anyhow; use derive_getters::Getters; -use num_traits::Num; use opencv::{ core::{MatTraitConst, Point, Scalar}, imgproc::{self, LINE_8}, diff --git a/src/vision/slalom.rs b/src/vision/slalom.rs new file mode 100644 index 00000000000..e6f0c5b8557 --- /dev/null +++ b/src/vision/slalom.rs @@ -0,0 +1,180 @@ +use crate::config::ColorProfile; + +use super::{image_prep::resize, MatWrapper, PosVector, VisualDetection, VisualDetector, Yuv}; +use opencv::{ + core::{in_range, Point, Scalar, Size, Vector}, + imgproc::{ + box_points, contour_area_def, cvt_color_def, find_contours_def, min_area_rect, + CHAIN_APPROX_SIMPLE, COLOR_BGR2YUV, RETR_EXTERNAL, + }, + prelude::{Mat, MatTraitConst, MatTraitConstManual}, +}; +use std::ops::RangeInclusive; + +#[derive(Debug)] +pub struct Slalom { + color_bounds: RangeInclusive, + area_bounds: RangeInclusive, + size: Size, + image: MatWrapper, +} + +impl Slalom { + pub fn new( + color_bounds: RangeInclusive, + area_bounds: RangeInclusive, + size: Size, + ) -> Self { + Self { + color_bounds, + area_bounds, + size, + image: Mat::default().into(), + } + } + + pub fn from_color_profile( + color_profile: &ColorProfile, + area_bounds: RangeInclusive, + ) -> Self { + Self::new( + color_profile.red.clone(), + area_bounds, + Size::from((400, 300)), + ) + } +} + +// TODO: Change these to match slalom, not path +impl Default for Slalom { + fn default() -> Self { + Self::new( + (Yuv { + y: 20, + u: 100, + v: 160, + })..=(Yuv { + y: 220, + u: 135, + v: 255, + }), + 1000.0..=11000.0, + Size::from((400, 300)), + ) + } +} + +impl VisualDetector for Slalom { + type ClassEnum = bool; + type Position = PosVector; + + fn detect( + &mut self, + input_image: &Mat, + ) -> anyhow::Result>> { + let areas = self.area_bounds.clone(); + let MIN_AREA = areas.start(); + let MAX_AREA = areas.end(); + + self.image = resize(input_image, &self.size)?.into(); + let mut yuv_image = Mat::default(); + + cvt_color_def(&self.image.0, &mut yuv_image, COLOR_BGR2YUV)?; + + let color_start = self.color_bounds.start(); + let color_end = self.color_bounds.end(); + let lower_red = Scalar::new( + color_start.y as f64, + color_start.u as f64, + color_start.v as f64, + 0., + ); + let upper_red = Scalar::new( + color_end.y as f64, + color_end.u as f64, + color_end.v as f64, + 0., + ); + + let mut mask = Mat::default(); + let _ = in_range(&yuv_image, &lower_red, &upper_red, &mut mask); + + let mut contours = Vector::>::new(); + find_contours_def(&mask, &mut contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE)?; + + let max_contour = contours.iter().max_by(|x, y| { + contour_area_def(&x) + .unwrap() + .partial_cmp(&contour_area_def(&y).unwrap()) + .unwrap() + }); + + if let Some(contour) = max_contour { + let area = contour_area_def(&contour)?; + #[cfg(feature = "logging")] + logln!("AREA: {area}"); + + if area > *MIN_AREA && area < *MAX_AREA { + let rect = min_area_rect(&contour)?; + + let mut box_rect = Mat::default(); + box_points(rect, &mut box_rect)?; + + let box_vec: Vec> = box_rect.to_vec_2d()?; + + let zero = box_vec[0].clone(); + let one = box_vec[1].clone(); + let two = box_vec[2].clone(); + + let edge1 = (one[0] - zero[0], one[1] - zero[1]); + let edge2 = (two[0] - one[0], two[1] - one[1]); + + let edge1mag = (edge1.0.powf(2.0) + edge1.1.powf(2.0)).sqrt(); + let edge2mag = (edge2.0.powf(2.0) + edge2.1.powf(2.0)).sqrt(); + let longest_edge = if edge2mag > edge1mag { edge2 } else { edge1 }; + + let mut angle = -(longest_edge.0 / longest_edge.1).atan().to_degrees(); + + angle = ((angle + 180.0) % 360.0) - 180.0; + if angle < -90.0 { + angle += 180.0; + } + + println!("{angle:?}"); + + let center_adjusted_x = rect.center.x as f64; + let center_adjusted_y = rect.center.y as f64; + + Ok(vec![VisualDetection { + class: true, + position: PosVector::new( + center_adjusted_x, + center_adjusted_y, + 0., + angle as f64, + ), + }]) + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } + } else { + Ok(vec![VisualDetection { + class: false, + position: PosVector::new(0., 0., 0., 0.), + }]) + } + } + + fn normalize(&mut self, pos: &Self::Position) -> Self::Position { + let img_size = self.image.size().unwrap(); + Self::Position::new( + ((*pos.x() / (img_size.width as f64)) - 0.5) * 2.0, + ((*pos.y() / (img_size.height as f64)) - 0.5) * 2.0, + 0., + *pos.angle(), + ) + } +} diff --git a/src/vision/slalom_yolo.rs b/src/vision/slalom_yolo.rs new file mode 100644 index 00000000000..e1e82d57962 --- /dev/null +++ b/src/vision/slalom_yolo.rs @@ -0,0 +1,93 @@ +use anyhow::Result; +use derive_getters::Getters; +use opencv::{core::Size, prelude::Mat}; + +use crate::load_onnx; + +use super::{ + nn_cv2::{OnnxModel, VisionModel, YoloClass, YoloDetection}, + yolo_model::YoloProcessor, +}; + +use core::hash::Hash; +use std::{error::Error, fmt::Display}; + +#[derive(Debug, PartialEq, Eq, Hash, Clone)] +pub enum Target { + Middle, + Side, +} + +impl From> for Target { + fn from(value: YoloClass) -> Self { + value.identifier + } +} + +#[derive(Debug)] +pub struct TargetError { + x: i32, +} + +impl Display for TargetError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{} is outside known classIDs [0, 1]", self.x) + } +} + +impl Error for TargetError {} + +impl TryFrom for Target { + type Error = TargetError; + fn try_from(value: i32) -> std::result::Result { + match value { + 0 => Ok(Self::Middle), + 1 => Ok(Self::Side), + x => Err(TargetError { x }), + } + } +} + +impl Display for Target { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + write!(f, "{self:?}") + } +} + +#[derive(Debug, Clone, Getters)] +pub struct Slalom { + model: T, + threshold: f64, +} + +impl Slalom { + pub fn new(model_name: &str, model_size: i32, threshold: f64) -> Result { + let model = OnnxModel::from_file(model_name, model_size, 2)?; + + Ok(Self { model, threshold }) + } + + pub fn load_640(threshold: f64) -> Self { + let model = load_onnx!("models/2025Slalom.onnx", 640, 2); + + Self { model, threshold } + } +} + +impl Default for Slalom { + fn default() -> Self { + Self::load_640(0.60) + } +} + +impl YoloProcessor for Slalom { + type Target = Target; + + fn detect_yolo_v5(&mut self, image: &Mat) -> Vec { + self.model.detect_yolo_v5(image, self.threshold) + } + + fn model_size(&self) -> Size { + self.model.size() + } +} diff --git a/src/vision/yolo_model.rs b/src/vision/yolo_model.rs index b68fcf4f9aa..d85837aab88 100644 --- a/src/vision/yolo_model.rs +++ b/src/vision/yolo_model.rs @@ -85,6 +85,7 @@ impl Draw for VisualDetection, DrawRect2d> { LINE_AA, false, )?; + Ok(()) } } diff --git a/tuning_tools/flake.lock b/tuning_tools/flake.lock new file mode 100644 index 00000000000..ab5f11367e0 --- /dev/null +++ b/tuning_tools/flake.lock @@ -0,0 +1,27 @@ +{ + "nodes": { + "nixpkgs": { + "locked": { + "lastModified": 1755186698, + "narHash": "sha256-wNO3+Ks2jZJ4nTHMuks+cxAiVBGNuEBXsT29Bz6HASo=", + "owner": "nixos", + "repo": "nixpkgs", + "rev": "fbcf476f790d8a217c3eab4e12033dc4a0f6d23c", + "type": "github" + }, + "original": { + "owner": "nixos", + "ref": "nixos-unstable", + "repo": "nixpkgs", + "type": "github" + } + }, + "root": { + "inputs": { + "nixpkgs": "nixpkgs" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/tuning_tools/flake.nix b/tuning_tools/flake.nix new file mode 100644 index 00000000000..44d92c29bba --- /dev/null +++ b/tuning_tools/flake.nix @@ -0,0 +1,56 @@ +{ + description = "A Nix-flake-based Python development environment"; + + inputs.nixpkgs.url = "github:nixos/nixpkgs/nixos-unstable"; + + outputs = { self, nixpkgs }: + let + supportedSystems = [ "x86_64-linux" "aarch64-linux" "x86_64-darwin" "aarch64-darwin" ]; + forEachSupportedSystem = f: nixpkgs.lib.genAttrs supportedSystems (system: f { + pkgs = import nixpkgs { inherit system; }; + }); + in + { + devShells = forEachSupportedSystem ({ pkgs }: { + default = pkgs.mkShell { + venvDir = ".venv"; + packages = let + customOpenCV = rec { + opencv4 = pkgs.python312Packages.opencv4.override { + enableGtk2 = true; + gtk2 = pkgs.gtk2; + }; + opencv-python = pkgs.python312Packages.opencv-python.override { + inherit opencv4; + }; + }; + in with pkgs; [ gtk2 python312 ] ++ + (with pkgs.gst_all_1; [ + gstreamer + gst-plugins-base + gst-plugins-good + gst-plugins-bad + gst-plugins-ugly + gst-libav + gst-vaapi + gst-rtsp-server + ]) ++ + (with pkgs.python312Packages; [ + pip + numpy + matplotlib + ipywidgets + nicegui + # pkgconfig + # opencv-python + venvShellHook + python-lsp-ruff + python-lsp-server + ultralytics + onnxruntime + pyside6 + ]) ++ [ customOpenCV.opencv-python ]; + }; + }); + }; +} diff --git a/tuning_tools/tuner.py b/tuning_tools/tuner.py new file mode 100644 index 00000000000..a920883b854 --- /dev/null +++ b/tuning_tools/tuner.py @@ -0,0 +1,146 @@ +import sys +import cv2 +import numpy as np +from PySide6.QtCore import Qt, QTimer +from PySide6.QtGui import QImage, QPixmap +from PySide6.QtWidgets import ( + QApplication, + QMainWindow, + QLabel, + QWidget, + QVBoxLayout, + QHBoxLayout, + QFormLayout, + QSlider, + QSpinBox, + QCheckBox, +) + +RTSP_URL = "rtsp://192.168.2.5:8554/front.mp4" + + +class RTSPViewer(QMainWindow): + def __init__(self): + super().__init__() + self.setWindowTitle("RTSP YUV Viewer (OpenCV)") + + # OpenCV RTSP capture using GStreamer + self.cap = cv2.VideoCapture( + f"rtspsrc location={RTSP_URL} latency=0 ! decodebin ! videoconvert ! appsink", + cv2.CAP_GSTREAMER, + ) + if not self.cap.isOpened(): + raise RuntimeError(f"Cannot open RTSP stream: {RTSP_URL}") + + # Default YUV ranges + self.yuv_min = np.array([0, 0, 0], dtype=np.uint8) + self.yuv_max = np.array([255, 255, 255], dtype=np.uint8) + self.show_mask_only = False + self.overlay_mode = True + + # UI: video + sliders + self.video_label = QLabel() + self.video_label.setAlignment(Qt.AlignCenter) + self.video_label.setStyleSheet("background-color: black;") + self.video_label.setMinimumSize(640, 360) + + controls = self._build_controls() + + central = QWidget() + layout = QHBoxLayout(central) + layout.addWidget(self.video_label, 3) + layout.addLayout(controls, 1) + self.setCentralWidget(central) + + # Timer for frame updates + self.timer = QTimer() + self.timer.timeout.connect(self.update_frame) + self.timer.start(30) # ~33 FPS + + def _build_controls(self): + layout = QVBoxLayout() + form = QFormLayout() + + def add_slider(label, idx, init, minv=0, maxv=255): + slider = QSlider(Qt.Horizontal) + slider.setRange(minv, maxv) + slider.setValue(init) + spin = QSpinBox() + spin.setRange(minv, maxv) + spin.setValue(init) + slider.valueChanged.connect(lambda v: spin.setValue(v)) + spin.valueChanged.connect(lambda v: slider.setValue(v)) + slider.valueChanged.connect(lambda v: self._on_slider(idx, v)) + spin.valueChanged.connect(lambda v: self._on_slider(idx, v)) + + h = QHBoxLayout() + h.addWidget(slider) + h.addWidget(spin) + form.addRow(label, h) + + add_slider("Y min", (0, "min"), 0) + add_slider("Y max", (0, "max"), 255) + add_slider("U min", (1, "min"), 0) + add_slider("U max", (1, "max"), 255) + add_slider("V min", (2, "min"), 0) + add_slider("V max", (2, "max"), 255) + + layout.addLayout(form) + + cb_mask = QCheckBox("Show mask only") + cb_mask.stateChanged.connect( + lambda s: setattr(self, "show_mask_only", s == Qt.Checked) + ) + layout.addWidget(cb_mask) + + cb_overlay = QCheckBox("Overlay on original") + cb_overlay.setChecked(True) + cb_overlay.stateChanged.connect( + lambda s: setattr(self, "overlay_mode", s == Qt.Checked) + ) + layout.addWidget(cb_overlay) + + layout.addStretch(1) + return layout + + def _on_slider(self, idx_info, value): + idx, bound = idx_info + if bound == "min": + self.yuv_min[idx] = value + else: + self.yuv_max[idx] = value + + def update_frame(self): + ret, frame = self.cap.read() + if not ret: + return + + # Convert to YUV + yuv = cv2.cvtColor(frame, cv2.COLOR_BGR2YUV) + mask = cv2.inRange(yuv, self.yuv_min, self.yuv_max) + + if self.show_mask_only: + disp = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR) + elif self.overlay_mode: + disp = cv2.bitwise_and(frame, frame, mask=mask) + else: + disp = frame + + # Convert to QImage + rgb = cv2.cvtColor(disp, cv2.COLOR_BGR2RGB) + h, w, ch = rgb.shape + qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888) + self.video_label.setPixmap(QPixmap.fromImage(qimg)) + + def closeEvent(self, event): + self.timer.stop() + self.cap.release() + super().closeEvent(event) + + +if __name__ == "__main__": + app = QApplication(sys.argv) + win = RTSPViewer() + win.resize(960, 540) + win.show() + sys.exit(app.exec())