mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-07-24 04:52:13 +08:00
IQ.Pilot Release Commit @ 70bd095
This commit is contained in:
IQ.Lvbs CI [bot]
@@ -16,23 +16,28 @@
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "839bc8e7b29e58f3182ee7b551df1687b858f47b5698ccfe1f4f0da3406727c2",
|
||||
"sha256": "ec590435888e03a83c9fbfb01102089b077de0840ae0bb2e0969e76213d633aa",
|
||||
"size": 135536
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "7188e3de16f358a1dfb7130cf8aa7f12d2861bd68f056bff9a805c14883f3016",
|
||||
"size": 67664
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "ee1376eeef9c4bf72673d4c34092697b2c5a4c86136db354190e9dfdcbee0b7a",
|
||||
"sha256": "7218ad7ac087c68fe7d466e4c3468a6ab3b48c0fe45711e546e53823039c365b",
|
||||
"size": 204624
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "bdc32de29822d49e40b9190e8116aeb0d6ecf48806ea856d64ab430130835bf7",
|
||||
"sha256": "56d60b113a982e2d4fee8b4f00eaf111fd09834fbe8bf95f382b49e0bf13c121",
|
||||
"size": 69752
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "59e3669444d7f125f753dcd5c3c4191d375ffb11fc77a9f49062307ceeb055b5",
|
||||
"size": 203056
|
||||
"sha256": "75f101e930d93044d2e074129207c8246ff7d94df77b0f83db7de665eef62ab6",
|
||||
"size": 203072
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/__init__.py": {
|
||||
"mode": 420,
|
||||
@@ -41,67 +46,67 @@
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip": {
|
||||
"mode": 420,
|
||||
"sha256": "b77e971e2f1e7ad2561c3da2159e2ad3c7121b57d5adee8767589b7d3988a25a",
|
||||
"sha256": "acc64ce2fe59583738ab5951f64e79d8043856317eb015dc1d0e6a4bffc51ee2",
|
||||
"size": 2100238
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "25883f278987578fb4bca7051508612fff5b76131e147497ca082f53e69550ed",
|
||||
"sha256": "ee2330e3ba4f488336d18d887fc19291f76a1733a24c203a642634b7106a85a1",
|
||||
"size": 268064
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "5604a2d97d179922df4f075fb6902310c2dac0a4b5fa5bafd02701767303034f",
|
||||
"sha256": "71568e01b8c983ca80d825bea610334b140c87da9e6ac3047b21d9c56c136423",
|
||||
"size": 340584
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "b17176800f2ca75c47a824c0cd17ff490e8cfce2e1c4fdda2591dac9ce6d62e5",
|
||||
"sha256": "7f47eb50931e9df19116736ef6564b2d5480a7424c24cd1d599424dc0da66537",
|
||||
"size": 68048
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "337099e876dd5cd0deeaf2034c5393efdd8101e4ece1edfaeddad1f8794f8b1a",
|
||||
"sha256": "e4c0882530ab3097ecadbb586ac7e35955b587f81d9af2c57fea1b5450c2e5ef",
|
||||
"size": 335728
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "8fb8d1b6b4e224d936a4908b7bd9d67c9448cbc2e0ab5363b5447482923ab726",
|
||||
"sha256": "62bbf38d79bece455bdb399d7124b37347c9ec2ebdf35ff2ce4b696b39c59462",
|
||||
"size": 271680
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "f165bcdba3b797c86b515d500265be43893232d32ea6f9df1ffd82afd1e5d397",
|
||||
"sha256": "b59b5d1bc50e5064206752cb30da118f2fa42f48c4cc2e6529ebbe04b28200a4",
|
||||
"size": 134408
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "f53fb96fe15ac11b2625390a031891f71e1312230bce78955adbd6b439db5ccd",
|
||||
"size": 3448176
|
||||
"sha256": "1d7b19f9d33649e91e5707574bdf436bc0e5b6dafd63d4b1aa63949bb6f7be20",
|
||||
"size": 3448272
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "1119f5b0c204999236cbf647f81d7c5d188934c243fe5c12f856db01093d1342",
|
||||
"sha256": "8a5e6e08eb15a8295f08bd4629f80f6bf2539ec7eea7b6a787af3ed5b2546e35",
|
||||
"size": 136448
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "c82f530ad3faa3239686b416041298f91d4207b842689de176776fee13412dda",
|
||||
"sha256": "ead654070e296ae23fd8afdbc0a1c2f3eb8d1ff04a905ca3f2fbd8e20316ed6b",
|
||||
"size": 68136
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "ce0e8e963d635499704fcc03358413e3f5c30c541ffc8f74a331066651e8200d",
|
||||
"sha256": "38c55fe55500aca574ab5bf3ed693e54ca192db276c247a31209470319d6578a",
|
||||
"size": 67840
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "826999c870d16354f1b740247a4c7e81e284e06cafe91dc080a9b63460d7bf3f",
|
||||
"sha256": "0547db3c95f399b9721ae4f7306b57d4124283adda0213f7df5069a3837a7f98",
|
||||
"size": 135648
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "c69b553bf43c002494df334a8c4e6385f5e65369b7ccc1215b60df2367e6c8ea",
|
||||
"sha256": "f7788e2023012537b40d6aa5109e9d913b7cb27e1de80eadc0ebe12a2ab7be1a",
|
||||
"size": 269568
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/uploaderd/__init__.py": {
|
||||
@@ -111,7 +116,7 @@
|
||||
},
|
||||
"python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": {
|
||||
"mode": 493,
|
||||
"sha256": "76643345ea8ef0af72daa1b649389fcd1c2574a877d25a14dc1ef061cba8d672",
|
||||
"sha256": "efbef0e4b63a65d846c4cd50f89cb3fdfe0d12da3301e8b1c9137f9a5f02c753",
|
||||
"size": 204184
|
||||
},
|
||||
"runtime": {
|
||||
@@ -143,22 +148,23 @@
|
||||
}
|
||||
},
|
||||
"signatures": {
|
||||
"python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": "8VLEwluJlz6qARphX2M2OmfZVupPKypS9LnaiPrMoV9thvudyMfKu9UYyPd+9LDI9KQ5Nv0AnBuoVZhqGIXbAw==",
|
||||
"python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": "4IAicJpyOSAye7WdpzAbkc71LrwsE37EqAtryK54E6CMDpxetiEGpsqOR28MRDBav01LEW89aHA48Yxz1m85Bg==",
|
||||
"python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": "BjisouIbl7J1epsKV/NU69JvByDhlaY4lQeD6syIE0myI81tFauyFenystnDUaOMhmYhkrN+s2Euu8kNas/XCQ==",
|
||||
"python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": "9INjJFxL8tPU2L0elHm+D6HifpVLciLkE+kCIkMHL5bfntldk+I7jir7YxH0+vby0Qo807IFcgWJXgY36zPwBg==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": "srRZKVgQW7QNDE4hN09mzF94+O2PUIKWCxsffd96n+Gg4heSxrlq3yiEP8Ssati1HlzS8+J8qGyEEYCbBSd/DQ==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": "RQnudpb8oix67avhaaWwk3xhv1jxojuTD7Dn+kZRDClaqoMGn7eBX0ny0lN50jTdHGaTNVrqtA7whh66JEwXDQ==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": "bWcpgy4X2iF6M9Ac9Dp0fUHgHa0JuEt+EM+ZggzLxHoI84Vla5krD3ZD9UBYbOL9oK0w8W0BGYmpeFqPFwP0Cw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": "+c7G7W5hHKg0tImEff+a6dcxC6LRi2Pk7dNzNTrUoSnahlK+B1F+WCHAX0vhVfj43zg83QEQx53905COgSwXCA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": "pPhZUUmVCbQz9cNz/z0AcC7DosMQ28xoxEa2xvyAj7n/QiO5/Oe2xy3Xl97AoOdY/TaBcR/3iOk9fDN5pfHJAQ==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": "oSE9/T3UFz/00VbijRvoYVBThNsu4fuUk2suzo9VPrVMEfzZ95wDsoiqla1wgBQB9pfT9g3CamBbhg87LjT+Ag==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": "N+I7b7kyZONsfEpoBZeLXExt3CUOidoU5DRoMyKotbFIUW3WTDZRRZMejWN9Ihh77zQEsiICPM/74GcU1KHNBA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": "Ag5tuuf3MxvxBdgBmvLZEEEftIHLq2jUYyO9R6viD20kpxsuG/t1rJVs5saliBL98ejoCkKzfxiW7XA+tAakCQ==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": "8xR3ozu7k/hYlMuW3U3n+yWWkxiII5pCM40qzUPP7DGKcjUBnH3mveHplPzlSnSoWVx+ZGfSbcFR5gz0Oip/AA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": "fODQbD6yDanJ06rrSbBhpMxNbAxXrlQ2KlElOerxBIMKQjJsE/Ew9AnLv0fEPUe7FSM+rL1hH8ur0UHnACyvBw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": "qCY1O6EwcROnnJtGNo0Tx+2bdCaDuG6tDaSaFV5VbwKh4Je1l0vclXJqqcYGhO3BtiDH9ehTSeDfbPGFwX9WCA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": "uNJGaiiojTXNXEDnNejYQZANEzPXktYJfrs7MzBt3a/ERmdzwRnJh+IXDzYPkZXxPUDXUUOkiyAzf+IZEMc7DA==",
|
||||
"python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": "2H4eWj9LmvC4sXbxkE8aE7H9M2q4aj06q25muZuwuZGkb42WTfhSq36PNBI07OzngF5B8+zk5+jbS6KPxBSEBA=="
|
||||
"python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": "tMSz9Dm9XQQkQzePUaDhhcM065JH2fcqAE3f+dlA2/+264m/j2ayWMYFMXTWZMTDm1YaQT8e9lvBe8sc4JSVDQ==",
|
||||
"python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": "fvD5Ii/rNZuynr8Nh+K+yNC+AMji7v53spO7rj7Fs61CvO604fWnKVUO7A/2d7KHh26YpUcjbv8wRb8tRTzfCg==",
|
||||
"python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": "KcxQfc5HVD8QOLY/KHWsrhWpbYETAxsxR9WjFOqqmYe0aEC7K4njmJPtU1eCU3B2S6MGMmUdABBHs8kr32ViDQ==",
|
||||
"python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": "9jTowLTfwEw9Iu3wgfFordJmJ+/+O+nPvq4TP+3CDddW1jISzSMhiw418iyH1f0m7rwB3hGyZYgGLoJszSHpBw==",
|
||||
"python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": "YvM513CnlMKSrIrCgrCNG/k7XyFjSlnEupgWH0Jhj0zKrE8dkeCiXek7iU9W+VpgXa6O8P3jTJUWchqLhnONBw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": "bhNLMkcIgi0F/CZsEDHRQsNHyPBD+pVh3DPowBz7mRYxQffVGzk1FeE2Gp3P7xxHsnyKsy635sAkdwKOlZLtBQ==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": "Huy5oGlX55p5bhoMQipY9/doTBBAfwnQhgPdM6aeYTrzBS/QOUqwjyk3qjanftHA4m0V4Jab8cVvjEt1OiTsBw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": "2BmMUg8uOE4eoIW/OTbuwrZCMA+Rx4SOZWLBYs4GUbD8JR5Ei0nQ6VKLar31Ue7NYk7qBfpgqFdGPnV1uST/Dw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": "6nFN6dJAmGczneUwRtJr+1TLNrD9reuMQcoa2G2j0np9xaSU401MdBssdVFSFBpse95iqQX1uV8ojQD327z1CA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": "8nKxC5LPkwPihlIdJzcD6OTp42avONqDkNgZrmTaQr8oueczFwXnHsgJ9aZ8jrpDK/nN+GLJrniSdFxZjrUUDA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": "y8dA+6ahkX+Evzn8oSQEPyDB6+d+t//v5rsEdg4zRbjdeLeXs5kqraMEaAALVH80CvNJ6Gba1CcO1ebrK00EBQ==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": "jczf5QX+Xm0tOLPBrmvJoPZmcKar5zTrxoX7RAOR1JQgHu3jWPFW+sWOTK8c8ByiVF5PdLeYOPdQgu+0KQvdCg==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": "s4HcwSXfBowW/vfY5M4GKg7gEueRJnbmq6zO1dAXx6HrTXYsL9iDZTeGHi7dNu/rw8mUFSkkpWsaFTlaYdK0DA==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": "LVgImFZgV/hgoBwMfMVWSEKuXUvc2jR/EPl6SOfmMncoyuIZUkpQa18jjdMTWqXbRGpFvAe+0KocmroFTrbqDg==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": "vXffOAHGUAdEkv321fidnhZvUMaX73l2kO5/Y5Sk43+mr9V95Wcv4TR54/2Y2N4UfgtxnGFz8N0llK/DsQLEAw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": "wXVM74ZUEwSVnM7cMqzXQYCWhQwVovszTjxYyMhDFjwZlWtNPPGeq4WghHoghGolGSG9Q4SOLxh8uF67BU2RDw==",
|
||||
"python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": "1cBBo9W9EBxW7XF9o2MimKA9Xm26zaCEsUNHLF81zm2zWxkoSkeVlLyrxHE9ZxGhgMRMfW3lz4XUJbWRJpjGDA==",
|
||||
"python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": "vqZyPMmER7bN7AvE4gwZVF4HypLwTxBfk4tidjRhVDZ2eKYbbG2yROd2z2xtk5Shf8GhdLsT7UhVrN+BKNnkBA=="
|
||||
}
|
||||
}
|
||||
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
+85
-8
@@ -70,12 +70,12 @@ struct OnroadEvent @0xc4fa6047f024e718 {
|
||||
longitudinalManeuver @30;
|
||||
steerTempUnavailableSilent @31;
|
||||
resumeRequired @32;
|
||||
preDriverDistracted @33;
|
||||
promptDriverDistracted @34;
|
||||
driverDistracted @35;
|
||||
preDriverUnresponsive @36;
|
||||
promptDriverUnresponsive @37;
|
||||
driverUnresponsive @38;
|
||||
driverDistracted1 @33;
|
||||
driverDistracted2 @34;
|
||||
driverDistracted3 @35;
|
||||
driverUnresponsive1 @36;
|
||||
driverUnresponsive2 @37;
|
||||
driverUnresponsive3 @38;
|
||||
belowSteerSpeed @39;
|
||||
lowBattery @40;
|
||||
accFaulted @41;
|
||||
@@ -2183,6 +2183,7 @@ struct DriverStateV2 {
|
||||
rightBlinkProb @8 :Float32;
|
||||
sunglassesProb @9 :Float32;
|
||||
phoneProb @13 :Float32;
|
||||
sleepProb @14 :Float32;
|
||||
notReadyProbDEPRECATED @12 :List(Float32);
|
||||
occludedProbDEPRECATED @10 :Float32;
|
||||
readyProbDEPRECATED @11 :List(Float32);
|
||||
@@ -2224,7 +2225,7 @@ struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 {
|
||||
stdDEPRECATED @2 :Float32;
|
||||
}
|
||||
|
||||
struct DriverMonitoringState @0xb83cda094a1da284 {
|
||||
struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 {
|
||||
events @18 :List(OnroadEvent);
|
||||
faceDetected @1 :Bool;
|
||||
isDistracted @2 :Bool;
|
||||
@@ -2250,6 +2251,81 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
|
||||
eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED);
|
||||
}
|
||||
|
||||
struct DriverMonitoringState {
|
||||
lockout @0 :Bool;
|
||||
lockoutCount @15 :Int8;
|
||||
lockoutMinutesRemaining @11 :Int8;
|
||||
alert3Count @12 :Int8;
|
||||
noResponseCount @13 :Int8;
|
||||
noResponseForceDecel @14 :Bool;
|
||||
|
||||
alwaysOn @3 :Bool;
|
||||
alwaysOnLockout @4 :Bool;
|
||||
|
||||
alertLevel @5 :AlertLevel;
|
||||
activePolicy @6 :MonitoringPolicy;
|
||||
isRHD @7 :Bool;
|
||||
rhdCalibration @8 :CalibrationState;
|
||||
|
||||
visionPolicyState @9 :VisionPolicyState;
|
||||
wheeltouchPolicyState @10 :WheeltouchPolicyState;
|
||||
|
||||
enum AlertLevel {
|
||||
none @0;
|
||||
one @1;
|
||||
two @2;
|
||||
three @3;
|
||||
}
|
||||
|
||||
enum MonitoringPolicy {
|
||||
wheeltouch @0;
|
||||
vision @1;
|
||||
}
|
||||
|
||||
struct VisionPolicyState {
|
||||
awarenessPercent @0 :Int8;
|
||||
awarenessStep @1 :Float32;
|
||||
isDistracted @2 :Bool;
|
||||
distractedTypes @3 :DistractedTypes;
|
||||
|
||||
faceDetected @4 :Bool;
|
||||
pose @5 :Pose;
|
||||
wheeltouchFallbackPercent @6 :Int8;
|
||||
uncertainOffroadAlertPercent @7 :Int8;
|
||||
|
||||
struct DistractedTypes {
|
||||
pose @0: Bool;
|
||||
eye @1: Bool;
|
||||
phone @2: Bool;
|
||||
}
|
||||
|
||||
struct Pose {
|
||||
pitch @0 :Float32;
|
||||
yaw @1 :Float32;
|
||||
pitchCalib @2 :CalibrationState;
|
||||
yawCalib @3 :CalibrationState;
|
||||
calibrated @4 :Bool;
|
||||
uncertainty @5 :Float32;
|
||||
}
|
||||
}
|
||||
|
||||
struct WheeltouchPolicyState {
|
||||
awarenessPercent @0 :Int8;
|
||||
awarenessStep @1 :Float32;
|
||||
driverInteracting @2 :Bool;
|
||||
}
|
||||
|
||||
struct CalibrationState {
|
||||
calibratedPercent @0 :Int8;
|
||||
offset @1 :Float32;
|
||||
}
|
||||
|
||||
deprecated :group {
|
||||
alertCountLockoutPercent @1 :Int8;
|
||||
alertTimeLockoutPercent @2 :Int8;
|
||||
}
|
||||
}
|
||||
|
||||
struct Boot {
|
||||
wallTimeNanos @0 :UInt64;
|
||||
pstore @4 :Map(Text, Data);
|
||||
@@ -2570,7 +2646,7 @@ struct Event {
|
||||
thumbnail @66: Thumbnail;
|
||||
onroadEvents @134: List(OnroadEvent);
|
||||
carParams @69: Car.CarParams;
|
||||
driverMonitoringState @71: DriverMonitoringState;
|
||||
driverMonitoringState @165: DriverMonitoringState;
|
||||
livePose @129 :LivePose;
|
||||
modelV2 @75 :ModelDataV2;
|
||||
drivingModelData @128 :DrivingModelData;
|
||||
@@ -2693,6 +2769,7 @@ struct Event {
|
||||
wifiScanDEPRECATED @29 :List(Legacy.WifiScan);
|
||||
uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent;
|
||||
liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED;
|
||||
driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED;
|
||||
gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints;
|
||||
gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan;
|
||||
applanixRawDEPRECATED @42 :Data;
|
||||
|
||||
@@ -41,6 +41,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}},
|
||||
{"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}},
|
||||
{"AlphaLongitudinalEnabled", {PERSISTENT, BOOL}},
|
||||
{"ExperimentalMode", {PERSISTENT, BOOL}},
|
||||
{"ExperimentalModeConfirmed", {PERSISTENT, BOOL}},
|
||||
|
||||
@@ -214,9 +214,10 @@ class NoEntryCard(AlertCard):
|
||||
def __init__(self,
|
||||
alert_text_2: str,
|
||||
alert_text_1: str = "openpilot Unavailable",
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none):
|
||||
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
|
||||
priority: Tier = Tier.LOW):
|
||||
primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
|
||||
super().__init__(primary, secondary, AlertStatus.normal, size, Tier.LOW, visual_alert, AudibleAlert.refuse, 3.0)
|
||||
super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
|
||||
|
||||
|
||||
class GentleDisableCard(AlertCard):
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -260,7 +260,7 @@ class Controls(IQControlsLayer):
|
||||
hudControl.leadFollowTime = 1.45
|
||||
hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
|
||||
hudControl.audibleAlert = self.sm['selfdriveState'].alertSound
|
||||
hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive'
|
||||
hudControl.driverUnresponsive = self.sm['driverMonitoringState'].noResponseForceDecel
|
||||
|
||||
hudControl.rightLaneVisible = True
|
||||
hudControl.leftLaneVisible = True
|
||||
@@ -292,7 +292,7 @@ class Controls(IQControlsLayer):
|
||||
cs.upAccelCmd = float(self.LoC.pid.p)
|
||||
cs.uiAccelCmd = float(self.LoC.pid.i)
|
||||
cs.ufAccelCmd = float(self.LoC.pid.f)
|
||||
cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
|
||||
cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or
|
||||
(self.sm['selfdriveState'].state == State.softDisabling))
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
|
||||
@@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False):
|
||||
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
|
||||
|
||||
# DM sequence
|
||||
(EventName.preDriverDistracted, ET.WARNING),
|
||||
(EventName.promptDriverDistracted, ET.WARNING),
|
||||
(EventName.driverDistracted, ET.WARNING),
|
||||
(EventName.driverDistracted1, ET.WARNING),
|
||||
(EventName.driverDistracted2, ET.WARNING),
|
||||
(EventName.driverDistracted3, ET.WARNING),
|
||||
]
|
||||
|
||||
# debug alerts
|
||||
|
||||
@@ -75,7 +75,7 @@ def parse_model_output(model_output):
|
||||
face_descs = model_output[f'face_descs_{ds_suffix}']
|
||||
parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
|
||||
parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
|
||||
for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']:
|
||||
parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
|
||||
return parsed
|
||||
|
||||
@@ -91,6 +91,7 @@ def fill_driver_data(msg, model_output, ds_suffix):
|
||||
msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
|
||||
msg.sleepProb = model_output[f'sleep_prob_{ds_suffix}'][0, 0].item()
|
||||
|
||||
def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
|
||||
msg = messaging.new_message('driverStateV2', valid=True)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,9 +1,9 @@
|
||||
{
|
||||
"dmonitoring_model": {
|
||||
"outputs": {
|
||||
"dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
|
||||
"dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52"
|
||||
"dmonitoring_model_metadata.pkl": "5999c262b1c25c62e485fb4ced5806d20a8ca59e3ae94e0ff499c0fe3497fedc",
|
||||
"dmonitoring_model_tinygrad.pkl": "5aca89a35b42376d56f67ccd28a1080806706d546dd8c63f6e1b4c681f8e2c01"
|
||||
},
|
||||
"signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349"
|
||||
"signature": "2364ebd4bb95c4b4e539c9b1ba68324b713cbb56617396b73262accf9cdcfbfc"
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
import pickle
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.modeld.dmonitoringmodeld import get_driverstate_packet, parse_model_output, slice_outputs
|
||||
from openpilot.selfdrive.modeld.dmonitoringmodeld import METADATA_PATH
|
||||
|
||||
|
||||
def test_sleep_probability_output():
|
||||
with open(METADATA_PATH, 'rb') as f:
|
||||
metadata = pickle.load(f)
|
||||
|
||||
output = np.zeros(metadata['output_shapes']['outputs'][1], dtype=np.float32)
|
||||
parsed = parse_model_output(slice_outputs(output, metadata['output_slices']))
|
||||
parsed['raw_pred'] = b''
|
||||
msg = get_driverstate_packet(parsed, 1, 0, 0., 0.)
|
||||
|
||||
assert msg.driverStateV2.leftDriverData.sleepProb == 0.5
|
||||
assert msg.driverStateV2.rightDriverData.sleepProb == 0.5
|
||||
@@ -1,15 +0,0 @@
|
||||
# driver monitoring (DM)
|
||||
|
||||
Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle.
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
Before creating a bug report, go through these troubleshooting steps.
|
||||
|
||||
* Ensure the driver-facing camera has a good view of the driver in normal driving positions.
|
||||
* This can be checked in Settings -> Device -> Preview Driver Camera (when car is off).
|
||||
* If the camera can't see the driver, the device should be re-mounted.
|
||||
|
||||
## Bug report
|
||||
|
||||
In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive.
|
||||
@@ -1,8 +1,20 @@
|
||||
#!/usr/bin/env python3
|
||||
from types import SimpleNamespace
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.selfdrive.monitoring.helpers import DriverMonitoring
|
||||
from openpilot.selfdrive.monitoring.policy import DriverMonitoring
|
||||
|
||||
|
||||
def get_dm_inputs(sm):
|
||||
return {
|
||||
'driverStateV2': sm['driverStateV2'],
|
||||
'liveCalibration': sm['liveCalibration'],
|
||||
'carState': sm['carState'],
|
||||
'selfdriveState': SimpleNamespace(enabled=sm['selfdriveState'].enabled or sm['carControl'].latActive),
|
||||
'modelV2': sm['modelV2'],
|
||||
}
|
||||
|
||||
|
||||
def dmonitoringd_thread():
|
||||
@@ -25,9 +37,9 @@ def dmonitoringd_thread():
|
||||
|
||||
valid = sm.all_checks()
|
||||
if demo_mode and sm.valid['driverStateV2']:
|
||||
DM.run_step(sm, demo=demo_mode)
|
||||
DM.run_step(get_dm_inputs(sm), demo=True)
|
||||
elif valid:
|
||||
DM.run_step(sm, demo=demo_mode)
|
||||
DM.run_step(get_dm_inputs(sm), demo=demo_mode)
|
||||
|
||||
# publish
|
||||
dat = DM.get_state_packet(valid=valid)
|
||||
@@ -40,9 +52,9 @@ def dmonitoringd_thread():
|
||||
|
||||
# save rhd virtual toggle every 5 mins
|
||||
if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
|
||||
DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
|
||||
DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
|
||||
params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
|
||||
DM.wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
|
||||
DM.wheel_on_right == (DM.wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
|
||||
params.put_bool("IsRhdDetected", DM.wheel_on_right)
|
||||
|
||||
def main():
|
||||
dmonitoringd_thread()
|
||||
|
||||
@@ -1,474 +0,0 @@
|
||||
from math import atan2
|
||||
import numpy as np
|
||||
|
||||
from cereal import car, log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.selfdrive.selfdrived.events import Events
|
||||
from openpilot.common.realtime import DT_DMON
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.stat_live import RunningStatFilter
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
# ******************************************************************************************
|
||||
# NOTE: To fork maintainers.
|
||||
# Disabling or nerfing safety features will get you and your users banned from our servers.
|
||||
# We recommend that you do not change these numbers from the defaults.
|
||||
# ******************************************************************************************
|
||||
|
||||
class DRIVER_MONITOR_SETTINGS:
|
||||
def __init__(self, device_type):
|
||||
self._DT_DMON = DT_DMON
|
||||
# ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
|
||||
self._AWARENESS_TIME = 30. # passive wheeltouch total timeout
|
||||
self._AWARENESS_PRE_TIME_TILL_TERMINAL = 15.
|
||||
self._AWARENESS_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||
self._DISTRACTED_TIME = 11. # active monitoring total timeout
|
||||
self._DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
|
||||
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||
|
||||
self._FACE_THRESHOLD = 0.7
|
||||
self._EYE_THRESHOLD = 0.65
|
||||
self._SG_THRESHOLD = 0.9
|
||||
self._BLINK_THRESHOLD = 0.865
|
||||
|
||||
self._PHONE_THRESH = 0.75 if device_type == 'mici' else 0.4
|
||||
self._PHONE_THRESH2 = 15.0
|
||||
self._PHONE_MAX_OFFSET = 0.06
|
||||
self._PHONE_MIN_OFFSET = 0.025
|
||||
self._PHONE_DATA_AVG = 0.05
|
||||
self._PHONE_DATA_VAR = 3*0.005
|
||||
self._PHONE_MAX_COUNT = int(360 / self._DT_DMON)
|
||||
|
||||
self._POSE_PITCH_THRESHOLD = 0.3133
|
||||
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
|
||||
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
|
||||
self._POSE_YAW_THRESHOLD = 0.4020
|
||||
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
|
||||
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
|
||||
self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_THRESHOLD = 0.449
|
||||
self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_VAR = 3*0.01
|
||||
self._YAW_NATURAL_VAR = 3*0.05
|
||||
self._PITCH_MAX_OFFSET = 0.124
|
||||
self._PITCH_MIN_OFFSET = -0.0881
|
||||
self._YAW_MAX_OFFSET = 0.289
|
||||
self._YAW_MIN_OFFSET = -0.0246
|
||||
|
||||
self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
|
||||
self._DCAM_UNCERTAIN_RESET_COUNT = int(20 / self._DT_DMON)
|
||||
self._POSESTD_THRESHOLD = 0.3
|
||||
self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s
|
||||
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||
self._ALWAYS_ON_ALERT_MIN_SPEED = 11
|
||||
|
||||
self._POSE_CALIB_MIN_SPEED = 13 # 30 mph
|
||||
self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative
|
||||
self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
|
||||
|
||||
self._WHEELPOS_CALIB_MIN_SPEED = 11
|
||||
self._WHEELPOS_THRESHOLD = 0.5
|
||||
self._WHEELPOS_FILTER_MIN_COUNT = int(15 / self._DT_DMON) # allow 15 seconds to converge wheel side
|
||||
self._WHEELPOS_DATA_AVG = 0.03
|
||||
self._WHEELPOS_DATA_VAR = 3*5.5e-5
|
||||
self._WHEELPOS_MAX_COUNT = -1
|
||||
|
||||
self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change
|
||||
self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
|
||||
|
||||
self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
|
||||
self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts
|
||||
|
||||
class DistractedType:
|
||||
|
||||
NOT_DISTRACTED = 0
|
||||
DISTRACTED_POSE = 1 << 0
|
||||
DISTRACTED_BLINK = 1 << 1
|
||||
DISTRACTED_PHONE = 1 << 2
|
||||
|
||||
class DriverPose:
|
||||
def __init__(self, settings):
|
||||
pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
|
||||
yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
|
||||
self.yaw = 0.
|
||||
self.pitch = 0.
|
||||
self.roll = 0.
|
||||
self.yaw_std = 0.
|
||||
self.pitch_std = 0.
|
||||
self.roll_std = 0.
|
||||
self.pitch_offseter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.yaw_offseter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.calibrated = False
|
||||
self.low_std = True
|
||||
self.cfactor_pitch = 1.
|
||||
self.cfactor_yaw = 1.
|
||||
|
||||
class DriverProb:
|
||||
def __init__(self, raw_priors, max_trackable):
|
||||
self.prob = 0.
|
||||
self.prob_offseter = RunningStatFilter(raw_priors=raw_priors, max_trackable=max_trackable)
|
||||
self.prob_calibrated = False
|
||||
|
||||
class DriverBlink:
|
||||
def __init__(self):
|
||||
self.left = 0.
|
||||
self.right = 0.
|
||||
|
||||
|
||||
# model output refers to center of undistorted+leveled image
|
||||
EFL = 598.0 # focal length in K
|
||||
cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw
|
||||
W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw
|
||||
|
||||
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
|
||||
# the output of these angles are in device frame
|
||||
# so from driver's perspective, pitch is up and yaw is right
|
||||
|
||||
pitch_net, yaw_net, roll_net = angles_desc
|
||||
|
||||
face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H)
|
||||
yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL)
|
||||
pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL)
|
||||
|
||||
pitch = pitch_net + pitch_focal_angle
|
||||
yaw = -yaw_net + yaw_focal_angle
|
||||
|
||||
# no calib for roll
|
||||
pitch -= rpy_calib[1]
|
||||
yaw -= rpy_calib[2]
|
||||
return roll_net, pitch, yaw
|
||||
|
||||
|
||||
class DriverMonitoring:
|
||||
def __init__(self, rhd_saved=False, settings=None, always_on=False):
|
||||
# init policy settings
|
||||
self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
|
||||
|
||||
# init driver status
|
||||
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
|
||||
phone_filter_raw_priors = (self.settings._PHONE_DATA_AVG, self.settings._PHONE_DATA_VAR, 2)
|
||||
self.wheelpos = DriverProb(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
|
||||
self.phone = DriverProb(raw_priors=phone_filter_raw_priors, max_trackable=self.settings._PHONE_MAX_COUNT)
|
||||
self.pose = DriverPose(settings=self.settings)
|
||||
self.blink = DriverBlink()
|
||||
|
||||
self.always_on = always_on
|
||||
self.distracted_types = []
|
||||
self.driver_distracted = False
|
||||
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
|
||||
self.wheel_on_right = False
|
||||
self.wheel_on_right_last = None
|
||||
self.wheel_on_right_default = rhd_saved
|
||||
self.face_detected = False
|
||||
self.terminal_alert_cnt = 0
|
||||
self.terminal_time = 0
|
||||
self.step_change = 0.
|
||||
self.active_monitoring_mode = True
|
||||
self.is_model_uncertain = False
|
||||
self.hi_stds = 0
|
||||
self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
|
||||
self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
|
||||
self.dcam_uncertain_cnt = 0
|
||||
self.dcam_reset_cnt = 0
|
||||
|
||||
self.params = Params()
|
||||
self.too_distracted = self.params.get_bool("DriverTooDistracted")
|
||||
|
||||
self._reset_awareness()
|
||||
self._set_timers(active_monitoring=True)
|
||||
self._reset_events()
|
||||
|
||||
def _reset_awareness(self):
|
||||
self.awareness = 1.
|
||||
self.awareness_active = 1.
|
||||
self.awareness_passive = 1.
|
||||
|
||||
def _reset_events(self):
|
||||
self.current_events = Events()
|
||||
|
||||
def _set_timers(self, active_monitoring):
|
||||
if self.active_monitoring_mode and self.awareness <= self.threshold_prompt:
|
||||
if active_monitoring:
|
||||
self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
|
||||
else:
|
||||
self.step_change = 0.
|
||||
return # no exploit after orange alert
|
||||
elif self.awareness <= 0.:
|
||||
return
|
||||
|
||||
if active_monitoring:
|
||||
# when falling back from passive mode to active mode, reset awareness to avoid false alert
|
||||
if not self.active_monitoring_mode:
|
||||
self.awareness_passive = self.awareness
|
||||
self.awareness = self.awareness_active
|
||||
|
||||
self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
|
||||
self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME
|
||||
self.step_change = self.settings._DT_DMON / self.settings._DISTRACTED_TIME
|
||||
self.active_monitoring_mode = True
|
||||
else:
|
||||
if self.active_monitoring_mode:
|
||||
self.awareness_active = self.awareness
|
||||
self.awareness = self.awareness_passive
|
||||
|
||||
self.threshold_pre = self.settings._AWARENESS_PRE_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
|
||||
self.threshold_prompt = self.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL / self.settings._AWARENESS_TIME
|
||||
self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME
|
||||
self.active_monitoring_mode = False
|
||||
|
||||
def _set_policy(self, brake_disengage_prob, car_speed):
|
||||
bp = brake_disengage_prob
|
||||
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
|
||||
bp_normal = max(min(bp / k1, 0.5),0)
|
||||
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
|
||||
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
|
||||
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
|
||||
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
|
||||
[self.settings._POSE_YAW_THRESHOLD_SLACK,
|
||||
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
|
||||
|
||||
def _get_distracted_types(self):
|
||||
distracted_types = []
|
||||
|
||||
if not self.pose.calibrated:
|
||||
pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
|
||||
yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
|
||||
else:
|
||||
pitch_error = self.pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
|
||||
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
|
||||
yaw_error = self.pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
|
||||
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
|
||||
pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
|
||||
yaw_error = abs(yaw_error)
|
||||
|
||||
pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
|
||||
yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
|
||||
|
||||
if pitch_error > pitch_threshold or yaw_error > yaw_threshold:
|
||||
distracted_types.append(DistractedType.DISTRACTED_POSE)
|
||||
|
||||
if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD:
|
||||
distracted_types.append(DistractedType.DISTRACTED_BLINK)
|
||||
|
||||
if self.phone.prob_calibrated:
|
||||
using_phone = self.phone.prob > max(min(self.phone.prob_offseter.filtered_stat.M, self.settings._PHONE_MAX_OFFSET), self.settings._PHONE_MIN_OFFSET) \
|
||||
* self.settings._PHONE_THRESH2
|
||||
else:
|
||||
using_phone = self.phone.prob > self.settings._PHONE_THRESH
|
||||
if using_phone:
|
||||
distracted_types.append(DistractedType.DISTRACTED_PHONE)
|
||||
|
||||
return distracted_types
|
||||
|
||||
def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, standstill, demo_mode=False):
|
||||
rhd_pred = driver_state.wheelOnRightProb
|
||||
# calibrates only when there's movement and either face detected
|
||||
if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
|
||||
driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
|
||||
self.wheelpos.prob_offseter.push_and_update(rhd_pred)
|
||||
|
||||
self.wheelpos.prob_calibrated = self.wheelpos.prob_offseter.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT
|
||||
|
||||
if self.wheelpos.prob_calibrated or demo_mode:
|
||||
self.wheel_on_right = self.wheelpos.prob_offseter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
|
||||
else:
|
||||
self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
|
||||
# make sure no switching when engaged
|
||||
if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
|
||||
self.wheel_on_right = self.wheel_on_right_last
|
||||
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
|
||||
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
|
||||
driver_data.faceOrientationStd, driver_data.facePositionStd)):
|
||||
return
|
||||
|
||||
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
|
||||
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
|
||||
if self.wheel_on_right:
|
||||
self.pose.yaw *= -1
|
||||
self.wheel_on_right_last = self.wheel_on_right
|
||||
self.pose.pitch_std = driver_data.faceOrientationStd[0]
|
||||
self.pose.yaw_std = driver_data.faceOrientationStd[1]
|
||||
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
|
||||
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
|
||||
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.phone.prob = driver_data.phoneProb
|
||||
|
||||
self.distracted_types = self._get_distracted_types()
|
||||
self.driver_distracted = (DistractedType.DISTRACTED_PHONE in self.distracted_types
|
||||
or DistractedType.DISTRACTED_POSE in self.distracted_types
|
||||
or DistractedType.DISTRACTED_BLINK in self.distracted_types) \
|
||||
and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
|
||||
self.driver_distraction_filter.update(self.driver_distracted)
|
||||
|
||||
# update offseter
|
||||
# only update when driver is actively driving the car above a certain speed
|
||||
if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
|
||||
self.pose.pitch_offseter.push_and_update(self.pose.pitch)
|
||||
self.pose.yaw_offseter.push_and_update(self.pose.yaw)
|
||||
self.phone.prob_offseter.push_and_update(self.phone.prob)
|
||||
|
||||
self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \
|
||||
self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
|
||||
self.phone.prob_calibrated = self.phone.prob_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT
|
||||
|
||||
if self.face_detected and not self.driver_distracted:
|
||||
if model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD:
|
||||
if not standstill:
|
||||
self.dcam_uncertain_cnt += 1
|
||||
self.dcam_reset_cnt = 0
|
||||
else:
|
||||
self.dcam_reset_cnt += 1
|
||||
if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
|
||||
self.dcam_uncertain_cnt = 0
|
||||
|
||||
self.is_model_uncertain = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME
|
||||
self._set_timers(self.face_detected and not self.is_model_uncertain)
|
||||
if self.face_detected and not self.pose.low_std and not self.driver_distracted:
|
||||
self.hi_stds += 1
|
||||
elif self.face_detected and self.pose.low_std:
|
||||
self.hi_stds = 0
|
||||
|
||||
def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
|
||||
self._reset_events()
|
||||
# Block engaging until ignition cycle after max number or time of distractions
|
||||
if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \
|
||||
self.terminal_time >= self.settings._MAX_TERMINAL_DURATION:
|
||||
if not self.too_distracted:
|
||||
self.params.put_bool_nonblocking("DriverTooDistracted", True)
|
||||
self.too_distracted = True
|
||||
|
||||
# Always-on distraction lockout is temporary
|
||||
if self.too_distracted or (self.always_on and self.awareness <= self.threshold_prompt):
|
||||
self.current_events.add(EventName.tooDistracted)
|
||||
|
||||
always_on_valid = self.always_on and not wrong_gear
|
||||
if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \
|
||||
(not always_on_valid and not op_engaged) or \
|
||||
(always_on_valid and not op_engaged and self.awareness <= 0):
|
||||
# always reset on disengage with normal mode; disengage resets only on red if always on
|
||||
self._reset_awareness()
|
||||
return
|
||||
|
||||
driver_attentive = self.driver_distraction_filter.x < 0.37
|
||||
awareness_prev = self.awareness
|
||||
|
||||
if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0):
|
||||
if driver_engaged:
|
||||
self._reset_awareness()
|
||||
return
|
||||
# only restore awareness when paying attention and alert is not red
|
||||
self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*
|
||||
(1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.)
|
||||
if self.awareness == 1.:
|
||||
self.awareness_passive = min(self.awareness_passive + self.step_change, 1.)
|
||||
# don't display alert banner when awareness is recovering and has cleared orange
|
||||
if self.awareness > self.threshold_prompt:
|
||||
return
|
||||
|
||||
_reaching_audible = self.awareness - self.step_change <= self.threshold_prompt
|
||||
_reaching_terminal = self.awareness - self.step_change <= 0
|
||||
standstill_orange_exemption = standstill and _reaching_audible
|
||||
always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal
|
||||
always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED
|
||||
|
||||
certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
|
||||
maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected
|
||||
|
||||
if certainly_distracted or maybe_distracted:
|
||||
# should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange
|
||||
# also will not be reaching 0 if DM is active when not engaged
|
||||
if not (standstill_orange_exemption or always_on_red_exemption or (always_on_lowspeed_exemption and _reaching_audible)):
|
||||
self.awareness = max(self.awareness - self.step_change, -0.1)
|
||||
|
||||
alert = None
|
||||
if self.awareness <= 0.:
|
||||
# terminal red alert: disengagement required
|
||||
alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive
|
||||
self.terminal_time += 1
|
||||
if awareness_prev > 0.:
|
||||
self.terminal_alert_cnt += 1
|
||||
elif self.awareness <= self.threshold_prompt:
|
||||
# prompt orange alert
|
||||
alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive
|
||||
elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption:
|
||||
# pre green alert
|
||||
alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive
|
||||
|
||||
if alert is not None:
|
||||
self.current_events.add(alert)
|
||||
|
||||
def get_state_packet(self, valid=True):
|
||||
# build driverMonitoringState packet
|
||||
dat = messaging.new_message('driverMonitoringState', valid=valid)
|
||||
dat.driverMonitoringState = {
|
||||
"events": self.current_events.to_msg(),
|
||||
"faceDetected": self.face_detected,
|
||||
"isDistracted": self.driver_distracted,
|
||||
"distractedType": sum(self.distracted_types),
|
||||
"awarenessStatus": self.awareness,
|
||||
"posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(),
|
||||
"posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n,
|
||||
"poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(),
|
||||
"poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n,
|
||||
"phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(),
|
||||
"phoneProbValidCount": self.phone.prob_offseter.filtered_stat.n,
|
||||
"stepChange": self.step_change,
|
||||
"awarenessActive": self.awareness_active,
|
||||
"awarenessPassive": self.awareness_passive,
|
||||
"isLowStd": self.pose.low_std,
|
||||
"hiStdCount": self.hi_stds,
|
||||
"isActiveMode": self.active_monitoring_mode,
|
||||
"isRHD": self.wheel_on_right,
|
||||
"uncertainCount": self.dcam_uncertain_cnt,
|
||||
}
|
||||
return dat
|
||||
|
||||
def run_step(self, sm, demo=False):
|
||||
if demo:
|
||||
highway_speed = 30
|
||||
enabled = True
|
||||
wrong_gear = False
|
||||
standstill = False
|
||||
driver_engaged = False
|
||||
brake_disengage_prob = 1.0
|
||||
rpyCalib = [0., 0., 0.]
|
||||
else:
|
||||
highway_speed = sm['carState'].vEgo
|
||||
enabled = sm['selfdriveState'].enabled or sm['carControl'].latActive
|
||||
wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
|
||||
standstill = sm['carState'].standstill
|
||||
driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
|
||||
brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
|
||||
calib_rpy = get_calibrated_rpy(sm['liveCalibration'])
|
||||
rpyCalib = calib_rpy.tolist() if calib_rpy is not None else [0., 0., 0.]
|
||||
self._set_policy(
|
||||
brake_disengage_prob=brake_disengage_prob,
|
||||
car_speed=highway_speed,
|
||||
)
|
||||
|
||||
# Parse data from dmonitoringmodeld
|
||||
self._update_states(
|
||||
driver_state=sm['driverStateV2'],
|
||||
cal_rpy=rpyCalib,
|
||||
car_speed=highway_speed,
|
||||
op_engaged=enabled,
|
||||
standstill=standstill,
|
||||
demo_mode=demo,
|
||||
)
|
||||
|
||||
# Update distraction events
|
||||
self._update_events(
|
||||
driver_engaged=driver_engaged,
|
||||
op_engaged=enabled,
|
||||
standstill=standstill,
|
||||
wrong_gear=wrong_gear,
|
||||
car_speed=highway_speed
|
||||
)
|
||||
@@ -0,0 +1,467 @@
|
||||
from collections import defaultdict
|
||||
from math import atan2, radians
|
||||
import numpy as np
|
||||
|
||||
from cereal import car, log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.realtime import DT_DMON
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.stat_live import RunningStatFilter
|
||||
from openpilot.common.transformations.camera import DEVICE_CAMERAS
|
||||
|
||||
AlertLevel = log.DriverMonitoringState.AlertLevel
|
||||
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
|
||||
|
||||
def to_percent(v):
|
||||
return int(min(max(v * 100., 0.), 100.))
|
||||
|
||||
# ******************************************************************************************
|
||||
# NOTE: To fork maintainers.
|
||||
# Disabling or nerfing safety features will get you and your users banned from our servers.
|
||||
# We recommend that you do not change these numbers from the defaults.
|
||||
# ******************************************************************************************
|
||||
|
||||
class DRIVER_MONITOR_SETTINGS:
|
||||
def __init__(self):
|
||||
# https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=OJ:L_202501899
|
||||
self._ALERT_MIN_SPEED = 2.8 # 10 km/h
|
||||
|
||||
self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5.
|
||||
self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15.
|
||||
self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25.
|
||||
self._VISION_POLICY_ALERT_1_TIMEOUT = 5.
|
||||
self._VISION_POLICY_ALERT_2_TIMEOUT = 8.
|
||||
self._VISION_POLICY_ALERT_3_TIMEOUT = 13.
|
||||
|
||||
# no response = alert_3 sustained for certain amount of time
|
||||
self._NO_RESPONSE_TIMEOUT = 5.
|
||||
|
||||
# lockout specs
|
||||
self._MAX_ALERT_3 = 2
|
||||
self._MAX_NO_RESPONSE = 1
|
||||
self._LOCKOUT_TIMES = [int(60 * n_min / DT_DMON) for n_min in [1, 5, 15, 30]]
|
||||
|
||||
self._TIMEOUT_RECOVERY_FACTOR_MAX = 5.
|
||||
self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25
|
||||
|
||||
self._FACE_THRESHOLD = 0.7
|
||||
self._EYE_THRESHOLD = 0.65
|
||||
self._SG_THRESHOLD = 0.9
|
||||
self._BLINK_THRESHOLD = 0.865
|
||||
self._PHONE_THRESH = 0.5
|
||||
self._POSE_PITCH_THRESHOLD = 0.3133
|
||||
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
|
||||
self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
|
||||
self._POSE_YAW_THRESHOLD = 0.4020
|
||||
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
|
||||
self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
|
||||
self._POSE_YAW_MIN_STEER_DEG = 30
|
||||
self._POSE_YAW_STEER_FACTOR = 0.15
|
||||
self._POSE_YAW_STEER_MAX_OFFSET = 0.3927
|
||||
self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_THRESHOLD = 0.449
|
||||
self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned
|
||||
self._PITCH_NATURAL_VAR = 3*0.01
|
||||
self._YAW_NATURAL_VAR = 3*0.05
|
||||
self._PITCH_MAX_OFFSET = 0.124
|
||||
self._PITCH_MIN_OFFSET = -0.0881
|
||||
self._YAW_MAX_OFFSET = 0.289
|
||||
self._YAW_MIN_OFFSET = -0.0246
|
||||
|
||||
self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1
|
||||
self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / DT_DMON)
|
||||
self._DCAM_UNCERTAIN_RESET_COUNT = int(2 / DT_DMON)
|
||||
self._HI_STD_THRESHOLD = 0.3
|
||||
self._HI_STD_FALLBACK_TIME = int(10 / DT_DMON) # fall back to wheel touch if model is uncertain for 10s
|
||||
self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||
|
||||
self._POSE_CALIB_MIN_SPEED = 13 # 30 mph
|
||||
self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON) # valid data counts before calibration completes, 1min cumulative
|
||||
self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
|
||||
self._WHEELPOS_CALIB_MIN_SPEED = 11
|
||||
self._WHEELPOS_THRESHOLD = 0.5
|
||||
self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side
|
||||
self._WHEELPOS_DATA_AVG = 0.03
|
||||
self._WHEELPOS_DATA_VAR = 3*5.5e-5
|
||||
self._WHEELPOS_MAX_COUNT = -1
|
||||
|
||||
class DriverPose:
|
||||
def __init__(self, settings):
|
||||
pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2)
|
||||
yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2)
|
||||
self.yaw = 0.
|
||||
self.pitch = 0.
|
||||
self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT)
|
||||
self.calibrated = False
|
||||
self.low_std = True
|
||||
self.cfactor_pitch = 1.
|
||||
self.cfactor_yaw = 1.
|
||||
self.steer_yaw_offset = 0.
|
||||
|
||||
class DriverBlink:
|
||||
def __init__(self):
|
||||
self.left = 0.
|
||||
self.right = 0.
|
||||
|
||||
# model output refers to center of undistorted+leveled image
|
||||
ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam
|
||||
dcam_undistorted_FL = 598.0
|
||||
dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height)
|
||||
|
||||
def face_orientation_from_model(orient_model, pos_model, rpy_calib):
|
||||
pitch_model = orient_model[0]
|
||||
yaw_model = orient_model[1]
|
||||
|
||||
face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H)
|
||||
yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL)
|
||||
pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL)
|
||||
|
||||
pitch = pitch_model + pitch_focal_angle
|
||||
yaw = -yaw_model + yaw_focal_angle
|
||||
|
||||
pitch -= rpy_calib[1]
|
||||
yaw -= rpy_calib[2]
|
||||
return pitch, yaw
|
||||
|
||||
|
||||
class DriverMonitoring:
|
||||
def __init__(self, rhd_saved=False, settings=None, always_on=False):
|
||||
# init policy settings
|
||||
self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS()
|
||||
|
||||
# init driver status
|
||||
wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2)
|
||||
self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT)
|
||||
self.pose = DriverPose(settings=self.settings)
|
||||
self.blink = DriverBlink()
|
||||
self.phone_prob = 0.
|
||||
|
||||
self.alert_level = AlertLevel.none
|
||||
self.always_on = always_on
|
||||
self.distracted_types = defaultdict(bool)
|
||||
self.driver_distracted = False
|
||||
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON)
|
||||
self.wheel_on_right = False
|
||||
self.wheel_on_right_last = None
|
||||
self.wheel_on_right_default = rhd_saved
|
||||
self.face_detected = False
|
||||
self.alert_3_cnt = 0
|
||||
self.cnt_since_alert_3 = 0
|
||||
self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON)
|
||||
self.no_response_cnt = 0
|
||||
self.lockout_active = Params().get_bool("DriverTooDistracted")
|
||||
self.lockout_count = Params().get("DriverLockoutCount") or 0
|
||||
self.lockout_duration = self.settings._LOCKOUT_TIMES[min(max(self.lockout_count - 1, 0), len(self.settings._LOCKOUT_TIMES) - 1)]
|
||||
self.lockout_time_elapsed = 0
|
||||
self.step_change = 0.
|
||||
self.active_policy = MonitoringPolicy.vision
|
||||
self.driver_interacting = False
|
||||
self.is_model_uncertain = False
|
||||
self.hi_stds = 0
|
||||
self.model_std_max = 0.
|
||||
self.threshold_alert_1 = 0.
|
||||
self.threshold_alert_2 = 0.
|
||||
self.dcam_uncertain_cnt = 0
|
||||
self.dcam_reset_cnt = 0
|
||||
|
||||
self._reset_awareness()
|
||||
self._set_policy(MonitoringPolicy.vision)
|
||||
|
||||
def _reset_awareness(self):
|
||||
self.awareness = 1.
|
||||
self.last_vision_awareness = 1.
|
||||
self.last_wheeltouch_awareness = 1.
|
||||
|
||||
def _set_policy(self, target_policy):
|
||||
if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2:
|
||||
if target_policy == MonitoringPolicy.vision:
|
||||
self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
|
||||
else:
|
||||
self.step_change = 0.
|
||||
return # no exploit after orange alert
|
||||
elif self.awareness <= 0.:
|
||||
return
|
||||
|
||||
if target_policy == MonitoringPolicy.vision:
|
||||
# when falling back from passive mode to active mode, reset awareness to avoid false alert
|
||||
if self.active_policy != MonitoringPolicy.vision:
|
||||
self.last_wheeltouch_awareness = self.awareness
|
||||
self.awareness = self.last_vision_awareness
|
||||
|
||||
self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
|
||||
self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
|
||||
self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT
|
||||
self.active_policy = MonitoringPolicy.vision
|
||||
else:
|
||||
if self.active_policy == MonitoringPolicy.vision:
|
||||
self.last_vision_awareness = self.awareness
|
||||
self.awareness = self.last_wheeltouch_awareness
|
||||
|
||||
self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
|
||||
self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
|
||||
self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT
|
||||
self.active_policy = MonitoringPolicy.wheeltouch
|
||||
|
||||
def _set_pose_strictness(self, brake_disengage_prob, car_speed):
|
||||
bp = brake_disengage_prob
|
||||
k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
|
||||
bp_normal = max(min(bp / k1, 0.5),0)
|
||||
self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
|
||||
[self.settings._POSE_PITCH_THRESHOLD_SLACK,
|
||||
self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
|
||||
self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
|
||||
[self.settings._POSE_YAW_THRESHOLD_SLACK,
|
||||
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
|
||||
|
||||
def _get_distracted_types(self):
|
||||
self.distracted_types = defaultdict(bool)
|
||||
|
||||
if not self.pose.calibrated:
|
||||
pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET
|
||||
yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET
|
||||
else:
|
||||
pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(),
|
||||
self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
|
||||
yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(),
|
||||
self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
|
||||
pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
|
||||
|
||||
if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional
|
||||
yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.)
|
||||
else:
|
||||
yaw_error = abs(yaw_error)
|
||||
|
||||
pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD
|
||||
yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw
|
||||
|
||||
self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold))
|
||||
self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD)
|
||||
self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH)
|
||||
|
||||
def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.):
|
||||
rhd_pred = driver_state.wheelOnRightProb
|
||||
# calibrates only when there's movement and either face detected
|
||||
if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or
|
||||
driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD):
|
||||
self.wheelpos_offsetter.push_and_update(rhd_pred)
|
||||
|
||||
wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT
|
||||
|
||||
if wheelpos_calibrated or demo_mode:
|
||||
self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
|
||||
else:
|
||||
self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished
|
||||
# make sure no switching when engaged
|
||||
if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode:
|
||||
self.wheel_on_right = self.wheel_on_right_last
|
||||
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
|
||||
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
|
||||
driver_data.faceOrientationStd, driver_data.facePositionStd)):
|
||||
return
|
||||
|
||||
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
|
||||
self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
|
||||
steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.)
|
||||
self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR
|
||||
if self.wheel_on_right:
|
||||
self.pose.yaw *= -1
|
||||
self.pose.steer_yaw_offset *= -1
|
||||
self.wheel_on_right_last = self.wheel_on_right
|
||||
self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1])
|
||||
self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD
|
||||
self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \
|
||||
* (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
|
||||
self.phone_prob = driver_data.phoneProb
|
||||
|
||||
self._get_distracted_types()
|
||||
self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
|
||||
self.driver_distraction_filter.update(self.driver_distracted)
|
||||
|
||||
# only update offsetter when driver is actively driving the car above a certain speed
|
||||
if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted):
|
||||
self.pose.pitch_offsetter.push_and_update(self.pose.pitch)
|
||||
self.pose.yaw_offsetter.push_and_update(self.pose.yaw)
|
||||
|
||||
self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \
|
||||
self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT
|
||||
|
||||
if self.face_detected and not self.driver_distracted:
|
||||
dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD
|
||||
if dcam_uncertain and not lowspeed:
|
||||
self.dcam_uncertain_cnt += 1
|
||||
self.dcam_reset_cnt = 0
|
||||
else:
|
||||
self.dcam_reset_cnt += 1
|
||||
if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT:
|
||||
self.dcam_uncertain_cnt = 0
|
||||
|
||||
self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME
|
||||
self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch)
|
||||
if self.face_detected and not self.pose.low_std and not self.driver_distracted:
|
||||
self.hi_stds += 1
|
||||
elif self.face_detected and self.pose.low_std:
|
||||
self.hi_stds = 0
|
||||
|
||||
def _update_events(self, driver_engaged, op_engaged, lowspeed, wrong_gear):
|
||||
self.alert_level = AlertLevel.none
|
||||
self.driver_interacting = driver_engaged
|
||||
|
||||
if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE:
|
||||
if not self.lockout_active:
|
||||
self.lockout_count += 1
|
||||
self.lockout_duration = self.settings._LOCKOUT_TIMES[min(self.lockout_count - 1, len(self.settings._LOCKOUT_TIMES) - 1)]
|
||||
Params().put("DriverLockoutCount", self.lockout_count)
|
||||
self.lockout_active = True
|
||||
|
||||
if self.lockout_active:
|
||||
self.lockout_time_elapsed += 1
|
||||
if self.lockout_time_elapsed > self.lockout_duration:
|
||||
self.lockout_active = False
|
||||
self.alert_3_cnt = 0
|
||||
self.cnt_since_alert_3 = 0
|
||||
self.no_response_cnt = 0
|
||||
self.lockout_time_elapsed = 0
|
||||
|
||||
always_on_valid = self.always_on and not wrong_gear
|
||||
if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \
|
||||
(not always_on_valid and not op_engaged) or \
|
||||
(always_on_valid and not op_engaged and self.awareness <= 0):
|
||||
# always reset on disengage with normal mode; disengage resets only on red if always on
|
||||
self._reset_awareness()
|
||||
return
|
||||
|
||||
awareness_prev = self.awareness
|
||||
_reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1
|
||||
_reaching_alert_3 = self.awareness - self.step_change <= 0
|
||||
lowspeed_exemption = lowspeed and _reaching_alert_1
|
||||
always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3
|
||||
|
||||
if self.awareness > 0 and \
|
||||
((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or lowspeed_exemption):
|
||||
if self.driver_interacting:
|
||||
self._reset_awareness()
|
||||
return
|
||||
# only restore awareness when paying attention and alert is not red
|
||||
self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*
|
||||
(1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.)
|
||||
if self.awareness == 1.:
|
||||
self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.)
|
||||
# don't display alert banner when awareness is recovering and has cleared orange
|
||||
if self.awareness > self.threshold_alert_2:
|
||||
return
|
||||
|
||||
certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected
|
||||
maybe_distracted = self.is_model_uncertain or not self.face_detected
|
||||
|
||||
if certainly_distracted or maybe_distracted:
|
||||
# should always be counting if distracted unless at low speed and reaching green
|
||||
# also will not be reaching 0 if DM is active when not engaged
|
||||
if not (lowspeed_exemption or always_on_exemption):
|
||||
self.awareness = max(self.awareness - self.step_change, -0.1)
|
||||
|
||||
if self.awareness <= 0.:
|
||||
# terminal alert: disengagement required
|
||||
self.alert_level = AlertLevel.three
|
||||
if awareness_prev > 0.:
|
||||
self.alert_3_cnt += 1
|
||||
self.cnt_since_alert_3 = 0
|
||||
else:
|
||||
self.cnt_since_alert_3 += 1
|
||||
if self.cnt_since_alert_3 == self.no_response_timeout:
|
||||
self.no_response_cnt += 1
|
||||
else:
|
||||
if self.awareness <= self.threshold_alert_2:
|
||||
self.alert_level = AlertLevel.two
|
||||
elif self.awareness <= self.threshold_alert_1:
|
||||
self.alert_level = AlertLevel.one
|
||||
|
||||
def get_state_packet(self, valid=True):
|
||||
# build driverMonitoringState packet
|
||||
dat = messaging.new_message('driverMonitoringState', valid=valid)
|
||||
dm = dat.driverMonitoringState
|
||||
|
||||
dm.lockout = self.lockout_active
|
||||
dm.lockoutCount = self.lockout_count
|
||||
if self.lockout_active:
|
||||
dm.lockoutMinutesRemaining = max(1, round((self.lockout_duration - self.lockout_time_elapsed) * DT_DMON / 60.))
|
||||
dm.alert3Count = self.alert_3_cnt
|
||||
dm.noResponseCount = self.no_response_cnt
|
||||
dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout
|
||||
dm.alwaysOn = self.always_on
|
||||
dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2
|
||||
dm.alertLevel = self.alert_level
|
||||
dm.activePolicy = self.active_policy
|
||||
dm.isRHD = self.wheel_on_right
|
||||
dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT)
|
||||
dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M
|
||||
|
||||
dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness)
|
||||
dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0.
|
||||
dm.visionPolicyState.isDistracted = self.driver_distracted
|
||||
dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose']
|
||||
dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye']
|
||||
dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone']
|
||||
dm.visionPolicyState.faceDetected = self.face_detected
|
||||
dm.visionPolicyState.pose.pitch = self.pose.pitch
|
||||
dm.visionPolicyState.pose.yaw = self.pose.yaw
|
||||
dm.visionPolicyState.pose.calibrated = self.pose.calibrated
|
||||
dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
|
||||
dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M
|
||||
dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT)
|
||||
dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M
|
||||
dm.visionPolicyState.pose.uncertainty = self.model_std_max
|
||||
dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME)
|
||||
dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT)
|
||||
|
||||
dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness)
|
||||
dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change
|
||||
dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting
|
||||
return dat
|
||||
|
||||
def run_step(self, sm, demo=False):
|
||||
if demo:
|
||||
car_speed = 30
|
||||
enabled = True
|
||||
wrong_gear = False
|
||||
lowspeed = False
|
||||
driver_engaged = False
|
||||
brake_disengage_prob = 1.0
|
||||
steering_angle_deg = 0.0
|
||||
rpyCalib = [0., 0., 0.]
|
||||
else:
|
||||
car_speed = sm['carState'].vEgo
|
||||
enabled = sm['selfdriveState'].enabled
|
||||
wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low)
|
||||
lowspeed = car_speed < self.settings._ALERT_MIN_SPEED
|
||||
driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed
|
||||
brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
|
||||
steering_angle_deg = sm['carState'].steeringAngleDeg
|
||||
rpyCalib = sm['liveCalibration'].rpyCalib
|
||||
|
||||
self._set_pose_strictness(
|
||||
brake_disengage_prob=brake_disengage_prob,
|
||||
car_speed=car_speed,
|
||||
)
|
||||
|
||||
# Parse data from dmonitoringmodeld
|
||||
self._update_states(
|
||||
driver_state=sm['driverStateV2'],
|
||||
cal_rpy=rpyCalib,
|
||||
car_speed=car_speed,
|
||||
op_engaged=enabled,
|
||||
lowspeed=lowspeed,
|
||||
demo_mode=demo,
|
||||
steering_angle_deg=steering_angle_deg,
|
||||
)
|
||||
|
||||
# Update distraction events
|
||||
self._update_events(
|
||||
driver_engaged=driver_engaged,
|
||||
op_engaged=enabled,
|
||||
lowspeed=lowspeed,
|
||||
wrong_gear=wrong_gear,
|
||||
)
|
||||
@@ -0,0 +1,24 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.monitoring.dmonitoringd import get_dm_inputs
|
||||
|
||||
|
||||
@pytest.mark.parametrize("enabled, lat_active, expected", [
|
||||
(False, False, False),
|
||||
(True, False, True),
|
||||
(False, True, True),
|
||||
(True, True, True),
|
||||
])
|
||||
def test_iq_enabled_adapter(enabled, lat_active, expected):
|
||||
sm = {
|
||||
'driverStateV2': object(),
|
||||
'liveCalibration': object(),
|
||||
'carState': object(),
|
||||
'selfdriveState': SimpleNamespace(enabled=enabled),
|
||||
'modelV2': object(),
|
||||
'carControl': SimpleNamespace(latActive=lat_active),
|
||||
}
|
||||
|
||||
assert get_dm_inputs(sm)['selfdriveState'].enabled == expected
|
||||
@@ -1,19 +1,15 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from cereal import log, car
|
||||
from cereal import log
|
||||
from openpilot.common.realtime import DT_DMON
|
||||
from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type())
|
||||
dm_settings = DRIVER_MONITOR_SETTINGS()
|
||||
|
||||
TEST_TIMESPAN = 120 # seconds
|
||||
DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1
|
||||
DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1
|
||||
INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1
|
||||
INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1
|
||||
DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1
|
||||
DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1
|
||||
INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1
|
||||
INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1
|
||||
|
||||
def make_msg(face_detected, distracted=False, model_uncertain=False):
|
||||
ds = log.DriverStateV2.new_message()
|
||||
@@ -37,7 +33,7 @@ msg_ATTENTIVE = make_msg(True)
|
||||
msg_DISTRACTED = make_msg(True, distracted=True)
|
||||
msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True)
|
||||
msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True)
|
||||
msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5)
|
||||
msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5)
|
||||
|
||||
# driver interaction with car
|
||||
car_interaction_DETECTED = True
|
||||
@@ -51,51 +47,66 @@ always_true = [True] * int(TEST_TIMESPAN / DT_DMON)
|
||||
always_false = [False] * int(TEST_TIMESPAN / DT_DMON)
|
||||
|
||||
class TestMonitoring:
|
||||
def _run_seq(self, msgs, interaction, engaged, standstill):
|
||||
def _run_seq(self, msgs, interaction, engaged, lowspeed):
|
||||
DM = DriverMonitoring()
|
||||
events = []
|
||||
alert_lvls = []
|
||||
for idx in range(len(msgs)):
|
||||
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx])
|
||||
DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx])
|
||||
# cal_rpy and car_speed don't matter here
|
||||
|
||||
# evaluate events at 10Hz for tests
|
||||
DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0)
|
||||
events.append(DM.current_events)
|
||||
assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs"
|
||||
return events, DM
|
||||
DM._update_events(interaction[idx], engaged[idx], lowspeed[idx], 0)
|
||||
alert_lvls.append(DM.alert_level)
|
||||
assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs"
|
||||
return alert_lvls, DM
|
||||
|
||||
def _assert_no_events(self, events):
|
||||
assert all(not len(e) for e in events)
|
||||
|
||||
# engaged, driver is attentive all the time
|
||||
def test_fully_aware_driver(self):
|
||||
events, _ = self._run_seq(always_attentive, always_false, always_true, always_false)
|
||||
self._assert_no_events(events)
|
||||
alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false)
|
||||
assert all(a == 0 for a in alert_lvls)
|
||||
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision
|
||||
|
||||
# engaged, driver is distracted and does nothing
|
||||
def test_fully_distracted_driver(self):
|
||||
events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
|
||||
assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
|
||||
assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \
|
||||
((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
|
||||
EventName.preDriverDistracted
|
||||
assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \
|
||||
((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverDistracted
|
||||
assert events[int((d_status.settings._DISTRACTED_TIME + \
|
||||
((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted
|
||||
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
|
||||
s = d_status.settings
|
||||
assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
|
||||
assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \
|
||||
(s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
|
||||
assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \
|
||||
(s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
|
||||
assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \
|
||||
(TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3
|
||||
assert isinstance(d_status.awareness, float)
|
||||
|
||||
# engaged, distracted past red and beyond the no-response window -> unavailability response + lockout
|
||||
def test_distracted_lockout(self):
|
||||
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false)
|
||||
assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3
|
||||
assert d_status.lockout_active
|
||||
assert d_status.lockout_time_elapsed > 0
|
||||
assert d_status.lockout_count >= 1
|
||||
|
||||
# no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout
|
||||
def test_invisible_lockout(self):
|
||||
_, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
|
||||
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
|
||||
assert d_status.lockout_active
|
||||
assert d_status.lockout_count >= 1
|
||||
|
||||
# engaged, no face detected the whole time, no action
|
||||
def test_fully_invisible_driver(self):
|
||||
events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
|
||||
assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0
|
||||
assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \
|
||||
((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \
|
||||
EventName.preDriverUnresponsive
|
||||
assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \
|
||||
((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
|
||||
assert events[int((d_status.settings._AWARENESS_TIME + \
|
||||
((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive
|
||||
alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false)
|
||||
s = d_status.settings
|
||||
assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0
|
||||
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \
|
||||
(s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1
|
||||
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \
|
||||
(s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2
|
||||
assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \
|
||||
(TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3
|
||||
assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch
|
||||
|
||||
# engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel
|
||||
# - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention
|
||||
@@ -106,13 +117,13 @@ class TestMonitoring:
|
||||
[msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON))
|
||||
interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \
|
||||
[car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON))
|
||||
events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
|
||||
assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
|
||||
assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
|
||||
assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0
|
||||
assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
|
||||
assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted
|
||||
assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0
|
||||
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
|
||||
assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0
|
||||
|
||||
# engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \
|
||||
# driver dodges, and then touches wheel to no avail, disengages and reengages
|
||||
@@ -130,31 +141,31 @@ class TestMonitoring:
|
||||
= [True] * int(1/DT_DMON)
|
||||
op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \
|
||||
= [False] * int(0.5/DT_DMON)
|
||||
events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
|
||||
assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.promptDriverDistracted
|
||||
assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted
|
||||
assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted
|
||||
assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0
|
||||
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3
|
||||
assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0
|
||||
|
||||
# engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears
|
||||
# - both actions should clear the alert, but momentary appearance should not
|
||||
def test_sometimes_transparent_commuter(self):
|
||||
_visible_time = np.random.choice([0.5, 10])
|
||||
ds_vector = always_no_face[:]*2
|
||||
interaction_vector = always_false[:]*2
|
||||
ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \
|
||||
[msg_ATTENTIVE] * int(_visible_time/DT_DMON)
|
||||
interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
|
||||
events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
|
||||
assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
|
||||
assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
|
||||
assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0
|
||||
if _visible_time == 0.5:
|
||||
assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
|
||||
assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive
|
||||
elif _visible_time == 10:
|
||||
assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
|
||||
assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0
|
||||
for _visible_time in (0.5, 10):
|
||||
ds_vector = always_no_face[:]*2
|
||||
interaction_vector = always_false[:]*2
|
||||
ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \
|
||||
[msg_ATTENTIVE] * int(_visible_time/DT_DMON)
|
||||
interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON)
|
||||
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false)
|
||||
assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0
|
||||
if _visible_time == 0.5:
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 2
|
||||
elif _visible_time == 10:
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0
|
||||
|
||||
# engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages
|
||||
# - only disengage will clear the alert
|
||||
@@ -166,105 +177,51 @@ class TestMonitoring:
|
||||
ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON)
|
||||
interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON)
|
||||
op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON)
|
||||
events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
|
||||
assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0
|
||||
assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive
|
||||
assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive
|
||||
assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive
|
||||
assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive
|
||||
assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0
|
||||
alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false)
|
||||
assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0
|
||||
|
||||
# disengaged, always distracted driver
|
||||
# - dm should stay quiet when not engaged
|
||||
def test_pure_dashcam_user(self):
|
||||
events, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
|
||||
assert sum(len(event) for event in events) == 0
|
||||
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false)
|
||||
assert all(a == 0 for a in alert_lvls)
|
||||
|
||||
# engaged, car stops at traffic light, down to orange, no action, then car starts moving
|
||||
# - should only reach green when stopped, but continues counting down on launch
|
||||
def test_long_traffic_light_victim(self):
|
||||
_redlight_time = 60 # seconds
|
||||
standstill_vector = always_true[:]
|
||||
standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON)
|
||||
events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector)
|
||||
assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \
|
||||
EventName.preDriverDistracted
|
||||
assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted
|
||||
assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted
|
||||
lowspeed_vector = always_true[:]
|
||||
lowspeed_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON)
|
||||
alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector)
|
||||
s = d_status.settings
|
||||
assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0
|
||||
_alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT
|
||||
assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1
|
||||
assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2
|
||||
|
||||
# engaged, distracted while moving, then car stops after reaching orange
|
||||
# - should reset timer to pre green at low speed
|
||||
def test_distracted_then_stops(self):
|
||||
_stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange
|
||||
lowspeed_vector = always_false[:]
|
||||
lowspeed_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON)
|
||||
alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector)
|
||||
# just before and briefly after stopping: orange alert; goes away quickly after stopped
|
||||
assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0
|
||||
|
||||
# engaged, model is somehow uncertain and driver is distracted
|
||||
# - should fall back to wheel touch after uncertain alert
|
||||
def test_somehow_indecisive_model(self):
|
||||
ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON)
|
||||
interaction_vector = always_false[:]
|
||||
events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
|
||||
assert EventName.preDriverUnresponsive in \
|
||||
events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names
|
||||
assert EventName.promptDriverUnresponsive in \
|
||||
events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
|
||||
assert EventName.driverUnresponsive in \
|
||||
events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names
|
||||
|
||||
|
||||
@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [
|
||||
(False, False, False), # Both Disabled
|
||||
(True, False, True), # OP Enabled, Lat Inactive
|
||||
(False, True, True), # OP Disabled, Lat Active (e.g. AOL)
|
||||
(True, True, True) # Both Active
|
||||
])
|
||||
def test_enabled_states(enabled_state, lat_active_state, expected):
|
||||
"""
|
||||
Test DriverMonitoring.run_step with all 4 combinations of:
|
||||
- selfdriveState.enabled (True/False)
|
||||
- carControl.latActive (True/False)
|
||||
"""
|
||||
cs = car.CarState.new_message()
|
||||
cs.vEgo = 30.0
|
||||
cs.gearShifter = car.CarState.GearShifter.drive
|
||||
cs.standstill = False
|
||||
cs.steeringPressed = False
|
||||
cs.gasPressed = False
|
||||
|
||||
ss = log.SelfdriveState.new_message()
|
||||
ss.enabled = enabled_state
|
||||
|
||||
cc = car.CarControl.new_message()
|
||||
cc.latActive = lat_active_state
|
||||
|
||||
mv2 = log.ModelDataV2.new_message()
|
||||
mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0]
|
||||
|
||||
lc = log.LiveCalibrationData.new_message()
|
||||
lc.rpyCalib = [0.0, 0.0, 0.0]
|
||||
|
||||
ds = make_msg(False)
|
||||
|
||||
sm = {
|
||||
'carState': cs,
|
||||
'selfdriveState': ss,
|
||||
'carControl': cc,
|
||||
'modelV2': mv2,
|
||||
'liveCalibration': lc,
|
||||
'driverStateV2': ds
|
||||
}
|
||||
|
||||
driver_monitoring = DriverMonitoring()
|
||||
|
||||
# run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value
|
||||
captured_args = []
|
||||
original_update_events = driver_monitoring._update_events
|
||||
|
||||
def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed):
|
||||
captured_args.append(op_engaged)
|
||||
return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed)
|
||||
|
||||
driver_monitoring._update_events = spy_update_events
|
||||
|
||||
driver_monitoring.run_step(sm, demo=False)
|
||||
|
||||
# Assertion
|
||||
assert len(captured_args) == 1, "Expected _update_events to be called exactly once"
|
||||
actual_enabled = captured_args[0]
|
||||
|
||||
assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}"
|
||||
|
||||
alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false)
|
||||
s = d_status.settings
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2
|
||||
assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3
|
||||
|
||||
@@ -79,6 +79,13 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4)
|
||||
|
||||
|
||||
def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
if sm['driverMonitoringState'].lockout:
|
||||
mins_left = sm['driverMonitoringState'].lockoutMinutesRemaining
|
||||
return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH)
|
||||
return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH)
|
||||
|
||||
|
||||
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
first_word = 'Recalibrating' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating'
|
||||
return Alert(
|
||||
@@ -360,15 +367,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
|
||||
},
|
||||
|
||||
EventName.preDriverDistracted: {
|
||||
EventName.driverDistracted1: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Pay Attention",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1),
|
||||
},
|
||||
|
||||
EventName.promptDriverDistracted: {
|
||||
EventName.driverDistracted2: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Pay Attention",
|
||||
"Driver Distracted",
|
||||
@@ -376,7 +383,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
|
||||
},
|
||||
|
||||
EventName.driverDistracted: {
|
||||
EventName.driverDistracted3: {
|
||||
ET.PERMANENT: Alert(
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"Driver Distracted",
|
||||
@@ -384,7 +391,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
|
||||
},
|
||||
|
||||
EventName.preDriverUnresponsive: {
|
||||
EventName.driverUnresponsive1: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Touch Steering Wheel: No Face Detected",
|
||||
"",
|
||||
@@ -392,7 +399,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1),
|
||||
},
|
||||
|
||||
EventName.promptDriverUnresponsive: {
|
||||
EventName.driverUnresponsive2: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Touch Steering Wheel",
|
||||
"Driver Unresponsive",
|
||||
@@ -400,7 +407,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
|
||||
},
|
||||
|
||||
EventName.driverUnresponsive: {
|
||||
EventName.driverUnresponsive3: {
|
||||
ET.PERMANENT: Alert(
|
||||
"DISENGAGE IMMEDIATELY",
|
||||
"Driver Unresponsive",
|
||||
@@ -632,7 +639,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
},
|
||||
|
||||
EventName.tooDistracted: {
|
||||
ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
|
||||
ET.NO_ENTRY: too_distracted_alert,
|
||||
},
|
||||
|
||||
EventName.excessiveActuation: {
|
||||
@@ -878,14 +885,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
|
||||
if HARDWARE.get_device_type() == 'mici':
|
||||
EVENTS.update({
|
||||
EventName.preDriverDistracted: {
|
||||
EventName.driverDistracted1: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Pay Attention",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 2),
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2),
|
||||
},
|
||||
EventName.promptDriverDistracted: {
|
||||
EventName.driverDistracted2: {
|
||||
ET.PERMANENT: Alert(
|
||||
"Pay Attention",
|
||||
"Driver Distracted",
|
||||
|
||||
@@ -54,6 +54,8 @@ EventName = log.OnroadEvent.EventName
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
SafetyModel = car.CarParams.SafetyModel
|
||||
TurnDirection = custom.IQTurnSignalDirection
|
||||
AlertLevel = log.DriverMonitoringState.AlertLevel
|
||||
MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
|
||||
|
||||
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
|
||||
|
||||
@@ -147,6 +149,7 @@ class SelfdriveD(GapButtonActions):
|
||||
self.events_prev = []
|
||||
self.logged_comm_issue = None
|
||||
self.not_running_prev = None
|
||||
self.dm_lockout_set = False
|
||||
self.experimental_mode = False
|
||||
self.personality = get_sanitize_int_param(
|
||||
"LongitudinalPersonality",
|
||||
@@ -183,7 +186,6 @@ class SelfdriveD(GapButtonActions):
|
||||
|
||||
self.events_iq = IQEvents()
|
||||
self.events_iq_prev = []
|
||||
self._cached_dm_event_names: tuple[int, ...] = ()
|
||||
self._cached_plan_event_names: tuple[int, ...] = ()
|
||||
self._cached_model_event_names: tuple[int, ...] = ()
|
||||
self._cached_nav_event_names: tuple[int, ...] = ()
|
||||
@@ -205,10 +207,6 @@ class SelfdriveD(GapButtonActions):
|
||||
if self.sm.updated['iqPlan']:
|
||||
self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events)
|
||||
|
||||
def _refresh_cached_dm_events(self) -> None:
|
||||
if self.sm.updated['driverMonitoringState']:
|
||||
self._cached_dm_event_names = tuple(event.name.raw for event in self.sm['driverMonitoringState'].events)
|
||||
|
||||
def _refresh_cached_model_events(self) -> None:
|
||||
if not self.sm.updated['iqDriveModelData']:
|
||||
return
|
||||
@@ -297,8 +295,24 @@ class SelfdriveD(GapButtonActions):
|
||||
self.events.add(EventName.resumeBlocked)
|
||||
|
||||
if not self.CP.notCar:
|
||||
self._refresh_cached_dm_events()
|
||||
self._add_event_names(self._cached_dm_event_names)
|
||||
if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set:
|
||||
self.params.put_bool("DriverTooDistracted", True)
|
||||
self.dm_lockout_set = True
|
||||
elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set:
|
||||
self.params.remove("DriverTooDistracted")
|
||||
self.dm_lockout_set = False
|
||||
|
||||
if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout:
|
||||
self.events.add(EventName.tooDistracted)
|
||||
|
||||
vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision
|
||||
if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one:
|
||||
self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1)
|
||||
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two:
|
||||
self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2)
|
||||
elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three:
|
||||
self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3)
|
||||
|
||||
self._refresh_cached_plan_events()
|
||||
self._add_iq_event_names(self._cached_plan_event_names)
|
||||
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from cereal import car, log
|
||||
|
||||
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
|
||||
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
|
||||
def test_driver_monitoring_alert_stages():
|
||||
expected_sounds = {
|
||||
EventName.driverDistracted1: AudibleAlert.preAlert,
|
||||
EventName.driverDistracted2: AudibleAlert.promptDistracted,
|
||||
EventName.driverDistracted3: AudibleAlert.warningImmediate,
|
||||
EventName.driverUnresponsive1: AudibleAlert.none,
|
||||
EventName.driverUnresponsive2: AudibleAlert.promptDistracted,
|
||||
EventName.driverUnresponsive3: AudibleAlert.warningImmediate,
|
||||
}
|
||||
|
||||
for event_name, audible_alert in expected_sounds.items():
|
||||
assert EVENTS[event_name][ET.PERMANENT].audible_alert == audible_alert
|
||||
|
||||
|
||||
def test_driver_monitoring_lockout_alert():
|
||||
callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY]
|
||||
sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=5)}
|
||||
alert = callback(None, None, sm, False, 0, None)
|
||||
|
||||
assert alert.alert_text_1 == "5 minutes Left"
|
||||
assert alert.alert_text_2 == "Too Distracted"
|
||||
@@ -455,21 +455,46 @@ def migrate_onroadEvents(msgs):
|
||||
return ops, [], []
|
||||
|
||||
|
||||
@migration(inputs=["driverMonitoringState"])
|
||||
@migration(inputs=["driverMonitoringStateDEPRECATED"])
|
||||
def migrate_driverMonitoringState(msgs):
|
||||
ops = []
|
||||
for index, msg in msgs:
|
||||
msg = msg.as_builder()
|
||||
events = []
|
||||
for event in msg.driverMonitoringState.eventsDEPRECATED:
|
||||
try:
|
||||
if not str(event.name).endswith('DEPRECATED'):
|
||||
# dict converts name enum into string representation
|
||||
events.append(log.OnroadEvent(**event.to_dict()))
|
||||
except RuntimeError: # Member was null
|
||||
traceback.print_exc()
|
||||
old = msg.driverMonitoringStateDEPRECATED
|
||||
new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime)
|
||||
dm = new_msg.driverMonitoringState
|
||||
dm.isRHD = old.isRHD
|
||||
dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \
|
||||
log.DriverMonitoringState.MonitoringPolicy.wheeltouch
|
||||
|
||||
msg.driverMonitoringState.events = events
|
||||
ops.append((index, msg.as_reader()))
|
||||
AlertLevel = log.DriverMonitoringState.AlertLevel
|
||||
event_to_alert_level = {
|
||||
'driverDistracted1': AlertLevel.one, 'driverUnresponsive1': AlertLevel.one,
|
||||
'driverDistracted2': AlertLevel.two, 'driverUnresponsive2': AlertLevel.two,
|
||||
'driverDistracted3': AlertLevel.three, 'driverUnresponsive3': AlertLevel.three,
|
||||
}
|
||||
for event in old.events:
|
||||
level = event_to_alert_level.get(str(event.name))
|
||||
if level is not None:
|
||||
dm.alertLevel = level
|
||||
break
|
||||
dm.lockout = any(str(event.name) == 'tooDistracted' for event in old.events)
|
||||
|
||||
dm.visionPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessStatus if old.isActiveMode else old.awarenessActive) * 100)))
|
||||
dm.visionPolicyState.awarenessStep = old.stepChange if old.isActiveMode else 0.
|
||||
dm.visionPolicyState.isDistracted = old.isDistracted
|
||||
dm.visionPolicyState.distractedTypes.pose = bool(old.distractedType & 1)
|
||||
dm.visionPolicyState.distractedTypes.eye = bool(old.distractedType & 2)
|
||||
dm.visionPolicyState.distractedTypes.phone = bool(old.distractedType & 4)
|
||||
dm.visionPolicyState.faceDetected = old.faceDetected
|
||||
dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset
|
||||
dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 1200 * 100))
|
||||
dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset
|
||||
dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 1200 * 100))
|
||||
dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 1200 and old.poseYawValidCount >= 1200
|
||||
dm.visionPolicyState.wheeltouchFallbackPercent = int(min(100, old.hiStdCount / 200 * 100))
|
||||
dm.visionPolicyState.uncertainOffroadAlertPercent = int(min(100, old.uncertainCount / 1200 * 100))
|
||||
dm.wheeltouchPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessPassive if old.isActiveMode else old.awarenessStatus) * 100)))
|
||||
dm.wheeltouchPolicyState.awarenessStep = 0. if old.isActiveMode else old.stepChange
|
||||
ops.append((index, new_msg.as_reader()))
|
||||
|
||||
return ops, [], []
|
||||
|
||||
@@ -200,7 +200,7 @@ class TrainingGuideDMTutorial(Widget):
|
||||
looking_center = False
|
||||
|
||||
# stay at 100% once reached
|
||||
if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99:
|
||||
if (dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99:
|
||||
slow = self._progress.x < 0.25
|
||||
duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION
|
||||
self._progress.x += 1.0 / (duration * gui_app.target_fps)
|
||||
|
||||
@@ -1,20 +1,14 @@
|
||||
import pyray as rl
|
||||
from cereal import log, messaging
|
||||
from cereal import car, log, messaging
|
||||
from msgq.visionipc import VisionStreamType
|
||||
from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView
|
||||
from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, device
|
||||
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.widgets.nav_widget import NavWidget
|
||||
from openpilot.system.ui.widgets.label import gui_label
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
EVENT_TO_INT = EventName.schema.enumerants
|
||||
|
||||
|
||||
class DriverCameraView(CameraView):
|
||||
def _calc_frame_matrix(self, rect: rl.Rectangle):
|
||||
base = super()._calc_frame_matrix(rect)
|
||||
@@ -116,10 +110,14 @@ class DriverCameraDialog(NavWidget):
|
||||
return
|
||||
|
||||
msg = messaging.new_message('selfdriveState')
|
||||
if dm_state is not None and len(dm_state.events):
|
||||
event_name = EVENT_TO_INT[dm_state.events[0].name]
|
||||
if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]:
|
||||
msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert
|
||||
if dm_state is not None:
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
alert_sounds = {
|
||||
'one': AudibleAlert.preAlert,
|
||||
'two': AudibleAlert.promptDistracted,
|
||||
'three': AudibleAlert.warningImmediate,
|
||||
}
|
||||
msg.selfdriveState.alertSound = alert_sounds.get(str(dm_state.alertLevel), AudibleAlert.none)
|
||||
self._pm.send('selfdriveState', msg)
|
||||
|
||||
def _render_dm_alerts(self, rect: rl.Rectangle):
|
||||
@@ -127,29 +125,30 @@ class DriverCameraDialog(NavWidget):
|
||||
dm_state = ui_state.sm["driverMonitoringState"]
|
||||
self._publish_alert_sound(dm_state)
|
||||
|
||||
is_vision = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
|
||||
awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent
|
||||
gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height),
|
||||
f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
|
||||
f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
|
||||
color=rl.Color(0, 0, 0, 180))
|
||||
gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
|
||||
gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM,
|
||||
alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP,
|
||||
color=rl.Color(255, 255, 255, int(255 * 0.9)))
|
||||
|
||||
if not dm_state.events:
|
||||
if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none:
|
||||
return
|
||||
|
||||
# Show first event (only one should be active at a time)
|
||||
event_name_str = str(dm_state.events[0].name).split('.')[-1]
|
||||
alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}"
|
||||
alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT
|
||||
|
||||
shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height)
|
||||
gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
|
||||
gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
|
||||
alignment=alignment,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
|
||||
color=rl.Color(0, 0, 0, 180))
|
||||
gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD,
|
||||
gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD,
|
||||
alignment=alignment,
|
||||
alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM,
|
||||
color=rl.Color(255, 255, 255, int(255 * 0.9)))
|
||||
@@ -166,7 +165,7 @@ class DriverCameraDialog(NavWidget):
|
||||
def _draw_face_detection(self, rect: rl.Rectangle):
|
||||
dm_state = ui_state.sm["driverMonitoringState"]
|
||||
driver_data = self.driver_state_renderer.get_driver_data()
|
||||
if not dm_state.faceDetected:
|
||||
if not dm_state.visionPolicyState.faceDetected:
|
||||
return
|
||||
|
||||
# Get face position and orientation
|
||||
|
||||
@@ -6,12 +6,12 @@ from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
|
||||
DEBUG = False
|
||||
ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF)
|
||||
CONE_COLOR_ORANGE = (255, 115, 0)
|
||||
|
||||
LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6)
|
||||
LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3)
|
||||
@@ -21,6 +21,7 @@ class DriverStateRenderer(Widget):
|
||||
BASE_SIZE = 60
|
||||
LINES_ANGLE_INCREMENT = 5
|
||||
LINES_STALE_ANGLES = 3.0 # seconds
|
||||
AWARENESS_UNFULL_PERCENT = 95
|
||||
|
||||
def __init__(self, lines: bool = False, inset: bool = False):
|
||||
super().__init__()
|
||||
@@ -35,11 +36,15 @@ class DriverStateRenderer(Widget):
|
||||
self._is_active = False
|
||||
self._is_rhd = False
|
||||
self._face_detected = False
|
||||
self._face_pitch = 0.
|
||||
self._face_yaw = 0.
|
||||
self._should_draw = False
|
||||
self._force_active = False
|
||||
self._looking_center = False
|
||||
self._awareness_unfull = False
|
||||
|
||||
self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps)
|
||||
self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps)
|
||||
self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
|
||||
self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False)
|
||||
self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False)
|
||||
@@ -95,6 +100,7 @@ class DriverStateRenderer(Widget):
|
||||
rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x)))
|
||||
|
||||
if self.effective_active:
|
||||
active_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0)
|
||||
source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height)
|
||||
dest_rect = rl.Rectangle(
|
||||
self._rect.x + self._rect.width / 2,
|
||||
@@ -104,13 +110,16 @@ class DriverStateRenderer(Widget):
|
||||
)
|
||||
|
||||
if not self._lines:
|
||||
r = int(round(ACTIVE_ACCENT.r * active_amount + CONE_COLOR_ORANGE[0] * (1 - active_amount)))
|
||||
g = int(round(ACTIVE_ACCENT.g * active_amount + CONE_COLOR_ORANGE[1] * (1 - active_amount)))
|
||||
b = int(round(ACTIVE_ACCENT.b * active_amount + CONE_COLOR_ORANGE[2] * (1 - active_amount)))
|
||||
rl.draw_texture_pro(
|
||||
self._dm_cone,
|
||||
source_rect,
|
||||
dest_rect,
|
||||
rl.Vector2(dest_rect.width / 2, dest_rect.height / 2),
|
||||
self._rotation_filter.x - 90,
|
||||
rl.Color(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)),
|
||||
rl.Color(r, g, b, int(255 * self._fade_filter.x)),
|
||||
)
|
||||
|
||||
else:
|
||||
@@ -150,9 +159,12 @@ class DriverStateRenderer(Widget):
|
||||
sm = ui_state.sm
|
||||
|
||||
dm_state = sm["driverMonitoringState"]
|
||||
self._is_active = dm_state.isActiveMode
|
||||
self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
|
||||
self._is_rhd = dm_state.isRHD
|
||||
self._face_detected = dm_state.faceDetected
|
||||
self._face_detected = dm_state.visionPolicyState.faceDetected
|
||||
self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT
|
||||
self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6)
|
||||
self._face_yaw = -dm_state.visionPolicyState.pose.yaw
|
||||
|
||||
driverstate = sm["driverStateV2"]
|
||||
driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData
|
||||
@@ -160,24 +172,9 @@ class DriverStateRenderer(Widget):
|
||||
|
||||
def _update_state(self):
|
||||
# Get monitoring state
|
||||
driver_data = self.get_driver_data()
|
||||
driver_orient = driver_data.faceOrientation
|
||||
|
||||
if len(driver_orient) != 3:
|
||||
return
|
||||
|
||||
# Calibrate orientation so looking straight ahead at the road (instead of at the device) reads
|
||||
# (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149)
|
||||
sm = ui_state.sm
|
||||
if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3:
|
||||
cal_rpy = sm['liveCalibration'].rpyCalib
|
||||
else:
|
||||
cal_rpy = [0.0, 0.0, 0.0]
|
||||
_, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy)
|
||||
yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention
|
||||
|
||||
pitch = self._pitch_filter.update(pitch)
|
||||
yaw = self._yaw_filter.update(yaw)
|
||||
_ = self.get_driver_data()
|
||||
pitch = self._pitch_filter.update(self._face_pitch)
|
||||
yaw = self._yaw_filter.update(self._face_yaw)
|
||||
|
||||
# hysteresis on looking center
|
||||
if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER:
|
||||
@@ -198,9 +195,8 @@ class DriverStateRenderer(Widget):
|
||||
rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN)
|
||||
rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN)
|
||||
|
||||
# filter head rotation, handling wrap-around (bias pitch up since calib/DM pose isn't exact,
|
||||
# and halve yaw sensitivity)
|
||||
rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw))
|
||||
# filter head rotation, handling wrap-around
|
||||
rotation = math.degrees(math.atan2(pitch * 2, yaw))
|
||||
angle_diff = rotation - self._rotation_filter.x
|
||||
angle_diff = ((angle_diff + 180) % 360) - 180
|
||||
self._rotation_filter.update(self._rotation_filter.x + angle_diff)
|
||||
|
||||
@@ -114,7 +114,7 @@ class DriverStateRenderer(Widget):
|
||||
|
||||
# Get monitoring state
|
||||
dm_state = sm["driverMonitoringState"]
|
||||
self.is_active = dm_state.isActiveMode
|
||||
self.is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision
|
||||
self.is_rhd = dm_state.isRHD
|
||||
|
||||
# Update fade state (smoother transition between active/inactive)
|
||||
|
||||
+20
-18
@@ -45,26 +45,28 @@ sound_list_iq: dict[int, tuple[str, int | None, float]] = {
|
||||
AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME),
|
||||
}
|
||||
|
||||
sound_list: dict[int, tuple[str, int | None, float]] = {
|
||||
# AudibleAlert, file name, play count (none for infinite)
|
||||
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME),
|
||||
|
||||
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME),
|
||||
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME),
|
||||
|
||||
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
|
||||
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
|
||||
|
||||
**sound_list_iq,
|
||||
}
|
||||
if HARDWARE.get_device_type() in ("tizi", "tici"):
|
||||
sound_list.update({
|
||||
def get_sound_list(device_type: str) -> dict[int, tuple[str, int | None, float]]:
|
||||
sounds = {
|
||||
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
|
||||
})
|
||||
AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME),
|
||||
AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME),
|
||||
AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME),
|
||||
AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME),
|
||||
**sound_list_iq,
|
||||
}
|
||||
if device_type in ("tizi", "tici"):
|
||||
sounds.update({
|
||||
AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME),
|
||||
AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME),
|
||||
})
|
||||
return sounds
|
||||
|
||||
|
||||
sound_list = get_sound_list(HARDWARE.get_device_type())
|
||||
|
||||
def check_selfdrive_timeout_alert(sm):
|
||||
ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
|
||||
|
||||
@@ -2,13 +2,19 @@ from cereal import car
|
||||
from cereal import messaging
|
||||
from cereal.messaging import SubMaster, PubMaster
|
||||
from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
|
||||
from openpilot.selfdrive.ui.soundd import get_sound_list
|
||||
|
||||
import pytest
|
||||
import time
|
||||
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
|
||||
class TestSoundd:
|
||||
@pytest.mark.parametrize("device_type", ["mici", "tici", "tizi"])
|
||||
def test_prompt_distracted_sound(self, device_type):
|
||||
assert get_sound_list(device_type)[AudibleAlert.promptDistracted][0] == "prompt_distracted.wav"
|
||||
|
||||
def test_check_selfdrive_timeout_alert(self):
|
||||
sm = SubMaster(['selfdriveState'])
|
||||
pm = PubMaster(['selfdriveState'])
|
||||
@@ -32,4 +38,3 @@ class TestSoundd:
|
||||
assert check_selfdrive_timeout_alert(sm)
|
||||
|
||||
# TODO: add test with micd for checking that soundd actually outputs sounds
|
||||
|
||||
|
||||
@@ -92,11 +92,12 @@ class SimulatedSensors:
|
||||
|
||||
# dmonitoringd output
|
||||
dat = messaging.new_message('driverMonitoringState', valid=True)
|
||||
dat.driverMonitoringState = {
|
||||
"faceDetected": True,
|
||||
"isDistracted": False,
|
||||
"awarenessStatus": 1.,
|
||||
}
|
||||
dm = dat.driverMonitoringState
|
||||
dm.alertLevel = log.DriverMonitoringState.AlertLevel.none
|
||||
dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision
|
||||
dm.visionPolicyState.faceDetected = True
|
||||
dm.visionPolicyState.isDistracted = False
|
||||
dm.visionPolicyState.awarenessPercent = 100
|
||||
self.pm.send('driverMonitoringState', dat)
|
||||
|
||||
def send_camera_images(self, world: 'World'):
|
||||
|
||||
Reference in New Issue
Block a user