From 44166e03596e766c1d5a1e3ff1c12734e14df38d Mon Sep 17 00:00:00 2001 From: mablin7 Date: Fri, 5 Apr 2024 14:26:27 +0200 Subject: [PATCH] Squashed commit of the following: commit c40828d82ec09cdde03f7093428873291d7ad993 Author: mablin7 Date: Fri Apr 5 14:18:25 2024 +0200 fix: formatting commit 3096877441b66e966e9e56bac38595fc1a7e894b Author: Renyi Yang Date: Sat Mar 30 01:39:53 2024 +0100 fix commit 8bd571c32a782b0e2ab54d47fe034fe30ce681fa Author: Renyi Yang Date: Sat Mar 30 00:59:50 2024 +0100 fix checkstyle feat: stop controller commit ad3b721ac0ecde4064d623bcb791f22d096c1760 Author: Renyi Yang Date: Fri Mar 29 19:33:15 2024 +0100 feat: halt controller commit 9045ef37ca44aa660f92c4df2d383cf3c7b165a8 Author: Renyi Yang Date: Fri Mar 29 18:45:16 2024 +0100 feat: playercontroller commit 85f4ce2b645248ce2da492a9d55b2ee57451740d Author: mablin7 Date: Thu Mar 28 20:40:49 2024 +0100 Squashed commit of the following: commit 0770d99b9bb3ab5be061a782b9fadd015e6847c2 Author: mablin7 Date: Wed Mar 27 15:18:37 2024 +0100 fix ball detection commit c7d9861c7c80c1acd9891129eae5546aa506cadb Author: mablin7 Date: Fri Mar 22 15:25:57 2024 +0100 fix: main.rs commit 21b47d06e3d8d991af8a78123101e8718b5bb978 Merge: a01cf44 1353d76 Author: mablin7 Date: Fri Mar 22 15:22:23 2024 +0100 Merge branch 'main' into simulator commit a01cf44a98e5c987cd5c8a0d79fe00df76936b0e Author: mablin7 Date: Thu Mar 21 20:37:25 2024 +0100 feat: sim Co-authored-by: Teodor Neagoe commit a96fbe976d70c0d7c1adb51567b2290ce2aa5d15 Author: Teodor Neagoe Date: Thu Mar 21 17:47:19 2024 +0100 step-1-simualtor commit 1353d76a280e842d1089653bd8d4817c478107ea Merge: 75b1407 e1ceccd Author: Balint Magyar Date: Thu Mar 21 15:32:21 2024 +0100 Merge pull request #13 from DelftMercurians/teo-optimize-run-on-server Teo optimize run on server commit e1ceccd1afac5aa45da20b770d8fb90e26420517 Merge: 2c54253 75b1407 Author: Balint Magyar Date: Thu Mar 21 15:32:03 2024 +0100 Merge branch 'main' into teo-optimize-run-on-server commit 2c542530bcac1ac014ddebcd12cc5a1422a78988 Author: mablin7 Date: Thu Mar 21 15:31:20 2024 +0100 fix commit 75b14071650caf38c3f0949333a62848fc59ece0 Merge: 89746a5 a6befa4 Author: Balint Magyar Date: Wed Mar 20 16:48:15 2024 +0100 Merge pull request #12 from DelftMercurians/filter Feat(world): Kalman Filter commit a6befa47d32d2effb51a2c3789e84c8eefc72760 Author: mablin7 Date: Wed Mar 20 16:47:04 2024 +0100 fix: fmt commit 708fb764a2733a807b445e09fae9290137130e29 Author: mablin7 Date: Wed Mar 20 15:41:19 2024 +0100 sim commit 434988de559a763d71598b60e903240507535ea1 Author: Renyi Yang Date: Tue Mar 19 21:00:18 2024 +0100 misc commit 5ba2ab4d32f9e3245ad04517010232725ee2ec7a Author: Renyi Yang Date: Tue Mar 19 20:51:12 2024 +0100 . commit 95622fd0c9cb0b368aec1bc09a032b3cfa7c3dfd Author: Renyi Yang Date: Wed Mar 13 01:47:37 2024 +0100 fix: test3d longer duration commit 253bccb8c73554878cb85cfe8bbb12341e90db06 Author: Renyi Yang Date: Wed Mar 13 01:41:51 2024 +0100 document commit 40e9b3e8aecc78267933eba3c898716ee361e4f4 Author: Renyi Yang Date: Wed Mar 13 00:46:38 2024 +0100 misc: checkstyle naming, dead code commit e95a0dea12350f147e94eabac86977e0e9fd2178 Author: Renyi Yang Date: Wed Mar 13 00:22:14 2024 +0100 refactor: remove builder commit 089da8ce5ef4ae9a9396a660483be5262485fb5c Author: Renyi Yang Date: Tue Mar 12 23:20:11 2024 +0100 refactor: switch to smatrix commit 99e78b88d7ccdbd8aa0e20fcbfdc118e644b103b Author: Renyi Yang Date: Tue Mar 12 19:32:01 2024 +0100 feat: update method not guarantee it functions. still need to test. commit 4d08d423c8b9cc36ac181f8abe57af69a3e8d1ea Author: Renyi Yang Date: Tue Mar 12 17:15:40 2024 +0100 outlier detect commit 79ec7e9a4d8c0ee7dc170b90fdff4a5166508d41 Author: Renyi Yang Date: Tue Mar 12 13:52:13 2024 +0100 test: builder test commit d149803e9d52a972044f9336b750bac254b200da Author: Renyi Yang Date: Tue Mar 12 01:40:16 2024 +0100 feat: new build for ball filter commit 31f2d465a504197c49ff3915de6cb29d8a0c3e9e Author: Renyi Yang Date: Mon Mar 11 23:40:11 2024 +0100 misc: var instead of std commit 53154bc485530db2ffcc95c3eb2149114fd56836 Author: mablin7 Date: Wed Mar 6 20:20:33 2024 +0100 fix commit 89746a5432ba0ef41eab80a6e8512a9b7d717e8b Author: mablin7 Date: Wed Mar 6 22:08:28 2024 +0100 feat: velocity measurement commit 259c4af991e6a92cbf8eef428b051e2beb685b8e Author: Teodor Neagoe Date: Wed Mar 6 19:40:48 2024 +0100 test-run-on-remote commit f1d5eab780cf5db178d27d76c6f8b18cf65b690f Author: mablin7 Date: Mon Mar 4 19:09:14 2024 +0100 ci: fix deps commit 165247fac82280f57804e8c81bc942e10efb353c Author: Renyi Yang Date: Mon Mar 4 13:06:23 2024 +0100 check style commit 20ad046925463a61f872f12ba31c8a30cc17be26 Author: Renyi Yang Date: Sat Mar 2 23:48:12 2024 +0100 unitest commit 1136805e994a2da1d8ca86a96a2f366bfcb3090c Author: Renyi Yang Date: Sat Mar 2 22:24:36 2024 +0100 builder feat: buidler commit 883622410ff99c81b3a68375e421d17d08c10e27 Author: Renyi Yang Date: Sat Mar 2 20:37:44 2024 +0100 feat: algorithm commit 903cbb24dd8f06969d504745f37c3b44e7eb1859 Author: Renyi Yang Date: Sat Mar 2 18:13:06 2024 +0100 feat: matrix generator commit 9e1ff1fdbae85a73a6df654f70ee75d6a615bbba Merge: 16c5810 002ebe9 Author: Balint Magyar Date: Mon Mar 4 08:04:39 2024 +0100 Merge pull request #11 from DelftMercurians/alexandru/webui Alexandru/webui commit a838560bd01c0ec41310f4614b8ab275851aeb7d Author: Teodor Neagoe Date: Wed Feb 28 19:37:53 2024 +0100 untested remove dirs commit a1cd9e0ada3c157a6f6de256c8153e32ca3f7f5d Author: Teodor Neagoe Date: Wed Feb 28 18:53:18 2024 +0100 added TODO to run-on-server commit 16c5810797c08b6ae12ff0b71f0836ad1d6d1465 Author: mablin7 Date: Wed Feb 28 18:44:38 2024 +0100 feat: move run on server into workspace commit 002ebe93b8efc26448cab759fafd2eccbee35fdd Merge: adf9639 f112108 Author: Alexandru Lolea Date: Wed Feb 28 17:28:35 2024 +0100 Solved conflicts commit adf9639c89de69289a3b3dc8065a17815b46d080 Author: Alexandru Lolea Date: Wed Feb 28 17:02:25 2024 +0100 Fixed prop reference in Bot.vue commit fa4d0a345d940cdbc4f5dac182722450d097cc98 Author: Alexandru Lolea Date: Wed Feb 28 16:54:44 2024 +0100 Fixed errors regarding null possibilities commit f0eefc398e6d084bc65fa23cb76cc2eff10212e1 Author: mablin7 Date: Wed Feb 28 18:01:27 2024 +0100 wip: pid --- Cargo.lock | 243 +++++++++---------- crates/dies-cli/Cargo.toml | 2 +- crates/dies-cli/src/executor.rs | 21 +- crates/dies-cli/src/main.rs | 2 +- crates/dies-control/Cargo.toml | 12 + crates/dies-control/src/lib.rs | 7 + crates/dies-control/src/pid.rs | 85 +++++++ crates/dies-control/src/player_controller.rs | 135 +++++++++++ crates/dies-control/src/team_controller.rs | 124 ++++++++++ crates/dies-control/src/variable.rs | 34 +++ crates/dies-core/src/lib.rs | 9 + crates/dies-python-rt/src/messages.rs | 4 +- docs/src/dev.md | 2 +- py/dies-py/src/dies_py/bridge.py | 29 +-- py/dies-py/src/dies_py/messages.py | 9 +- webui/src/App.svelte | 36 +++ 16 files changed, 596 insertions(+), 158 deletions(-) create mode 100644 crates/dies-control/Cargo.toml create mode 100644 crates/dies-control/src/lib.rs create mode 100644 crates/dies-control/src/pid.rs create mode 100644 crates/dies-control/src/player_controller.rs create mode 100644 crates/dies-control/src/team_controller.rs create mode 100644 crates/dies-control/src/variable.rs diff --git a/Cargo.lock b/Cargo.lock index b81abbf5..4f451701 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -46,7 +46,7 @@ version = "1.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" dependencies = [ - "memchr 2.7.1", + "memchr", ] [[package]] @@ -158,18 +158,18 @@ checksum = "16e62a023e7c117e27523144c5d2459f4397fcc3cab0085af8e2224f643a0193" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] name = "async-trait" -version = "0.1.78" +version = "0.1.79" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "461abc97219de0eaaf81fe3ef974a540158f3d079c2ab200f891f1a2ef201e85" +checksum = "a507401cad91ec6a857ed5513a2073c82a9b9048762b885bb98655b306964681" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -195,15 +195,15 @@ checksum = "62af46d040ba9df09edc6528dae9d8e49f5f3e82f55b7d2ec31a733c38dbc49d" [[package]] name = "autocfg" -version = "1.1.0" +version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" +checksum = "f1fdabc7756949593fe60f30ec81974b613357de856987752631dea1e3394c80" [[package]] name = "backtrace" -version = "0.3.70" +version = "0.3.71" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "95d8e92cac0961e91dbd517496b00f7e9b92363dbe6d42c3198268323798860c" +checksum = "26b05800d2e817c8b3b4b54abd461726265fa9789ae34330622f2db9ee696f9d" dependencies = [ "addr2line", "cc", @@ -279,9 +279,9 @@ checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "bytes" -version = "1.5.0" +version = "1.6.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a2bd12c1caf447e69cd4528f47f94d203fd2582878ecb9e9465484c4148a8223" +checksum = "514de17de45fdb8dc022b1a7975556c53c86f9f0aa5f534b98977b171857c2c9" [[package]] name = "bzip2" @@ -322,9 +322,9 @@ checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" [[package]] name = "chrono" -version = "0.4.35" +version = "0.4.37" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8eaf5903dcbc0a39312feb77df2ff4c76387d591b9fc7b04a238dcf8bb62639a" +checksum = "8a0d04d43504c61aa6c7531f1871dd0d418d91130162063b789da00fd7057a5e" dependencies = [ "android-tzdata", "iana-time-zone", @@ -359,9 +359,9 @@ dependencies = [ [[package]] name = "clap" -version = "4.5.3" +version = "4.5.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "949626d00e063efc93b6dca932419ceb5432f99769911c0b995f7e884c778813" +checksum = "90bc066a67923782aa8515dbaea16946c5bcc5addbd668bb80af688e53e548a0" dependencies = [ "clap_builder", "clap_derive", @@ -381,14 +381,14 @@ dependencies = [ [[package]] name = "clap_derive" -version = "4.5.3" +version = "4.5.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90239a040c80f5e14809ca132ddc4176ab33d5e17e49691793296e3fcb34d72f" +checksum = "528131438037fd55894f62d6e9f068b8f45ac57ffa77517819645d10aed04f64" dependencies = [ "heck", "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -411,9 +411,9 @@ checksum = "245097e9a4535ee1e3e3931fcfcd55a796a44c643e8596ff6566d68f09b87bbc" [[package]] name = "cookie" -version = "0.18.0" +version = "0.18.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3cd91cf61412820176e137621345ee43b3f4423e589e7ae4e50d601d93e35ef8" +checksum = "4ddef33a339a91ea89fb53151bd0a4689cfce27055c291dfa69945475d22c747" dependencies = [ "percent-encoding", "time", @@ -559,7 +559,7 @@ dependencies = [ "proc-macro2", "proc-macro2-diagnostics", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -569,6 +569,7 @@ dependencies = [ "anyhow", "chrono", "clap", + "dies-control", "dies-core", "dies-protos", "dies-python-rt", @@ -579,13 +580,22 @@ dependencies = [ "dies-world", "dirs", "nalgebra", - "npy", "tokio", "tracing", "tracing-appender", "tracing-subscriber", ] +[[package]] +name = "dies-control" +version = "0.1.0" +dependencies = [ + "dies-core", + "log", + "nalgebra", + "num-traits", +] + [[package]] name = "dies-core" version = "0.1.0" @@ -630,7 +640,7 @@ dependencies = [ "toml", "tracing", "zip", - "zstd 0.13.0", + "zstd 0.13.1", ] [[package]] @@ -802,9 +812,9 @@ dependencies = [ [[package]] name = "fastrand" -version = "2.0.1" +version = "2.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "25cbce373ec4653f1a01a31e8a5e5ec0c622dc27ff9c4e6606eefef5cbbed4a5" +checksum = "658bd65b1cf4c852a3cc96f18a8ce7b5640f6b703f905c7d74532294c2a63984" [[package]] name = "figment" @@ -916,7 +926,7 @@ checksum = "87750cf4b7a4c0625b1529e4c543c2182106e4dedc60a2a6455e00d212c489ac" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -943,7 +953,7 @@ dependencies = [ "futures-macro", "futures-sink", "futures-task", - "memchr 2.7.1", + "memchr", "pin-project-lite", "pin-utils", "slab", @@ -1012,9 +1022,9 @@ checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" [[package]] name = "h2" -version = "0.3.25" +version = "0.3.26" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fbd2820c5e49886948654ab546d0688ff24530286bdcf8fca3cefb16d4618eb" +checksum = "81fe527a889e1532da5c525686d96d4c2e74cdd345badf8dfef9f6b39dd5f5e8" dependencies = [ "bytes", "fnv", @@ -1022,7 +1032,7 @@ dependencies = [ "futures-sink", "futures-util", "http", - "indexmap 2.2.5", + "indexmap 2.2.6", "slab", "tokio", "tokio-util", @@ -1198,9 +1208,9 @@ dependencies = [ [[package]] name = "indexmap" -version = "2.2.5" +version = "2.2.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7b0b929d511467233429c45a44ac1dcaa21ba0f5ba11e4879e6ed28ddb4f9df4" +checksum = "168fb715dda47215e360912c096649d23d58bf392ac62f73919e831745e40f26" dependencies = [ "equivalent", "hashbrown 0.14.3", @@ -1209,9 +1219,9 @@ dependencies = [ [[package]] name = "indoc" -version = "2.0.4" +version = "2.0.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e186cfbae8084e513daff4240b4797e342f988cecda4fb6c939150f96315fd8" +checksum = "b248f5224d1d606005e02c97f5aa4e88eeb230488bcc03bc9ca4d7991399f2b5" [[package]] name = "inlinable_string" @@ -1263,9 +1273,9 @@ dependencies = [ [[package]] name = "itoa" -version = "1.0.10" +version = "1.0.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b1a46d1a171d865aa5f83f92695765caa047a9b4cbae2cbf37dbd613a793fd4c" +checksum = "49f1f14873335454500d59611f1cf4a4b0f786f9ac11f4312a78e4cf2566695b" [[package]] name = "jobserver" @@ -1319,13 +1329,12 @@ checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" [[package]] name = "libredox" -version = "0.0.1" +version = "0.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "85c833ca1e66078851dba29046874e38f08b2c883700aa29a03ddd3b23814ee8" +checksum = "c0ff37bd590ca25063e35af745c343cb7a0271906fb7b37e4813e8f79f00268d" dependencies = [ "bitflags 2.5.0", "libc", - "redox_syscall", ] [[package]] @@ -1441,18 +1450,9 @@ dependencies = [ [[package]] name = "memchr" -version = "1.0.2" +version = "2.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "148fab2e51b4f1cfc66da2a7c32981d1d3c083a803978268bb11fe4b86925e7a" -dependencies = [ - "libc", -] - -[[package]] -name = "memchr" -version = "2.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" +checksum = "6c8640c5d730cb13ebd907d8d04b52f55ac9a2eec55b440c8892f40d56c76c1d" [[package]] name = "mime" @@ -1492,7 +1492,7 @@ dependencies = [ "http", "httparse", "log", - "memchr 2.7.1", + "memchr", "mime", "spin", "tokio", @@ -1502,9 +1502,9 @@ dependencies = [ [[package]] name = "nalgebra" -version = "0.32.4" +version = "0.32.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4541eb06dce09c0241ebbaab7102f0a01a0c8994afed2e5d0d66775016e25ac2" +checksum = "3ea4908d4f23254adda3daa60ffef0f1ac7b8c3e9a864cf3cc154b251908a2ef" dependencies = [ "approx", "matrixmultiply", @@ -1557,25 +1557,6 @@ dependencies = [ "libc", ] -[[package]] -name = "nom" -version = "3.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "05aec50c70fd288702bcd93284a8444607f3292dbdf2a30de5ea5dcdbe72287b" -dependencies = [ - "memchr 1.0.2", -] - -[[package]] -name = "npy" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "428bbb4dd63ca4318d430b61e64e9ba7809d11525605905eaa88c73b01d91ed4" -dependencies = [ - "byteorder", - "nom", -] - [[package]] name = "nu-ansi-term" version = "0.46.0" @@ -1610,7 +1591,7 @@ checksum = "ed3955f1a9c7c0c15e092f9c887db08b1fc683305fdf6eb6684f22555355e202" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -1659,7 +1640,7 @@ version = "0.32.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a6a622008b6e321afc04970976f62ee297fdbaa6f95318ca343e3eebb9648441" dependencies = [ - "memchr 2.7.1", + "memchr", ] [[package]] @@ -1691,7 +1672,7 @@ checksum = "a948666b637a0f465e8564c73e89d4dde00d72d4d473cc972f390fc3dcee7d9c" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -1702,9 +1683,9 @@ checksum = "ff011a302c396a5197692431fc1948019154afc178baf7d8e37367442a4601cf" [[package]] name = "openssl-sys" -version = "0.9.101" +version = "0.9.102" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dda2b0f344e78efc2facf7d195d098df0dd72151b26ab98da807afc26c198dff" +checksum = "c597637d56fbc83893a35eb0dd04b2b8e7a50c91e64e9493e398b5df4fb45fa2" dependencies = [ "cc", "libc", @@ -1749,9 +1730,9 @@ dependencies = [ [[package]] name = "parry3d" -version = "0.13.6" +version = "0.13.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "13d0bdaf533851feec5cba9af11cefcc753ecefba05f758cf6abe886086bc3f5" +checksum = "6ccba18a65dba56c08dadfa936e0c9efbc883b3a26dc77d2685f78be10f7667c" dependencies = [ "approx", "arrayvec", @@ -1824,7 +1805,7 @@ dependencies = [ "proc-macro2", "proc-macro2-diagnostics", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -1844,9 +1825,9 @@ checksum = "e3148f5046208a5d56bcfc03053e3ca6334e51da8dfb19b6cdc8b306fae3283e" [[package]] name = "pin-project-lite" -version = "0.2.13" +version = "0.2.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8afb450f006bf6385ca15ef45d71d2288452bc3683ce2e2cacc0d18e4be60b58" +checksum = "bda66fc9667c18cb2758a2ac84d1167245054bcf85d5d1aaa6923f45801bdd02" [[package]] name = "pin-utils" @@ -1889,7 +1870,7 @@ checksum = "af066a9c399a26e020ada66a034357a868728e72cd426f3adcd35f80d88d88c8" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", "version_check", "yansi", ] @@ -2040,9 +2021,9 @@ dependencies = [ [[package]] name = "redox_users" -version = "0.4.4" +version = "0.4.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a18479200779601e498ada4e8c1e1f50e3ee19deb0259c25825a98b5603b2cb4" +checksum = "bd283d9651eeda4b2a83a43c1c91b266c40fd76ecd39a50a8c630ae69dc72891" dependencies = [ "getrandom", "libredox", @@ -2066,19 +2047,19 @@ checksum = "5fddb4f8d99b0a2ebafc65a87a69a7b9875e4b1ae1f00db265d300ef7f28bccc" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] name = "regex" -version = "1.10.3" +version = "1.10.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b62dbe01f0b06f9d8dc7d49e05a0785f153b00b2c227856282f671e0318c9b15" +checksum = "c117dbdfde9c8308975b6a18d71f3f385c89461f7b3fb054288ecf2a2058ba4c" dependencies = [ "aho-corasick", - "memchr 2.7.1", + "memchr", "regex-automata 0.4.6", - "regex-syntax 0.8.2", + "regex-syntax 0.8.3", ] [[package]] @@ -2097,7 +2078,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "59b23e92ee4318893fa3fe3e6fb365258efbfe6ac6ab30f090cdcbb7aa37efa9" dependencies = [ "aho-corasick", - "memchr 2.7.1", + "memchr", "regex-syntax 0.7.5", ] @@ -2108,8 +2089,8 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "86b83b8b9847f9bf95ef68afb0b8e6cdb80f498442f5179a29fad448fcc1eaea" dependencies = [ "aho-corasick", - "memchr 2.7.1", - "regex-syntax 0.8.2", + "memchr", + "regex-syntax 0.8.3", ] [[package]] @@ -2126,9 +2107,9 @@ checksum = "dbb5fb1acd8a1a18b3dd5be62d25485eb770e05afb408a9627d14d451bae12da" [[package]] name = "regex-syntax" -version = "0.8.2" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c08c74e62047bb2de4ff487b251e4a92e24f48745648451635cec7d591162d9f" +checksum = "adad44e29e4c806119491a7f06f03de4d1af22c3a680dd47f1e6e179439d1f56" [[package]] name = "reqwest" @@ -2192,9 +2173,9 @@ dependencies = [ "either", "figment", "futures", - "indexmap 2.2.5", + "indexmap 2.2.6", "log", - "memchr 2.7.1", + "memchr", "multer", "num_cpus", "parking_lot", @@ -2224,11 +2205,11 @@ checksum = "896efc56a554a4e1fd03df1d8186c955b803aa652bbe8725a731f64e20f9cee8" dependencies = [ "devise", "glob", - "indexmap 2.2.5", + "indexmap 2.2.6", "proc-macro2", "quote", "rocket_http", - "syn 2.0.53", + "syn 2.0.58", "unicode-xid", "version_check", ] @@ -2244,9 +2225,9 @@ dependencies = [ "futures", "http", "hyper", - "indexmap 2.2.5", + "indexmap 2.2.6", "log", - "memchr 2.7.1", + "memchr", "pear", "percent-encoding", "pin-project-lite", @@ -2347,9 +2328,9 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" [[package]] name = "security-framework" -version = "2.9.2" +version = "2.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "05b64fb303737d99b81884b2c63433e9ae28abebe5eb5045dcdd175dc2ecf4de" +checksum = "770452e37cad93e0a50d5abc3990d2bc351c36d0328f86cefec2f2fb206eaef6" dependencies = [ "bitflags 1.3.2", "core-foundation", @@ -2360,9 +2341,9 @@ dependencies = [ [[package]] name = "security-framework-sys" -version = "2.9.1" +version = "2.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e932934257d3b408ed8f30db49d85ea163bfe74961f017f405b025af298f0c7a" +checksum = "41f3cc463c0ef97e11c3461a9d3787412d30e8e7eb907c79180c4a57bf7c04ef" dependencies = [ "core-foundation-sys", "libc", @@ -2385,14 +2366,14 @@ checksum = "7eb0b34b42edc17f6b7cac84a52a1c5f0e1bb2227e997ca9011ea3dd34e8610b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] name = "serde_json" -version = "1.0.114" +version = "1.0.115" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c5f09b1bd632ef549eaa9f60a1f8de742bdbc698e6cee2095fc84dde5f549ae0" +checksum = "12dc5c46daa8e9fdf4f5e71b6cf9a53f2487da0e86e55808e2d35539666497dd" dependencies = [ "itoa", "ryu", @@ -2542,7 +2523,7 @@ version = "0.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4564168c00635f88eaed410d5efa8131afa8d8699a612c80c455a0ba05c21045" dependencies = [ - "memchr 2.7.1", + "memchr", ] [[package]] @@ -2569,9 +2550,9 @@ dependencies = [ [[package]] name = "strsim" -version = "0.11.0" +version = "0.11.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5ee073c9e4cd00e28217186dbe12796d692868f432bf2e97ee73bed0c56dfa01" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" [[package]] name = "subtle" @@ -2592,9 +2573,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.53" +version = "2.0.58" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7383cd0e49fff4b6b90ca5670bfd3e9d6a733b3f90c686605aa7eec8c4996032" +checksum = "44cfb93f38070beee36b3fef7d4f5a16f27751d94b187b666a5cc5e9b0d30687" dependencies = [ "proc-macro2", "quote", @@ -2670,7 +2651,7 @@ checksum = "c8f546451eaa38373f549093fe9fd05e7d2bade739e2ddf834b9968621d60107" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -2690,7 +2671,7 @@ checksum = "c61f3ba182994efc43764a46c018c347bc492c79f024e705f46567b418f6d4f7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -2751,9 +2732,9 @@ checksum = "1f3ccbac311fea05f86f61904b462b55fb3df8837a366dfc601a0161d0532f20" [[package]] name = "tokio" -version = "1.36.0" +version = "1.37.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61285f6515fa018fb2d1e46eb21223fff441ee8db5d0f1435e8ab4f5cdb80931" +checksum = "1adbebffeca75fcfd058afa480fb6c0b81e165a0323f9c9d39c9697e37c46787" dependencies = [ "backtrace", "bytes", @@ -2775,7 +2756,7 @@ checksum = "5b8a1e28f2deaa14e508979454cb3a223b10b938b45af148bc0986de36f1923b" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -2840,7 +2821,7 @@ version = "0.22.9" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8e40bb779c5187258fd7aad0eb68cb8706a0a81fa712fbea808ab43c4b8374c4" dependencies = [ - "indexmap 2.2.5", + "indexmap 2.2.6", "serde", "serde_spanned", "toml_datetime", @@ -2884,7 +2865,7 @@ checksum = "34704c8d6ebcbc939824180af020566b01a7c01f80641264eba0999f6c2b6be7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -3087,7 +3068,7 @@ dependencies = [ "once_cell", "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", "wasm-bindgen-shared", ] @@ -3121,7 +3102,7 @@ checksum = "e94f17b526d0a461a191c78ea52bbce64071ed5c04c9ffe424dcb38f74171bb7" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", "wasm-bindgen-backend", "wasm-bindgen-shared", ] @@ -3364,7 +3345,7 @@ version = "0.6.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dffa400e67ed5a4dd237983829e66475f0a4a26938c4b04c21baede6262215b8" dependencies = [ - "memchr 2.7.1", + "memchr", ] [[package]] @@ -3414,7 +3395,7 @@ checksum = "9ce1b18ccd8e73a9321186f97e46f9f04b778851177567b1975109d26a08d2a6" dependencies = [ "proc-macro2", "quote", - "syn 2.0.53", + "syn 2.0.58", ] [[package]] @@ -3448,11 +3429,11 @@ dependencies = [ [[package]] name = "zstd" -version = "0.13.0" +version = "0.13.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bffb3309596d527cfcba7dfc6ed6052f1d39dfbd7c867aa2e865e4a449c10110" +checksum = "2d789b1514203a1120ad2429eae43a7bd32b90976a7bb8a05f7ec02fa88cc23a" dependencies = [ - "zstd-safe 7.0.0", + "zstd-safe 7.1.0", ] [[package]] @@ -3467,18 +3448,18 @@ dependencies = [ [[package]] name = "zstd-safe" -version = "7.0.0" +version = "7.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43747c7422e2924c11144d5229878b98180ef8b06cca4ab5af37afc8a8d8ea3e" +checksum = "1cd99b45c6bc03a018c8b8a86025678c87e55526064e38f9df301989dce7ec0a" dependencies = [ "zstd-sys", ] [[package]] name = "zstd-sys" -version = "2.0.9+zstd.1.5.5" +version = "2.0.10+zstd.1.5.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9e16efa8a874a0481a574084d34cc26fdb3b99627480f785888deb6386506656" +checksum = "c253a4914af5bafc8fa8c86ee400827e83cf6ec01195ec1f1ed8441bf00d65aa" dependencies = [ "cc", "pkg-config", diff --git a/crates/dies-cli/Cargo.toml b/crates/dies-cli/Cargo.toml index ada69329..c633effe 100644 --- a/crates/dies-cli/Cargo.toml +++ b/crates/dies-cli/Cargo.toml @@ -21,4 +21,4 @@ tracing-subscriber = { version = "0.3.18", features = ["json"] } tracing-appender = "0.2.3" nalgebra = "0.32.4" dies-protos = { version = "0.1.0", path = "../dies-protos" } -npy = "0.4.0" +dies-control = { version = "0.1.0", path = "../dies-control" } diff --git a/crates/dies-cli/src/executor.rs b/crates/dies-cli/src/executor.rs index 83f83188..b3598f71 100644 --- a/crates/dies-cli/src/executor.rs +++ b/crates/dies-cli/src/executor.rs @@ -6,12 +6,14 @@ use std::{ use anyhow::{Context, Result}; -use dies_core::PlayerCmd; +use dies_control::TeamController; +use dies_core::{PlayerCmd, WorldData}; use dies_python_rt::{PyRuntime, PyRuntimeConfig, RuntimeEvent, RuntimeMsg}; use dies_serial_client::{SerialClient, SerialClientConfig}; use dies_ssl_client::{VisionClient, VisionClientConfig}; use dies_webui::spawn_webui; use dies_world::{WorldConfig, WorldTracker}; +use nalgebra::Vector2; pub struct ExecutorConfig { pub py_config: Option, @@ -49,6 +51,8 @@ pub async fn run(config: ExecutorConfig) -> Result<()> { }; let robot_ids = config.robot_ids; + let mut team_controller = TeamController::new(); + // Launch webui let (webui_sender, mut webui_cmd_rx, webui_handle) = if config.webui { let (webui_sender, webui_cmd_rx, webui_handle) = spawn_webui(); @@ -62,6 +66,7 @@ pub async fn run(config: ExecutorConfig) -> Result<()> { let fail: HashMap = HashMap::new(); let mut robots: HashSet = HashSet::new(); + let mut world_data: Option = None; loop { let runtime_msg_fut = async { if let Some(runtime) = &mut runtime { @@ -96,7 +101,8 @@ pub async fn run(config: ExecutorConfig) -> Result<()> { match vision_msg { Ok(vision_msg) => { tracker.update_from_vision(&vision_msg); - if let Some(world_data) = tracker.get() { + if let Some(new_world_data) = tracker.get() { + world_data = Some(new_world_data.clone()); // Failsafe: if one of our robots is not detected, we send stop to runtime // for player in world_data.own_players.iter() { // if SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_secs() as f64 - player.timestamp > 0.5 { @@ -138,12 +144,14 @@ pub async fn run(config: ExecutorConfig) -> Result<()> { // Send update to runtime if let Some(runtime) = &mut runtime { - let _ = runtime.send(&RuntimeMsg::World(world_data.clone())).await; + if let Some(world_data) = &world_data { + let _ = runtime.send(&RuntimeMsg::World(world_data.clone())).await; + } } // Send update to webui if let Some(ref webui_sender) = webui_sender { - if let Err(err) = webui_sender.send(world_data) { + if let Err(err) = webui_sender.send(new_world_data) { tracing::error!("Failed to send world data to webui: {}", err); } } @@ -171,6 +179,11 @@ pub async fn run(config: ExecutorConfig) -> Result<()> { tracing::error!("Received player cmd but serial is not configured"); } } + Ok(RuntimeEvent::PlayerPosCmd(cmd)) => { + let rid = *robot_ids.get(&cmd.id).unwrap_or(&0); + robots.insert(rid); + team_controller.set_target_pos(cmd.id, Vector2::new(cmd.x, cmd.y)); + } Ok(RuntimeEvent::Debug { msg }) => { tracing::debug!("Runtime debug: {}", msg); } diff --git a/crates/dies-cli/src/main.rs b/crates/dies-cli/src/main.rs index 45eb37d5..a0cb825d 100644 --- a/crates/dies-cli/src/main.rs +++ b/crates/dies-cli/src/main.rs @@ -56,7 +56,7 @@ struct Args { #[clap(long, default_value = "224.5.23.2:10006")] vision_addr: SocketAddr, - #[clap(long, default_value = "info")] + #[clap(long, default_value = "debug")] log_level: String, #[clap(long, default_value = "auto")] diff --git a/crates/dies-control/Cargo.toml b/crates/dies-control/Cargo.toml new file mode 100644 index 00000000..2d77444a --- /dev/null +++ b/crates/dies-control/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "dies-control" +version = "0.1.0" +edition.workspace = true + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +dies-core = { version = "0.1.0", path = "../dies-core" } +log = "0.4.20" +nalgebra = "0.32.4" +num-traits = "0.2.18" diff --git a/crates/dies-control/src/lib.rs b/crates/dies-control/src/lib.rs new file mode 100644 index 00000000..6bbe7e36 --- /dev/null +++ b/crates/dies-control/src/lib.rs @@ -0,0 +1,7 @@ +mod pid; +mod player_controller; +mod team_controller; +mod variable; + +pub use player_controller::PlayerController; +pub use team_controller::TeamController; diff --git a/crates/dies-control/src/pid.rs b/crates/dies-control/src/pid.rs new file mode 100644 index 00000000..20dbe01b --- /dev/null +++ b/crates/dies-control/src/pid.rs @@ -0,0 +1,85 @@ +use std::time::Instant; + +use crate::variable::Variable; + +pub struct PID { + kp: f32, + ki: f32, + kd: f32, + setpoint: Option, + integral: T, + last_error: Option, + last_time: Instant, +} + +impl PID +where + T: Variable, +{ + pub fn new(kp: f32, ki: f32, kd: f32) -> Self { + Self { + kp, + ki, + kd, + setpoint: None, + integral: T::zero(), + last_error: None, + last_time: Instant::now(), + } + } + + pub fn set_setpoint(&mut self, setpoint: T) { + self.setpoint = Some(setpoint); + } + + pub fn update(&mut self, input: T) -> T { + if let Some(setpoint) = self.setpoint { + let error = setpoint - input; + let dt = self.last_time.elapsed().as_secs_f32(); + self.last_time = Instant::now(); + + self.integral += error * dt; + + let last_error = self.last_error.unwrap_or(error); + let derivative = (error - last_error) / dt; + self.last_error = Some(error); + + error * self.kp + self.integral * self.ki + derivative * self.kd + } else { + T::zero() + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use nalgebra::Vector2; + + #[test] + fn test_pid_update_without_setpoint() { + let mut pid = PID::::new(1.0, 1.0, 1.0); + + let output = pid.update(1.0); + + assert_eq!(output, 0.0); + } + + #[test] + fn test_pid_update_with_setpoint() { + let mut pid = PID::::new(1.0, 1.0, 1.0); + pid.set_setpoint(2.0); + + let output = pid.update(1.0); + assert_ne!(output, 0.0); + } + + #[test] + fn test_update_with_vector() { + let mut pid = PID::new(1.0, 1.0, 1.0); + pid.set_setpoint(Vector2::new(2.0, 2.0)); + + let output = pid.update(Vector2::new(1.0, 1.0)); + assert_ne!(output, Vector2::zeros()); + } +} diff --git a/crates/dies-control/src/player_controller.rs b/crates/dies-control/src/player_controller.rs new file mode 100644 index 00000000..eda38fc8 --- /dev/null +++ b/crates/dies-control/src/player_controller.rs @@ -0,0 +1,135 @@ +use crate::pid::PID; +use dies_core::{FieldGeometry, GameState, PlayerCmd, PlayerData}; +use nalgebra::Vector2; +use std::time::{Duration, Instant}; + +pub struct PlayerController { + id: u32, + position_pid: PID>, + heading_pid: PID, + kick_timer: Option, + last_pos: Vector2, + last_orientation: f32, + frame_missings: u32, +} + +impl PlayerController { + /// Create a new player controller with the given ID. + pub fn new(id: u32) -> Self { + let mut heading_pid = PID::new(2.0, 0.002, 0.0); + // Fix the heading for now + heading_pid.set_setpoint(0.0); + Self { + id, + position_pid: PID::new(0.7, 0.0, 0.0), + heading_pid, + kick_timer: None, + last_pos: Vector2::new(0.0, 0.0), + last_orientation: 0.0, + frame_missings: 0, + } + } + + /// Increment the missing frame count, return whether it's missing for too long + pub fn increment_frame_missings(&mut self) -> bool { + let threshold: u32 = 100; // for example 1.5s + self.frame_missings += 1; + self.frame_missings > threshold + } + + /// Set the target position for the player. + pub fn set_target_pos(&mut self, setpoint: Vector2) { + self.position_pid.set_setpoint(setpoint); + } + + /// Keep track of current position from the frame + pub fn update_current_pos(&mut self, state: &PlayerData) { + self.frame_missings = 0; + self.last_pos = state.position; + self.last_orientation = state.orientation; + } + + /// Update the controller with the current state of the player. + pub fn update( + &mut self, + state: &PlayerData, + is_dribbling: bool, + is_kicking: bool, + ) -> PlayerCmd { + let mut cmd: PlayerCmd = PlayerCmd { + id: self.id, + ..Default::default() + }; + self.set_speed(&mut cmd); + + self.handle_dribbling(&mut cmd, is_dribbling); + self.handle_kicking(&mut cmd, is_kicking); + + cmd + } + + /// set speed of the robot based on pids + pub fn set_speed(&mut self, cmd: &mut PlayerCmd) { + let pos_u = self.position_pid.update(self.last_pos); + let head_u = self.heading_pid.update(self.last_orientation); + let local_u = rotate_vector(Vector2::new(pos_u.x, pos_u.y), -self.last_orientation); + cmd.sx = local_u.x; + cmd.sy = local_u.y; + cmd.w = head_u; + } + + /// handle the dribbling. + pub fn handle_dribbling(&mut self, cmd: &mut PlayerCmd, is_dribbling: bool) { + // for example, 60 rad/s + if is_dribbling { + cmd.dribble_speed = 60.0; + } + } + + /// handle the kicking. + pub fn handle_kicking(&mut self, cmd: &mut PlayerCmd, is_kicking: bool) { + if !is_kicking { + return; + } + if self.kick_timer.is_none() { + self.kick_timer = Some(Instant::now()); + cmd.arm = true; + } else { + let elapsed = self.kick_timer.unwrap().elapsed(); + if elapsed >= Duration::from_millis(1000) { + cmd.kick = true; + self.kick_timer = None; + } + } + } + + /// speed limit check + pub fn speed_limit_check(&self, cmd: &mut PlayerCmd, velocity: Vector2) { + // for example 1500 mm/s + let max_speed = 1500.0; + let speed = (velocity.x.powi(2) + velocity.y.powi(2)).sqrt(); + if speed > max_speed { + let scale = max_speed / speed; + cmd.sx *= scale; + cmd.sy *= scale; + } + } + + /// out of bound detection + pub fn out_of_bound_detection( + &self, + cmd: &mut PlayerCmd, + game_state: GameState, + field_geometry: FieldGeometry, + ) { + // TODO: + // what is the excepted behavior of this?? I think pid will drag it back when + // it is out of bound, and since pid is the only way to control the robot, it may be unnecessary + // to do this check. + } +} + +fn rotate_vector(v: Vector2, angle: f32) -> Vector2 { + let rot = nalgebra::Rotation2::new(angle); + rot * v +} diff --git a/crates/dies-control/src/team_controller.rs b/crates/dies-control/src/team_controller.rs new file mode 100644 index 00000000..d065009b --- /dev/null +++ b/crates/dies-control/src/team_controller.rs @@ -0,0 +1,124 @@ +use std::collections::HashMap; + +use crate::player_controller::PlayerController; +use dies_core::GameState; +use dies_core::{PlayerCmd, WorldData}; +use nalgebra::{Vector2, Vector3}; +use std::cell::RefCell; +use std::rc::Rc; + +pub struct HaltController { + players: Rc>>, +} +impl HaltController { + /// everyone stops, notice that this only interrupts the players, so if the game + ///recovers the players will head to their original goal. + pub fn new(players: Rc>>) -> Self { + Self { players } + } + + pub fn update(&mut self) -> Vec { + self.players + .borrow() + .iter() + .map(|(id, _)| PlayerCmd::zero(*id)) + .collect() + } +} + +pub struct StopController { + players: Rc>>, +} +impl StopController { + /// After generating the commands, everyone's speed is capped at 1.5m/s. + /// and if the player is less than 500mm from the ball, the goal is set to the point 500mm away from the ball. + /// and in the opposite of the ball's speed direction. + /// dribbler is also stopped. + /// we only issue commands for players in the current frame, + /// and we assume the world covers every player's state in a long enough time. + pub fn new(players: Rc>>) -> Self { + Self { players } + } + + pub fn update(&mut self, world_data: WorldData) -> Vec { + let mut commands = Vec::new(); + let ball_pos = world_data.ball.clone().unwrap().position; + let ball_speed = world_data.ball.clone().unwrap().velocity; + let ball_pos_v2 = Vector2::new(ball_pos.x, ball_pos.y); + let ball_speed_v2 = Vector2::new(ball_speed.x, ball_speed.y); + let ball_speed_norm = ball_speed_v2.norm(); + let mut players_borrow_mut = self.players.borrow_mut(); + + for playerData in world_data.own_players { + let id = playerData.id; + let player_controller = players_borrow_mut.get_mut(&id).unwrap(); + + if ball_speed_norm > 0.1 { + let ball_speed_dir: Vector2 = ball_speed_v2 / ball_speed_norm; + let target_pos: Vector2 = ball_pos_v2 - ball_speed_dir * 500.0; + player_controller.set_target_pos(target_pos); + } else { + let target_pos_dir: Vector2 = (playerData.position - ball_pos_v2).normalize(); + let target_pos: Vector2 = ball_pos_v2 + target_pos_dir * 500.0; + player_controller.set_target_pos(target_pos); + } + let mut cmd = player_controller.update(&playerData, false, false); + let player_speed = (cmd.sx * cmd.sx + cmd.sy * cmd.sy).sqrt(); + if player_speed > 1500.0 { + cmd.sx = cmd.sx * 1500.0 / player_speed; + cmd.sy = cmd.sy * 1500.0 / player_speed; + } + commands.push(cmd); + } + commands + } +} + +pub struct TeamController { + players: Rc>>, + halt_controller: HaltController, + stop_controller: StopController, +} + +impl TeamController { + /// Create a new team controller. + pub fn new() -> Self { + let players = Rc::new(RefCell::new(HashMap::new())); + Self { + halt_controller: HaltController::new(Rc::clone(&players)), + stop_controller: StopController::new(Rc::clone(&players)), + players, + } + } + + /// Set the target position for the player with the given ID. + pub fn set_target_pos(&mut self, id: u32, setpoint: Vector2) { + let mut players = self.players.borrow_mut(); + if let Some(player) = players.get_mut(&id) { + player.set_target_pos(setpoint); + } + } + + /// Update the controllers with the current state of the players. + pub fn update(&mut self, world_data: WorldData) -> Vec { + let mut players = self.players.borrow_mut(); + let tracked_ids: Vec = world_data.own_players.iter().map(|p| p.id).collect(); + for player in &world_data.own_players { + let id = player.id; + let player_controller = players + .entry(id) + .or_insert_with(|| PlayerController::new(id)); + player_controller.update_current_pos(&player); + } + for (id, player_controller) in players.iter_mut() { + if !tracked_ids.contains(id) { + player_controller.increment_frame_missings(); + } + } + match world_data.current_game_state { + GameState::Halt | GameState::Timeout => self.halt_controller.update(), + GameState::Stop => self.stop_controller.update(world_data), + _ => Vec::new(), + } + } +} diff --git a/crates/dies-control/src/variable.rs b/crates/dies-control/src/variable.rs new file mode 100644 index 00000000..64a18450 --- /dev/null +++ b/crates/dies-control/src/variable.rs @@ -0,0 +1,34 @@ +use std::ops::{Add, AddAssign, Div, Mul, Sub}; + +use nalgebra::{Vector2, Vector3}; + +/// A trait for types that can be used as variables in a PID controller. +pub trait Variable: + Copy + + Add + + AddAssign + + Mul + + Sub + + Div +{ + /// Returns the zero value for this type. + fn zero() -> Self; +} + +impl Variable for f32 { + fn zero() -> Self { + 0.0 + } +} + +impl Variable for Vector2 { + fn zero() -> Self { + Vector2::zeros() + } +} + +impl Variable for Vector3 { + fn zero() -> Self { + Vector3::zeros() + } +} diff --git a/crates/dies-core/src/lib.rs b/crates/dies-core/src/lib.rs index b54d359e..70b578a8 100644 --- a/crates/dies-core/src/lib.rs +++ b/crates/dies-core/src/lib.rs @@ -46,3 +46,12 @@ impl PlayerCmd { } } } + +/// A position command to one of our players +#[derive(Clone, Copy, Debug, Serialize, Deserialize)] +pub struct PlayerPosCmd { + pub id: u32, + pub x: f32, + pub y: f32, + pub orientation: f32, +} diff --git a/crates/dies-python-rt/src/messages.rs b/crates/dies-python-rt/src/messages.rs index 5a2a025e..0a5f7cf4 100644 --- a/crates/dies-python-rt/src/messages.rs +++ b/crates/dies-python-rt/src/messages.rs @@ -1,4 +1,4 @@ -use dies_core::{PlayerCmd, WorldData}; +use dies_core::{PlayerCmd, PlayerPosCmd, WorldData}; use serde::{Deserialize, Serialize}; /// A message to the runtime @@ -15,6 +15,8 @@ pub enum RuntimeMsg { #[derive(Debug, Clone, Deserialize)] #[serde(tag = "type")] pub enum RuntimeEvent { + /// A position command to one of our players + PlayerPosCmd(PlayerPosCmd), /// A command to one of our players PlayerCmd(PlayerCmd), /// A debug message diff --git a/docs/src/dev.md b/docs/src/dev.md index c055a308..5eb68195 100644 --- a/docs/src/dev.md +++ b/docs/src/dev.md @@ -57,7 +57,7 @@ Throughout the codebase, the [`anyhow`](https://docs.rs/anyhow/latest/anyhow/) c Logging is really important for Dies, as it is the only way to debug issues that occured during a match. Logs should contain all the information needed to replay matches, including all the data we receive from the vision system, the world state, and the commands we send to the robots. -Dies uses the [`log`](https://docs.rs/log/latest/log/) crate for logging. The following log levels should be used: +Dies uses the [`tracing`](https://docs.rs/tracing/latest/tracing/) crate for logging. The following log levels should be used: - `error`: should be used only for fatal errors, which cause the executor to stop, ie. errors should only be logged in the executor - `warn`: should be used for unexpected events or recoverable errors, which do not cause the executor to stop and do not get propagated up diff --git a/py/dies-py/src/dies_py/bridge.py b/py/dies-py/src/dies_py/bridge.py index 7d95ca86..e4c5290c 100644 --- a/py/dies-py/src/dies_py/bridge.py +++ b/py/dies-py/src/dies_py/bridge.py @@ -33,10 +33,8 @@ def init(): recv_thread = Thread(target=_recv_loop, daemon=True) recv_thread.start() - # Set signal handler for SIGTERM, SIGINT, and SIGHUP - signal.signal(signal.SIGTERM, _sig_handler) + # Set signal handler for SIGINT signal.signal(signal.SIGINT, _sig_handler) - signal.signal(signal.SIGHUP, _sig_handler) def next_world() -> bool: @@ -78,22 +76,17 @@ def should_stop(): def _recv_loop(): global _world_state, _world_update_ev, _stop, _sock + buf = bytearray(4096) while not _stop: - try: - data = _sock.recv(4096) - msg: Msg = msg_dec.decode(data) - if isinstance(msg, Term): - _world_update_ev.set() - _stop = True - break - elif isinstance(msg, World): - _world_state = msg - _world_update_ev.set() - except socket.timeout: - pass - except msgspec.DecodeError: - sys.stderr.write("Error decoding message\n") - sys.stderr.flush() + n = _sock.recv_into(buf) + msg: Msg = msg_dec.decode(buf[:n]) + if isinstance(msg, Term): + _world_update_ev.set() + _stop = True + break + elif isinstance(msg, World): + _world_state = msg + _world_update_ev.set() def _sig_handler(signum, frame): diff --git a/py/dies-py/src/dies_py/messages.py b/py/dies-py/src/dies_py/messages.py index 17e347fc..6109c0c3 100644 --- a/py/dies-py/src/dies_py/messages.py +++ b/py/dies-py/src/dies_py/messages.py @@ -79,6 +79,13 @@ class PlayerCmd(Struct, tag=True): kick: bool = False +class PlayerPosCmd(Struct, tag=True): + id: int + x: float + y: float + orientation: float = 0.0 + + class Debug(Struct, tag=True): message: str @@ -87,4 +94,4 @@ class Ping(Struct, tag=True): pass -Cmd = Union[Debug, PlayerCmd, Ping] +Cmd = Union[Debug, PlayerCmd, Ping, PlayerPosCmd] diff --git a/webui/src/App.svelte b/webui/src/App.svelte index 9e70f2f3..48445b8a 100644 --- a/webui/src/App.svelte +++ b/webui/src/App.svelte @@ -243,6 +243,39 @@ data.push(velocityMagnitude); velocityChart.update(); } + if (state?.own_players.length > 0) { + let selectedPlayer = state.own_players[0]; + if (selectedPlayerId === null) { + selectedPlayerId = state.own_players[0].id; + } else { + const _selectedPlayer = state.own_players.find( + (player) => player.id === selectedPlayerId + ); + if (_selectedPlayer) { + selectedPlayer = _selectedPlayer; + } else { + selectedPlayerId = selectedPlayer.id; + } + } + + const velocityMagnitude = Math.sqrt( + selectedPlayer.velocity[0] ** 2 + selectedPlayer.velocity[1] ** 2 + ); + if (firstTs === null) { + firstTs = selectedPlayer.timestamp; + } + const ts = selectedPlayer.timestamp - firstTs; + + const labels = velocityChart.data.labels!; + const data = velocityChart.data.datasets[0].data; + if (data.length > 100) { + labels.shift(); + data.shift(); + } + labels.push(ts); + data.push(velocityMagnitude); + velocityChart.update(); + } }; @@ -250,6 +283,9 @@
+
+ +