Torch model (#2452)
* refactor draw model * rebase master * correct valid_len * rename function * rename variables * white space * rebase to master * e16c13ac-927d-455e-ae0a-81b482a2c787 * start rewriting * save proress * compiles! * oops * many fixes * seems to work * fix desires * finally cleaned * wrong std for ll * dont pulse none * compiles! * ready to test * WIP does not compile * compiles * various fixes * does something! * full 3d * not needed * draw up to 100m * fix segfault * wrong sign * fix flicker * add road edges * finish v2 packet * Added pytorch supercombo * fix rebase * no more keras * Hacky solution to the NCHW/NHWC incompatibility between SNPE and our frame data * dont break dmonitoringd, final model 229e3ce1-7259-412b-85e6-cc646d70f1d8/430 * fix hack * Revert "fix hack" This reverts commit 5550fc01a7881d065a5eddbbb42dac55ef7ec36c. * Removed axis permutation hack * Folded padding layers into conv layers * Removed the last pad layer from the dlc * Revert "Removed the last pad layer from the dlc" This reverts commit b85f24b9e1d04abf64e85901a7ff49e00d82020a. * Revert "Folded padding layers into conv layers" This reverts commit b8d1773e4e76dea481acebbfad6a6235fbb58463. * vision model: 5034ac8b-5703-4a49-948b-11c064d10880/780 temporal model: 229e3ce1-7259-412b-85e6-cc646d70f1d8/430 with permute + pool opt * fix ui drawing with clips * ./compile_torch.py 5034ac8b-5703-4a49-948b-11c064d10880/780 dfcd2375-81d8-49df-95bf-1d2d6ad86010/450 with variable history length * std::clamp * not sure how this compiled before * 2895ace6-a296-47ac-86e6-17ea800a74e5/550 * db090195-8810-42de-ab38-bb835d775d87/601 * 5m is very little * onnx runner * add onnxruntime to pipfile * run in real time without using the whole CPU * bump cereal; * add stds * set road edge opacity based on stddev * don't access the model packet in paint * convert mat.h to a c++ header file (#2499) * update tests * safety first Co-authored-by: deanlee <deanlee3@gmail.com> Co-authored-by: mitchell <mitchell@comma.ai> Co-authored-by: Comma Device <device@comma.ai> Co-authored-by: George Hotz <george@comma.ai> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>pull/2530/head
HaraldSchafer
GitHub
parent
faf8b1fd82
commit
08846b5c0e
|
|
@ -1,5 +1,6 @@
|
|||
*.keras filter=lfs diff=lfs merge=lfs -text
|
||||
*.dlc filter=lfs diff=lfs merge=lfs -text
|
||||
*.onnx filter=lfs diff=lfs merge=lfs -text
|
||||
*.pb filter=lfs diff=lfs merge=lfs -text
|
||||
*.bin filter=lfs diff=lfs merge=lfs -text
|
||||
*.apk filter=lfs diff=lfs merge=lfs -text
|
||||
|
|
|
|||
1
Pipfile
1
Pipfile
|
|
@ -115,6 +115,7 @@ pycryptodome = "*"
|
|||
"Jinja2" = "*"
|
||||
PyJWT = "*"
|
||||
pyserial = "*"
|
||||
onnxruntime = "*"
|
||||
|
||||
[requires]
|
||||
python_version = "3.8"
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
{
|
||||
"_meta": {
|
||||
"hash": {
|
||||
"sha256": "58d0a6326cce8f5b34dc12b07aa239d737f8b290a2fdd85b35b0daf8a086036d"
|
||||
"sha256": "6f3b68a47ca0d9acc1f6519dd90ef055ac7a3816c5e71c689c34871b8a65e76d"
|
||||
},
|
||||
"pipfile-spec": 6,
|
||||
"requires": {
|
||||
|
|
@ -34,10 +34,10 @@
|
|||
},
|
||||
"certifi": {
|
||||
"hashes": [
|
||||
"sha256:5930595817496dd21bb8dc35dad090f1c2cd0adfaf21204bf6732ca5d8ee34d3",
|
||||
"sha256:8fc0819f1f30ba15bdb34cceffb9ef04d99f420f68eb75d901e9560b8749fc41"
|
||||
"sha256:1f422849db327d534e3d0c5f02a263458c3955ec0aae4ff09b95f195c59f4edd",
|
||||
"sha256:f05def092c44fbf25834a51509ef6e631dc19765ab8a57b4e7ab85531f0a9cf4"
|
||||
],
|
||||
"version": "==2020.6.20"
|
||||
"version": "==2020.11.8"
|
||||
},
|
||||
"cffi": {
|
||||
"hashes": [
|
||||
|
|
@ -392,6 +392,27 @@
|
|||
"index": "pypi",
|
||||
"version": "==1.19.4"
|
||||
},
|
||||
"onnxruntime": {
|
||||
"hashes": [
|
||||
"sha256:2c31ad5bd9391c00773e05c3e6410f31e2b65ff280f5a1fe6f575c4c5d46a9f1",
|
||||
"sha256:651f1d7674b9c6c44209f1a0492b91f2d996b46392de45fca87e3919f5d92a0a",
|
||||
"sha256:700a57ac232aecdf1bf756113fb5693b4f7196d0a9898f456baa68ef30ea2656",
|
||||
"sha256:74935ad47b19f989c21788371e77b1d881493e65205192906647671402d2286a",
|
||||
"sha256:8d7d1f0990386ee166b366288b6bb99961362e4574574bf892ba9c27dae435fb",
|
||||
"sha256:9ce8b166c7eba95fde87f87813804b7bc143c74d3b7c6b1f6010433424885dc8",
|
||||
"sha256:b8e6c97a7691174bbe37e2be922857d2d626b6be6747d9e9a4a8b5d5bc3ae3f3",
|
||||
"sha256:bf43878c15ba6bb4a919e66057ecd2c5d6870dc7bfbd3e3a446d1ed481f4b023",
|
||||
"sha256:c89026fe51b7687a2cc799151b56c78d514821e2721186a2be146372775cc3b9",
|
||||
"sha256:c8b245cc004f3a27c8857f1a8b28bb70c3ec3812c229bc74ee3a9a3130c9408a",
|
||||
"sha256:cf895d4c2eaadd0ce86c97323ff4123bc8744e0283f95c272d291d6e645f31b1",
|
||||
"sha256:edfac621a44bf0509c0ea767cf59bed712036f4fd0fc678dd68fa5a6fc07602f",
|
||||
"sha256:eea54734b4ff22847084c48354677acfa0155b4468b427076f5063af434fe6f4",
|
||||
"sha256:f164d5ef9bfeb72feff4095a341c33366db309afb67b83784451e249782da990",
|
||||
"sha256:fc481e9413d987fdf10aabf4eaa37f6c2e8752d61e8758b3a430e87e74d39140"
|
||||
],
|
||||
"index": "pypi",
|
||||
"version": "==1.5.2"
|
||||
},
|
||||
"pillow": {
|
||||
"hashes": [
|
||||
"sha256:006de60d7580d81f4a1a7e9f0173dc90a932e3905cc4d47ea909bc946302311a",
|
||||
|
|
@ -426,6 +447,29 @@
|
|||
"index": "pypi",
|
||||
"version": "==8.0.1"
|
||||
},
|
||||
"protobuf": {
|
||||
"hashes": [
|
||||
"sha256:0bba42f439bf45c0f600c3c5993666fcb88e8441d011fad80a11df6f324eef33",
|
||||
"sha256:1e834076dfef9e585815757a2c7e4560c7ccc5962b9d09f831214c693a91b463",
|
||||
"sha256:339c3a003e3c797bc84499fa32e0aac83c768e67b3de4a5d7a5a9aa3b0da634c",
|
||||
"sha256:361acd76f0ad38c6e38f14d08775514fbd241316cce08deb2ce914c7dfa1184a",
|
||||
"sha256:3dee442884a18c16d023e52e32dd34a8930a889e511af493f6dc7d4d9bf12e4f",
|
||||
"sha256:4d1174c9ed303070ad59553f435846a2f877598f59f9afc1b89757bdf846f2a7",
|
||||
"sha256:5db9d3e12b6ede5e601b8d8684a7f9d90581882925c96acf8495957b4f1b204b",
|
||||
"sha256:6a82e0c8bb2bf58f606040cc5814e07715b2094caeba281e2e7d0b0e2e397db5",
|
||||
"sha256:8c35bcbed1c0d29b127c886790e9d37e845ffc2725cc1db4bd06d70f4e8359f4",
|
||||
"sha256:91c2d897da84c62816e2f473ece60ebfeab024a16c1751aaf31100127ccd93ec",
|
||||
"sha256:9c2e63c1743cba12737169c447374fab3dfeb18111a460a8c1a000e35836b18c",
|
||||
"sha256:9edfdc679a3669988ec55a989ff62449f670dfa7018df6ad7f04e8dbacb10630",
|
||||
"sha256:c0c5ab9c4b1eac0a9b838f1e46038c3175a95b0f2d944385884af72876bd6bc7",
|
||||
"sha256:c8abd7605185836f6f11f97b21200f8a864f9cb078a193fe3c9e235711d3ff1e",
|
||||
"sha256:d69697acac76d9f250ab745b46c725edf3e98ac24763990b24d58c16c642947a",
|
||||
"sha256:df3932e1834a64b46ebc262e951cd82c3cf0fa936a154f0a42231140d8237060",
|
||||
"sha256:e7662437ca1e0c51b93cadb988f9b353fa6b8013c0385d63a70c8a77d84da5f9",
|
||||
"sha256:f68eb9d03c7d84bd01c790948320b768de8559761897763731294e3bc316decb"
|
||||
],
|
||||
"version": "==3.13.0"
|
||||
},
|
||||
"psutil": {
|
||||
"hashes": [
|
||||
"sha256:01bc82813fbc3ea304914581954979e637bcc7084e59ac904d870d6eb8bb2bc7",
|
||||
|
|
@ -867,11 +911,11 @@
|
|||
},
|
||||
"azure-cli-core": {
|
||||
"hashes": [
|
||||
"sha256:703774a8101e06e83c21b6714d41723b9721bd11ae37d7188c3a7ce90ecb11c3",
|
||||
"sha256:a748173ed646fd227f3b76281448c722e9eca9de9a238fe935b6078fb878529c"
|
||||
"sha256:e9c977ddf17867f1074ee33bde49179b787914123e1ddd7b73e3dc56390750ee",
|
||||
"sha256:fe259a3ef8c842180e09d288182596a1da7dd01cb07d5ec1a3b4d5e618fd7db7"
|
||||
],
|
||||
"index": "pypi",
|
||||
"version": "==2.14.0"
|
||||
"version": "==2.14.2"
|
||||
},
|
||||
"azure-cli-telemetry": {
|
||||
"hashes": [
|
||||
|
|
@ -890,11 +934,11 @@
|
|||
},
|
||||
"azure-core": {
|
||||
"hashes": [
|
||||
"sha256:621b53271f7988b766f8a7d7f7a2c44241e3d2c1d8db13e68089d6da6241748e",
|
||||
"sha256:be23d411e19874f375c2ef0327c452be10b1e9a1023ac6afe334598f2920136b"
|
||||
"sha256:a4db9e64fc0de9d19d004d6b3b908459aa7d9cb6730734c485f738cb56a1ef27",
|
||||
"sha256:ef8ae93a2ce8b595f231395579be11aadc1838168cbc2582e2d0bbd8b15c461f"
|
||||
],
|
||||
"index": "pypi",
|
||||
"version": "==1.8.2"
|
||||
"version": "==1.9.0"
|
||||
},
|
||||
"azure-mgmt-core": {
|
||||
"hashes": [
|
||||
|
|
@ -995,18 +1039,18 @@
|
|||
},
|
||||
"boto3": {
|
||||
"hashes": [
|
||||
"sha256:74c6c1757178b08cf7d43238473b8557342f007491d435b2beb312d0fe67c62f",
|
||||
"sha256:f1cad6493686f6ad5b84a671cea86401330ab3992c9b427833741c9c028f2059"
|
||||
"sha256:60cc37e027d8911f4890275bcd8d1e3f9f5bdb18b3506641a343ae7e60a1d41a",
|
||||
"sha256:904d2f1935241c4437781769f9c0d90826470c59eef0d62ea7df4aaf63295d7c"
|
||||
],
|
||||
"index": "pypi",
|
||||
"version": "==1.16.12"
|
||||
"version": "==1.16.15"
|
||||
},
|
||||
"botocore": {
|
||||
"hashes": [
|
||||
"sha256:bb14ea33b32b831262610ed5525770c49da42d1a6cbbc1839d11e6a05bad96fc",
|
||||
"sha256:d0a39209551381752ca5257ae9b3eef53e7fe7eb16ccb9854fb1b14bc3d43519"
|
||||
"sha256:4e9dc37fb3cc47425c6480dc22999d556ca3cf71714f2937df0fc3db2a7f6581",
|
||||
"sha256:a2d789c8bed5bf1165cc57c95e2db1e74ec50508beb770a89f7c89bc68523281"
|
||||
],
|
||||
"version": "==1.19.12"
|
||||
"version": "==1.19.15"
|
||||
},
|
||||
"cachetools": {
|
||||
"hashes": [
|
||||
|
|
@ -1018,10 +1062,10 @@
|
|||
},
|
||||
"certifi": {
|
||||
"hashes": [
|
||||
"sha256:5930595817496dd21bb8dc35dad090f1c2cd0adfaf21204bf6732ca5d8ee34d3",
|
||||
"sha256:8fc0819f1f30ba15bdb34cceffb9ef04d99f420f68eb75d901e9560b8749fc41"
|
||||
"sha256:1f422849db327d534e3d0c5f02a263458c3955ec0aae4ff09b95f195c59f4edd",
|
||||
"sha256:f05def092c44fbf25834a51509ef6e631dc19765ab8a57b4e7ab85531f0a9cf4"
|
||||
],
|
||||
"version": "==2020.6.20"
|
||||
"version": "==2020.11.8"
|
||||
},
|
||||
"cffi": {
|
||||
"hashes": [
|
||||
|
|
@ -1225,11 +1269,11 @@
|
|||
},
|
||||
"elasticsearch": {
|
||||
"hashes": [
|
||||
"sha256:5e08776fbb30c6e92408c7fa8c37d939210d291475ae2f364f0497975918b6fe",
|
||||
"sha256:8c7e2374f53ee1b891ff2804116e0c7fb517585d6d5788ba668686bbc9d82e2d"
|
||||
"sha256:9053ca99bc9db84f5d80e124a79a32dfa0f7079b2112b546a03241c0dbeda36d",
|
||||
"sha256:9a21bfa7dc6a0b0dc142088bd653d8ce5ab284b4f7a3ded716185adf5276a7fe"
|
||||
],
|
||||
"index": "pypi",
|
||||
"version": "==7.9.1"
|
||||
"version": "==7.10.0"
|
||||
},
|
||||
"entrypoints": {
|
||||
"hashes": [
|
||||
|
|
@ -1384,6 +1428,7 @@
|
|||
"sha256:7fda62846ef8d86caf06bd1ecfddcae2c7e59479a4ee28808120e170064d36cc",
|
||||
"sha256:85e56ab125b35b1373205b3802f58119e70ffedfe0d7e2821999126058f7c44f",
|
||||
"sha256:88f2a102cbc67e91f42b4323cec13348bf6255b25f80426088079872bd4f3c5c",
|
||||
"sha256:89add4f4cda9546f61cb8a6988bc5b22101dd8ca4af610dff6f28105d1f78695",
|
||||
"sha256:8cf67b8493bff50fa12b4bc30ab40ce1f1f216eb54145962b525852959b0ab3d",
|
||||
"sha256:a8c84db387907e8d800c383e4c92f39996343adedf635ae5206a684f94df8311",
|
||||
"sha256:abaf30d18874310d4439a23a0afb6e4b5709c4266966401de7c4ae345cc810ee",
|
||||
|
|
@ -1395,6 +1440,7 @@
|
|||
"sha256:c89510381cbf8c8317e14e747a8b53988ad226f0ed240824064a9297b65f921d",
|
||||
"sha256:d386630af995fd4de225d550b6806507ca09f5a650f227fddb29299335cda55e",
|
||||
"sha256:d51ddfb3d481a6a3439db09d4b08447fb9f6b60d862ab301238f37bea8f60a6d",
|
||||
"sha256:dd47fac2878f6102efa211461eb6fa0a6dd7b4899cd1ade6cdcb9fa9748363eb",
|
||||
"sha256:eff55d318a114742ed2a06972f5daacfe3d5ad0c0c0d9146bcaf10acb427e6be",
|
||||
"sha256:f2673c51e8535401c68806d331faba614bcff3ee16373481158a2e74f510b7f6",
|
||||
"sha256:fa78bd55ec652d4a88ba254c8dae623c9992e2ce647bd17ba1a37ca2b7b42222",
|
||||
|
|
@ -1974,11 +2020,11 @@
|
|||
},
|
||||
"notebook": {
|
||||
"hashes": [
|
||||
"sha256:07b6e8b8a61aa2f780fe9a97430470485bc71262bc5cae8521f1441b910d2c88",
|
||||
"sha256:687d01f963ea20360c0b904ee7a37c3d8cda553858c8d6e33fd0afd13e89de32"
|
||||
"sha256:3db37ae834c5f3b6378381229d0e5dfcbfb558d08c8ce646b1ad355147f5e91d",
|
||||
"sha256:508cf9dad7cdb3188f1aa27017dc78179029dfe83814fc505329f689bc2ab50f"
|
||||
],
|
||||
"markers": "python_version >= '3.5'",
|
||||
"version": "==6.1.4"
|
||||
"version": "==6.1.5"
|
||||
},
|
||||
"numpy": {
|
||||
"hashes": [
|
||||
|
|
@ -2065,33 +2111,33 @@
|
|||
},
|
||||
"osmium": {
|
||||
"hashes": [
|
||||
"sha256:04e2f3380525c9b100d7c9350e107ae0d75c378c4c36d17cc4999c5952b33960",
|
||||
"sha256:0a5e9cccd9c376ab31fe79cdb1efcd1eb8175e7ff74a1f7ac218bd084d1a819d",
|
||||
"sha256:32e0b94c9a6f24c3f86787a5e23d7289c00467a19d13ff2552feb2810d8b76c3",
|
||||
"sha256:396557cc44f9655419060fb56623822b64af3f835df1f35ca0bb5aad2b5f33fe",
|
||||
"sha256:44cd17932ef17c1c749ff5585a76e68b4615bfbe94828b7765bdebb39dfb2d85",
|
||||
"sha256:6909a93173acd4111cc179bd7317e8a4e656c3b7b1a9041c5a5a8fcd01dbc3b5",
|
||||
"sha256:7638dd47f667ab2f18e0092036851e4ead4a47fb5840ff138e20c02bd47d14d7",
|
||||
"sha256:7c3782400d001229302bdffd6f410c460b270aef29a4a58c45fd479c897825b6",
|
||||
"sha256:8451c8ebdb39b7f3d5bc038c0230f592d4dffa50ec6baef64ee8baa72e814e8a",
|
||||
"sha256:84a7259ab1c2a37b399cfd53b82c402daf8df611ce9223dc443ccf3142b97223",
|
||||
"sha256:8bf205bed661aa0ab7cb39f00981ced6b1f46d923c5aec246e89616b45f64dd2",
|
||||
"sha256:97de71233b4741992008ca49e77c0a92999e9077cec9d6511759ba5d55faedde",
|
||||
"sha256:a4a2a7c55696ad32f809dfb48f0eca1acffc6d447063d2062aaca6d6f9a26946",
|
||||
"sha256:a60445b2122e159d7775d57f4958191ea5a32395b289c91156841ef04bdc2182",
|
||||
"sha256:a8f4ecebe1d536ab7cdd02fa9f82ff745dd4aa8ed478ccf588f586b4c4af38a9",
|
||||
"sha256:c3526f17eeb58eda0525888542ae14586745b0faaaf00dca2ccf2d376b75899f",
|
||||
"sha256:c85c2bc37b118b93dc8eb6c141bf415eff34c35084613be29ced3b969b49c7f6",
|
||||
"sha256:ce18818b7260113fcedb54e3167da0e9a07723a92f23a594318086d8c09645df",
|
||||
"sha256:cf78640c5cd9b8e76ee37b5c0522c5130469d3db26ffdfd54e8fb751477787f5",
|
||||
"sha256:d5da39de1c00d0c0ef8771df29424abb8bdc3fe093641c1df7448f0a22266f28",
|
||||
"sha256:e25a2f8f495ff7d96c1c3a4c636ea5aa5176b58d5323350f913b30a6a1b67159",
|
||||
"sha256:ede6fed48da8512415a4a895c2ded2777dacddfacc1b342f7938aa4271c97bd1",
|
||||
"sha256:f247a74c6a02016c7f3749f5fc6fad3abaa5c7874ae2be9516ef1649dc276792",
|
||||
"sha256:fe43969f2fb021051c2c202c28bd42bd7453f3f41d5d2499126b16437bb52f63"
|
||||
"sha256:04f8ba3fd87c2273b8ae66749d3d9695a1574f983ef12dd97b9c4a4663c818ae",
|
||||
"sha256:1115671bc47d3f08f8db15d7b9c1a6a77d8abcebbb9a06dc351b652becd9e38f",
|
||||
"sha256:1c912d4b3ac6e23296cbd923c0a8960a3a5277bb3fb9423d0323c7631e791bc9",
|
||||
"sha256:1fbd3c52c8c16a546021fa8686977df2aa283e8800904c6393a275670b9fa0d6",
|
||||
"sha256:22719008b2631f8c6c3d14bbdfe65f314cefce695e0887149bd0ea120e16fe62",
|
||||
"sha256:2d9f2ff65e2628f9747807e7e3c3ce022a83bd788cdff4e447a41cf6550f730f",
|
||||
"sha256:44e51d8fa6f505b091799a3a6ed4b1eaaf92d897605bfca85fbafbef4a34ec55",
|
||||
"sha256:4f614088b2aa50bab7b90b5b9ef710de9b74904755ddc89f4ed186f21982d2fa",
|
||||
"sha256:5ab52b4867f25e481dacc2db8af17a4baff6fe09cdbc5bf5bc1a7cc549c29c6b",
|
||||
"sha256:77c4b9038e83543c078b9efe23720cc6df565c5064a602dadf62e15f97d7fa28",
|
||||
"sha256:b026e91007e5a7e09c258c97e1d26797e6776d711ab1dc4d8049065a80e9686c",
|
||||
"sha256:b83e2f668b1c1f69aee9571a62580a23be4420eaf7e30419520eb420d3a268f1",
|
||||
"sha256:c0c068c5c3437d3926674be640ecb9e100093b0f2a14f9acaf306616a8ead8b0",
|
||||
"sha256:c1dda4ac95deebe0a13a1afffcf55a1fc3439997bf24e780a23a605584c95d04",
|
||||
"sha256:c73ab75148fd079f3d0ee6e69ec638d665feba199dc62e8a11f5c3f396831431",
|
||||
"sha256:cfd8f75bc9dbc6e0e5d0299d81a29abf6732da52ae665dc6056c5c39a53cc475",
|
||||
"sha256:d223328781ace310de51bfb52648b1e95d0360d1c467f9e690df66841ae81d27",
|
||||
"sha256:d4a632a51b005cdcc6397c87df69030f87b44ba26aa3df982c98009a37b1471e",
|
||||
"sha256:df58bddd01bdb70d126587c77dda3d9f05c287facd9d6ee733250fe52c19adc3",
|
||||
"sha256:e39d0b8b9a3ce8f8b086ec33cdcc782adfe194ae1f97c7832f0db84ee4422dad",
|
||||
"sha256:ed7870ef352bad8d731b5140a9808c5ca34aa0bc645b7e9b068359a7f3eecb59",
|
||||
"sha256:f19ad99acf2ec002f5a06c4c4f34129e220736c8d0ebb2b61e151dfdcd18545e",
|
||||
"sha256:f82e44595b3dc5375d2eb303547ef635accb7bdb4c7c23a33e581ce132edfa92",
|
||||
"sha256:fa3e118a41cfbd2539b30760a81dea2e5db9017382fb3357fd604f53cd391922"
|
||||
],
|
||||
"index": "pypi",
|
||||
"version": "==3.0.1"
|
||||
"version": "==3.1.0"
|
||||
},
|
||||
"packaging": {
|
||||
"hashes": [
|
||||
|
|
@ -2614,16 +2660,22 @@
|
|||
"sha256:076ca8907001fdfe4205484f719d12b4a0262dfe6652fa1cfc3c5c362d14dc84",
|
||||
"sha256:18a51b3f9416a2ae6e9a35c4af32cf520dd7895f2b69714f4aa2f4342fca47f9",
|
||||
"sha256:1a64b40f6acb4ffbaccce0545d7fc641744f95351f62e4c6aaa40549326008c9",
|
||||
"sha256:2b634a54241c190ee989a4af87669d377b37c91bcc9cf0efe33c10ff847f7841",
|
||||
"sha256:2f7429eeb5bf9c7068002d0d7f094ed654c77a70ce5e6198737fd68ab85f8311",
|
||||
"sha256:35959c041ec014648575085a97b498eafbbaa824f86f6e4a59bfdef8a3fe6308",
|
||||
"sha256:39c74740718e420d38c78ca4498568fa57976d78d5096277358e0fa9629a7aea",
|
||||
"sha256:411e17ca6ed8cf5e18a7ca5ee06a91c25800cc6c58c77986202abf98d749273a",
|
||||
"sha256:55e39ec848ceec13c9fa1598253ae9dd5c31d09dfd48059462860d2b908fb224",
|
||||
"sha256:6162dc0ae04669ea04b4b51420777b9ea2d30b0a9d02901b2a3b4d61d159c2e9",
|
||||
"sha256:68b5c33741d26c827074b3d8f0251de1c3019bb9567b8d303eb093c822ce28f1",
|
||||
"sha256:6bc78fb9c42a716309b4ace56f51965d8b5662c3ba19d4591749f31773db1125",
|
||||
"sha256:6ebfefebb5c6494a3af41ad8c60248a95da267a24b79ed143723d4502b1fe4d7",
|
||||
"sha256:720dbcdd3d91c6dfead79c80bf8b00a1d8aa4e5d551dc528c6d5151e4efc3403",
|
||||
"sha256:732bab78435c48be5d6bc75486ef629d7c8f112e07b313bf1f1a2220ab437277",
|
||||
"sha256:7947e51ca05489b85928af52a34fe67022ab5b81d4ae32a4109a99e883a0635e",
|
||||
"sha256:79f5b54f9dc353e5ee47f0c3f02bebd2c899d49780633aa771fed43fa20b3149",
|
||||
"sha256:80b924edbc012ded8aa8b91cb2fd6207fb1a9a3a377beb4049b8a07445cec6f0",
|
||||
"sha256:83c5e3eb78ce111c2f0b45f46106cc697c3cb6c4e5f51308e1f81b512c70c8fb",
|
||||
"sha256:889d4c5c5205a9c90118c1980df526857929841df33e4cd1ff1eff77c6817a65",
|
||||
"sha256:935ff247b8b78bdf77647fee962b1cc208c51a7b229db30b9ba5f6da3e675178",
|
||||
"sha256:98b2669c5af842a70cfab33a7043fcb5e7535a690a00cd251b44c9be0be418e5",
|
||||
|
|
@ -2829,6 +2881,7 @@
|
|||
"sha256:1641724c1055459a7e2b8bbe47ba25bdc89554582e62aec23cb3f3ca25f9b129",
|
||||
"sha256:17df66e87d0fe0193910aeaa938c99f0b04f67b430edb8adae01e7be557b141b",
|
||||
"sha256:182716ffb500d114b5d1b75d7fd9d14b7d3414cef3c38c0490534cc9ce20981a",
|
||||
"sha256:2df5260d0f2983309776cb41bfa85c464ec07018d88c0ecfca23d40bfadae2f1",
|
||||
"sha256:35be1c5d869966569d3dfd4ec31832d7c780e9df760e1fe52131105685941891",
|
||||
"sha256:4c10f317e379cc404f8fc510cd9982d5d3e7ba13a9cfd39aa251d894c6366798",
|
||||
"sha256:4f3c59f6dbf86a9fc293546de492f5e07344e045f9333f3a753f2dda903c45d1",
|
||||
|
|
@ -2840,6 +2893,7 @@
|
|||
"sha256:8f15b6ce67dcc05b61f19c689b60f3fe58550ba994290ff8332f711f5aaa9840",
|
||||
"sha256:90a3e2ae0d6d7d50ff2370ba168fbd416a53e7d8448410758c5d6a5920646c1d",
|
||||
"sha256:a3774516c8a83abfd1ddffb8b6ec1b0935d7fe6ea0ff5c31a18bfdae567b4eba",
|
||||
"sha256:a5c3a50d823c192f32615a2a6920e8c046b09e07a58eba220407335a9cd2e8ea",
|
||||
"sha256:da38ed3d65b8091447dc3717e5218cc336d20303b77b0634b261bc5c1aa2bae8",
|
||||
"sha256:de618e67b64a51a0768d26a9963ecd7d338a2cf6e9e7582d2385f88ad005b3d1",
|
||||
"sha256:e3afccf0437edc108eef1e2bb9cc4c7073e7705924eb4cd0bf7715cd1ef0ce1b"
|
||||
|
|
|
|||
2
cereal
2
cereal
|
|
@ -1 +1 @@
|
|||
Subproject commit eb0ede91af24aba72adcefa545233107e4a970fe
|
||||
Subproject commit 72b40bf2eea3209b71fc75811632f9eaee7db7ac
|
||||
BIN
models/supercombo.dlc (Stored with Git LFS)
BIN
models/supercombo.dlc (Stored with Git LFS)
Binary file not shown.
BIN
models/supercombo.keras (Stored with Git LFS)
BIN
models/supercombo.keras (Stored with Git LFS)
Binary file not shown.
Binary file not shown.
|
|
@ -388,7 +388,7 @@ selfdrive/modeld/constants.py
|
|||
selfdrive/modeld/modeld
|
||||
selfdrive/modeld/dmonitoringmodeld
|
||||
|
||||
selfdrive/modeld/models/commonmodel.c
|
||||
selfdrive/modeld/models/commonmodel.cc
|
||||
selfdrive/modeld/models/commonmodel.h
|
||||
selfdrive/modeld/models/driving.cc
|
||||
selfdrive/modeld/models/driving.h
|
||||
|
|
@ -397,7 +397,8 @@ selfdrive/modeld/models/dmonitoring.h
|
|||
|
||||
selfdrive/modeld/transforms/loadyuv.[c,h]
|
||||
selfdrive/modeld/transforms/loadyuv.cl
|
||||
selfdrive/modeld/transforms/transform.[c,h]
|
||||
selfdrive/modeld/transforms/transform.cc
|
||||
selfdrive/modeld/transforms/transform.h
|
||||
selfdrive/modeld/transforms/transform.cl
|
||||
|
||||
selfdrive/modeld/thneed/thneed.*
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
#ifndef COMMON_MAT_H
|
||||
#define COMMON_MAT_H
|
||||
#pragma once
|
||||
|
||||
typedef struct vec3 {
|
||||
float v[3];
|
||||
|
|
@ -17,7 +16,7 @@ typedef struct mat4 {
|
|||
float v[4*4];
|
||||
} mat4;
|
||||
|
||||
static inline mat3 matmul3(const mat3 a, const mat3 b) {
|
||||
static inline mat3 matmul3(const mat3 &a, const mat3 &b) {
|
||||
mat3 ret = {{0.0}};
|
||||
for (int r=0; r<3; r++) {
|
||||
for (int c=0; c<3; c++) {
|
||||
|
|
@ -31,7 +30,7 @@ static inline mat3 matmul3(const mat3 a, const mat3 b) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
|
||||
static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) {
|
||||
vec3 ret = {{0.0}};
|
||||
for (int r=0; r<3; r++) {
|
||||
for (int c=0; c<3; c++) {
|
||||
|
|
@ -41,7 +40,7 @@ static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
static inline mat4 matmul(const mat4 a, const mat4 b) {
|
||||
static inline mat4 matmul(const mat4 &a, const mat4 &b) {
|
||||
mat4 ret = {{0.0}};
|
||||
for (int r=0; r<4; r++) {
|
||||
for (int c=0; c<4; c++) {
|
||||
|
|
@ -55,7 +54,7 @@ static inline mat4 matmul(const mat4 a, const mat4 b) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
static inline vec4 matvecmul(const mat4 a, const vec4 b) {
|
||||
static inline vec4 matvecmul(const mat4 &a, const vec4 &b) {
|
||||
vec4 ret = {{0.0}};
|
||||
for (int r=0; r<4; r++) {
|
||||
for (int c=0; c<4; c++) {
|
||||
|
|
@ -67,7 +66,7 @@ static inline vec4 matvecmul(const mat4 a, const vec4 b) {
|
|||
|
||||
// scales the input and output space of a transformation matrix
|
||||
// that assumes pixel-center origin.
|
||||
static inline mat3 transform_scale_buffer(const mat3 in, float s) {
|
||||
static inline mat3 transform_scale_buffer(const mat3 &in, float s) {
|
||||
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
|
||||
|
||||
mat3 transform_out = {{
|
||||
|
|
@ -84,5 +83,3 @@ static inline mat3 transform_scale_buffer(const mat3 in, float s) {
|
|||
|
||||
return matmul3(transform_in, matmul3(in, transform_out));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,6 +1,9 @@
|
|||
#pragma once
|
||||
|
||||
#define MODEL_PATH_DISTANCE 192
|
||||
#define TRAJECTORY_SIZE 33
|
||||
#define MIN_DRAW_DISTANCE 10.0
|
||||
#define MAX_DRAW_DISTANCE 100.0
|
||||
#define POLYFIT_DEGREE 4
|
||||
#define SPEED_PERCENTILES 10
|
||||
#define DESIRE_PRED_SIZE 32
|
||||
|
|
|
|||
|
|
@ -59,8 +59,8 @@ class LanePlanner:
|
|||
self.r_prob = md.rightLane.prob # right line prob
|
||||
|
||||
if len(md.meta.desireState):
|
||||
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1]
|
||||
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1]
|
||||
self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft]
|
||||
self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight]
|
||||
|
||||
def update_d_poly(self, v_ego):
|
||||
# only offset left and right lane lines; offsetting p_poly does not make sense
|
||||
|
|
|
|||
|
|
@ -6,10 +6,10 @@ libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'z
|
|||
TEST_THNEED = False
|
||||
|
||||
common_src = [
|
||||
"models/commonmodel.c",
|
||||
"models/commonmodel.cc",
|
||||
"runners/snpemodel.cc",
|
||||
"transforms/loadyuv.c",
|
||||
"transforms/transform.c"
|
||||
"transforms/transform.cc"
|
||||
]
|
||||
|
||||
if arch == "aarch64":
|
||||
|
|
@ -23,12 +23,12 @@ elif arch == "larch64":
|
|||
else:
|
||||
libs += ['pthread']
|
||||
|
||||
# for tensorflow support
|
||||
common_src += ['runners/tfmodel.cc']
|
||||
# for onnx support
|
||||
common_src += ['runners/onnxmodel.cc']
|
||||
|
||||
# tell runners to use tensorflow
|
||||
lenv['CFLAGS'].append("-DUSE_TF_MODEL")
|
||||
lenv['CXXFLAGS'].append("-DUSE_TF_MODEL")
|
||||
# tell runners to use onnx
|
||||
lenv['CFLAGS'].append("-DUSE_ONNX_MODEL")
|
||||
lenv['CXXFLAGS'].append("-DUSE_ONNX_MODEL")
|
||||
|
||||
if arch == "Darwin":
|
||||
# fix OpenCL
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ int main(int argc, char **argv) {
|
|||
assert(err == 0);
|
||||
|
||||
// messaging
|
||||
PubMaster pm({"model", "cameraOdometry"});
|
||||
PubMaster pm({"modelV2", "model", "cameraOdometry"});
|
||||
SubMaster sm({"pathPlan", "frame"});
|
||||
|
||||
#if defined(QCOM) || defined(QCOM2)
|
||||
|
|
@ -173,7 +173,7 @@ int main(int argc, char **argv) {
|
|||
|
||||
if (sm.update(0) > 0){
|
||||
// TODO: path planner timeout?
|
||||
desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1;
|
||||
desire = ((int)sm["pathPlan"].getPathPlan().getDesire());
|
||||
frame_id = sm["frame"].getFrame().getFrameId();
|
||||
}
|
||||
|
||||
|
|
@ -200,6 +200,7 @@ int main(int argc, char **argv) {
|
|||
float frame_drop_perc = frames_dropped / MODEL_FREQ;
|
||||
|
||||
model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
|
||||
model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
|
||||
posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
|
||||
|
||||
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc);
|
||||
|
|
|
|||
|
|
@ -59,6 +59,29 @@ void frame_free(ModelFrame* frame) {
|
|||
clReleaseMemObject(frame->transformed_y_cl);
|
||||
}
|
||||
|
||||
void softmax(const float* input, float* output, size_t len) {
|
||||
float max_val = -FLT_MAX;
|
||||
for(int i = 0; i < len; i++) {
|
||||
const float v = input[i];
|
||||
if( v > max_val ) {
|
||||
max_val = v;
|
||||
}
|
||||
}
|
||||
|
||||
float denominator = 0;
|
||||
for(int i = 0; i < len; i++) {
|
||||
float const v = input[i];
|
||||
float const v_exp = expf(v - max_val);
|
||||
denominator += v_exp;
|
||||
output[i] = v_exp;
|
||||
}
|
||||
|
||||
const float inv_denominator = 1. / denominator;
|
||||
for(int i = 0; i < len; i++) {
|
||||
output[i] *= inv_denominator;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float sigmoid(float input) {
|
||||
return 1 / (1 + expf(-input));
|
||||
|
|
@ -8,6 +8,7 @@
|
|||
#include <CL/cl.h>
|
||||
#endif
|
||||
|
||||
#include <float.h>
|
||||
#include "common/mat.h"
|
||||
#include "transforms/transform.h"
|
||||
#include "transforms/loadyuv.h"
|
||||
|
|
@ -16,6 +17,7 @@
|
|||
extern "C" {
|
||||
#endif
|
||||
|
||||
void softmax(const float* input, float* output, size_t len);
|
||||
float softplus(float input);
|
||||
float sigmoid(float input);
|
||||
|
||||
|
|
|
|||
|
|
@ -9,14 +9,17 @@
|
|||
#include "common/params.h"
|
||||
#include "driving.h"
|
||||
|
||||
#define PATH_IDX 0
|
||||
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1
|
||||
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2
|
||||
#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2
|
||||
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
|
||||
#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
|
||||
#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
|
||||
#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2
|
||||
#define MIN_VALID_LEN 10.0
|
||||
#define TRAJECTORY_SIZE 33
|
||||
#define TRAJECTORY_TIME 10.0
|
||||
#define TRAJECTORY_DISTANCE 192.0
|
||||
#define PLAN_IDX 0
|
||||
#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE)
|
||||
#define LL_PROB_IDX LL_IDX + 4*2*2*33
|
||||
#define RE_IDX LL_PROB_IDX + 4
|
||||
#define LEAD_IDX RE_IDX + 2*2*2*33
|
||||
#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE)
|
||||
#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3
|
||||
#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN
|
||||
#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
|
||||
#define OUTPUT_SIZE POSE_IDX + POSE_SIZE
|
||||
|
|
@ -29,6 +32,8 @@
|
|||
// #define DUMP_YUV
|
||||
|
||||
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
|
||||
float X_IDXS[TRAJECTORY_SIZE];
|
||||
float T_IDXS[TRAJECTORY_SIZE];
|
||||
|
||||
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
|
||||
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
|
||||
|
|
@ -63,9 +68,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
|
|||
#endif
|
||||
|
||||
// Build Vandermonde matrix
|
||||
for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
|
||||
for(int i = 0; i < TRAJECTORY_SIZE; i++) {
|
||||
for(int j = 0; j < POLYFIT_DEGREE - 1; j++) {
|
||||
vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
|
||||
X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2));
|
||||
T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2));
|
||||
vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -76,7 +83,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
|
|||
float *desire_in) {
|
||||
#ifdef DESIRE
|
||||
if (desire_in != NULL) {
|
||||
for (int i = 0; i < DESIRE_LEN; i++) {
|
||||
for (int i = 1; i < DESIRE_LEN; i++) {
|
||||
// Model decides when action is completed
|
||||
// so desire input is just a pulse triggered on rising edge
|
||||
if (desire_in[i] - s->prev_desire[i] > .99) {
|
||||
|
|
@ -107,13 +114,12 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
|
|||
|
||||
// net outputs
|
||||
ModelDataRaw net_outputs;
|
||||
net_outputs.path = &s->output[PATH_IDX];
|
||||
net_outputs.left_lane = &s->output[LL_IDX];
|
||||
net_outputs.right_lane = &s->output[RL_IDX];
|
||||
net_outputs.plan = &s->output[PLAN_IDX];
|
||||
net_outputs.lane_lines = &s->output[LL_IDX];
|
||||
net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX];
|
||||
net_outputs.road_edges = &s->output[RE_IDX];
|
||||
net_outputs.lead = &s->output[LEAD_IDX];
|
||||
net_outputs.long_x = &s->output[LONG_X_IDX];
|
||||
net_outputs.long_v = &s->output[LONG_V_IDX];
|
||||
net_outputs.long_a = &s->output[LONG_A_IDX];
|
||||
net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
|
||||
net_outputs.meta = &s->output[DESIRE_STATE_IDX];
|
||||
net_outputs.pose = &s->output[POSE_IDX];
|
||||
return net_outputs;
|
||||
|
|
@ -151,35 +157,40 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
|
|||
out[3] = y0;
|
||||
}
|
||||
|
||||
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) {
|
||||
float points_arr[MODEL_PATH_DISTANCE];
|
||||
float stds_arr[MODEL_PATH_DISTANCE];
|
||||
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) {
|
||||
float points_arr[TRAJECTORY_SIZE];
|
||||
float stds_arr[TRAJECTORY_SIZE];
|
||||
float poly_arr[POLYFIT_DEGREE];
|
||||
float std;
|
||||
float prob;
|
||||
float valid_len;
|
||||
|
||||
// clamp to 5 and MODEL_PATH_DISTANCE
|
||||
valid_len = fmin(MODEL_PATH_DISTANCE, fmax(5, data[MODEL_PATH_DISTANCE*2]));
|
||||
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
|
||||
points_arr[i] = data[i] + offset;
|
||||
stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]) + 1e-6;
|
||||
for (int i=0; i<TRAJECTORY_SIZE; i++) {
|
||||
// negative sign because mpc has left positive
|
||||
points_arr[i] = -data[30*i + 16];
|
||||
stds_arr[i] = exp(data[30*(33 + i) + 16]);
|
||||
}
|
||||
if (has_prob) {
|
||||
prob = sigmoid(data[MODEL_PATH_DISTANCE*2 + 1]);
|
||||
} else {
|
||||
prob = 1.0;
|
||||
}
|
||||
std = softplus(data[MODEL_PATH_DISTANCE]) + 1e-6;
|
||||
poly_fit(points_arr, stds_arr, poly_arr, valid_len);
|
||||
std = stds_arr[0];
|
||||
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
|
||||
|
||||
if (std::getenv("DEBUG")){
|
||||
kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
|
||||
path.setStds(stds);
|
||||
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
|
||||
path.setPoly(poly);
|
||||
path.setProb(1.0);
|
||||
path.setStd(std);
|
||||
path.setValidLen(valid_len);
|
||||
}
|
||||
|
||||
kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
|
||||
path.setPoints(points);
|
||||
void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) {
|
||||
float points_arr[TRAJECTORY_SIZE];
|
||||
float stds_arr[TRAJECTORY_SIZE];
|
||||
float poly_arr[POLYFIT_DEGREE];
|
||||
float std;
|
||||
|
||||
for (int i=0; i<TRAJECTORY_SIZE; i++) {
|
||||
// negative sign because mpc has left positive
|
||||
points_arr[i] = -data[2*33*ll_idx + 2*i];
|
||||
stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]);
|
||||
}
|
||||
std = stds_arr[0];
|
||||
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
|
||||
|
||||
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
|
||||
path.setPoly(poly);
|
||||
|
|
@ -188,48 +199,197 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo
|
|||
path.setValidLen(valid_len);
|
||||
}
|
||||
|
||||
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) {
|
||||
const double x_scale = 10.0;
|
||||
const double y_scale = 10.0;
|
||||
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) {
|
||||
lead.setProb(prob);
|
||||
lead.setT(t);
|
||||
float xyva_arr[LEAD_MHP_VALS];
|
||||
float xyva_stds_arr[LEAD_MHP_VALS];
|
||||
for (int i=0; i<LEAD_MHP_VALS; i++) {
|
||||
xyva_arr[i] = data[LEAD_MHP_VALS + i];
|
||||
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
|
||||
}
|
||||
kj::ArrayPtr<const float> xyva(xyva_arr, LEAD_MHP_VALS);
|
||||
kj::ArrayPtr<const float> xyva_stds(xyva_stds_arr, LEAD_MHP_VALS);
|
||||
lead.setXyva(xyva);
|
||||
lead.setXyvaStd(xyva_stds);
|
||||
}
|
||||
|
||||
lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset]));
|
||||
lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
|
||||
lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
|
||||
lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
|
||||
lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
|
||||
lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]);
|
||||
lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]));
|
||||
lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]);
|
||||
lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]));
|
||||
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) {
|
||||
lead.setProb(prob);
|
||||
lead.setDist(data[0]);
|
||||
lead.setStd(exp(data[LEAD_MHP_VALS]));
|
||||
// TODO make all msgs same format
|
||||
lead.setRelY(-data[1]);
|
||||
lead.setRelYStd(exp(data[LEAD_MHP_VALS + 1]));
|
||||
lead.setRelVel(data[2]);
|
||||
lead.setRelVelStd(exp(data[LEAD_MHP_VALS + 2]));
|
||||
lead.setRelA(data[3]);
|
||||
lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
|
||||
}
|
||||
|
||||
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
|
||||
kj::ArrayPtr<const float> desire_state(&meta_data[0], DESIRE_LEN);
|
||||
float desire_state_softmax[DESIRE_LEN];
|
||||
float desire_pred_softmax[4*DESIRE_LEN];
|
||||
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
|
||||
for (int i=0; i<4; i++) {
|
||||
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
|
||||
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
|
||||
}
|
||||
kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
|
||||
meta.setDesireState(desire_state);
|
||||
meta.setEngagedProb(meta_data[DESIRE_LEN]);
|
||||
meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]);
|
||||
meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]);
|
||||
meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]);
|
||||
kj::ArrayPtr<const float> desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE);
|
||||
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
|
||||
meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
|
||||
meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
|
||||
meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
|
||||
kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
|
||||
meta.setDesirePrediction(desire_pred);
|
||||
}
|
||||
|
||||
void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_x_data, const float * long_v_data, const float * long_a_data) {
|
||||
// just doing 10 vals, 1 every sec for now
|
||||
float dist_arr[TIME_DISTANCE/10];
|
||||
float speed_arr[TIME_DISTANCE/10];
|
||||
float accel_arr[TIME_DISTANCE/10];
|
||||
for (int i=0; i<TIME_DISTANCE/10; i++) {
|
||||
dist_arr[i] = long_x_data[i*10];
|
||||
speed_arr[i] = long_v_data[i*10];
|
||||
accel_arr[i] = long_a_data[i*10];
|
||||
void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
|
||||
float desire_state_softmax[DESIRE_LEN];
|
||||
float desire_pred_softmax[4*DESIRE_LEN];
|
||||
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
|
||||
for (int i=0; i<4; i++) {
|
||||
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
|
||||
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
|
||||
}
|
||||
kj::ArrayPtr<const float> dist(&dist_arr[0], ARRAYSIZE(dist_arr));
|
||||
longi.setDistances(dist);
|
||||
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
|
||||
longi.setSpeeds(speed);
|
||||
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
|
||||
longi.setAccelerations(accel);
|
||||
kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
|
||||
meta.setDesireState(desire_state);
|
||||
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
|
||||
meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
|
||||
meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
|
||||
meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
|
||||
kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
|
||||
meta.setDesirePrediction(desire_pred);
|
||||
}
|
||||
|
||||
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
|
||||
int columns, int column_offset, float * plan_t_arr) {
|
||||
float x_arr[TRAJECTORY_SIZE];
|
||||
float y_arr[TRAJECTORY_SIZE];
|
||||
float z_arr[TRAJECTORY_SIZE];
|
||||
//float x_std_arr[TRAJECTORY_SIZE];
|
||||
//float y_std_arr[TRAJECTORY_SIZE];
|
||||
//float z_std_arr[TRAJECTORY_SIZE];
|
||||
float t_arr[TRAJECTORY_SIZE];
|
||||
for (int i=0; i<TRAJECTORY_SIZE; i++) {
|
||||
// column_offset == -1 means this data is X indexed not T indexed
|
||||
if (column_offset >= 0) {
|
||||
t_arr[i] = T_IDXS[i];
|
||||
x_arr[i] = data[i*columns + 0 + column_offset];
|
||||
//x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset];
|
||||
} else {
|
||||
t_arr[i] = plan_t_arr[i];
|
||||
x_arr[i] = X_IDXS[i];
|
||||
//x_std_arr[i] = NAN;
|
||||
}
|
||||
y_arr[i] = data[i*columns + 1 + column_offset];
|
||||
//y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset];
|
||||
z_arr[i] = data[i*columns + 2 + column_offset];
|
||||
//z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
|
||||
}
|
||||
kj::ArrayPtr<const float> x(x_arr, TRAJECTORY_SIZE);
|
||||
kj::ArrayPtr<const float> y(y_arr, TRAJECTORY_SIZE);
|
||||
kj::ArrayPtr<const float> z(z_arr, TRAJECTORY_SIZE);
|
||||
//kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
|
||||
//kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
|
||||
//kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
|
||||
kj::ArrayPtr<const float> t(t_arr, TRAJECTORY_SIZE);
|
||||
xyzt.setX(x);
|
||||
xyzt.setY(y);
|
||||
xyzt.setZ(z);
|
||||
//xyzt.setXStd(x_std);
|
||||
//xyzt.setYStd(y_std);
|
||||
//xyzt.setZStd(z_std);
|
||||
xyzt.setT(t);
|
||||
}
|
||||
|
||||
|
||||
void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
||||
uint32_t vipc_dropped_frames, float frame_drop,
|
||||
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
|
||||
// make msg
|
||||
MessageBuilder msg;
|
||||
auto framed = msg.initEvent(frame_drop < MAX_FRAME_DROP).initModelV2();
|
||||
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
|
||||
framed.setFrameId(vipc_frame_id);
|
||||
framed.setFrameAge(frame_age);
|
||||
framed.setFrameDropPerc(frame_drop * 100);
|
||||
framed.setTimestampEof(timestamp_eof);
|
||||
|
||||
// plan
|
||||
int plan_mhp_max_idx = 0;
|
||||
for (int i=1; i<PLAN_MHP_N; i++) {
|
||||
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
|
||||
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
|
||||
plan_mhp_max_idx = i;
|
||||
}
|
||||
}
|
||||
float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
|
||||
valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
|
||||
int valid_len_idx = 0;
|
||||
for (int i=1; i<TRAJECTORY_SIZE; i++) {
|
||||
if (valid_len >= X_IDXS[valid_len_idx]){
|
||||
valid_len_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
|
||||
float plan_t_arr[TRAJECTORY_SIZE];
|
||||
for (int i=0; i<TRAJECTORY_SIZE; i++) {
|
||||
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
|
||||
}
|
||||
|
||||
auto position = framed.initPosition();
|
||||
fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
|
||||
auto orientation = framed.initOrientation();
|
||||
fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
|
||||
auto velocity = framed.initVelocity();
|
||||
fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 6, plan_t_arr);
|
||||
auto orientation_rate = framed.initOrientationRate();
|
||||
fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
|
||||
|
||||
// lane lines
|
||||
auto lane_lines = framed.initLaneLines(4);
|
||||
float lane_line_probs_arr[4];
|
||||
float lane_line_stds_arr[4];
|
||||
for (int i = 0; i < 4; i++) {
|
||||
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
|
||||
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
|
||||
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
|
||||
}
|
||||
kj::ArrayPtr<const float> lane_line_probs(lane_line_probs_arr, 4);
|
||||
framed.setLaneLineProbs(lane_line_probs);
|
||||
framed.setLaneLineStds(lane_line_stds_arr);
|
||||
|
||||
// road edges
|
||||
auto road_edges = framed.initRoadEdges(2);
|
||||
float road_edge_stds_arr[2];
|
||||
for (int i = 0; i < 2; i++) {
|
||||
fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr);
|
||||
road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]);
|
||||
}
|
||||
framed.setRoadEdgeStds(road_edge_stds_arr);
|
||||
|
||||
// meta
|
||||
auto meta = framed.initMeta();
|
||||
fill_meta_v2(meta, net_outputs.meta);
|
||||
|
||||
// leads
|
||||
auto leads = framed.initLeads(LEAD_MHP_SELECTION);
|
||||
int mdn_max_idx = 0;
|
||||
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
|
||||
for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
|
||||
for (int i=1; i<LEAD_MHP_N; i++) {
|
||||
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
|
||||
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) {
|
||||
mdn_max_idx = i;
|
||||
fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)],
|
||||
sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]);
|
||||
}
|
||||
}
|
||||
}
|
||||
pm.send("modelV2", msg);
|
||||
}
|
||||
|
||||
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
||||
|
|
@ -243,29 +403,58 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
|||
framed.setFrameDropPerc(frame_drop * 100);
|
||||
framed.setTimestampEof(timestamp_eof);
|
||||
|
||||
fill_path(framed.initPath(), net_outputs.path, false, 0);
|
||||
fill_path(framed.initLeftLane(), net_outputs.left_lane, true, 1.8);
|
||||
fill_path(framed.initRightLane(), net_outputs.right_lane, true, -1.8);
|
||||
fill_longi(framed.initLongitudinal(), net_outputs.long_x, net_outputs.long_v, net_outputs.long_a);
|
||||
// Find the distribution that corresponds to the most probable plan
|
||||
int plan_mhp_max_idx = 0;
|
||||
for (int i=1; i<PLAN_MHP_N; i++) {
|
||||
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
|
||||
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
|
||||
plan_mhp_max_idx = i;
|
||||
}
|
||||
}
|
||||
// x pos at 10s is a good valid_len
|
||||
float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
|
||||
// clamp to 5 and MODEL_PATH_DISTANCE
|
||||
valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
|
||||
int valid_len_idx = 0;
|
||||
for (int i=1; i<TRAJECTORY_SIZE; i++) {
|
||||
if (valid_len >= X_IDXS[valid_len_idx]){
|
||||
valid_len_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
auto lpath = framed.initPath();
|
||||
fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx);
|
||||
|
||||
auto left_lane = framed.initLeftLane();
|
||||
int ll_idx = 1;
|
||||
fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
|
||||
sigmoid(net_outputs.lane_lines_prob[ll_idx]));
|
||||
auto right_lane = framed.initRightLane();
|
||||
ll_idx = 2;
|
||||
fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
|
||||
sigmoid(net_outputs.lane_lines_prob[ll_idx]));
|
||||
|
||||
// Find the distribution that corresponds to the current lead
|
||||
int mdn_max_idx = 0;
|
||||
int t_offset = 0;
|
||||
for (int i=1; i<LEAD_MDN_N; i++) {
|
||||
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
|
||||
for (int i=1; i<LEAD_MHP_N; i++) {
|
||||
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
|
||||
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
|
||||
mdn_max_idx = i;
|
||||
}
|
||||
}
|
||||
fill_lead(framed.initLead(), net_outputs.lead, mdn_max_idx, t_offset);
|
||||
fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
|
||||
// Find the distribution that corresponds to the lead in 2s
|
||||
mdn_max_idx = 0;
|
||||
t_offset = 1;
|
||||
for (int i=1; i<LEAD_MDN_N; i++) {
|
||||
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) {
|
||||
for (int i=1; i<LEAD_MHP_N; i++) {
|
||||
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
|
||||
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
|
||||
mdn_max_idx = i;
|
||||
}
|
||||
}
|
||||
fill_lead(framed.initLeadFuture(), net_outputs.lead, mdn_max_idx, t_offset);
|
||||
fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
|
||||
|
||||
fill_meta(framed.initMeta(), net_outputs.meta);
|
||||
|
||||
pm.send("model", msg);
|
||||
|
|
@ -280,10 +469,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
|||
|
||||
for (int i =0; i < 3; i++) {
|
||||
trans_arr[i] = net_outputs.pose[i];
|
||||
trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6;
|
||||
trans_std_arr[i] = exp(net_outputs.pose[6 + i]);
|
||||
|
||||
rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0;
|
||||
rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0;
|
||||
rot_arr[i] = net_outputs.pose[3 + i];
|
||||
rot_std_arr[i] = exp(net_outputs.pose[9 + i]);
|
||||
}
|
||||
|
||||
MessageBuilder msg;
|
||||
|
|
|
|||
|
|
@ -17,33 +17,40 @@
|
|||
#include <memory>
|
||||
#include "messaging.hpp"
|
||||
|
||||
#define MODEL_NAME "supercombo_dlc"
|
||||
|
||||
#define MODEL_WIDTH 512
|
||||
#define MODEL_HEIGHT 256
|
||||
#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
|
||||
#define MODEL_NAME "supercombo_dlc"
|
||||
|
||||
#define DESIRE_LEN 8
|
||||
#define TRAFFIC_CONVENTION_LEN 2
|
||||
#define LEAD_MDN_N 5 // probs for 5 groups
|
||||
#define MDN_VALS 4 // output xyva for each lead group
|
||||
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
|
||||
#define MDN_GROUP_SIZE 11
|
||||
#define TIME_DISTANCE 100
|
||||
|
||||
#define PLAN_MHP_N 5
|
||||
#define PLAN_MHP_COLUMNS 30
|
||||
#define PLAN_MHP_VALS 30*33
|
||||
#define PLAN_MHP_SELECTION 1
|
||||
#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION)
|
||||
|
||||
#define LEAD_MHP_N 5
|
||||
#define LEAD_MHP_VALS 4
|
||||
#define LEAD_MHP_SELECTION 3
|
||||
#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION)
|
||||
|
||||
#define POSE_SIZE 12
|
||||
|
||||
#define MODEL_FREQ 20
|
||||
#define MAX_FRAME_DROP 0.05
|
||||
|
||||
struct ModelDataRaw {
|
||||
float *path;
|
||||
float *left_lane;
|
||||
float *right_lane;
|
||||
float *plan;
|
||||
float *lane_lines;
|
||||
float *lane_lines_prob;
|
||||
float *road_edges;
|
||||
float *lead;
|
||||
float *long_x;
|
||||
float *long_v;
|
||||
float *long_a;
|
||||
float *lead_prob;
|
||||
float *desire_state;
|
||||
float *meta;
|
||||
float *desire_pred;
|
||||
float *pose;
|
||||
};
|
||||
|
||||
|
|
@ -72,6 +79,8 @@ void poly_fit(float *in_pts, float *in_stds, float *out);
|
|||
|
||||
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
||||
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof);
|
||||
void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
||||
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof);
|
||||
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
|
||||
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, uint64_t timestamp_eof);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,49 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# TODO: why are the keras models saved with python 2?
|
||||
from __future__ import print_function
|
||||
|
||||
import tensorflow as tf # pylint: disable=import-error
|
||||
import os
|
||||
import sys
|
||||
import numpy as np
|
||||
from tensorflow.keras.models import load_model # pylint: disable=import-error
|
||||
|
||||
def read(sz):
|
||||
dd = []
|
||||
gt = 0
|
||||
while gt < sz * 4:
|
||||
st = os.read(0, sz * 4 - gt)
|
||||
assert(len(st) > 0)
|
||||
dd.append(st)
|
||||
gt += len(st)
|
||||
return np.frombuffer(b''.join(dd), dtype=np.float32)
|
||||
|
||||
def write(d):
|
||||
os.write(1, d.tobytes())
|
||||
|
||||
def run_loop(m):
|
||||
ishapes = [[1]+ii.shape[1:] for ii in m.inputs]
|
||||
print("ready to run keras model", ishapes, file=sys.stderr)
|
||||
while 1:
|
||||
inputs = []
|
||||
for shp in ishapes:
|
||||
ts = np.product(shp)
|
||||
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
|
||||
inputs.append(read(ts).reshape(shp))
|
||||
ret = m.predict_on_batch(inputs)
|
||||
#print(ret, file=sys.stderr)
|
||||
for r in ret:
|
||||
write(r)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(tf.__version__, file=sys.stderr)
|
||||
# limit gram alloc
|
||||
gpus = tf.config.experimental.list_physical_devices('GPU')
|
||||
if len(gpus) > 0:
|
||||
tf.config.experimental.set_virtual_device_configuration(gpus[0], [tf.config.experimental.VirtualDeviceConfiguration(memory_limit=1024)])
|
||||
|
||||
m = load_model(sys.argv[1])
|
||||
print(m, file=sys.stderr)
|
||||
|
||||
run_loop(m)
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#!/usr/bin/env python3
|
||||
# TODO: why are the keras models saved with python 2?
|
||||
from __future__ import print_function
|
||||
|
||||
import os
|
||||
import sys
|
||||
import numpy as np
|
||||
import onnxruntime as ort
|
||||
|
||||
def read(sz):
|
||||
dd = []
|
||||
gt = 0
|
||||
while gt < sz * 4:
|
||||
st = os.read(0, sz * 4 - gt)
|
||||
assert(len(st) > 0)
|
||||
dd.append(st)
|
||||
gt += len(st)
|
||||
return np.frombuffer(b''.join(dd), dtype=np.float32)
|
||||
|
||||
def write(d):
|
||||
os.write(1, d.tobytes())
|
||||
|
||||
def run_loop(m):
|
||||
ishapes = [[1]+ii.shape[1:] for ii in m.get_inputs()]
|
||||
keys = [x.name for x in m.get_inputs()]
|
||||
print("ready to run onnx model", keys, ishapes, file=sys.stderr)
|
||||
while 1:
|
||||
inputs = []
|
||||
for shp in ishapes:
|
||||
ts = np.product(shp)
|
||||
#print("reshaping %s with offset %d" % (str(shp), offset), file=sys.stderr)
|
||||
inputs.append(read(ts).reshape(shp))
|
||||
ret = m.run(None, dict(zip(keys, inputs)))
|
||||
#print(ret, file=sys.stderr)
|
||||
for r in ret:
|
||||
write(r)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(ort.get_available_providers(), file=sys.stderr)
|
||||
if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
|
||||
print("OnnxJit is using openvino", file=sys.stderr)
|
||||
options = ort.SessionOptions()
|
||||
options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL
|
||||
provider = 'OpenVINOExecutionProvider'
|
||||
else:
|
||||
print("OnnxJit is using CPU", file=sys.stderr)
|
||||
options = ort.SessionOptions()
|
||||
options.intra_op_num_threads = 4
|
||||
options.inter_op_num_threads = 8
|
||||
provider = 'CPUExecutionProvider'
|
||||
|
||||
ort_session = ort.InferenceSession(sys.argv[1], options)
|
||||
ort_session.set_providers([provider], None)
|
||||
run_loop(ort_session)
|
||||
|
||||
|
|
@ -1,7 +1,8 @@
|
|||
#include "tfmodel.h"
|
||||
#include "onnxmodel.h"
|
||||
#include <stdio.h>
|
||||
#include <string>
|
||||
#include <string.h>
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
|
|
@ -12,33 +13,33 @@
|
|||
#include <cassert>
|
||||
|
||||
|
||||
TFModel::TFModel(const char *path, float *_output, size_t _output_size, int runtime) {
|
||||
ONNXModel::ONNXModel(const char *path, float *_output, size_t _output_size, int runtime) {
|
||||
output = _output;
|
||||
output_size = _output_size;
|
||||
|
||||
char tmp[1024];
|
||||
strncpy(tmp, path, sizeof(tmp));
|
||||
strstr(tmp, ".dlc")[0] = '\0';
|
||||
strcat(tmp, ".keras");
|
||||
strcat(tmp, ".onnx");
|
||||
LOGD("loading model %s", tmp);
|
||||
|
||||
assert(pipe(pipein) == 0);
|
||||
assert(pipe(pipeout) == 0);
|
||||
|
||||
std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe"));
|
||||
std::string keras_runner = exe_dir + "/runners/keras_runner.py";
|
||||
std::string onnx_runner = exe_dir + "/runners/onnx_runner.py";
|
||||
|
||||
proc_pid = fork();
|
||||
if (proc_pid == 0) {
|
||||
LOGD("spawning keras process %s", keras_runner.c_str());
|
||||
char *argv[] = {(char*)keras_runner.c_str(), tmp, NULL};
|
||||
LOGD("spawning onnx process %s", onnx_runner.c_str());
|
||||
char *argv[] = {(char*)onnx_runner.c_str(), tmp, NULL};
|
||||
dup2(pipein[0], 0);
|
||||
dup2(pipeout[1], 1);
|
||||
close(pipein[0]);
|
||||
close(pipein[1]);
|
||||
close(pipeout[0]);
|
||||
close(pipeout[1]);
|
||||
execvp(keras_runner.c_str(), argv);
|
||||
execvp(onnx_runner.c_str(), argv);
|
||||
}
|
||||
|
||||
// parent
|
||||
|
|
@ -46,13 +47,13 @@ TFModel::TFModel(const char *path, float *_output, size_t _output_size, int runt
|
|||
close(pipeout[1]);
|
||||
}
|
||||
|
||||
TFModel::~TFModel() {
|
||||
ONNXModel::~ONNXModel() {
|
||||
close(pipein[1]);
|
||||
close(pipeout[0]);
|
||||
kill(proc_pid, SIGTERM);
|
||||
}
|
||||
|
||||
void TFModel::pwrite(float *buf, int size) {
|
||||
void ONNXModel::pwrite(float *buf, int size) {
|
||||
char *cbuf = (char *)buf;
|
||||
int tw = size*sizeof(float);
|
||||
while (tw > 0) {
|
||||
|
|
@ -65,35 +66,41 @@ void TFModel::pwrite(float *buf, int size) {
|
|||
LOGD("host write of size %d done", size);
|
||||
}
|
||||
|
||||
void TFModel::pread(float *buf, int size) {
|
||||
void ONNXModel::pread(float *buf, int size) {
|
||||
char *cbuf = (char *)buf;
|
||||
int tr = size*sizeof(float);
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = pipeout[0];
|
||||
fds[0].events = POLLIN;
|
||||
while (tr > 0) {
|
||||
LOGD("host read remaining %d/%d", tr, size*sizeof(float));
|
||||
int err = read(pipeout[0], cbuf, tr);
|
||||
assert(err >= 0);
|
||||
int err;
|
||||
err = poll(fds, 1, 10000); // 10 second timeout
|
||||
assert(err == 1 || (err == -1 && errno == EINTR));
|
||||
LOGD("host read remaining %d/%d poll %d", tr, size*sizeof(float), err);
|
||||
err = read(pipeout[0], cbuf, tr);
|
||||
assert(err > 0 || (err == 0 && errno == EINTR));
|
||||
cbuf += err;
|
||||
tr -= err;
|
||||
}
|
||||
LOGD("host read done");
|
||||
}
|
||||
|
||||
void TFModel::addRecurrent(float *state, int state_size) {
|
||||
void ONNXModel::addRecurrent(float *state, int state_size) {
|
||||
rnn_input_buf = state;
|
||||
rnn_state_size = state_size;
|
||||
}
|
||||
|
||||
void TFModel::addDesire(float *state, int state_size) {
|
||||
void ONNXModel::addDesire(float *state, int state_size) {
|
||||
desire_input_buf = state;
|
||||
desire_state_size = state_size;
|
||||
}
|
||||
|
||||
void TFModel::addTrafficConvention(float *state, int state_size) {
|
||||
void ONNXModel::addTrafficConvention(float *state, int state_size) {
|
||||
traffic_convention_input_buf = state;
|
||||
traffic_convention_size = state_size;
|
||||
}
|
||||
|
||||
void TFModel::execute(float *net_input_buf, int buf_size) {
|
||||
void ONNXModel::execute(float *net_input_buf, int buf_size) {
|
||||
// order must be this
|
||||
pwrite(net_input_buf, buf_size);
|
||||
if (desire_input_buf != NULL) {
|
||||
|
|
@ -1,15 +1,13 @@
|
|||
#ifndef TFMODEL_H
|
||||
#define TFMODEL_H
|
||||
#ifndef ONNXMODEL_H
|
||||
#define ONNXMODEL_H
|
||||
|
||||
#include <stdlib.h>
|
||||
#include "runmodel.h"
|
||||
|
||||
struct TFState;
|
||||
|
||||
class TFModel : public RunModel {
|
||||
class ONNXModel : public RunModel {
|
||||
public:
|
||||
TFModel(const char *path, float *output, size_t output_size, int runtime);
|
||||
~TFModel();
|
||||
ONNXModel(const char *path, float *output, size_t output_size, int runtime);
|
||||
~ONNXModel();
|
||||
void addRecurrent(float *state, int state_size);
|
||||
void addDesire(float *state, int state_size);
|
||||
void addTrafficConvention(float *state, int state_size);
|
||||
|
|
@ -7,9 +7,9 @@
|
|||
#ifdef QCOM
|
||||
#define DefaultRunModel SNPEModel
|
||||
#else
|
||||
#ifdef USE_TF_MODEL
|
||||
#include "tfmodel.h"
|
||||
#define DefaultRunModel TFModel
|
||||
#ifdef USE_ONNX_MODEL
|
||||
#include "onnxmodel.h"
|
||||
#define DefaultRunModel ONNXModel
|
||||
#else
|
||||
#define DefaultRunModel SNPEModel
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -99,14 +99,14 @@ void transform_queue(Transform* s,
|
|||
err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl);
|
||||
assert(err == 0);
|
||||
|
||||
const size_t work_size_y[2] = {out_y_width, out_y_height};
|
||||
const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height};
|
||||
|
||||
err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
|
||||
(const size_t*)&work_size_y, NULL, 0, 0, NULL);
|
||||
assert(err == 0);
|
||||
|
||||
|
||||
const size_t work_size_uv[2] = {out_uv_width, out_uv_height};
|
||||
const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height};
|
||||
|
||||
err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width);
|
||||
assert(err == 0);
|
||||
|
|
@ -48,7 +48,7 @@ endif
|
|||
all: visiontest
|
||||
|
||||
libvisiontest_inputs := visiontest.c \
|
||||
transforms/transform.c \
|
||||
transforms/transform.cc \
|
||||
transforms/loadyuv.c \
|
||||
../common/clutil.c \
|
||||
$(BASEDIR)/selfdrive/common/util.c \
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
734ee7118a5bcab4accdd73ae8fdc03df104157a
|
||||
f67a4ac1f387514a42c9f4d890495c4872288a63
|
||||
|
|
@ -16,7 +16,7 @@ def cputime_total(ct):
|
|||
def print_cpu_usage(first_proc, last_proc):
|
||||
procs = [
|
||||
("selfdrive.controls.controlsd", 47.0),
|
||||
("./loggerd", 38.0),
|
||||
("./loggerd", 42.0),
|
||||
("selfdrive.locationd.locationd", 35.0),
|
||||
("selfdrive.locationd.paramsd", 12.0),
|
||||
("selfdrive.controls.plannerd", 10.0),
|
||||
|
|
|
|||
|
|
@ -4,6 +4,8 @@
|
|||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include "common/util.h"
|
||||
#include <algorithm>
|
||||
|
||||
|
||||
#define NANOVG_GLES3_IMPLEMENTATION
|
||||
#include "nanovg_gl.h"
|
||||
|
|
@ -37,26 +39,23 @@ const mat3 intrinsic_matrix = (mat3){{
|
|||
|
||||
// Projects a point in car to space to the corresponding point in full frame
|
||||
// image space.
|
||||
vec3 car_space_to_full_frame(const UIState *s, vec4 car_space_projective) {
|
||||
const UIScene *scene = &s->scene;
|
||||
|
||||
bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y) {
|
||||
const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}};
|
||||
// We'll call the car space point p.
|
||||
// First project into normalized image coordinates with the extrinsics matrix.
|
||||
const vec4 Ep4 = matvecmul(scene->extrinsic_matrix, car_space_projective);
|
||||
const vec4 Ep4 = matvecmul(s->scene.extrinsic_matrix, car_space_projective);
|
||||
|
||||
// The last entry is zero because of how we store E (to use matvecmul).
|
||||
const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}};
|
||||
const vec3 KEp = matvecmul3(intrinsic_matrix, Ep);
|
||||
|
||||
// Project.
|
||||
const vec3 p_image = {{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2], 1.}};
|
||||
return p_image;
|
||||
*out_x = KEp.v[0] / KEp.v[2];
|
||||
*out_y = KEp.v[1] / KEp.v[2];
|
||||
|
||||
return *out_x >= 0 && *out_y >= 0;
|
||||
}
|
||||
|
||||
// Calculate an interpolation between two numbers at a specific increment
|
||||
static float lerp(float v0, float v1, float t) {
|
||||
return (1 - t) * v0 + t * v1;
|
||||
}
|
||||
|
||||
static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, float size, NVGcolor color, int font){
|
||||
nvgFontFaceId(vg, font);
|
||||
|
|
@ -67,12 +66,8 @@ static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, f
|
|||
|
||||
static void draw_chevron(UIState *s, float x_in, float y_in, float sz,
|
||||
NVGcolor fillColor, NVGcolor glowColor) {
|
||||
const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}};
|
||||
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
|
||||
|
||||
float x = p_full_frame.v[0];
|
||||
float y = p_full_frame.v[1];
|
||||
if (x < 0 || y < 0.){
|
||||
float x, y;
|
||||
if (!car_space_to_full_frame(s, x_in, y_in, 0.0, &x, &y)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -134,110 +129,52 @@ static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &le
|
|||
draw_chevron(s, d_rel, lead.getYRel(), 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW);
|
||||
}
|
||||
|
||||
static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) {
|
||||
if (pvd->cnt == 0) return;
|
||||
static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcolor *color, NVGpaint *paint) {
|
||||
if (cnt == 0) return;
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y);
|
||||
for (int i=1; i<pvd->cnt; i++) {
|
||||
nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
|
||||
nvgMoveTo(s->vg, std::clamp<float>(v[0].x, 0, s->fb_w), std::clamp<float>(v[0].y, 0, s->fb_h));
|
||||
for (int i = 1; i < cnt; i++) {
|
||||
nvgLineTo(s->vg, std::clamp<float>(v[i].x, 0, s->fb_w), std::clamp<float>(v[i].y, 0, s->fb_h));
|
||||
}
|
||||
nvgClosePath(s->vg);
|
||||
nvgFillColor(s->vg, color);
|
||||
if (color) {
|
||||
nvgFillColor(s->vg, *color);
|
||||
} else if (paint) {
|
||||
nvgFillPaint(s->vg, *paint);
|
||||
}
|
||||
nvgFill(s->vg);
|
||||
}
|
||||
|
||||
static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) {
|
||||
static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) {
|
||||
const UIScene *scene = &s->scene;
|
||||
const float *points = scene->path_points;
|
||||
const float *mpc_x_coords = &scene->mpc_x[0];
|
||||
const float *mpc_y_coords = &scene->mpc_y[0];
|
||||
|
||||
float off = is_mpc?0.3:0.5;
|
||||
float lead_d = scene->lead_data[0].getDRel()*2.;
|
||||
float path_height = is_mpc?(lead_d>5.)?fmin(lead_d, 25.)-fmin(lead_d*0.35, 10.):20.
|
||||
:(lead_d>0.)?fmin(lead_d, 50.)-fmin(lead_d*0.35, 10.):49.;
|
||||
path_height = fmin(path_height, scene->model.getPath().getValidLen());
|
||||
pvd->cnt = 0;
|
||||
// left side up
|
||||
for (int i=0; i<=path_height; i++) {
|
||||
float px, py, mpx;
|
||||
if (is_mpc) {
|
||||
mpx = i==0?0.0:mpc_x_coords[i];
|
||||
px = lerp(mpx+1.0, mpx, i/100.0);
|
||||
py = mpc_y_coords[i] - off;
|
||||
} else {
|
||||
px = lerp(i+1.0, i, i/100.0);
|
||||
py = points[i] - off;
|
||||
}
|
||||
|
||||
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
|
||||
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
|
||||
if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) {
|
||||
continue;
|
||||
}
|
||||
pvd->v[pvd->cnt].x = p_full_frame.v[0];
|
||||
pvd->v[pvd->cnt].y = p_full_frame.v[1];
|
||||
pvd->cnt += 1;
|
||||
}
|
||||
|
||||
// right side down
|
||||
for (int i=path_height; i>=0; i--) {
|
||||
float px, py, mpx;
|
||||
if (is_mpc) {
|
||||
mpx = i==0?0.0:mpc_x_coords[i];
|
||||
px = lerp(mpx+1.0, mpx, i/100.0);
|
||||
py = mpc_y_coords[i] + off;
|
||||
} else {
|
||||
px = lerp(i+1.0, i, i/100.0);
|
||||
py = points[i] + off;
|
||||
}
|
||||
|
||||
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
|
||||
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
|
||||
if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) {
|
||||
continue;
|
||||
}
|
||||
pvd->v[pvd->cnt].x = p_full_frame.v[0];
|
||||
pvd->v[pvd->cnt].y = p_full_frame.v[1];
|
||||
pvd->cnt += 1;
|
||||
}
|
||||
}
|
||||
|
||||
static void update_all_track_data(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
// Draw vision path
|
||||
update_track_data(s, false, &s->track_vertices[0]);
|
||||
|
||||
if (scene->controls_state.getEnabled()) {
|
||||
// Draw MPC path when engaged
|
||||
update_track_data(s, true, &s->track_vertices[1]);
|
||||
}
|
||||
}
|
||||
|
||||
static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) {
|
||||
if (pvd->cnt == 0) return;
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgMoveTo(s->vg, pvd->v[0].x, pvd->v[0].y);
|
||||
for (int i=1; i<pvd->cnt; i++) {
|
||||
nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
|
||||
}
|
||||
nvgClosePath(s->vg);
|
||||
|
||||
NVGpaint track_bg;
|
||||
if (is_mpc) {
|
||||
// Draw colored MPC track
|
||||
const NVGcolor clr = bg_colors[s->status];
|
||||
track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
|
||||
nvgRGBA(clr.r, clr.g, clr.b, 255), nvgRGBA(clr.r, clr.g, clr.b, 255/2));
|
||||
const float off = 0.5;
|
||||
int max_idx;
|
||||
float lead_d;
|
||||
if(s->sm->updated("radarState")) {
|
||||
lead_d = scene->lead_data[0].getDRel()*2.;
|
||||
} else {
|
||||
// Draw white vision track
|
||||
track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h*.4,
|
||||
COLOR_WHITE, COLOR_WHITE_ALPHA(0));
|
||||
lead_d = MAX_DRAW_DISTANCE;
|
||||
}
|
||||
nvgFillPaint(s->vg, track_bg);
|
||||
nvgFill(s->vg);
|
||||
float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE;
|
||||
path_length = fmin(path_length, scene->max_distance);
|
||||
|
||||
|
||||
vertex_data *v = &pvd->v[0];
|
||||
for (int i = 0; line.getX()[i] <= path_length and i < TRAJECTORY_SIZE; i++) {
|
||||
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y);
|
||||
max_idx = i;
|
||||
}
|
||||
for (int i = max_idx; i >= 0; i--) {
|
||||
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y);
|
||||
}
|
||||
pvd->cnt = v - pvd->v;
|
||||
}
|
||||
|
||||
static void ui_draw_track(UIState *s, track_vertices_data *pvd) {
|
||||
NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4,
|
||||
COLOR_WHITE, COLOR_WHITE_ALPHA(0));
|
||||
ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg);
|
||||
}
|
||||
|
||||
static void draw_frame(UIState *s) {
|
||||
|
|
@ -271,72 +208,48 @@ static void draw_frame(UIState *s) {
|
|||
glBindVertexArray(0);
|
||||
}
|
||||
|
||||
static inline bool valid_frame_pt(UIState *s, float x, float y) {
|
||||
return x >= 0 && x <= s->stream.bufs_info.width && y >= 0 && y <= s->stream.bufs_info.height;
|
||||
}
|
||||
|
||||
static void update_lane_line_data(UIState *s, const float *points, float off, model_path_vertices_data *pvd, float valid_len) {
|
||||
pvd->cnt = 0;
|
||||
int rcount = fmin(MODEL_PATH_MAX_VERTICES_CNT / 2, valid_len);
|
||||
for (int i = 0; i < rcount; i++) {
|
||||
float px = (float)i;
|
||||
float py = points[i] - off;
|
||||
const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
|
||||
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
|
||||
if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1]))
|
||||
continue;
|
||||
pvd->v[pvd->cnt].x = p_full_frame.v[0];
|
||||
pvd->v[pvd->cnt].y = p_full_frame.v[1];
|
||||
pvd->cnt += 1;
|
||||
static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) {
|
||||
// TODO check that this doesn't overflow max vertex buffer
|
||||
int max_idx;
|
||||
vertex_data *v = &pvd->v[0];
|
||||
for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) {
|
||||
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y);
|
||||
max_idx = i;
|
||||
}
|
||||
for (int i = rcount - 1; i > 0; i--) {
|
||||
float px = (float)i;
|
||||
float py = points[i] + off;
|
||||
const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
|
||||
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
|
||||
if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1]))
|
||||
continue;
|
||||
pvd->v[pvd->cnt].x = p_full_frame.v[0];
|
||||
pvd->v[pvd->cnt].y = p_full_frame.v[1];
|
||||
pvd->cnt += 1;
|
||||
for (int i = max_idx - 1; i > 0; i--) {
|
||||
v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y);
|
||||
}
|
||||
pvd->cnt = v - pvd->v;
|
||||
}
|
||||
|
||||
static void update_all_lane_lines_data(UIState *s, const cereal::ModelData::PathData::Reader &path, const float *points, model_path_vertices_data *pstart) {
|
||||
update_lane_line_data(s, points, 0.025*path.getProb(), pstart, path.getValidLen());
|
||||
update_lane_line_data(s, points, fmin(path.getStd(), 0.7), pstart + 1, path.getValidLen());
|
||||
}
|
||||
|
||||
static void ui_draw_lane(UIState *s, model_path_vertices_data *pstart, NVGcolor color) {
|
||||
ui_draw_lane_line(s, pstart, color);
|
||||
color.a /= 25;
|
||||
ui_draw_lane_line(s, pstart + 1, color);
|
||||
}
|
||||
|
||||
static void ui_draw_vision_lanes(UIState *s) {
|
||||
static void ui_draw_vision_lane_lines(UIState *s) {
|
||||
const UIScene *scene = &s->scene;
|
||||
model_path_vertices_data *pvd = &s->model_path_vertices[0];
|
||||
if(s->sm->updated("model")) {
|
||||
update_all_lane_lines_data(s, scene->model.getLeftLane(), scene->left_lane_points, pvd);
|
||||
update_all_lane_lines_data(s, scene->model.getRightLane(), scene->right_lane_points, pvd + MODEL_LANE_PATH_CNT);
|
||||
// paint lanelines
|
||||
line_vertices_data *pvd_ll = &s->lane_line_vertices[0];
|
||||
for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
|
||||
if(s->sm->updated("modelV2")) {
|
||||
update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance);
|
||||
}
|
||||
NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]);
|
||||
ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr);
|
||||
}
|
||||
|
||||
// Draw left lane edge
|
||||
ui_draw_lane(s, pvd, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getLeftLane().getProb()));
|
||||
|
||||
// Draw right lane edge
|
||||
ui_draw_lane(s, pvd + MODEL_LANE_PATH_CNT, nvgRGBAf(1.0, 1.0, 1.0, scene->model.getRightLane().getProb()));
|
||||
|
||||
if(s->sm->updated("radarState")) {
|
||||
update_all_track_data(s);
|
||||
|
||||
// paint road edges
|
||||
line_vertices_data *pvd_re = &s->road_edge_vertices[0];
|
||||
for (int re_idx = 0; re_idx < 2; re_idx++) {
|
||||
if(s->sm->updated("modelV2")) {
|
||||
update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance);
|
||||
}
|
||||
NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp<float>(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0));
|
||||
ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr);
|
||||
}
|
||||
|
||||
// Draw vision path
|
||||
ui_draw_track(s, false, &s->track_vertices[0]);
|
||||
if (scene->controls_state.getEnabled()) {
|
||||
// Draw MPC path when engaged
|
||||
ui_draw_track(s, true, &s->track_vertices[1]);
|
||||
|
||||
// paint path
|
||||
if(s->sm->updated("modelV2")) {
|
||||
update_track_data(s, scene->model.getPosition(), &s->track_vertices);
|
||||
}
|
||||
ui_draw_track(s, &s->track_vertices);
|
||||
}
|
||||
|
||||
// Draw all world space objects.
|
||||
|
|
@ -359,7 +272,7 @@ static void ui_draw_world(UIState *s) {
|
|||
nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
|
||||
|
||||
// Draw lane edges and vision/mpc tracks
|
||||
ui_draw_vision_lanes(s);
|
||||
ui_draw_vision_lane_lines(s);
|
||||
|
||||
// Draw lead indicators if openpilot is handling longitudinal
|
||||
if (s->longitudinal_control) {
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
#include <stdio.h>
|
||||
#include <cmath>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <signal.h>
|
||||
|
|
@ -24,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param
|
|||
}
|
||||
|
||||
void ui_init(UIState *s) {
|
||||
s->sm = new SubMaster({"model", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal",
|
||||
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal",
|
||||
"health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"});
|
||||
|
||||
s->started = false;
|
||||
|
|
@ -106,13 +107,6 @@ destroy:
|
|||
s->vision_connected = false;
|
||||
}
|
||||
|
||||
static inline void fill_path_points(const cereal::ModelData::PathData::Reader &path, float *points) {
|
||||
const capnp::List<float>::Reader &poly = path.getPoly();
|
||||
for (int i = 0; i < path.getValidLen(); i++) {
|
||||
points[i] = poly[0] * (i * i * i) + poly[1] * (i * i) + poly[2] * i + poly[3];
|
||||
}
|
||||
}
|
||||
|
||||
void update_sockets(UIState *s) {
|
||||
|
||||
UIScene &scene = s->scene;
|
||||
|
|
@ -178,11 +172,24 @@ void update_sockets(UIState *s) {
|
|||
scene.extrinsic_matrix.v[i] = extrinsicl[i];
|
||||
}
|
||||
}
|
||||
if (sm.updated("model")) {
|
||||
scene.model = sm["model"].getModel();
|
||||
fill_path_points(scene.model.getPath(), scene.path_points);
|
||||
fill_path_points(scene.model.getLeftLane(), scene.left_lane_points);
|
||||
fill_path_points(scene.model.getRightLane(), scene.right_lane_points);
|
||||
if (sm.updated("modelV2")) {
|
||||
scene.model = sm["modelV2"].getModelV2();
|
||||
scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE);
|
||||
for (int ll_idx = 0; ll_idx < 4; ll_idx++) {
|
||||
if (scene.model.getLaneLineProbs().size() > ll_idx) {
|
||||
scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx];
|
||||
} else {
|
||||
scene.lane_line_probs[ll_idx] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
for (int re_idx = 0; re_idx < 2; re_idx++) {
|
||||
if (scene.model.getRoadEdgeStds().size() > re_idx) {
|
||||
scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx];
|
||||
} else {
|
||||
scene.road_edge_stds[re_idx] = 1.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sm.updated("uiLayoutState")) {
|
||||
auto data = sm["uiLayoutState"].getUiLayoutState();
|
||||
|
|
|
|||
|
|
@ -55,9 +55,8 @@ const Rect home_btn = {60, 1080 - 180 - 40, 180, 180};
|
|||
|
||||
const int UI_FREQ = 20; // Hz
|
||||
|
||||
const int MODEL_PATH_MAX_VERTICES_CNT = 98;
|
||||
const int MODEL_LANE_PATH_CNT = 2;
|
||||
const int TRACK_POINTS_MAX_CNT = 50 * 2;
|
||||
const int MODEL_PATH_MAX_VERTICES_CNT = TRAJECTORY_SIZE*2;
|
||||
const int TRACK_POINTS_MAX_CNT = TRAJECTORY_SIZE*4;
|
||||
|
||||
const int SET_SPEED_NA = 255;
|
||||
|
||||
|
|
@ -83,10 +82,14 @@ static std::map<UIStatus, NVGcolor> bg_colors = {
|
|||
{STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)},
|
||||
};
|
||||
|
||||
typedef struct UIScene {
|
||||
typedef struct {
|
||||
float x[TRAJECTORY_SIZE];
|
||||
float y[TRAJECTORY_SIZE];
|
||||
float z[TRAJECTORY_SIZE];
|
||||
} line;
|
||||
|
||||
float mpc_x[50];
|
||||
float mpc_y[50];
|
||||
|
||||
typedef struct UIScene {
|
||||
|
||||
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
|
||||
bool world_objects_visible;
|
||||
|
|
@ -112,10 +115,17 @@ typedef struct UIScene {
|
|||
cereal::ControlsState::Reader controls_state;
|
||||
cereal::DriverState::Reader driver_state;
|
||||
cereal::DMonitoringState::Reader dmonitoring_state;
|
||||
cereal::ModelData::Reader model;
|
||||
float left_lane_points[MODEL_PATH_DISTANCE];
|
||||
float path_points[MODEL_PATH_DISTANCE];
|
||||
float right_lane_points[MODEL_PATH_DISTANCE];
|
||||
cereal::ModelDataV2::Reader model;
|
||||
line path;
|
||||
line outer_left_lane_line;
|
||||
line left_lane_line;
|
||||
line right_lane_line;
|
||||
line outer_right_lane_line;
|
||||
line left_road_edge;
|
||||
line right_road_edge;
|
||||
float max_distance;
|
||||
float lane_line_probs[4];
|
||||
float road_edge_stds[2];
|
||||
} UIScene;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -125,7 +135,7 @@ typedef struct {
|
|||
typedef struct {
|
||||
vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
|
||||
int cnt;
|
||||
} model_path_vertices_data;
|
||||
} line_vertices_data;
|
||||
|
||||
typedef struct {
|
||||
vertex_data v[TRACK_POINTS_MAX_CNT];
|
||||
|
|
@ -190,8 +200,9 @@ typedef struct UIState {
|
|||
bool alert_blinked;
|
||||
float alert_blinking_alpha;
|
||||
|
||||
track_vertices_data track_vertices[2];
|
||||
model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
|
||||
track_vertices_data track_vertices;
|
||||
line_vertices_data lane_line_vertices[4];
|
||||
line_vertices_data road_edge_vertices[2];
|
||||
|
||||
Rect video_rect;
|
||||
} UIState;
|
||||
|
|
|
|||
Loading…
Reference in New Issue