diff --git a/artifacts/iqpilot_hephaestusd_private/manifest.json b/artifacts/iqpilot_hephaestusd_private/manifest.json index e8be689..6ca0455 100644 --- a/artifacts/iqpilot_hephaestusd_private/manifest.json +++ b/artifacts/iqpilot_hephaestusd_private/manifest.json @@ -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==" } } diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so index d98eb54..1c1b60f 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 0000000..0ee00bb Binary files /dev/null and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so index 11cef1f..451c554 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so index e4561d9..a0d5e41 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so index 399b676..2921640 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip index 67dd229..3af1b89 100644 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so index 22466cc..73de247 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so index e9f83e9..6e5ec02 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so index d8312a7..bf5de9d 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so index a5d5eab..d32d379 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so index 5c7e288..5507056 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so index f1b76ca..fc00647 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so index c447b9e..4b935ba 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so index 0830f98..97b04a1 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so index 8f14878..a3fc091 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so index 696ad0c..4ab0517 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so index c2ed59e..7d34cfc 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so index 805302c..fecd933 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so index d5d9815..e48e0fe 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so differ diff --git a/cereal/log.capnp b/cereal/log.capnp index 590c92a..39e9f50 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -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; diff --git a/common/params_keys.h b/common/params_keys.h index c6fc6b5..e311527 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -41,6 +41,7 @@ inline static std::unordered_map 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}}, diff --git a/iqpilot/common/atlas_alerts.py b/iqpilot/common/atlas_alerts.py index 5339155..8060fda 100644 --- a/iqpilot/common/atlas_alerts.py +++ b/iqpilot/common/atlas_alerts.py @@ -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): diff --git a/selfdrive/assets/sounds/pre_alert.wav b/selfdrive/assets/sounds/pre_alert.wav new file mode 100644 index 0000000..f83b710 Binary files /dev/null and b/selfdrive/assets/sounds/pre_alert.wav differ diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav index c3d4475..6cad556 100644 Binary files a/selfdrive/assets/sounds/prompt_distracted.wav and b/selfdrive/assets/sounds/prompt_distracted.wav differ diff --git a/selfdrive/assets/sounds/warning_immediate.wav b/selfdrive/assets/sounds/warning_immediate.wav index b1815a9..99b4b03 100644 Binary files a/selfdrive/assets/sounds/warning_immediate.wav and b/selfdrive/assets/sounds/warning_immediate.wav differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1bd7b31..3def978 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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() diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 5b25d8c..c1f30bc 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -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 diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 0e50932..73a4af2 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -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) diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 51d1849..873c74b 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model.onnx and b/selfdrive/modeld/models/dmonitoring_model.onnx differ diff --git a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl index 7869f9d..9aad8d4 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl and b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl differ diff --git a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl index a89f4f1..8f33bbb 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl and b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl differ diff --git a/selfdrive/modeld/models/prebuilt_check.json b/selfdrive/modeld/models/prebuilt_check.json index 6d0839d..a40a808 100644 --- a/selfdrive/modeld/models/prebuilt_check.json +++ b/selfdrive/modeld/models/prebuilt_check.json @@ -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" } } diff --git a/selfdrive/modeld/test_dmonitoringmodeld.py b/selfdrive/modeld/test_dmonitoringmodeld.py new file mode 100644 index 0000000..af23f46 --- /dev/null +++ b/selfdrive/modeld/test_dmonitoringmodeld.py @@ -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 diff --git a/selfdrive/monitoring/README.md b/selfdrive/monitoring/README.md deleted file mode 100644 index 2a29ea0..0000000 --- a/selfdrive/monitoring/README.md +++ /dev/null @@ -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. diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 022415a..ffefe32 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -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() diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py deleted file mode 100644 index 6f8752d..0000000 --- a/selfdrive/monitoring/helpers.py +++ /dev/null @@ -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 - ) diff --git a/selfdrive/monitoring/policy.py b/selfdrive/monitoring/policy.py new file mode 100644 index 0000000..1c55b4d --- /dev/null +++ b/selfdrive/monitoring/policy.py @@ -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, + ) diff --git a/selfdrive/monitoring/test_dmonitoringd.py b/selfdrive/monitoring/test_dmonitoringd.py new file mode 100644 index 0000000..8aae92e --- /dev/null +++ b/selfdrive/monitoring/test_dmonitoringd.py @@ -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 diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index fef6a7e..37606fe 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -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 diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 1085e11..a76a620 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -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", diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 285d46f..15da7cc 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -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) diff --git a/selfdrive/selfdrived/tests/test_driver_monitoring.py b/selfdrive/selfdrived/tests/test_driver_monitoring.py new file mode 100644 index 0000000..3d47a9b --- /dev/null +++ b/selfdrive/selfdrived/tests/test_driver_monitoring.py @@ -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" diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 010ce3f..b135a68 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -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, [], [] diff --git a/selfdrive/ui/mici/layouts/onboarding.py b/selfdrive/ui/mici/layouts/onboarding.py index 11a8154..8efcdd9 100644 --- a/selfdrive/ui/mici/layouts/onboarding.py +++ b/selfdrive/ui/mici/layouts/onboarding.py @@ -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) diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 68ff785..1ad90a2 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -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 diff --git a/selfdrive/ui/mici/onroad/driver_state.py b/selfdrive/ui/mici/onroad/driver_state.py index 8a20137..bd2e694 100644 --- a/selfdrive/ui/mici/onroad/driver_state.py +++ b/selfdrive/ui/mici/onroad/driver_state.py @@ -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) diff --git a/selfdrive/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py index 39aa6d1..e34b2c1 100644 --- a/selfdrive/ui/onroad/driver_state.py +++ b/selfdrive/ui/onroad/driver_state.py @@ -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) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index e956e73..9d92e80 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -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'] diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index a9da845..a1e18e8 100644 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -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 - diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py index a8374a0..6ac7c4f 100644 --- a/tools/sim/lib/simulated_sensors.py +++ b/tools/sim/lib/simulated_sensors.py @@ -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'):