From 27cc0023e1e3d2487d1bf881dcc4e76aafbd45e4 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 8 Apr 2021 23:22:40 -0700 Subject: [PATCH 01/32] hotfix: remove ubloxRaw check in controlsd (#20631) * remove ubloxRaw from controlsd SubMaster * reduce STATUS_PACKET logging frequency to every 10 minutes Co-authored-by: Willem Melching --- selfdrive/controls/controlsd.py | 4 ++-- selfdrive/thermald/thermald.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9b37834bbc0c88..b575d6eb996c88 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -55,8 +55,8 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.sm = sm if self.sm is None: - ignore = ['ubloxRaw', 'driverCameraState', 'managerState'] if SIMULATION else None - self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'ubloxRaw', + ignore = ['driverCameraState', 'managerState'] if SIMULATION else None + self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 9f769801746a79..78a5350837b580 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -407,8 +407,8 @@ def thermald_thread(): should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() - # report to server once per minute - if (count % int(60. / DT_TRML)) == 0: + # report to server once every 10 minutes + if (count % int(600. / DT_TRML)) == 0: location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, From ba34a7ccb8c478c0b66211103f4a77086f5ce88d Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 13 Apr 2021 10:19:32 +0200 Subject: [PATCH 02/32] Remove .python-version --- .python-version | 1 - 1 file changed, 1 deletion(-) delete mode 100644 .python-version diff --git a/.python-version b/.python-version deleted file mode 100644 index 0cbfaed0d9fe75..00000000000000 --- a/.python-version +++ /dev/null @@ -1 +0,0 @@ -3.8.5 From 8d8b35f3a386cfb188e61865fa0d98a3044eb360 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 18 Aug 2020 15:27:56 +0200 Subject: [PATCH 03/32] Expand logs utility for usage with unlogger --- tools/lib/expand_logs.py | 64 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 tools/lib/expand_logs.py diff --git a/tools/lib/expand_logs.py b/tools/lib/expand_logs.py new file mode 100644 index 00000000000000..880ae3f37c8340 --- /dev/null +++ b/tools/lib/expand_logs.py @@ -0,0 +1,64 @@ +import os +import sys +import re +import shutil +import argparse + +DONGLE_ID = '52a8edf5ce93e696' +SEGMENT_NAME_RE = r'[0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2}--[0-9]+' +OP_SEGMENT_DIR_RE = r'^({})$'.format(SEGMENT_NAME_RE) + + +class Expander(object): + def __init__(self, data_dir): + self._expand_directories(data_dir) + + def _expand_directories(self, data_dir): + files = os.listdir(data_dir) + + for f in files: + fullpath = os.path.join(data_dir, f) + if not os.path.isdir(fullpath): + continue + + op_match = re.match(OP_SEGMENT_DIR_RE, f) + if not op_match: + continue + + subfiles = os.listdir(fullpath) + for sf in subfiles: + oldpath = os.path.join(fullpath, sf) + newname = '{}|{}--{}'.format(DONGLE_ID, f, sf) + newpath = os.path.join(data_dir, newname) + print('Will move froom {} to {}'.format(oldpath, newpath)) + os.rename(oldpath, newpath) + + print('Will remove source dir {}'.format(fullpath)) + shutil.rmtree(fullpath) + + +def get_arg_parser(): + parser = argparse.ArgumentParser( + description="Expand downloaded segments from device into single folder", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("data_dir", nargs='?', default=os.getenv('UNLOGGER_DATA_DIR'), + help="Path to directory in which log and camera files are located.") + + return parser + + +def main(argv): + args = get_arg_parser().parse_args(sys.argv[1:]) + + if args.data_dir is not None: + Expander(args.data_dir) + print("done") + else: + print("missing data dir") + + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv[1:])) From 5191c203ab81d02b50b4670045c992f5927f74a6 Mon Sep 17 00:00:00 2001 From: alfhern Date: Thu, 16 Jul 2020 14:39:07 +0200 Subject: [PATCH 04/32] Point cereal to move-fast repo --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 1e6110b7a6941e..d0a6c7e1cbdb7d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -9,7 +9,7 @@ url = ../../commaai/laika.git [submodule "cereal"] path = cereal - url = ../../commaai/cereal.git + url = ../../move-fast/cereal.git [submodule "rednose_repo"] path = rednose_repo url = ../../commaai/rednose.git From 85c41103bf3bbd6ac001eec538220282e29b8903 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 13 Apr 2021 09:56:54 +0200 Subject: [PATCH 05/32] Hands on wheel monitoring: Bump Cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index e1741865f8117f..965ae80e42c61a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e1741865f8117f27206b0225b4c48fa7432b830f +Subproject commit 965ae80e42c61a42b0276fb11a4bccbd15ae8245 From e78bb0e9bdd6e08ae5f4abd9683b5fb4b96f30f9 Mon Sep 17 00:00:00 2001 From: alfhern Date: Fri, 17 Jul 2020 15:23:38 +0200 Subject: [PATCH 06/32] Hands on wheel monitoring: Implementation according to r079r4e regulation --- common/params_pyx.pyx | 1 + selfdrive/assets/img_hands_on_wheel.png | Bin 0 -> 21194 bytes selfdrive/controls/lib/events.py | 20 +++ .../debug/internal/hands_on_wheel_moniotr.py | 80 ++++++++++ selfdrive/manager/manager.py | 5 + selfdrive/monitoring/dmonitoringd.py | 19 ++- .../monitoring/hands_on_wheel_monitor.py | 51 +++++++ selfdrive/monitoring/test_hands_monitoring.py | 139 ++++++++++++++++++ selfdrive/ui/paint.cc | 12 ++ selfdrive/ui/qt/offroad/settings.cc | 6 + 10 files changed, 329 insertions(+), 4 deletions(-) create mode 100644 selfdrive/assets/img_hands_on_wheel.png create mode 100644 selfdrive/debug/internal/hands_on_wheel_moniotr.py create mode 100644 selfdrive/monitoring/hands_on_wheel_monitor.py create mode 100644 selfdrive/monitoring/test_hands_monitoring.py diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 3d366b08f1645a..1f051118492430 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -38,6 +38,7 @@ keys = { b"GithubSshKeys": [TxType.PERSISTENT], b"GithubUsername": [TxType.PERSISTENT], b"HardwareSerial": [TxType.PERSISTENT], + b"HandsOnWheelMonitoring": [TxType.PERSISTENT], b"HasAcceptedTerms": [TxType.PERSISTENT], b"HasCompletedSetup": [TxType.PERSISTENT], b"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], diff --git a/selfdrive/assets/img_hands_on_wheel.png b/selfdrive/assets/img_hands_on_wheel.png new file mode 100644 index 0000000000000000000000000000000000000000..0b06b4c6207fb0842c30bdbe37a988ea0d7775e3 GIT binary patch literal 21194 zcmV*0KzYB3P)dPC+00001b5ch_0Itp) z=>Px#L}ge>W=%~1DgXcg2mk?xX#fNO00031000^Q000001E2u_0{{R30RRC20H6W@ z1ONa40RR923ZMf31ONa40RR923IG5A03UYUu>b%-07*naRCodHod>uiMfLVwGAubR zu)qRK&N;JWB!dLWN%Sun6a^#*eg=|AFn|am2q>sz$vH1b&N-)rCBqW;`@OR>cjwNX znL1Tn)zfotpXYgR_jI2+sj5#`S5@~N^sSQmT?EFC9Xlyb4Ff~KOjXQWx36PU0r54% z1RVdo1wQ5YJ7823yv?x}1`QhY0o#)LQ3M9{BVPuTD)3!ZD(K2|1?PE|=Zy=9`v7Qb9dlN2UL(c)m~Dwblq7?=cf6OF~#=fVB;AbF^V zQc{5@IMVK;n3+zdG8jewB%$05wNqm>x4m z040Vsz@lIv*UcOFe>12h4n4+{)WHZ0O>NS+)9|6s_Tn3aht|w`X@fT5+@Qpy3 zD10O4$xm`vkRrQ)9l#u(#Y^_~5%`4Dmx8mw4P{S*C?ujXQ6vZ~$zfW=ei!TtRs=<) zu^6Sh@fSdACTd<81?rmD(DM`u&MGUPO8oqs{?mlQ5nsqJiCP(|}DBT+CBaaaxBU zX!=nXjxLZFz#}v^Egmux@)XN;!EQj07n6Zb(3dEE8ORUeMIc|awn5X7HH^l`ulemo z_&GHNW&oO*sfX5X%w30R*zXkGf#S2lKk)HmUnB5wPRh&&CI+fk#q??L3i8xW1{^5} z1BPe-3*hW_U>l%Ef_DDv;?lG5-@&b|8VE9{;QY?u03i8!k-FWFfQP|D;4z@Y@G7<{ z;dPXV%XAo^#4$h68jvNbn4N82^Z|;_0H+cYCC{dWty;mV9^h6$c52mXGg8ud5d0fl zgv{5PF_p)A{-d zK=5zy7D!EBq2QmO*JB{R7Xw#-uYoUpLxC%S5=+0Qo`&BaJ*5)$PQyt+y@(jT-iw^y zf=$8JKwZ-m^dUm;0Jm2$lDn726Z$1lPwI87TE*&YwIn?i-GkuYgArhNkTv0|_oiOw zLDZ3EWcnq=xOfT~7#iPF7iQ|RB^50KG@k20QRp)a`5Wjp(q6Qf^#^;1@l4=g@H{9A-P#{w zTl1mc^eAW@+v1eE^gi@B%o-=}!L+!qgOu6LA{-vAjp=&o`Y{SB zLeHh|0oPR7xjZj=CkoXtI`n$@&0rp2r5j+}1f8E*3#8z~cGdChU|uj3=w&!P9yQz6 zj_JAl6QFk+YTNq#+A)p6=z0Em{QQ({M+sjK!+>9U81XK=Yh(LE9%Xu1(Wfynjg7S< z)&Df;__7^S(a#k!QBVQ8AlC*9fVsi+;2WS|zX&v7cptbEED9a~?MUV3r?Osb$JFi| zHiaY$&7~_@jAp5(j6d8(>l0iKbM?vRP(;Fywt{L$Eoz%*S!9nOa-a}8OBq2TM z9L`2lpcCkK8(N8TS~|Wg&|tYH0tb;?adefn?0*huP z3!fPl8GAJdN*0ux2rLWs2NwdL1_yf~X$)4~4m9YvDVQ;|ekl8Y;2U72Xh}jgqxAn- z6$kKrNtr)`qAef{t*)?g1Ym3M7D!Dygjv|U6voa8e3Qjhlrh~&;hPQUE~%-wkHJ8o zCvox=usU#c$$XNvH?fa$3B3ZMQ#)ydQwn^bQKPYi~(EJd4v>_n>bp%`381J^QC{9@!zu;!4VC5KG-wjWrwu?gj%rIS_)Tr16c@ z!MdQbT9)lr;45IIBuS!Lhlg8@h{Shpini>Mnft5@QNOD{!*V8V)t)m+GJkNQEbAc;i{|;P+ZCcS%QS&+d|)!I5A?FjE$rQ3xJRm)NHT z(4qAhR?lfDb(lv%W8o zjF7L{j2C^3a`0*`#ic-V_Uh($HOYsnFY+noIuRv_U+`SXF-fR(h6P`v~*!LNU5 zpliWGo0tU`oe^*Kr6(8ZP7`ziQ%>KG@S&$*)?-XELVng{p{496gY|&(PL#}%#De8p z)kpa%pF`;b3a*gST;$ISUiC0gNn+y=n<%G6cb4AQ-$LXUvW(H;U9%XEj>CWtX(iy0 zEIM6fksp#Q?x)-WF4_I{ynF|Wfe+=8B<8^&J-S*cI7!s%7Dc-nl)YZ#@9Hg%4ly{Z zkHbum)lp>Z1@t^Pp|0u&|@&C zP8M^c!#7#{G-h0iEQ{Fu)ThGW%_B*Kyea(4m~qBW%j1Vus&+RS9o-a=LYA{0%zZHU0Dz^X>Ui@Jf1T8 z%%46xw-i_!%nw9bg!VOf6KHYK-QYjyd5vwy2z#eO`T9Upm0EN)Jy2WI;-qiDV?bXe zyB{nGjsaHEGJO3ft~;#-eEG0nYi(`yFRfNF!EUvPi&n=(;~7F^tFMD zeiBh-k0_Le^ei5t+zdTJ66_#<9`G_qL3;K2TA)OkE1`2`a31&sq@a%wx=Ko?kUuNX zS{om_3FfFbyPA*4{fWU4trG2S!bx@ETy#S!FHx*yahuD4hVu{dyPK0m&7B+BSt3MM9^-RWi`G95#9NV0fiPcj1Ai2n& z1!xA$hpvY?*}r8#wi2+ejdt`_%D5zzjaSP8&4>RT{11EqT=bumMxi_;S!m(UbcyZC zBUvc+XMxOXRcf)2EN})$O&V`Xog`*Phi1Nf=sK8_B?-kD`IcUquC4pPx!`9&V_3x{ z44f7m37!I>bYMykF)Acks1i~qiycA(=2`Bq@LS(ZY*`h^P?9(sy}>&mD`_BZ_mrL?KO|Y`1)U^8N3K?r zT6$ctMWJ71jW1T55XBOkkhfuTzD*qB6LMz&8oc(Qt6@%^By{7@wUCuYAz)A7^D2Bu zDBG98fZ4!E5R<+^*xv0mXFH4hkYpi0r^%*yd}(A&4$c92lNw;~C^=;}0;;1tNjB>a znYlMqs{v$94>V})L*_53*Yx;qF=kuf8kg{~S-kgvq%T3X^b^HP0iQSNVE3XVQ>HbC zup}f|DBxPtkcp6?aS7i>N|9cqMMrM|EgANuS190pSES4|KzH~~(UmC9Jiv^M>45Kx zXVRVYEnehr1-=9+NOi7tIQbC@mjD`xPEAQ)j>7nmWN{bef|CVhbw_>=gwW*_`Zmyl zrV6n?8CVsZ2{glDCGQoV2=-YTuU^L`U<)vljAg4{)Nrsn@a^(bq(U$9SBtoUh_V_> zyRRb2{zq}2BuJsL5c0PKsggv^Q(d#kr^_oYqez24s;s=|5M!)^*l;2wS=>pvo{QC@ z7=8@^9tA?W?P}PoyY#@Li%`8D3%vuAdDI@>8G&XXETqA-wxjQyS3GtBA-z&1#Q$@} zqwzdt)FbkJRKAQd{UJ%bO*eV}EN{lYj+p*qL$&?uc4h12Dc#lx7?P5L7PX*Kq^r4GkHYaFn9P9azpJhx! zvGRG;Z1g?NB0RJV25&ZMYxDa9&YcR@2Xg^kh;xGVz$xH;;7dhZXK^b|G+pP<@m;`Y zz?bwvorUTfmG3^~*D@$oGIGu&z@r|Kg*NrIvz{LXYk)T_iW+z{u)AfwtlB=u)$S}w zu3qH7`BB6qaU*u^3WkEp;yJeJk!=Xh0zT91x|W-^+gj{`wg^2OSm|^a+rBHoc{%te ztH3(G{em1Y7D7)*&*B!!^#q-Lb^reeLg=3qYTHXOy7DdLSVs2ebIC)_ng&OnaLKWp zN6!#&lR;lImJ>PH)1_u#H#a46ANm)}Ju)md`|=>)wr=lIeXO z12hQYBHax(_7M*M_keGga~~fazP#8Ik}PhboVxtV_P)=;6se4OAK+_iJAPyyEWr+H zdMB=zOZfvGGIaXL`Z8Jeku@RsHTVYj((M$``X1GV61Wdt2y@Yr#Dmym{^&Hk6NB@B zgDsf8p%D)tKO|XbAWv%>eCag`M1Or6nHv}k z`MxV`$k0XWqD@?Ke9xm$Q_1hT6gN4qI#6Bska}`OOA-%Z(~JReQ;*jAxD{`@6s1Q1 z7yZ-Mp?ZFVZw-8th4&O$y-wFY_Jkx0SDYHp-^76=ANWP?(v8ZR{NCRXhu<|R*LY|- zmz1RFV90H4uYJ%$wVMKa84Nt*H5Jaff3 z1fXO0mKKaZVHu-1WITJzz11yRUR zyj=8XE8D$~qiM85+yA_Uwkd$F&4AB440>s3*ES+EpF)1Do`s$}hi5j(AOJmQ0~hJL zmTd_EJGq2#{;zF?L^_F{1%NK5taJ_nqJ5vEnS03h?MXyS60!&V^B96Ox|!|X$51q} zLD}`*f*l3Wxe4%17CSq{W;%^MS(C-zkYQdnoyA!6%m6+C?Pv@qoiB&Mqvu^0Nmu3< zwk$rQM>86hB-qMk)kD#egnI2!dJ?r5B5!hFd6bW{W&~xQfp+vz&A6Cb6l&C4joC`- zS?n0ohDKS)KgnuL9iM#oVWX^+ztMMco&Hw4H(G5#fMv1WiB@?r_^@dY;A)%Ad_OCO z{#G(j=-ZP}l9;)bywKyw)HJiMiFWj9sQf`HMQeM^F8fG_DllfzO>o$s*`IiB@(YPiskh23cVrdo^ggLba8? z(BtUa5?CJDK84TxUIx4?0t+drCm*P-j}Q8$23mIQL%P#C$7kyGxb~6fn2q+s z1JVV42`oJc*v6h-U*57$nR`{xK)MKz1iASy`_XqG@JZ%cpAp4I64km$tOc<~>_{3sJl}E|KIsNA{2N~OW=+sKW4O}u~o=4wI z;Cc@mH9hOyNGUVILx)(e0(tkSRzl}1Qf{Qo|Beqm+W^a;Va=qZNkVN4MSItjIkxLY zYl|qF3|KDEPvJ}YJdXS|jkds6A>CoN$Px_7TkoV#!8UqEF)%4;qaOYOlx+NfJX^m>lIWHZ2Ww4O-}oC`#*vpmkng z?Ox@PELOuFWkf5fv2IfDlVzVW#~TzVE=hY*^|qpGDqvZj+i3a&9O!u+zFwx!J~9>r zdVKLEEtk%$-)bwm76z6<@MEkpYkbI6lCZo>0b7Nh{$e;(fN}6h&En;tmVNdUnAhxnt=t9 z1*kzVx=u6Lcxywkwm*>UktLW zl~adB7YFDTtv=` z89BZ^iKi%I{^(4D>878^S5Ly?A8Zv;uX+WO@T^QI`t3NK=D%`e$*g+Yl}En1!ta?a z8u#qFjzi{R26^|klacit`sW0i@AoAwxn47?eoJ}u=weht^10K(UZFeqJG(_sC=c@Q z8KdfpDTuU$EG&jxO|go3;Wz)7Le>UtnjM7{()K$C}VK+ov0K!CS;6p*9^mNz{kssKz+7{Y}Z! z>Psiu&m&nZhV9ChR?-5lRqBJo?@JAZAAo1V`f|Sel-~#F&gM&hqreot`tst(w)w#$ zzD~ifY3WQKPo}&c;r<6)^q2a0z`QN+8JK+*{-O0UV(%kAb&^0ug??+Y9oD_71@hSB zkt`O)5uX7z-2qps1&jSXjDa|)R>o(G?7=`^f%YX`NI&q=)hS-=RQLZJUk9uFDi@Rp zDR-bzXV3C}_y=;5sD*3SwrvG2t&wm&&((y*+A3Kr-qx^;Cy~D}&~%}d^t`{K)Uq7{ zG|=WE-GR3BF%|xWfJUHw=>-Zz>6-i4o0S)ve+ZP|e98OK3I*Q*mR^B+k(ETm);CE! zN13LBpjl&8E06rVdJ?GWA=Oc({nza1*>XYyP*3-;`t4}x=+s6>jYYb>lphoITS5P3@U|&xI$Y}@Lmjy5SIt{;)_xC{C zSrN`p2#y5Swpe-31Af(qwG6Cd^GFhlqenN=5RxytCUtFw(4!RUGr+4m=l4COsljdu zrmnRy_VrAt0lDLV??sSDvRDAS6g(>_*hhe6fTdSaldDFw9~fSXjkf%r75o6)4}3}b zlT07h+nC2beoPPk>l=V7|46n<7`H`WVLLf$yW=^OQ;5lUN)bK7;YF zSFSl-d%6aDE^_v#2D(-?qvS)qztus>$-t{d39rG|XDYF=fx$B~unYv&t5slHE_`X! z5%L@P^8@+dL$d2Vp8|Efx)Vy@=(=ORyKhq<*@C2yIA$$_Kk8K-4tx?Y?7c|qWz=pJ z;}Ft#lus{0I+P^l%<6x;^2iTK61vu$pE-bM1hBS$_58+vuYw=SHWqpkCg+;?S`gq> zV>sR-xQ-`}WTCsyD|I@Hyn6@6C0)Q%tFKOM@ogQTMA!|KN0QJ?ovr~N(lyrhk*vOM z;Z@I8*Rq8+>nRKO*@hCz+Vl4m^4OuA@0yG}lEu8(^|Hb5PO>C_Y09G-ld%%uq2SI= z2HK$e1$feYzo8mgAYJ25!9XcbYl=LNQTBniuPSRW$q5AM`A4FnxrFRaD>n;ajc z0k%b2Fw`H`z+})|c7tId$zsuZ8L{{AYZ%b8SW!r$&AENpC3ZZs%ENc9h*Wzk3aM=j z%j#FV^2pa(pInoK>_O+`KsO~9Ehy%7U0j_1cRNRY&LQhqgT8te1#loF3k~Z14rETq zKJ`&VUSgnbvBqXPO+`%-$HuBR0(_An$ZIQ6>||g(ES6!CgTCVoI_p{bd$PC}XY=`( zNX&{%D2sULk$7KVd5(rHI`#V`QN0!qF+^@hU9<-o825g@nRV?XwXkFP-T8%EI1QiP zw`v#u73}x<4BU8ED|^fG_jh35hU3wGGFbCtjFr3}8HH*vt2V&-xUr0t?M(yYxJ+wc zEE)@O)JsVV|$!3F;{6V<>u3k)^I)jjfzG>k;`TptfqI z6N&RdR^~uHUCYZE%4%?a9N~;!`_xTkUQe zO<`g|I4}=TBEAf~143yO1tsH0KuM-62wU}vzKa%#=?u!$lTh3Ap%Y`;&`cJ_9BDL2 zIYF~ZO8w-o=i18)U?4hHh$pZbB<>qUp@ji}Je3$|J6g&V@k!1e_%n--wG*Ynd zhlT*G8%sO776L^~5=R)~P-8j0Pdqp?D|oNQVb~ttM@ZxY*r4f0A5z6c=~*D>K7--~ zBfA_$9CqtYb~ezWuDnRPq%+xURF>di*QSO*{J$(ZYGu(Ak|gvfkd(0Na6=4gEXoDV zKPkb9J!)LWZPcaR&7y=IzR5zLyUTO|Q(hkq`3AHjeI2Pb#?#F7+>c?8-iG}f(0e1D zBI%Y5bLSp_HVUyvk6tc%0)`3Nh;g2xd{L8x<bVX3KWiy4(g|#^ zEDEVNHB+)ckmZwM*F|zKvl#3h1P%as_8?H#mSl@;8kE_=NCL3MvOIssP<6;(5UB0> z&`F{CtmQDqGJ{udvvNS|gvrST>bX7k549Q{%7-nvBn$Ld-hEMM4h;1rW;yJb9b6}a z3QDqV_Lx40%EFGn8e*h@Uhm!zWr~_44mJ4PjHUHQs^H$PSqbB@OdqzfR6ch2e%Rz~ z%B*gYEgpE4BrM(b&*9T_Vn+!ZwgIC-(MdLL)6rm|92-_M2I_J6%*g`Y<$-Tc;^frv zz$VL^p|CZj?ONIg`|(YS*bn}+rSM26Fy(Z+ITcPVwbrW7mKQeXG$!$1V5M^-xf3J? zBehuOEUU2v;*-rC9oIqsW5#%>XR#A#*#+m^WI#6;-^caSf-2sl5H=kQvQV#8s=b~c zk*YV^*7UHa8S773EYeF!Gp5jE*rGa`_y=E%hY+mC-%D>~;mvlID4=mW#JqMr9=<3(1F$^(@eRV;18xlvSIJ zGJW5w-jJ^zP7Klwq%R@aGJWJ@z>oWCiFX7;F7h!Z67Su}N%nSOyXZ(S_)i8uZl&x- zV6t|4EaxNxYz!nbl;lgNVmTDudbG%!F0{lqlm}fG#J2AA#@)W;OH}y zStBolr3*->FC|~J*D3}Ws#&8vA7L{s1Z?BSfu2Es6@FdIF8WO$os_u7Rf_W~g5f}l zeCU8a3Ns!+`E))8WZ-?6vcof)8(bXSntJoS^Ibq0%O}o)?P^v~I)QZikbGOdnY=uX zp?h=ip^w2Ccw-F+_k;#pw5ZL8bnWUI*0t=SYkCybb||5{ri-Q#KTILXV*i@17+Y+Z z(`9VN^WRYZ3@}9udqZTPdrIJQm;My~?}zA$Sw=Ph*i81D$zs2H8Cmu*R9)RvbQ^*@ zc9*Ng1sl%vaPWP2m#LNQ^j5Y2*<__NJ83gUP6+k_qpSu8@U0QcM|AJ!ssDBO=dah# zZ`YH7a)Oo-j3a)ihv1@XYy7n>1?B@TWsP#9eE0^T$2JOX^7|hgy8~EOTdGH;#axBB z*sx;@U?ojgu2QR)qHJXo@R>^8qSLlx?_lsfpcyXL%@dXiJ;&dBtNZOX$Hedt1^V{| zKGY{k$PNTtYY_f>{eIw1}x4ox%c)K93KSRafR_Fk8FA4d?bn*6$mRnSY{l za+se5E}e6s=VkcyS@b1=UV&B_Wl8z|26zSB2cH(!6oRl<(}quizSC2EE&PH*`n>$L zeNVMNQjO!faIFtCqoddeB6T~3mw=vCuS19BUpv4$p3ouLmw`#fA8on!JZBa}&zCJZ zay)_U`o_ixpthq$Rx^XI!RJ76c^r(y-pAnmHv3nBcBFU?4~b(U_KmEQIp^x-uT5Dk#H?hN-6-UumIIO$RL?@sKR&c~MR~vT}`6ZauEJNNsl=UxO&)bMuf5Z-Ta~Ksmpq z7#;NU9$R6wRHE}koX81D7AH_{q8?+~?7+7pu-*liZX>F&r|@qFU?E*l(^?&YS9hZ? zErN|al4b4{!mB&3Y_`x2R=Twds|5390_z_{2chgsu%AQj%Dtnasvx@XcZn ze4WbG1~Z>#UxSjZNP{<%GpI4P6+``Tl+Vxao58I0#|`@*!E*=D0HX%9^ai5>=_5xr zu8ys@v+XJ(Zu*Bp4;zfW82{Qn3Q7j@v^@F}u!(*rq|1@LIW~Rc(t%C?fya{M7DL{n zE_p>ekADYZlkbZ^mUDnrf$!RqQIygD<~79)TXp?wfO=BUik?Q^xW`Ni zdWRxm0%$|A35 zJou&_oL2MZOoP48gT>3e+*5I^O7-wQFA(w5~(&Q2F%YZ1y| zyQQ0XpT$;WX*?%yvOag;XE5v$i=Rd0!MCM>#v$^g*Fs`}ftK|j)2nN0>E4VEF@iEP z1eOaa_AZ4sqMOwGJ^B!dIhLWq=0u(4j_^m9S;$hdJk`b5;{1mk7eeTis%}?Vrw<5N zv>FqatiqkA4c?DY8jy89ibXVjX5v*DO7N6vxwZ!@QA}YFY!TK=3`T2Fl#mC#g_8BD zpy%>XmPp4c2DwUt7Z~_{29I?{M2-18urZj57)~Q-zj{pPtPNfXO1fPnA1n#o5)+rM zj+`fk8XMXZXn|H9qygpW0~A^6p^_*D8-!bgHHw+B@j#;Ak0EyjD&~KQq#o@WUKf0+~C4E5XM#2JKgm>2YESAYKdYQ?;QbFLLFB20&e8 ziHS=_$Ij#DpHaRA_%tUYH2CjJmQH{a1h3C zV{qUe1J6$IOlIJ_0iIQeopXe|%A>pcjRv{WEgM`k(#62}Z4?{D$iQ1j7GkG_s5MA; z=3w9~0Xi(#wmDIJ6kve0>+37}OA7oQEJ-CC3fkUbeTC=2Ps7*ZqdkBwyqCf8U}l4E z%f+>sLC&-AY(?yPdJwKMRo*S(Q&|^TY_RyEm?$=SU+2bZ%u=;0mrudxiQVHAUJd*Z zya-ZJ$fJj)4NPGjqtM0aOP0IZaR@yC+ych5QJ{4k*8|;o=O*ww0X=eb4bjLa16nfs zAB=tzoB`GZssjrt25r0l$es-z=L%YQ4j_K-Ta?!GRLiKoT%;9$vZWn8#7X&NA;n0s zQp~h8Sxd+jOG_KG$WP8g?7jh~qi8r#@9|P#xkz=B{wKH)xJcs^zVwBmz+efsj8RyG zHR_wR@j#Q`uMxacIDRsfuEsDAP#{%SwtWbm2QR}HlB_7&IjZ`k2EGH(-FWH@);vfI zT=lRD@&`F|Jb;`l9Xu^ht1Ny!2*Z*sna0^5TP|wFBVUlCWU+iJo)*W|HuT&tXzjqi z*=oS`CS+O%U19GGVdK|mR$MN^cD>W0#6BNT+t&=F&g-W640r-OOqtK&-H&}Gn0C}} z6Ge~H)k;>k*3z}@DObcDDs>s`QgYZ9%o@qYI}CDWH}G6+;Bm32SJW<1sJz9N+u`#~ z6p~#dCEnet(2I2cVE;VYQdbOYW2X|o>eW)WusLZ$Cr4if*Ud*I%wrT5VT~qqO}DVo z@Q~j$(s{uV;7Q8g2Mz?Wl7yC}UjSS*n~Q<-F)kvD42v!D$@E%)!7s&HG1p=)Exc1* z%xfsupIA~Qipm&;x<4-Z(#6o^yhU!4V%>9yhNwj@`r6RlSgoGWFTer`JUE)WkCKmz zhPW6w|E7V*#bS}+w596P(Jo~H=T!%)i)U3g)jFCvKu)(62}u-|YkSfOh{PB}G(rr} zvzCjxuGc}Mn-c5|P@`%~DGMW*Fi`6@tx<=P&j!4FHK27{#{3N1o+cv0QVFIwxs7(B$`Lo$gVjxagYr zX+K0`SrqOaz;<9Rdr5Cjbj?pO1h;K#C-xF7A~;k1#XS81;_DRUk%^KxF0PC+}*f4e3I&j(Ic_qq44xRW|5C z&rFI zwq(ghxlw-82=pC}AAnmB_!2kU#n%y2>Rd=6Q^n9Oy+O4V-DUTS|Hq;fI1b zWBTBdg}#lAPRak>UGi$@#S5%4(}kQHVb`2Mu~y7elKfZOQ5%{(HwRIKzN>(X7WK6W zftIhJjgSGBPm`S)V8ARcuQ;!}c~U}${#D$1;BP=nep8a}KF5N^k~-B!IJyS`EjqH2 z-kVscl}McfKA-rP?m?~Or9O`C#enKSb&-pob}=;r$o* zJGdQxQgnko%&FXc)~3MqUWLZBftxAwTD9ElY}WuwRDWAH>kxw*tjwwSsDHJ)Re{Df zwgM?18{R_xKfw9;K9X&8!k@M|Eo4UeNG`)1L~XhInn#T66#UR4P114 z?{c&wea@v zXR5oD^fbcN2Ir3%3*>$5Fj_Y(Z$jqn4!Jr#w}LpSf$<4%3n+lxrwp<+-4;y99|H8R z(JUVcHJHt{o+E*f=HF1*k4nrzuHoQf*TM zufat5nGHr-rpEi&fLy&7<3alz8IqE&w|YhgVf_PsHo^@*DdxpK8F(6t#LHft?D=>sg0_ws?ARA>l$U22QJd= zk2fK7ltb{_Jwjo8z`-m>HRj+&lsVTSSEskJ2!(GFuo<`zdZCv+a3X04*GNMVBn&o8vStnM&K})P|jDzowVRe^y;`x z+rPuwo^5{ex#ov^9Ua!|7e6p=32+>{FM*rE_rY`^HhqY|v%$JxC?m!Pg8SI-SA@GR;vgl%#7w*TOKxjqTM2Wh?u}7@4~n)pi%{jnqRY;EH39leK zXpUY&kgd8`_j&}-qwc-nLS#P4w(F@{AKT%`4#4#t(Nz9^L_9-gQ_jJlog7AuUqFYm z3j!B~PQO^DNL{*p&OZ|Kt3yJ&`ElOA(gEq+rk zTlnoxQ#6v0?Qf>CBAFG&cOSwSQLL7?ZB6nfZC+xu>rPBbV{Rs%WpKg zW)jt=dXM9A;4y=#8U^+uWLgGH466IEe9V#afEwgPUPcv^89ycfYlAbv`yhnWI6E2- z^zRQ`H=bs`ea69mf=$6B1+6A8v+!dgpf;+usy3T~LM9r98Vu3K)($nfVWx}p^3Vz#Qsb&zo3Ar~KWH++tQAksA!@Lz1v;e*g0lJ{>0eR7z zDB3kuNRYiQcsws>(4%&77??YiPkGD57p);uTbFOXbYqCUC^^rdX#GC2DWpd1B7INN zn-IE@OK|nP?i|Mf@2-@$Q8^XBm#M)n;93y!+)lA5bOlnPKKCO}k4v2--HrDI{i&y^ zM|E8Tx)v<7QAkrpu)YiGqULYCQtB;7NsUkkjZ9PS=PhK9=J=n%8lbYSBgF9{mBA0a zz_vNK2xx$=D5PcaQ3!{2EQ(Xuq33-)_iqRK#n=^o3*LV^k3wC!M3Xj-!$MR|F#G6 zaaxdy9!IbiNj1;-t+js4sp*LKSFKrWXYh9tRKZ$%aZ2-hl3|v!OD2PHbtl|XrC!Uj!g`H2Yg1&7l~09=yY{m zVX2U8lZ_KM>gAZ$jmWn%fP76!&m;U-AZ2f+(ZxdkCI;6V0<9(K&56Qu83cR;&xFk- z^xvcStk03{1N7gkQj*3rG`6t?P?yt5@T~%@Nh{ApA)8ilIsk)w1@c+GD+Vd)euV!R z47J$SpLprx>QB4-kG9SR3>i8>+Xd+xGG2X5gZ&oqi!fOG`BX)sDX4WKb8`X+Vw) z_^>$8(%_dtO49liy+ylXO81c8uM=estOkHb#V9P{$|K>!nOuhbY+{Xt!(&^f5 zN4oZH3)oG~N z9c;n$6eW)Y!)yAxX^Y(&EOyZYW}lFBf=e3b|7;f6VMc6KFKI8JxqMe|36@||w!R!} z3#Q21ZFCN}L{T4O#k?AO)q(5I2}>rro6`FM?@!Rx+nnbn~Icplz{dGGjzb!9+l`H(21fa5bUTfs&$h3=I|AIt_C}Sm-@zZVjC=O+@o%0%G^nd+`uhnBl5tihz&v(vz8pw~N94il4{#s&?@ zy2$^PbtL}KC6)6Ec1MDM-bKN$!HXazjYhbh?M8r(_-buA*jFYBW3W`0U<6Rzs*Y3A zO9(#(EM_z>DjRy`t0gi1&|5KrH2`RNwl^)&^){eM0qmI*91I=;DM?F+F9Ld1yii@p zCSmD0gso+w=!u4XM|E7SYt?z`<>QZ_;}Fn(iL~8(Cnvhp<93m*b=$OPqecp9kj6zP zHa;rYG{CQG)J3}nhYrcpfH5u&oZl>1NnJtYObLzyAAywg48o5F z^Vg~G`d$1f6Gft0R9mUG8MU31q_+P{5ap2USwPWTy#n1u%SWSAs1=UNv3!E zDSx_4Nx*q!=X1dNAal?&2Q#q%d@K`1;uzP9SDUJ~Ew!-}z2N=o_4)2&(@>ym(?#3* z%I?FDhQZ(ymnP01(WkKS0A)3O^S(icg(cbZp3$9-u@RgyQFJK!#C{5(n`+E)wL7x7 zgDpn@7wOveTV&PBCnTxcZA?TG{%FX2x?l_;@2y&|!72!BBDicKkDuQ~L0cbWu zvl>}u>OKf{2fY&jU6U^QN2rcI<{d_2PmtNSxc{_(Tqnpa0tk4PTJs3 z$5@C%nJ79I{pO0*Hi6ox+Nv+z*IcUmD2gnDn6gJ-j*mwOv@CMEHB=+=FPGH@dJmps z!Q!F1`fE92QYMN5SFYM@we6}6TjIud zVd0;R=z|#-^f}S@fEJ{j)c0_Cq>MmUL_lrVvZ(49t_jQa*34z zX*yreThPaeHiUL;QRR^`0%;>qZL6mkwH(E@@R^af zkxkd0Yt4|ZVO`7V2#5a-Tr>uT*+Zc+CBLCyLJ;JVP8dP%w2fv!Cl{ky4vHV1us6cTl68+o-;y3-$C z`{A=Cla7M_H^wrD6OoR*<8QRDWGf?}*Avu6t=GfBTm!CNd#k8QhozToxrSTmuph^9 z%T%ex8iHmgD5viOJPs`M08D-Uuch6a{=-Qe${t0*s#9$fsEu0a(opjeJ{PdOX&p2r z8q%YXFGm|3(%WxAj|ODaFDR?8?Tj_(Su#|QL3YVfMj&ehbPcGDy66)a_H{9}IWIky z8>Z#jZKKBH6q!r!W?25k+n4YK{|^Mpo(U`@Jz#QEtr1#(2;bSFpvt3fXXKkPLmf*{4&Tc}ksz+VBGm?g+MJZixPNl_w^$hKgvfKja0@A9S_l{@zq8>V$1ItELtSv2*UR=QN#&lCMjxz zYKTVqU8pkT>kUy|TNWA-swYZ0EWFlYB|JX}RY=((;BBKGy)UvwsIG1wQ=x_z9 z?E%X>OR$Z77+9t=n|ejrA4rISu5BtH&x07byunbh=t2+vE+1t#kq{$&1m7Zg#-Rwh z@VQJB9jpkoIiR*@A^lr`MMKS{>@>i7^ISeuPpw?=<3LRSQ<)>#zpVDV&2}cv1-~|n zp35o!r#ABIPff{>O9|w3^#{xSG6Jb0P;F~k2cfoSAuX(W)WX<;N7s_BDHmNCY1+{nswbizw=ntsH6*WhV#Lg;=nTy#@p=hYhu{ljZQW>``=}0@TK;ZH*@_v1a< zqaGKAgqlfNpH<7h4b_vZ91iGza=Zd8bW|2YP_Vm2NgdA)S@d;FS^O;%Mb4E)%y+13 zXRci>ipn(mK>4FwN~-5|Em|Lg@{JmXX~fq;dJkmbEY?w8|5w3E8aV3AVv{URdX~k< zGEsQOEDO8Z6j0l;(momWEsP>vI~FST%pV?HZt()1`$3keA$^edCSWB!u56H{qKaM? z|H?#>QzfY_0iQ?UGqae6u1SHe84GFPzu1Hq!+_qSxV?EsI%{-^!$E9Md6LbmdwWd-V?fqekc6hi{%N z22x&?XPKU@nWSW~X%?MDEQ?QNqDWXVYBRt(MgVK&OKnSMF$o#-09_*%(luv!)jW$y zam!-xYQQqrs{q^!f~QPnIpuc+R(-HmKJu}7T(3JT4WG(Hk*ErY?dH`s!^T8eJ7LB0u{!WGEpR`54DkM zTVZ1&y%D-qR#T91l1bY*rW3O2>sWd0S4PywGOU#O4v(VatjelVVCx}$oh=g~Nj(@V=^D$_BhO@PjKVlDAF#eCGaBBY?`+oFij1>> z4{3a3|9TnSxQ`EIqKH^=Y7@X`>`XpT@LdIM`*Mfy~ZOxZSVNnX>$SxL-t2~-Y z$}}KDd9A%aw<^zPa~jOSUmEIkys^DZ6cH*xZ2+h(_|h-4sz;$F0(1>nX_u@z3soMk z&$oIG-^p2xM}|J?afXM^8te7!bmd#8*j*-yj15P157Y*H$@)3|wicrFWDmvXw-wfn zCvkpi@VrM5fWv^n$+OaB6a$TfrDEK>@y%ZPq2M!+UibCCaObf1mDn z$g#6b6rQ!9x&^9ZU(z7w9-iu&vLiCmQv`qM!op~r!XtPz3lW03h%UWy@d z^m@8ag}`1Rt$16g==Fe&Wuow?B-JTU-TIPd8lt>2gdFv<)yA!)YbW@lHy+{XJS&ds zOI0JSk%8|Wus{sUk+T+f5BQQ^Cq4*d{(N8zKbpzFwlYyP3rxc?)nTq<(w^hlhZdtr3FnB@px<(i_8woCU#iAcXFu(EN=u zi|`w}%0%I)2-O`>9fr{T6w361SIs=+%nsD1t)y!u_{&%|yVlGjizzW2D5a!=CGkm6m^xL zI;qx;>L`TXrqH@|A~WqGOG!d)*M|kCUU3yL=Rs5-_vL-D3PUiQXT}M^Z^Bx z@H+N#4!X-k!G)nZsMdw*B$RHYV3hF)U8~5N4yeug(CPi{I?wD6(bM|+|N2A`UOnf< ztjx)f=bNw^O@5IByMf&Qp9ne!bfQqqtFc!dgpzt6$AD<>{2*&K;4>xxd*xb)W<%x+ zv`7L7jxN+|6(tW+qlhwfT8jnpwghi^*wBpiNe*rUa+~u;*MLkEig7j8ig^}NTUw{q z4q|MPrzue1#HDLsvKadNtqcOB=eQey4~>F(xtKvg-VES;A0t}u>TzaUko@j|jscY@ z6w_*Kvpg~)qOuhGG`3SkZbQo3gT9SRay%wd>;;V;`C80kugVQk(CJ*9Bt4D2faM zHW>I!BdJ2PJbT+H2Gj{brUs(U0WnCcLXQP=*QtoIi_CtVDDZEdYV1N*)>5<=X)Hoh z-7ZW2P^bm#D06r~2RUkX2wyBg5yyci@q_1%N*K&Jos zQ1cZz{Vq``Hq{s@R*G2+Qkz<~7PpvN_kKljb=~(B5`>TJori3evv2?gK8`k6U8PLy^5e6KyZv(T`0)Zc!}Da zFR5+!&+kgK3RbG)1acY>k}R~Qf6f$kRc$(ft#22TB5mnq&i)Rp3cOYW!r!%tLcUgg zmhXx|Od5l*Ux6fDd*sdu)Yg1SZF-s%?Q_7&B%o7(i+yVX{&zvLC9udI4$cAkK0$1H z9f2B+*L3aluF8j}s}hBLs`@4$V+zwRcBk=2)@pzEwqrDE>=0pD9K#GKc*vk7b_p<#BSO@eOgyUGD6NPN9+AbgD zi{4{MK}ry3g6IRrwKjn4iGkXb?|1}7Ds9-Sw^-9#ZapGEpf$}H(#YQA^HCpI2-H@G`?{OaM2<)TeECY;)=g)kxjBqHp*648CWur62w_xo-Dq($|7Hn z8fr_vB>xX^$uG}Gi~s@u4*1l!rr=jMag&IqG!=!8dBI8GeNZIID^cX>^Lr>e5hUwD z)G8Srn}Co<4Yk#N^Iz#|@#u;zfnO244MIo*k;xuKY5_#YV6Z399kmG5pAv;)s@NtO zq^#AUw&>8KhT2RBsg15!FR$FsIRaFG?%Yp@)E;Hjea!7^Rp?p^Xb?+XjCRcyUGsyGSvQJR+UC6Njqy>C2X#}TP*3saV0gP2rZ|VrWr5m62&oOKEkKlv_*_Qf(^ zKhdoo=`KKZA4+O-Q4+DQ{Uv{f2vBK10$+sII^{-!LVP?IJLUk}08Inl3B~|l+AKpj zdlg0TQoz>+ts{6C=qqBof~X%H>SdtE5A0Y8sIEgvZR|%LDofU)MSu#_qMsK)C~4*0 z-$BxaTE20^o=Lz8U?1>DpvRThKr{N7gZujGaPcEkiL2F`0)30&UT`kZqr|$PP%l4W z&s;!tt2z#)7bqCzKiRqB-O2ME>ssgOG!?1;1#$}5DTCfGD0>F@^`Jq6Ue6#WX;FOD z>&m)QF9`HK(HFpN*!(8j=!8Cv>g!9@(l;7B0iHzPD{Ko%*sQo52KE41K8AcQ<@dwa z_t}=zUm`#Q(K`~)f-LkIh1LDfZJMsX#J|&isLrZ&r#j3+YD2lLZn>*sZHdPP8WQmvaT1KNnFklIRF$GsvvKz>m1dTm{+{XPUS=o*AX`_EDXq#B~T zsX73vix{M~p*B*!79PE(x=bLAU?_;SE&x$up8`JtneR1iyJ;zWQ3qw$L!!E+!sX?pF~pd;@y z#EDS8QWUFd%oMxWq&ikzm*WumR%`#!M&p?n>7MCnP}2+)`YgG0dUAQfpLkVfkd0E>bFGp-?Dt3Jzj#USgV zC`7a*)uHM#`IoWUHmWWJ>Hsu;;~;ZND`dLoYM zJuMGbsX{N`X{mim`X9ni0v8fTEy*v*7=bcT7z5RejmEAwoKL2s7s1HKPZ z-xV9x>-Q+|ICv50B`~43{~8y_H~daU_^&}gEoGk@46ANy`wR1XCXhG1hr+*r-{b$w zc`NU4`pQHRu|~OI^vt_6*cGT-T?A5+7!BS9?=);bvZ0v%xteh)>v-6d?j-qKFX0W+F%$^KyK} zA5baD9szwY=s!Tchr37+W9>yt&MXlq6GfKjWh_hL&?AXHfFi5`^mVF|#z){Ic5epP z3nT_TVwBW@2$YGU13_=5lB6*a$5#Ng#G(B>rVpE$)2U;c{=ElO5`*?h4qtW3m{RZf z2$YFpd@KsJN8-@`78wDS14~xX-^Mpy!mkIbxR>oMB!-XKmQ=6^l!>BX71GWel8659 zMJ;L6whM8r=sgeh6rKRJq)^$C6tvc%q%Md+nJBuTlG>R_^3alUeF0mGzUBZ!n`|fJ zSV|g$@K?brO}1L>q_1+9OQs1#w^ErXy0zv!>?WCLfs~TS%wUQtw0KJU+Sc#unCMfW t1zELi{l1O~Z-ZAz0$RvbQa_Hs{|EF|^o> 0 or \ + v_cruise != v_cruise_last or sm['carState'].steeringPressed + v_cruise_last = v_cruise + + # Get status from our own instance of SteeringStatus + steering_status.update(Events(), steering_wheel_engaged, sm['carState'].cruiseState.enabled, sm['carState'].vEgo) + steering_state = steering_status.hands_on_wheel_state + state_name = "Unknown " + if steering_state == HandsOnWheelState.none: + state_name = "Not Active " + elif steering_state == HandsOnWheelState.ok: + state_name = "Hands On Wheel " + elif steering_state == HandsOnWheelState.minor: + state_name = "Hands Off Wheel - Minor " + elif steering_state == HandsOnWheelState.warning: + state_name = "Hands Off Wheel - Warning " + elif steering_state == HandsOnWheelState.critical: + state_name = "Hands Off Wheel - Critical" + elif steering_state == HandsOnWheelState.terminal: + state_name = "Hands Off Wheel - Terminal" + + # Get events from `dMonitoringState` + events = sm['dMonitoringState'].events + event_name = events[0].name if len(events) else "None" + event_name = "{:<30}".format(event_name[:30]) + + # Print output + sys.stdout.write(f'\rSteering State: {state_name} | event: {event_name}') + sys.stdout.flush() + + except Exception as e: + print(e) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Sniff a communcation socket') + parser.add_argument('--addr', default='127.0.0.1') + args = parser.parse_args() + + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + status_monitor() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 393b513939e439..d7a3e796dff288 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -33,6 +33,7 @@ def manager_init(): ("RecordFront", "0"), ("HasAcceptedTerms", "0"), ("HasCompletedSetup", "0"), + ("HandsOnWheelMonitoring", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "0"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), @@ -49,6 +50,10 @@ def manager_init(): if params.get(k) is None: params.put(k, v) + # parameters set by Enviroment Varables + if os.getenv("HANDSMONITORING") is not None: + params.put("HandsOnWheelMonitoring", str(int(os.getenv("HANDSMONITORING")))) + # is this dashcam? if os.getenv("PASSIVE") is not None: params.put("Passive", str(int(os.getenv("PASSIVE")))) diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 83313b8200842c..6bd832a4ecd04f 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -5,6 +5,7 @@ from selfdrive.controls.lib.events import Events from selfdrive.monitoring.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION from selfdrive.locationd.calibrationd import Calibration +from selfdrive.monitoring.hands_on_wheel_monitor import HandsOnWheelStatus def dmonitoringd_thread(sm=None, pm=None): @@ -15,6 +16,7 @@ def dmonitoringd_thread(sm=None, pm=None): sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1") + hands_on_wheel_status = HandsOnWheelStatus() sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] @@ -23,6 +25,8 @@ def dmonitoringd_thread(sm=None, pm=None): v_cruise_last = 0 driver_engaged = False + steering_wheel_engaged = False + hands_on_wheel_monitoring_enabled = Params().get("HandsOnWheelMonitoring", encoding='utf8') == "1" # 10Hz <- dmonitoringmodeld while True: @@ -34,12 +38,15 @@ def dmonitoringd_thread(sm=None, pm=None): # Get interaction if sm.updated['carState']: v_cruise = sm['carState'].cruiseState.speed - driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ - v_cruise != v_cruise_last or \ - sm['carState'].steeringPressed or \ - sm['carState'].gasPressed + steering_wheel_engaged = len(sm['carState'].buttonEvents) > 0 or \ + v_cruise != v_cruise_last or \ + sm['carState'].steeringPressed + driver_engaged = steering_wheel_engaged or sm['carState'].gasPressed if driver_engaged: driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) + # Update events and state from hands on wheel monitoring status when steering wheel in engaged + if steering_wheel_engaged and hands_on_wheel_monitoring_enabled: + hands_on_wheel_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].vEgo) v_cruise_last = v_cruise if sm.updated['modelV2']: @@ -55,6 +62,9 @@ def dmonitoringd_thread(sm=None, pm=None): # Update events from driver state driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) + # Update events and state from hands on wheel monitoring status + if hands_on_wheel_monitoring_enabled: + hands_on_wheel_status.update(events, steering_wheel_engaged, sm['controlsState'].enabled, sm['carState'].vEgo) # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState') @@ -73,6 +83,7 @@ def dmonitoringd_thread(sm=None, pm=None): "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, "isActiveMode": driver_status.active_monitoring_mode, + "handsOnWheelState": hands_on_wheel_status.hands_on_wheel_state, } pm.send('driverMonitoringState', dat) diff --git a/selfdrive/monitoring/hands_on_wheel_monitor.py b/selfdrive/monitoring/hands_on_wheel_monitor.py new file mode 100644 index 00000000000000..3148aab40ffe07 --- /dev/null +++ b/selfdrive/monitoring/hands_on_wheel_monitor.py @@ -0,0 +1,51 @@ +from cereal import log, car +from selfdrive.config import Conversions as CV + +EventName = car.CarEvent.EventName +HandsOnWheelState = log.DriverMonitoringState.HandsOnWheelState + +_PRE_ALERT_THRESHOLD = 150 # 15s +_PROMPT_ALERT_THRESHOLD = 300 # 30s +_TERMINAL_ALERT_THRESHOLD = 600 # 60s + +_MIN_MONITORING_SPEED = 10 * CV.KPH_TO_MS # No monitoring underd 10kph + + +class HandsOnWheelStatus(): + def __init__(self): + self.hands_on_wheel_state = HandsOnWheelState.none + self.hands_off_wheel_cnt = 0 + + def update(self, events, steering_wheel_engaged, ctrl_active, v_ego): + if v_ego < _MIN_MONITORING_SPEED or not ctrl_active: + self.hands_on_wheel_state = HandsOnWheelState.none + self.hands_off_wheel_cnt = 0 + return + + if steering_wheel_engaged: + # Driver has hands on steering wheel + self.hands_on_wheel_state = HandsOnWheelState.ok + self.hands_off_wheel_cnt = 0 + return + + self.hands_off_wheel_cnt += 1 + alert = None + + if self.hands_off_wheel_cnt >= _TERMINAL_ALERT_THRESHOLD: + # terminal red alert: disengagement required + self.hands_on_wheel_state = HandsOnWheelState.terminal + alert = EventName.keepHandsOnWheel + elif self.hands_off_wheel_cnt >= _PROMPT_ALERT_THRESHOLD: + # prompt orange alert + self.hands_on_wheel_state = HandsOnWheelState.critical + alert = EventName.promptKeepHandsOnWheel + elif self.hands_off_wheel_cnt >= _PRE_ALERT_THRESHOLD: + # pre green alert + self.hands_on_wheel_state = HandsOnWheelState.warning + alert = EventName.preKeepHandsOnWheel + else: + # hands off wheel for acceptable period of time. + self.hands_on_wheel_state = HandsOnWheelState.minor + + if alert is not None: + events.add(alert) diff --git a/selfdrive/monitoring/test_hands_monitoring.py b/selfdrive/monitoring/test_hands_monitoring.py new file mode 100644 index 00000000000000..ef998037ed25d3 --- /dev/null +++ b/selfdrive/monitoring/test_hands_monitoring.py @@ -0,0 +1,139 @@ +# flake8: noqa + +import unittest +import numpy as np +from cereal import car, log +from common.realtime import DT_DMON +from selfdrive.controls.lib.events import Events +from selfdrive.monitoring.hands_on_wheel_monitor import HandsOnWheelStatus, _PRE_ALERT_THRESHOLD, \ + _PROMPT_ALERT_THRESHOLD, _TERMINAL_ALERT_THRESHOLD, \ + _MIN_MONITORING_SPEED + +EventName = car.CarEvent.EventName +HandsOnWheelState = log.DriverMonitoringState.HandsOnWheelState + +_TEST_TIMESPAN = 120 # seconds + +# some common state vectors +test_samples = int(_TEST_TIMESPAN / DT_DMON) +half_test_samples = int(test_samples / 2.) +always_speed_over_threshold = [_MIN_MONITORING_SPEED + 1.] * test_samples +always_speed_under_threshold = [_MIN_MONITORING_SPEED - 1.] * test_samples +always_true = [True] * test_samples +always_false = [False] * test_samples +true_then_false = [True] * half_test_samples + [False] * (test_samples - half_test_samples) + + +def run_HOWState_seq(steering_wheel_interaction, openpilot_status, speed_status): + # inputs are all 10Hz + HOWS = HandsOnWheelStatus() + events_from_HOWM = [] + hands_on_wheel_state_from_HOWM = [] + + for idx in range(len(steering_wheel_interaction)): + e = Events() + # evaluate events at 10Hz for tests + HOWS.update(e, steering_wheel_interaction[idx], openpilot_status[idx], speed_status[idx]) + events_from_HOWM.append(e) + hands_on_wheel_state_from_HOWM.append(HOWS.hands_on_wheel_state) + + assert len(events_from_HOWM) == len(steering_wheel_interaction), 'somethings wrong' + assert len(hands_on_wheel_state_from_HOWM) == len(steering_wheel_interaction), 'somethings wrong' + return events_from_HOWM, hands_on_wheel_state_from_HOWM + + +class TestHandsMonitoring(unittest.TestCase): + # 0. op engaged over monitoring speed, driver has hands on wheel all the time + def test_hands_on_all_the_time(self): + events_output, state_output = run_HOWState_seq(always_true, always_true, always_speed_over_threshold) + self.assertTrue(np.sum([len(event) for event in events_output]) == 0) + self.assertEqual(state_output, [HandsOnWheelState.ok for x in range(len(state_output))]) + + # 1. op engaged under monitoring speed, steering wheel interaction is irrelevant + def test_monitoring_under_threshold_speed(self): + events_output, state_output = run_HOWState_seq(true_then_false, always_true, always_speed_under_threshold) + self.assertTrue(np.sum([len(event) for event in events_output]) == 0) + self.assertEqual(state_output, [HandsOnWheelState.none for x in range(len(state_output))]) + + # 2. op engaged over monitoring speed, driver has no hands on wheel all the time + def test_hands_off_all_the_time(self): + events_output, state_output = run_HOWState_seq(always_false, always_true, always_speed_over_threshold) + # Assert correctness before _PRE_ALERT_THRESHOLD + self.assertTrue(np.sum([len(event) for event in events_output[:_PRE_ALERT_THRESHOLD - 1]]) == 0) + self.assertEqual(state_output[:_PRE_ALERT_THRESHOLD - 1], + [HandsOnWheelState.minor for x in range(_PRE_ALERT_THRESHOLD - 1)]) + # Assert correctness before _PROMPT_ALERT_THRESHOLD + self.assertEqual([event.names[0] for event in events_output[_PRE_ALERT_THRESHOLD:_PROMPT_ALERT_THRESHOLD - 1]], + [EventName.preKeepHandsOnWheel for x in range(_PROMPT_ALERT_THRESHOLD - 1 - _PRE_ALERT_THRESHOLD)]) + self.assertEqual(state_output[_PRE_ALERT_THRESHOLD:_PROMPT_ALERT_THRESHOLD - 1], + [HandsOnWheelState.warning for x in range(_PROMPT_ALERT_THRESHOLD - 1 - _PRE_ALERT_THRESHOLD)]) + # Assert correctness before _TERMINAL_ALERT_THRESHOLD + self.assertEqual( + [event.names[0] for event in events_output[_PROMPT_ALERT_THRESHOLD:_TERMINAL_ALERT_THRESHOLD - 1]], + [EventName.promptKeepHandsOnWheel for x in range(_TERMINAL_ALERT_THRESHOLD - 1 - _PROMPT_ALERT_THRESHOLD)]) + self.assertEqual( + state_output[_PROMPT_ALERT_THRESHOLD:_TERMINAL_ALERT_THRESHOLD - 1], + [HandsOnWheelState.critical for x in range(_TERMINAL_ALERT_THRESHOLD - 1 - _PROMPT_ALERT_THRESHOLD)]) + # Assert correctness after _TERMINAL_ALERT_THRESHOLD + self.assertEqual([event.names[0] for event in events_output[_TERMINAL_ALERT_THRESHOLD:]], + [EventName.keepHandsOnWheel for x in range(test_samples - _TERMINAL_ALERT_THRESHOLD)]) + self.assertEqual(state_output[_TERMINAL_ALERT_THRESHOLD:], + [HandsOnWheelState.terminal for x in range(test_samples - _TERMINAL_ALERT_THRESHOLD)]) + + # 3. op engaged over monitoring speed, alert status resets to none when going under monitoring speed + def test_status_none_when_speeds_goes_down(self): + speed_vector = always_speed_over_threshold[:-1] + [_MIN_MONITORING_SPEED - 1.] + events_output, state_output = run_HOWState_seq(always_false, always_true, speed_vector) + # Assert correctness after _TERMINAL_ALERT_THRESHOLD + self.assertEqual([event.names[0] for event in events_output[_TERMINAL_ALERT_THRESHOLD:test_samples - 1]], + [EventName.keepHandsOnWheel for x in range(test_samples - 1 - _TERMINAL_ALERT_THRESHOLD)]) + self.assertEqual(state_output[_TERMINAL_ALERT_THRESHOLD:test_samples - 1], + [HandsOnWheelState.terminal for x in range(test_samples - 1 - _TERMINAL_ALERT_THRESHOLD)]) + # Assert correctes on last sample where speed went under monitoring threshold + self.assertEqual(len(events_output[-1]), 0) + self.assertEqual(state_output[-1], HandsOnWheelState.none) + + # 4. op engaged over monitoring speed, alert status resets to ok when user interacts with steering wheel, + # process repeats once hands are off wheel. + def test_status_ok_after_interaction_with_wheel(self): + interaction_vector = always_false[:_TERMINAL_ALERT_THRESHOLD] + [True + ] + always_false[_TERMINAL_ALERT_THRESHOLD + 1:] + events_output, state_output = run_HOWState_seq(interaction_vector, always_true, always_speed_over_threshold) + # Assert correctness after _TERMINAL_ALERT_THRESHOLD + self.assertEqual(events_output[_TERMINAL_ALERT_THRESHOLD - 1].names[0], EventName.keepHandsOnWheel) + self.assertEqual(state_output[_TERMINAL_ALERT_THRESHOLD - 1], HandsOnWheelState.terminal) + # Assert correctnes for one sample when user interacts with steering wheel + self.assertEqual(len(events_output[_TERMINAL_ALERT_THRESHOLD]), 0) + self.assertEqual(state_output[_TERMINAL_ALERT_THRESHOLD], HandsOnWheelState.ok) + # Assert process correctness on second run + offset = _TERMINAL_ALERT_THRESHOLD + 1 + self.assertTrue(np.sum([len(event) for event in events_output[offset:offset + _PRE_ALERT_THRESHOLD - 1]]) == 0) + self.assertEqual(state_output[offset:offset + _PRE_ALERT_THRESHOLD - 1], + [HandsOnWheelState.minor for x in range(_PRE_ALERT_THRESHOLD - 1)]) + self.assertEqual( + [event.names[0] for event in events_output[offset + _PRE_ALERT_THRESHOLD:offset + _PROMPT_ALERT_THRESHOLD - 1]], + [EventName.preKeepHandsOnWheel for x in range(_PROMPT_ALERT_THRESHOLD - 1 - _PRE_ALERT_THRESHOLD)]) + self.assertEqual(state_output[offset + _PRE_ALERT_THRESHOLD:offset + _PROMPT_ALERT_THRESHOLD - 1], + [HandsOnWheelState.warning for x in range(_PROMPT_ALERT_THRESHOLD - 1 - _PRE_ALERT_THRESHOLD)]) + self.assertEqual([ + event.names[0] + for event in events_output[offset + _PROMPT_ALERT_THRESHOLD:offset + _TERMINAL_ALERT_THRESHOLD - 1] + ], [EventName.promptKeepHandsOnWheel for x in range(_TERMINAL_ALERT_THRESHOLD - 1 - _PROMPT_ALERT_THRESHOLD)]) + self.assertEqual( + state_output[offset + _PROMPT_ALERT_THRESHOLD:offset + _TERMINAL_ALERT_THRESHOLD - 1], + [HandsOnWheelState.critical for x in range(_TERMINAL_ALERT_THRESHOLD - 1 - _PROMPT_ALERT_THRESHOLD)]) + self.assertEqual([event.names[0] for event in events_output[offset + _TERMINAL_ALERT_THRESHOLD:]], + [EventName.keepHandsOnWheel for x in range(test_samples - offset - _TERMINAL_ALERT_THRESHOLD)]) + self.assertEqual(state_output[offset + _TERMINAL_ALERT_THRESHOLD:], + [HandsOnWheelState.terminal for x in range(test_samples - offset - _TERMINAL_ALERT_THRESHOLD)]) + + # 5. op not engaged, always hands off wheel + # - monitor should stay quiet when not engaged + def test_pure_dashcam_user(self): + events_output, state_output = run_HOWState_seq(always_false, always_false, always_speed_over_threshold) + self.assertTrue(np.sum([len(event) for event in events_output]) == 0) + self.assertEqual(state_output, [HandsOnWheelState.none for x in range(len(state_output))]) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 563dec36a303b5..ccf0fea983a44f 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -219,6 +219,17 @@ static void ui_draw_vision_event(UIState *s) { const int bg_wheel_x = s->viz_rect.right() - bg_wheel_size - bdr_s * 2; const int bg_wheel_y = s->viz_rect.y + (bg_wheel_size / 2) + (bdr_s * 1.5); ui_draw_circle_image(s, bg_wheel_x, bg_wheel_y, bg_wheel_size, "wheel", bg_colors[s->status], 1.0f, bg_wheel_y - 25); + + // draw hands on wheel pictogram under wheel pictogram. + auto handsOnWheelState = s->scene.dmonitoring_state.getHandsOnWheelState(); + if (handsOnWheelState >= cereal::DriverMonitoringState::HandsOnWheelState::WARNING) { + NVGcolor color = COLOR_RED; + if (handsOnWheelState == cereal::DriverMonitoringState::HandsOnWheelState::WARNING) { + color = COLOR_YELLOW; + } + const int wheel_y = bg_wheel_y + bdr_s + 2 * bg_wheel_size; + ui_draw_circle_image(s, bg_wheel_x, wheel_y, bg_wheel_size, "hands_on_wheel", color, 1.0f, wheel_y - 25); + } } } @@ -517,6 +528,7 @@ void ui_nvg_init(UIState *s) { // init images std::vector> images = { {"wheel", "../assets/img_chffr_wheel.png"}, + {"hands_on_wheel", "../assets/img_hands_on_wheel.png"}, {"trafficSign_turn", "../assets/img_trafficSign_turn.png"}, {"driver_face", "../assets/img_driver_face.png"}, {"button_settings", "../assets/images/button_settings.png"}, diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 8abaeb7ba89a3a..05007df697d99b 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -60,6 +60,12 @@ QWidget * toggles_panel() { "\U0001f96c Disable use of lanelines (Alpha) \U0001f96c", "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.", "../assets/offroad/icon_road.png")); + toggles_list->addWidget(horizontal_line()); + toggles_list->addWidget(new ParamControl("HandsOnWheelMonitoring", + "Enable Hands on Wheel Monitoring", + "Monitor and alert when driver is not keeping the hands on the steering wheel.", + "../assets/offroad/icon_openpilot.png" + )); bool record_lock = Params().read_db_bool("RecordFrontLock"); record_toggle->setEnabled(!record_lock); From e65e733a0803ee5db4b946c35ef22fa48bc7a52e Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 13 Apr 2021 11:46:22 +0200 Subject: [PATCH 07/32] Turn Controller: Bump Cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 965ae80e42c61a..92d44afdaf2e2b 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 965ae80e42c61a42b0276fb11a4bccbd15ae8245 +Subproject commit 92d44afdaf2e2b3506259da34eefd72ab17e0f66 From e4619079c3b9ea1ce138c71eece67782f72aad39 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 15 Sep 2020 15:59:03 +0200 Subject: [PATCH 08/32] Turn Controller: Provide longitudinal control on turns via a solution to Planner --- cereal | 2 +- common/params_pyx.pyx | 2 + selfdrive/controls/controlsd.py | 1 + .../controls/lib/longitudinal_planner.py | 13 +- selfdrive/controls/lib/turn_controller.py | 249 ++++++++++++++++++ selfdrive/manager/manager.py | 2 + selfdrive/ui/paint.cc | 8 +- selfdrive/ui/qt/offroad/settings.cc | 6 + 8 files changed, 280 insertions(+), 3 deletions(-) create mode 100644 selfdrive/controls/lib/turn_controller.py diff --git a/cereal b/cereal index 92d44afdaf2e2b..0da4b251ae7c51 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 92d44afdaf2e2b3506259da34eefd72ab17e0f66 +Subproject commit 0da4b251ae7c51f866725bc661817e2051084247 diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index 1f051118492430..a5dcad68943efb 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -55,6 +55,7 @@ keys = { b"LastUpdateException": [TxType.PERSISTENT], b"LastUpdateTime": [TxType.PERSISTENT], b"LiveParameters": [TxType.PERSISTENT], + b"MaxDecelerationForTurns": [TxType.PERSISTENT], b"OpenpilotEnabledToggle": [TxType.PERSISTENT], b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], b"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], @@ -69,6 +70,7 @@ keys = { b"TermsVersion": [TxType.PERSISTENT], b"Timezone": [TxType.PERSISTENT], b"TrainingVersion": [TxType.PERSISTENT], + b"TurnVisionControl": [TxType.PERSISTENT], b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], b"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START], b"Version": [TxType.PERSISTENT], diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b575d6eb996c88..61d65dfdc29e8d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -523,6 +523,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.ufAccelCmd = float(self.LoC.pid.f) controlsState.vTargetLead = float(v_acc) controlsState.aTarget = float(a_acc) + controlsState.decelForModelDEPRECATED = self.sm['longitudinalPlan'].decelForTurnDEPRECATED controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2f18a753cc028e..f7caa6cce5f0be 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -13,6 +13,7 @@ from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from selfdrive.controls.lib.turn_controller import TurnController LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -62,6 +63,7 @@ def __init__(self, CP): self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) + self.turn_controller = TurnController(CP) self.v_acc_start = 0.0 self.a_acc_start = 0.0 @@ -90,6 +92,8 @@ def choose_solution(self, v_cruise_setpoint, enabled): solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: solutions['mpc2'] = self.mpc2.v_mpc + if self.turn_controller.is_active: + solutions['turn'] = self.turn_controller.v_turn slowest = min(solutions, key=solutions.get) @@ -104,8 +108,13 @@ def choose_solution(self, v_cruise_setpoint, enabled): elif slowest == 'cruise': self.v_acc = self.v_cruise self.a_acc = self.a_cruise + elif slowest == 'turn': + self.v_acc = self.turn_controller.v_turn + self.a_acc = self.turn_controller.a_turn self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) + if self.turn_controller.is_active: + self.v_acc_future = min(self.v_acc_future, self.turn_controller.v_turn_future) def update(self, sm, CP): """Gets called when new radarState is available""" @@ -164,6 +173,7 @@ def update(self, sm, CP): self.mpc1.update(sm['carState'], lead_1) self.mpc2.update(sm['carState'], lead_2) + self.turn_controller.update(enabled, self.v_acc_start, self.a_acc_start, v_cruise_setpoint, sm) self.choose_solution(v_cruise_setpoint, enabled) @@ -200,7 +210,6 @@ def publish(self, sm, pm): longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState'] - longitudinalPlan.vCruise = float(self.v_cruise) longitudinalPlan.aCruise = float(self.a_cruise) longitudinalPlan.vStart = float(self.v_acc_start) @@ -212,6 +221,8 @@ def publish(self, sm, pm): longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource longitudinalPlan.fcw = self.fcw + longitudinalPlan.decelForTurnDEPRECATED = bool(self.turn_controller.is_active) + longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/turn_controller.py b/selfdrive/controls/lib/turn_controller.py new file mode 100644 index 00000000000000..3f1e2785bfcd2f --- /dev/null +++ b/selfdrive/controls/lib/turn_controller.py @@ -0,0 +1,249 @@ +import numpy as np +import math +from enum import Enum +from common.numpy_fast import interp +from common.params import Params +from common.realtime import sec_since_boot +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE + + +_LON_MPC_STEP = 0.2 # Time stemp of longitudinal control (5 Hz) +_MIN_V = 5.6 # Do not operate under 20km/h + +_ENTERING_PRED_CURVATURE_TH = 0.003 # Predicitve curvature threshold to trigger entering turn state. +_ENTERING_PRED_LAT_ACC_TH = 1.0 # Predicted Lat Acc threshold to trigger entering turn state. +_ABORT_ENTERING_CURVATURE_TH = 0.0015 # Curvature threshold to abort entering state if road straightens. + +_TURNING_CURVATURE_TH = 0.0022 # Curvature threshold to trigger turning turn state. +_LEAVING_CURVATURE_TH = 0.002 # Curvature threshold to trigger leaving turn state. +_FINISH_CURVATURE_TH = 0.0015 # Curvature threshold to trigger the end of turn cycle. + +_ENTERING_SMOOTH_DECEL = -0.3 # Smooth decel when entering curve without overshooting lat acc limits. +_LEAVING_ACC = 0.0 # Allowed acceleration when leaving the turn. + +_EVAL_STEP = 5. # evaluate curvature every 5mts +_EVAL_START = 0. # start evaluating 0 mts ahead +_EVAL_LENGHT = 195. # evaluate curvature for 130mts +_EVAL_RANGE = np.arange(_EVAL_START, _EVAL_LENGHT, _EVAL_STEP) + +_MAX_JERK_ACC_INCREASE = 0.5 # Maximum jerk allowed when increasing acceleration. + +# Lookup table for maximum lateral acceleration according +# to R079r4e regulation for M1 category vehicles. +_A_LAT_REG_MAX_V = [2., 2., 2., 2.] # Currently all the same for all speed ranges +_A_LAT_REG_MAX_BP = [2.8, 16.7, 27.8, 36.1] # 10, 60, 100, 130 km/h + +# Lookup table for the minimum deceleration during the ENTERING state +# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead. +_ENTERING_SMOOTH_DECEL_V = [-0.3, -1.] # min decel value allowed on ENTERING state +_ENTERING_SMOOTH_DECEL_BP = [1., 3] # absolute value of lat acc ahead + +# Lookup table for the acceleration for the TURNING state +# depending on the current lateral acceleration of the vehicle. +_TURNING_ACC_V = [0.5, -0.2, -0.4] # acc value +_TURNING_ACC_BP = [1., 2., 3.] # absolute value of current lat acc + + +def eval_curvature(poly, x_vals): + """ + This function returns a vector with the curvature based on path defined by `poly` + evaluated on distance vector `x_vals` + """ + # https://en.wikipedia.org/wiki/Curvature# Local_expressions + def curvature(x): + a = abs(2 * poly[1] + 6 * poly[0] * x) / (1 + (3 * poly[0] * x**2 + 2 * poly[1] * x + poly[2])**2)**(1.5) + return a + + return np.vectorize(curvature)(x_vals) + + +def eval_lat_acc(v_ego, x_curv): + """ + This function returns a vector with the lateral acceleration based + for the provided speed `v_ego` evaluated over curvature vector `x_curv` + """ + + def lat_acc(curv): + a = v_ego**2 * curv + return a + + return np.vectorize(lat_acc)(x_curv) + + +class TurnState(Enum): + DISABLED = 1 + ENTERING = 2 + TURNING = 3 + LEAVING = 4 + + @property + def description(self): + if self == TurnState.DISABLED: + return 'DISABLED' + if self == TurnState.ENTERING: + return 'ENTERING' + if self == TurnState.TURNING: + return 'TURNING' + if self == TurnState.LEAVING: + return 'LEAVING' + + +class TurnController(): + def __init__(self, CP): + self._params = Params() + self._CP = CP + self._op_enabled = False + self._is_enabled = self._params.get("TurnVisionControl", encoding='utf8') == "1" + self._min_braking_acc = float(self._params.get("MaxDecelerationForTurns")) + self._jerk_limits = [self._min_braking_acc, _MAX_JERK_ACC_INCREASE] + self._last_params_update = 0.0 + self._v_cruise_setpoint = 0.0 + self._v_ego = 0.0 + self._state = TurnState.DISABLED + + self._reset() + + @property + def v_turn_future(self): + return float(self._v_turn_future) if self.state != TurnState.DISABLED else self._v_cruise_setpoint + + @property + def state(self): + return self._state + + @property + def is_active(self): + return self._state != TurnState.DISABLED + + @state.setter + def state(self, value): + if value != self._state: + print(f'TurnController state: {value.description}') + if value == TurnState.DISABLED: + self._reset() + self._state = value + + def _reset(self): + self._v_turn_future = 0.0 + self._current_curvature = 0.0 + self._max_pred_curvature = 0.0 + self._max_pred_lat_acc = 0.0 + self._v_target_distance = 200.0 + self._v_target = 0.0 + self._lat_acc_overshoot_ahead = False + + self.a_turn = 0.0 + self.v_turn = 0.0 + + def _update_params(self): + time = sec_since_boot() + if time > self._last_params_update + 5.0: + self._is_enabled = self._params.get("TurnVisionControl", encoding='utf8') == "1" + self._last_params_update = time + + def _update_calculations(self): + # Get path poly aproximation from model data + md = self._model_data + if len(md.position.x) == TRAJECTORY_SIZE: + path_poly = np.polyfit(md.position.x, md.position.y, 3) + else: + path_poly = np.array([0., 0., 0., 0.]) + + pred_curvatures = eval_curvature(path_poly, _EVAL_RANGE) + max_pred_curvature_idx = np.argmax(pred_curvatures) + self._max_pred_curvature = pred_curvatures[max_pred_curvature_idx] + self._max_pred_lat_acc = self._v_ego**2 * self._max_pred_curvature + + a_lat_reg_max = interp(self._v_ego, _A_LAT_REG_MAX_BP, _A_LAT_REG_MAX_V) + max_curvature_for_vego = a_lat_reg_max / max(self._v_ego, 0.1)**2 + lat_acc_overshoot_idxs = np.nonzero(pred_curvatures >= max_curvature_for_vego)[0] + self._lat_acc_overshoot_ahead = len(lat_acc_overshoot_idxs) > 0 + + if self._lat_acc_overshoot_ahead: + self._v_target_distance = max(lat_acc_overshoot_idxs[0] * _EVAL_STEP + _EVAL_START, _EVAL_STEP) + self._v_target = min(math.sqrt(a_lat_reg_max / self._max_pred_curvature), self._v_cruise_setpoint) + print(f'High Lat Acc ahead. Distance: {self._v_target_distance:.2f}, target v: {self._v_target:.2f}') + + def _state_transition(self): + # In any case, if system is disabled or the feature is disabeld or min braking param has been + # set to non negative value, disable. + if not self._op_enabled or not self._is_enabled or self._min_braking_acc >= 0.0: + self.state = TurnState.DISABLED + return + + # DISABLED + if self.state == TurnState.DISABLED: + # Do not enter a turn control cycle if speed is low. + if self._v_ego <= _MIN_V: + pass + # If substantial curvature ahead is detected, and a minimum lateral + # acceleration is predicted, then move to Entering turn state. + elif self._max_pred_curvature >= _ENTERING_PRED_CURVATURE_TH \ + and self._max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: + self.state = TurnState.ENTERING + # ENTERING + elif self.state == TurnState.ENTERING: + # Transition to Turning if current curvature over threshold. + if self._current_curvature >= _TURNING_CURVATURE_TH: + self.state = TurnState.TURNING + # Abort if road straightens. + elif self._max_pred_curvature < _ABORT_ENTERING_CURVATURE_TH: + self.state = TurnState.DISABLED + # TURNING + elif self.state == TurnState.TURNING: + # Transition to Leaving if current curvature under threshold. + if self._current_curvature < _LEAVING_CURVATURE_TH: + self.state = TurnState.LEAVING + # LEAVING + elif self.state == TurnState.LEAVING: + # Transition back to Turning if current curvature over threshold. + if self._current_curvature >= _TURNING_CURVATURE_TH: + self.state = TurnState.TURNING + elif self._current_curvature < _FINISH_CURVATURE_TH: + self.state = TurnState.DISABLED + + def _update_solution(self): + # Calculate target acceleration based on turn state. + # DISABLED + if self.state == TurnState.DISABLED: + a_target = self._a_ego + # ENTERING + elif self.state == TurnState.ENTERING: + entering_smooth_decel = interp(self._max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V) + print(f'Overshooting {self._lat_acc_overshoot_ahead}, _entering_smooth_decel {entering_smooth_decel:.2f}') + if self._lat_acc_overshoot_ahead: + a_target = min((self._v_target**2 - self._v_ego**2) / (2 * self._v_target_distance), entering_smooth_decel) + else: + a_target = entering_smooth_decel + # TURNING + elif self.state == TurnState.TURNING: + current_lat_acc = self._current_curvature * self._v_ego**2 + a_target = interp(current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V) + # LEAVING + elif self.state == TurnState.LEAVING: + a_target = _LEAVING_ACC + + # smooth out acceleration using jerk limits. + j_limits = np.array(self._jerk_limits) + a_limits = self._a_ego + j_limits * _LON_MPC_STEP + a_target = max(min(a_target, a_limits[1]), a_limits[0]) + + # calculate solution values. + self.a_turn = max(a_target, self._min_braking_acc) # acceleration in next Longitudinal control step. + self.v_turn = self._v_ego + self.a_turn * _LON_MPC_STEP # speed in next Longitudinal control step. + self._v_turn_future = self._v_ego + self.a_turn * 4. # speed in 4 seconds. + + def update(self, enabled, v_ego, a_ego, v_cruise_setpoint, sm): + self._op_enabled = enabled + self._v_ego = v_ego + self._a_ego = a_ego + self._v_cruise_setpoint = v_cruise_setpoint + self._current_curvature = abs( + sm['carState'].steeringAngleDeg * CV.DEG_TO_RAD / (self._CP.steerRatio * self._CP.wheelbase)) + self._model_data = sm['modelV2'] + + self._update_params() + self._update_calculations() + self._state_transition() + self._update_solution() diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index d7a3e796dff288..52fb8e9a036e89 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -37,7 +37,9 @@ def manager_init(): ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "0"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), + ("MaxDecelerationForTurns", "-3.0"), ("OpenpilotEnabledToggle", "1"), + ("TurnVisionControl", "1"), ("VisionRadarToggle", "0"), ("IsDriverViewEnabled", "0"), ] diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index ccf0fea983a44f..74d3f52e7e4715 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -213,7 +213,13 @@ static void ui_draw_vision_speed(UIState *s) { } static void ui_draw_vision_event(UIState *s) { - if (s->scene.controls_state.getEngageable()) { + if (s->scene.controls_state.getDecelForModelDEPRECATED() && s->scene.controls_state.getEnabled()) { + // draw winding road sign + const int img_turn_size = 96; + const int img_turn_x = s->viz_rect.right() - img_turn_size - bdr_s; + const int img_turn_y = s->viz_rect.y + (bdr_s * 1.5); + ui_draw_image(s, {img_turn_x, img_turn_y, img_turn_size, img_turn_size}, "trafficSign_turn", 1.0f); + } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; const int bg_wheel_x = s->viz_rect.right() - bg_wheel_size - bdr_s * 2; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 05007df697d99b..e14d367709f742 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -66,6 +66,12 @@ QWidget * toggles_panel() { "Monitor and alert when driver is not keeping the hands on the steering wheel.", "../assets/offroad/icon_openpilot.png" )); + toggles_list->addWidget(horizontal_line()); + toggles_list->addWidget(new ParamControl("TurnVisionControl", + "Enable vision based turn control", + "Use vision path predictions to estimate the appropiate speed to drive through turns ahead.", + "../assets/offroad/icon_road.png" + )); bool record_lock = Params().read_db_bool("RecordFrontLock"); record_toggle->setEnabled(!record_lock); From 87a882657a58c60d9840b2129c4e483210abac63 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 29 Sep 2020 11:12:00 +0200 Subject: [PATCH 09/32] Speed Limit Controller: Implemnetation --- common/params_pyx.pyx | 2 + selfdrive/car/toyota/carstate.py | 102 ++++++++ selfdrive/controls/controlsd.py | 3 + selfdrive/controls/lib/events.py | 24 ++ .../controls/lib/longitudinal_planner.py | 20 ++ .../controls/lib/speed_limit_controller.py | 221 ++++++++++++++++++ selfdrive/manager/manager.py | 4 + selfdrive/ui/paint.cc | 50 ++++ selfdrive/ui/qt/home.cc | 14 +- selfdrive/ui/qt/offroad/settings.cc | 6 + selfdrive/ui/ui.cc | 2 + selfdrive/ui/ui.hpp | 14 ++ 12 files changed, 461 insertions(+), 1 deletion(-) create mode 100644 selfdrive/controls/lib/speed_limit_controller.py diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index a5dcad68943efb..fa304dece0dd59 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -65,6 +65,8 @@ keys = { b"RecordFrontLock": [TxType.PERSISTENT], # for the internal fleet b"ReleaseNotes": [TxType.PERSISTENT], b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], + b"SpeedLimitControl": [TxType.PERSISTENT], + b"SpeedLimitPercOffset": [TxType.PERSISTENT], b"SubscriberInfo": [TxType.PERSISTENT], b"SshEnabled": [TxType.PERSISTENT], b"TermsVersion": [TxType.PERSISTENT], diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 42bce1c48d668a..b4aaf0c8e9791c 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -6,6 +6,13 @@ from selfdrive.config import Conversions as CV from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR +_TRAFFIC_SINGAL_MAP = { + 1: "kph", + 36: "mph", + 65: "No overtake", + 66: "No overtake" +} + class CarState(CarStateBase): def __init__(self, CP): @@ -19,6 +26,7 @@ def __init__(self, CP): self.needs_angle_offset = True self.accurate_steer_angle_seen = False self.angle_offset = 0. + self._init_traffic_signals() def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -104,8 +112,89 @@ def update(self, cp, cp_cam): ret.leftBlindspot = (cp.vl["BSM"]['L_ADJACENT'] == 1) or (cp.vl["BSM"]['L_APPROACHING'] == 1) ret.rightBlindspot = (cp.vl["BSM"]['R_ADJACENT'] == 1) or (cp.vl["BSM"]['R_APPROACHING'] == 1) + self._update_traffic_signals(cp_cam) + ret.cruiseState.speedLimit = self._calculate_speed_limit() + return ret + def _init_traffic_signals(self): + self._tsgn1 = None + self._spdval1 = None + self._splsgn1 = None + self._tsgn2 = None + self._splsgn2 = None + self._tsgn3 = None + self._splsgn3 = None + self._tsgn4 = None + self._splsgn4 = None + + def _update_traffic_signals(self, cp_cam): + # Print out car signals for traffic signal detection + tsgn1 = cp_cam.vl["RSA1"]['TSGN1'] + spdval1 = cp_cam.vl["RSA1"]['SPDVAL1'] + splsgn1 = cp_cam.vl["RSA1"]['SPLSGN1'] + tsgn2 = cp_cam.vl["RSA1"]['TSGN2'] + splsgn2 = cp_cam.vl["RSA1"]['SPLSGN2'] + tsgn3 = cp_cam.vl["RSA2"]['TSGN3'] + splsgn3 = cp_cam.vl["RSA2"]['SPLSGN3'] + tsgn4 = cp_cam.vl["RSA2"]['TSGN4'] + splsgn4 = cp_cam.vl["RSA2"]['SPLSGN4'] + + has_changed = tsgn1 != self._tsgn1 \ + or spdval1 != self._spdval1 \ + or splsgn1 != self._splsgn1 \ + or tsgn2 != self._tsgn2 \ + or splsgn2 != self._splsgn2 \ + or tsgn3 != self._tsgn3 \ + or splsgn3 != self._splsgn3 \ + or tsgn4 != self._tsgn4 \ + or splsgn4 != self._splsgn4 + + self._tsgn1 = tsgn1 + self._spdval1 = spdval1 + self._splsgn1 = splsgn1 + self._tsgn2 = tsgn2 + self._splsgn2 = splsgn2 + self._tsgn3 = tsgn3 + self._splsgn3 = splsgn3 + self._tsgn4 = tsgn4 + self._splsgn4 = splsgn4 + + if not has_changed: + return + + print('---- TRAFFIC SIGNAL UPDATE -----') + if tsgn1 is not None and tsgn1 != 0: + print(f'TSGN1: {self._traffic_signal_description(tsgn1)}') + if spdval1 is not None and spdval1 != 0: + print(f'SPDVAL1: {spdval1}') + if splsgn1 is not None and splsgn1 != 0: + print(f'SPLSGN1: {splsgn1}') + if tsgn2 is not None and tsgn2 != 0: + print(f'TSGN2: {self._traffic_signal_description(tsgn2)}') + if splsgn2 is not None and splsgn2 != 0: + print(f'SPLSGN2: {splsgn2}') + if tsgn3 is not None and tsgn3 != 0: + print(f'TSGN3: {self._traffic_signal_description(tsgn3)}') + if splsgn3 is not None and splsgn3 != 0: + print(f'SPLSGN3: {splsgn3}') + if tsgn4 is not None and tsgn4 != 0: + print(f'TSGN4: {self._traffic_signal_description(tsgn4)}') + if splsgn4 is not None and splsgn4 != 0: + print(f'SPLSGN4: {splsgn4}') + print('------------------------') + + def _traffic_signal_description(self, tsgn): + desc = _TRAFFIC_SINGAL_MAP.get(int(tsgn)) + return f'{tsgn}: {desc}' if desc is not None else f'{tsgn}' + + def _calculate_speed_limit(self): + if self._tsgn1 == 1: + return self._spdval1 * CV.KPH_TO_MS + if self._tsgn1 == 36: + return self._spdval1 * CV.MPH_TO_MS + return 0 + @staticmethod def get_can_parser(CP): @@ -184,6 +273,19 @@ def get_cam_can_parser(CP): ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0) ] + # Include traffic singal signals. + signals += [ + ("TSGN1", "RSA1", 0), + ("SPDVAL1", "RSA1", 0), + ("SPLSGN1", "RSA1", 0), + ("TSGN2", "RSA1", 0), + ("SPLSGN2", "RSA1", 0), + ("TSGN3", "RSA2", 0), + ("SPLSGN3", "RSA2", 0), + ("TSGN4", "RSA2", 0), + ("SPLSGN4", "RSA2", 0), + ] + # use steering message to check if panda is connected to frc checks = [ ("STEERING_LKA", 42) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 61d65dfdc29e8d..8c83a8e04c7dda 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -157,6 +157,7 @@ def update_events(self, CS): self.events.clear() self.events.add_from_msg(CS.events) self.events.add_from_msg(self.sm['driverMonitoringState'].events) + self.events.add_from_msg(self.sm['longitudinalPlan'].eventsDEPRECATED) # Handle startup event if self.startup_event is not None: @@ -518,6 +519,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.v_cruise_kph) + controlsState.speedLimit = float(CS.cruiseState.speedLimit) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) @@ -528,6 +530,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter + controlsState.speedLimitControlState = self.sm['longitudinalPlan'].speedLimitControlState if self.CP.steerControlType == car.CarParams.SteerControlType.angle: controlsState.lateralControlState.angleState = lac_log diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 23f1e2c7ff8dbc..9b5d951c0307e7 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -493,6 +493,30 @@ def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: boo ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Contact Support"), }, + EventName.speedLimitActive: { + ET.WARNING: Alert( + "Cruise set to speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.chimePrompt, 1., 0., 2.), + }, + + EventName.speedLimitDecrease: { + ET.WARNING: Alert( + "Decreasing speed to match new speed limit", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.chimePrompt, 1., 0., 2.), + }, + + EventName.speedLimitIncrease: { + ET.WARNING: Alert( + "Higher speed limit detected", + "Increasing vehicle speed after short delay", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimePrompt, 1., 0., 2.), + }, + # ********** events that affect controls state transitions ********** EventName.pcmEnable: { diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f7caa6cce5f0be..99e44fb84701e9 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -10,10 +10,13 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.longcontrol import LongCtrlState +from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.turn_controller import TurnController +from selfdrive.controls.lib.speed_limit_controller import SpeedLimitController + LON_MPC_STEP = 0.2 # first step is 0.2s AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted @@ -64,6 +67,7 @@ def __init__(self, CP): self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.turn_controller = TurnController(CP) + self.speed_limit_controller = SpeedLimitController(CP) self.v_acc_start = 0.0 self.a_acc_start = 0.0 @@ -85,6 +89,8 @@ def __init__(self, CP): self.params = Params() self.first_loop = True + self.events = Events() + def choose_solution(self, v_cruise_setpoint, enabled): if enabled: solutions = {'cruise': self.v_cruise} @@ -94,6 +100,8 @@ def choose_solution(self, v_cruise_setpoint, enabled): solutions['mpc2'] = self.mpc2.v_mpc if self.turn_controller.is_active: solutions['turn'] = self.turn_controller.v_turn + if self.speed_limit_controller.is_active: + solutions['limit'] = self.speed_limit_controller.v_limit slowest = min(solutions, key=solutions.get) @@ -111,10 +119,15 @@ def choose_solution(self, v_cruise_setpoint, enabled): elif slowest == 'turn': self.v_acc = self.turn_controller.v_turn self.a_acc = self.turn_controller.a_turn + elif slowest == 'limit': + self.v_acc = self.speed_limit_controller.v_limit + self.a_acc = self.speed_limit_controller.a_limit self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) if self.turn_controller.is_active: self.v_acc_future = min(self.v_acc_future, self.turn_controller.v_turn_future) + if self.speed_limit_controller.is_active: + self.v_acc_future = min(self.v_acc_future, self.speed_limit_controller.v_limit_future) def update(self, sm, CP): """Gets called when new radarState is available""" @@ -130,6 +143,7 @@ def update(self, sm, CP): lead_1 = sm['radarState'].leadOne lead_2 = sm['radarState'].leadTwo + self.events = Events() enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 @@ -156,6 +170,9 @@ def update(self, sm, CP): # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) + # update speed limit solution calculation. + self.speed_limit_controller.update(enabled, self.v_acc_start, self.a_acc_start, sm['carState'], + v_cruise_setpoint, accel_limits_turns, jerk_limits, self.events) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) @@ -167,6 +184,7 @@ def update(self, sm, CP): self.a_acc_start = reset_accel self.v_cruise = reset_speed self.a_cruise = reset_accel + self.speed_limit_controller.deactivate() # Deactivate speed limit controller to provide no solution. self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) @@ -222,6 +240,8 @@ def publish(self, sm, pm): longitudinalPlan.fcw = self.fcw longitudinalPlan.decelForTurnDEPRECATED = bool(self.turn_controller.is_active) + longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state + longitudinalPlan.eventsDEPRECATED = self.events.to_msg() longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py new file mode 100644 index 00000000000000..61800ce4ba171a --- /dev/null +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -0,0 +1,221 @@ +import numpy as np +from cereal import log, car +from common.params import Params +from common.realtime import sec_since_boot +from selfdrive.controls.lib.speed_smoother import speed_smoother +from selfdrive.controls.lib.events import Events + +_LON_MPC_STEP = 0.2 # Time stemp of longitudinal control (5 Hz) +_WAIT_TIME_LIMIT_RISE = 2.0 # Waiting time before raising the speed limit. + +_MIN_ADAPTING_BRAKE_ACC = -1.5 # Minimum acceleration allowed when adapting to lower speed limit. +_MIN_ADAPTING_BRAKE_JERK = -1.0 # Minimum jerk allowed when adapting to lower speed limit. +_SPEED_OFFSET_TH = -3.0 # m/s Maximum offset between speed limit and current speed for adapting state. +_LIMIT_ADAPT_TIME = 5.0 # Ideal time (s) to adapt to lower speed limit. i.e. braking. + +_MAX_SPEED_OFFSET_DELTA = 1.0 # m/s Maximum delta for speed limit changes. + +SpeedLimitControlState = log.ControlsState.SpeedLimitControlState +EventName = car.CarEvent.EventName + + +def _description_for_state(speed_limit_control_state): + if speed_limit_control_state == SpeedLimitControlState.inactive: + return 'INACTIVE' + if speed_limit_control_state == SpeedLimitControlState.tempInactive: + return 'TEMP_INACTIVE' + if speed_limit_control_state == SpeedLimitControlState.adapting: + return 'ADAPTING' + if speed_limit_control_state == SpeedLimitControlState.active: + return 'ACTIVE' + + +class SpeedLimitController(): + def __init__(self, CP): + self._params = Params() + self._last_params_update = 0.0 + self._is_metric = self._params.get("IsMetric", encoding='utf8') == "1" + self._is_enabled = self._params.get("SpeedLimitControl", encoding='utf8') == "1" + self._speed_limit_perc_offset = float(self._params.get("SpeedLimitPercOffset")) + self._CP = CP + self._op_enabled = False + self._active_jerk_limits = [0.0, 0.0] + self._active_accel_limits = [0.0, 0.0] + self._adapting_jerk_limits = [_MIN_ADAPTING_BRAKE_JERK, 1.0] + self._v_ego = 0.0 + self._a_ego = 0.0 + self._v_offset = 0.0 + self._v_cruise_setpoint = 0.0 + self._v_cruise_setpoint_prev = 0.0 + self._v_cruise_setpoint_changed = False + self._speed_limit_set = 0.0 + self._speed_limit_set_prev = 0.0 + self._speed_limit_set_change = 0.0 + self._speed_limit = 0.0 + self._speed_limit_prev = 0.0 + self._speed_limit_changed = False + self._last_speed_limit_set_change_ts = 0.0 + self._state = SpeedLimitControlState.inactive + self._state_prev = SpeedLimitControlState.inactive + self._adapting_cycles = 0 + + self.v_limit = 0.0 + self.a_limit = 0.0 + self.v_limit_future = 0.0 + + @property + def state(self): + return self._state + + @state.setter + def state(self, value): + if value != self._state: + print(f'Speed Limit Controller state: {_description_for_state(value)}') + if value == SpeedLimitControlState.adapting: + self._adapting_cycles = 0 # Reset adapting state cycle count when entereing state. + elif value == SpeedLimitControlState.tempInactive: + # Make sure speed limit is set to `set` value, this will have the effect + # of canceling delayed increase limit, if pending. + self._speed_limit = self._speed_limit_set + self._speed_limit_prev = self._speed_limit + self._state = value + + @property + def is_active(self): + return self.state > SpeedLimitControlState.tempInactive + + @property + def speed_limit(self): + return self._speed_limit * (1.0 + self._speed_limit_perc_offset / 100.0) + + def _update_params(self): + time = sec_since_boot() + if time > self._last_params_update + 5.0: + self._speed_limit_perc_offset = float(self._params.get("SpeedLimitPercOffset")) + self._is_enabled = self._params.get("SpeedLimitControl", encoding='utf8') == "1" + print(f'Updated Speed limit params. enabled: {self._is_enabled}, \ + perc_offset: {self._speed_limit_perc_offset:.1f}') + self._last_params_update = time + + def _update_calculations(self): + # Track the time when speed limit set value changes. + time = sec_since_boot() + if self._speed_limit_set != self._speed_limit_set_prev: + self._last_speed_limit_set_change_ts = time + # Update speed limit from the set value. + # - Imediate when changing from 0 or when updating to a lower speed limit. + # - After a predefined period of time when increasing speed limit. + if self._speed_limit != self._speed_limit_set: + if self._speed_limit == 0.0 or self._speed_limit_set < self._speed_limit: + self._speed_limit = self._speed_limit_set + elif time > self._last_speed_limit_set_change_ts + _WAIT_TIME_LIMIT_RISE: + self._speed_limit = self._speed_limit_set + # Update current velocity offset (error) + self._v_offset = self.speed_limit - self._v_ego + # Update change tracking variables + self._speed_limit_changed = self._speed_limit != self._speed_limit_prev + self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev + self._speed_limit_set_change = self._speed_limit_set - self._speed_limit_set_prev + self._speed_limit_prev = self._speed_limit + self._v_cruise_setpoint_prev = self._v_cruise_setpoint + self._speed_limit_set_prev = self._speed_limit_set + + def _state_transition(self): + self._state_prev = self._state + # In any case, if op is disabled, or speed limit control is disabled + # or the reported speed limit is 0, deactivate. + if not self._op_enabled or not self._is_enabled or self._speed_limit == 0: + self.state = SpeedLimitControlState.inactive + return + + # inactive + if self.state == SpeedLimitControlState.inactive: + # If the limit speed offset is negative (i.e. reduce speed) and lower than threshold + # we go to adapting state to quickly reduce speed, otherwise we go directly to active + if self._v_offset < _SPEED_OFFSET_TH: + self.state = SpeedLimitControlState.adapting + else: + self.state = SpeedLimitControlState.active + # tempInactive + elif self.state == SpeedLimitControlState.tempInactive: + # if speed limit changes, transition to inactive, + # proper active state will be set on next iteration. + if self._speed_limit_changed: + self.state = SpeedLimitControlState.inactive + # adapting + elif self.state == SpeedLimitControlState.adapting: + self._adapting_cycles += 1 + # If user changes the cruise speed, deactivate temporarely + if self._v_cruise_setpoint_changed: + self.state = SpeedLimitControlState.tempInactive + # Go to active once the speed offset is over threshold. + elif self._v_offset >= _SPEED_OFFSET_TH: + self.state = SpeedLimitControlState.active + # active + elif self.state == SpeedLimitControlState.active: + # If user changes the cruise speed, deactivate temporarely + if self._v_cruise_setpoint_changed: + self.state = SpeedLimitControlState.tempInactive + # Go to adapting if the speed offset goes below threshold. + elif self._v_offset < _SPEED_OFFSET_TH: + self.state = SpeedLimitControlState.adapting + + def _update_solution(self): + # inactive + if self.state == SpeedLimitControlState.inactive: + # Preserve values + self.v_limit = self._v_ego + self.a_limit = self._a_ego + self.v_limit_future = self._v_ego + # adapting + elif self.state == SpeedLimitControlState.adapting: + # Calculate to adapt speed on target time. + adapting_time = max(_LIMIT_ADAPT_TIME - self._adapting_cycles * _LON_MPC_STEP, 1.0) # min adapt time 1 sec. + a_target = (self.speed_limit - self._v_ego) / adapting_time + # smooth out acceleration using jerk limits. + j_limits = np.array(self._adapting_jerk_limits) + a_limits = self._a_ego + j_limits * _LON_MPC_STEP + a_target = max(min(a_target, a_limits[1]), a_limits[0]) + # calculate the solution values + self.a_limit = max(a_target, _MIN_ADAPTING_BRAKE_ACC) # acceleration in next Longitudinal control step. + self.v_limit = self._v_ego + self.a_limit * _LON_MPC_STEP # speed in next Longitudinal control step. + self.v_limit_future = max(self._v_ego + self.a_limit * 4., self.speed_limit_offseted) # speed in 4 seconds. + # active + elif self.state == SpeedLimitControlState.active: + # Calculate following same cruise logic in planner.py + self.v_limit, self.a_limit = speed_smoother(self._v_ego, self._a_ego, self.speed_limit, + self._active_accel_limits[1], self._active_accel_limits[0], + self._active_jerk_limits[1], self._active_jerk_limits[0], + _LON_MPC_STEP) + self.v_limit = max(self.v_limit, 0.) + self.v_limit_future = self._speed_limit + + def _update_events(self, events): + if not self.is_active: + # no event while inactive or deactivating + return + + if self._state_prev <= SpeedLimitControlState.tempInactive: + events.add(EventName.speedLimitActive) + elif self._speed_limit_set_change > 0: + events.add(EventName.speedLimitIncrease) + elif self._speed_limit_set_change < 0: + events.add(EventName.speedLimitDecrease) + + def update(self, enabled, v_ego, a_ego, CS, v_cruise_setpoint, accel_limits, jerk_limits, events=Events()): + self._op_enabled = enabled + self._v_ego = v_ego + self._a_ego = a_ego + self._speed_limit_set = CS.cruiseState.speedLimit + self._v_cruise_setpoint = v_cruise_setpoint + self._active_accel_limits = accel_limits + self._active_jerk_limits = jerk_limits + + self._update_params() + self._update_calculations() + self._state_transition() + self._update_solution() + self._update_events(events) + + def deactivate(self): + self.state = SpeedLimitControlState.inactive diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 52fb8e9a036e89..efcf2d7e0e170d 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -40,6 +40,8 @@ def manager_init(): ("MaxDecelerationForTurns", "-3.0"), ("OpenpilotEnabledToggle", "1"), ("TurnVisionControl", "1"), + ("SpeedLimitControl", "1"), + ("SpeedLimitPercOffset", "10.0"), ("VisionRadarToggle", "0"), ("IsDriverViewEnabled", "0"), ] @@ -55,6 +57,8 @@ def manager_init(): # parameters set by Enviroment Varables if os.getenv("HANDSMONITORING") is not None: params.put("HandsOnWheelMonitoring", str(int(os.getenv("HANDSMONITORING")))) + if os.getenv("FOLLOWSPEEDLIMIT") is not None: + params.put("SpeedLimitControl", str(int(os.getenv("FOLLOWSPEEDLIMIT")))) # is this dashcam? if os.getenv("PASSIVE") is not None: diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 74d3f52e7e4715..c00886deda2261 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -28,6 +28,30 @@ static void ui_draw_text(const UIState *s, float x, float y, const char *string, nvgText(s->vg, x, y, string, NULL); } +static void ui_draw_circle(UIState *s, float x, float y, float size, NVGcolor color) { + nvgBeginPath(s->vg); + nvgCircle(s->vg, x, y + (bdr_s * 1.5), size); + nvgFillColor(s->vg, color); + nvgFill(s->vg); +} + +static void ui_draw_speed_sign(UIState *s, float x, float y, int size, float speed, float speed_offset, const char *font_name, int ring_alpha, int inner_alpha) { + ui_draw_circle(s, x, y, float(size), COLOR_RED_ALPHA(ring_alpha)); + ui_draw_circle(s, x, y, float(size) * 0.8, COLOR_WHITE_ALPHA(inner_alpha)); + + char speedlimit_str[16]; + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + snprintf(speedlimit_str, sizeof(speedlimit_str), "%d", int(speed)); + ui_draw_text(s, x, y + (bdr_s * 1.5), speedlimit_str, 120, COLOR_BLACK_ALPHA(inner_alpha), font_name); + + if (int(speed_offset) == 0) { + return; + } + char speedlimitoffset_str[16]; + snprintf(speedlimitoffset_str, sizeof(speedlimitoffset_str), "%+d", int(speed_offset)); + ui_draw_text(s, x, y + (bdr_s * 1.5) + 55, speedlimitoffset_str, 50, COLOR_BLACK_ALPHA(inner_alpha), font_name); +} + static void draw_chevron(UIState *s, float x, float y, float sz, NVGcolor fillColor, NVGcolor glowColor) { // glow float g_xo = sz/5; @@ -204,6 +228,31 @@ static void ui_draw_vision_maxspeed(UIState *s) { } } +static void ui_draw_vision_speedlimit(UIState *s) { + const float speedLimit = s->scene.controls_state.getSpeedLimit(); + const float speedLimitOffset = speedLimit * s->scene.speed_limit_perc_offset / 100.0; + + if (speedLimit > 0.0 && s->scene.controls_state.getEnabled()) { + const int viz_maxspeed_w = 184; + const int viz_maxspeed_h = 202; + const float sign_center_x = s->viz_rect.x + bdr_s * 3 + viz_maxspeed_w + speed_sgn_r; + const float sign_center_y = s->viz_rect.y + viz_maxspeed_h / 2; + const float speed = (s->scene.is_metric ? speedLimit * 3.6 : speedLimit * 2.2369363) + 0.5; + const float speed_offset = (s->scene.is_metric ? speedLimitOffset * 3.6 : speedLimitOffset * 2.2369363) + 0.5; + + auto speedLimitControlState = s->scene.controls_state.getSpeedLimitControlState(); + const bool force_active = s->scene.speed_limit_control_enabled && seconds_since_boot() < s->scene.last_speed_limit_sign_tap + 5.0; + const bool inactive = !force_active && (!s->scene.speed_limit_control_enabled || speedLimitControlState == cereal::ControlsState::SpeedLimitControlState::INACTIVE); + const bool temp_inactive = !force_active && (s->scene.speed_limit_control_enabled && speedLimitControlState == cereal::ControlsState::SpeedLimitControlState::TEMP_INACTIVE); + const int ring_alpha = inactive ? 100 : 255; + const int inner_alpha = inactive || temp_inactive ? 100 : 255; + + ui_draw_speed_sign(s, sign_center_x, sign_center_y, speed_sgn_r, speed, speed_offset, "sans-bold", ring_alpha, inner_alpha); + s->scene.ui_speed_sgn_x = sign_center_x - speed_sgn_r; + s->scene.ui_speed_sgn_y = sign_center_y - speed_sgn_r; + } +} + static void ui_draw_vision_speed(UIState *s) { const float speed = std::max(0.0, s->scene.car_state.getVEgo() * (s->scene.is_metric ? 3.6 : 2.2369363)); const std::string speed_str = std::to_string((int)std::nearbyint(speed)); @@ -298,6 +347,7 @@ static void ui_draw_vision_header(UIState *s) { ui_fill_rect(s->vg, {s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, header_h}, gradient); ui_draw_vision_maxspeed(s); + ui_draw_vision_speedlimit(s); ui_draw_vision_speed(s); ui_draw_vision_event(s); } diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index fead4ae1b1e86a..87813f1a287d1d 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -62,8 +62,20 @@ void HomeWindow::mousePressEvent(QMouseEvent* e) { emit openSettings(); } + // Toggle speed limit control enabled + else if (ui_state->scene.controls_state.getSpeedLimit() > 0.0 + && e->x() >= ui_state->scene.ui_speed_sgn_x - speed_sgn_touch_pad + && e->x() < ui_state->scene.ui_speed_sgn_x + 2 * speed_sgn_r + speed_sgn_touch_pad + && e->y() >= ui_state->scene.ui_speed_sgn_y - speed_sgn_touch_pad + && e->y() < ui_state->scene.ui_speed_sgn_y + 2 * speed_sgn_r + speed_sgn_touch_pad) { + // If touching the speed limit sign area when visible + ui_state->scene.last_speed_limit_sign_tap = seconds_since_boot(); + ui_state->scene.speed_limit_control_enabled = !ui_state->scene.speed_limit_control_enabled; + Params().write_db_value("SpeedLimitControl", ui_state->scene.speed_limit_control_enabled ? "1" : "0", 1); + } + // Handle sidebar collapsing - if (ui_state->scene.started && (e->x() >= ui_state->viz_rect.x - bdr_s)) { + else if (ui_state->scene.started && (e->x() >= ui_state->viz_rect.x - bdr_s)) { ui_state->sidebar_collapsed = !ui_state->sidebar_collapsed; } } diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index e14d367709f742..79f1c97f82d0bf 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -72,6 +72,12 @@ QWidget * toggles_panel() { "Use vision path predictions to estimate the appropiate speed to drive through turns ahead.", "../assets/offroad/icon_road.png" )); + toggles_list->addWidget(horizontal_line()); + toggles_list->addWidget(new ParamControl("SpeedLimitControl", + "Enable Speed Limit Control", + "Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits.", + "../assets/offroad/icon_speed_limit.png" + )); bool record_lock = Params().read_db_bool("RecordFrontLock"); record_toggle->setEnabled(!record_lock); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2d268f7955b289..b6dbf86443ad7c 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -336,6 +336,8 @@ static void update_status(UIState *s) { read_param(&s->scene.is_rhd, "IsRHD"); read_param(&s->scene.end_to_end, "EndToEndToggle"); + read_param(&s->scene.speed_limit_control_enabled, "SpeedLimitControl"); + read_param(&s->scene.speed_limit_perc_offset, "SpeedLimitPercOffset"); s->sidebar_collapsed = true; s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; s->vipc_client = s->scene.driver_view ? s->vipc_client_front : s->vipc_client_rear; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index aec9a225631038..861f0b17ea02c2 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -36,6 +36,7 @@ #define COLOR_RED_ALPHA(x) nvgRGBA(201, 34, 49, x) #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) +#define COLOR_RED_ALPHA(x) nvgRGBA(201, 34, 49, x) #define UI_BUF_COUNT 4 @@ -56,6 +57,8 @@ const int header_h = 420; const int footer_h = 280; const Rect settings_btn = {50, 35, 200, 117}; const Rect home_btn = {60, 1080 - 180 - 40, 180, 180}; +const int speed_sgn_r = 96; +const int speed_sgn_touch_pad = 50; const int UI_FREQ = 20; // Hz @@ -98,6 +101,12 @@ typedef struct UIScene { bool is_rhd; bool driver_view; + // Speed limit control + int ui_speed_sgn_x, ui_speed_sgn_y; // speed sign position + bool speed_limit_control_enabled; + float speed_limit_perc_offset; + double last_speed_limit_sign_tap; + std::string alert_text1; std::string alert_text2; std::string alert_type; @@ -164,6 +173,11 @@ typedef struct UIState { // device state bool awake; + // speed limit controll state + bool speed_limit_control_enabled; + float speed_limit_perc_offset; + double last_speed_limit_sign_tap; + bool sidebar_collapsed; Rect video_rect, viz_rect; float car_space_transform[6]; From c67ce1c8f562da0b3c6064db5cc374b12609f196 Mon Sep 17 00:00:00 2001 From: alfhern Date: Fri, 30 Apr 2021 14:59:32 +0200 Subject: [PATCH 10/32] TurnController: use model data compensated wiht lines for turn prediction --- selfdrive/controls/lib/lateral_planner.py | 7 +- selfdrive/controls/lib/turn_controller.py | 7 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/manager/process_config.py | 1 + selfdrive/mapd/lib/NodesData.py | 273 +++++++++++++++ selfdrive/mapd/lib/Route.py | 266 +++++++++++++++ selfdrive/mapd/lib/WayCollection.py | 36 ++ selfdrive/mapd/lib/WayRelation.py | 383 ++++++++++++++++++++++ selfdrive/mapd/lib/__init__.py | 0 selfdrive/mapd/lib/geo.py | 179 ++++++++++ selfdrive/mapd/lib/mock_data.py | 88 +++++ selfdrive/mapd/lib/osm.py | 26 ++ selfdrive/mapd/lib/test_NodesData.py | 94 ++++++ selfdrive/mapd/lib/test_geo.py | 223 +++++++++++++ selfdrive/mapd/mapd.py | 191 +++++++++++ selfdrive/mapd/speed.py | 76 +++++ 16 files changed, 1846 insertions(+), 6 deletions(-) create mode 100644 selfdrive/mapd/lib/NodesData.py create mode 100644 selfdrive/mapd/lib/Route.py create mode 100644 selfdrive/mapd/lib/WayCollection.py create mode 100644 selfdrive/mapd/lib/WayRelation.py create mode 100644 selfdrive/mapd/lib/__init__.py create mode 100644 selfdrive/mapd/lib/geo.py create mode 100644 selfdrive/mapd/lib/mock_data.py create mode 100644 selfdrive/mapd/lib/osm.py create mode 100644 selfdrive/mapd/lib/test_NodesData.py create mode 100644 selfdrive/mapd/lib/test_geo.py create mode 100644 selfdrive/mapd/mapd.py create mode 100644 selfdrive/mapd/speed.py diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 3b7772df0d72d2..d0d559a7190255 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -66,6 +66,7 @@ def __init__(self, CP): self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) self.y_pts = np.zeros(TRAJECTORY_SIZE) + self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE,3)) def setup_mpc(self): self.libmpc = libmpc_py.libmpc @@ -160,8 +161,9 @@ def update(self, sm, CP): if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob + self.d_path_w_lines_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) if self.use_lanelines: - d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) + d_path_xyz = self.d_path_w_lines_xyz else: d_path_xyz = self.path_xyz y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) @@ -240,6 +242,9 @@ def publish(self, sm, pm): plan_send.lateralPlan.laneChangeState = self.lane_change_state plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction + plan_send.lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]] + plan_send.lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]] + pm.send('lateralPlan', plan_send) if LOG_MPC: diff --git a/selfdrive/controls/lib/turn_controller.py b/selfdrive/controls/lib/turn_controller.py index 3f1e2785bfcd2f..d02a81dcf17ad0 100644 --- a/selfdrive/controls/lib/turn_controller.py +++ b/selfdrive/controls/lib/turn_controller.py @@ -144,9 +144,8 @@ def _update_params(self): def _update_calculations(self): # Get path poly aproximation from model data - md = self._model_data - if len(md.position.x) == TRAJECTORY_SIZE: - path_poly = np.polyfit(md.position.x, md.position.y, 3) + if self._lateral_planner_data is not None and len(self._lateral_planner_data.dPathWLinesX) > 0: + path_poly = np.polyfit(self._lateral_planner_data.dPathWLinesX, self._lateral_planner_data.dPathWLinesY, 3) else: path_poly = np.array([0., 0., 0., 0.]) @@ -241,7 +240,7 @@ def update(self, enabled, v_ego, a_ego, v_cruise_setpoint, sm): self._v_cruise_setpoint = v_cruise_setpoint self._current_curvature = abs( sm['carState'].steeringAngleDeg * CV.DEG_TO_RAD / (self._CP.steerRatio * self._CP.wheelbase)) - self._model_data = sm['modelV2'] + self._lateral_planner_data = sm['lateralPlan'] if sm.valid.get('lateralPlan', False) else None self._update_params() self._update_calculations() diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ceed00c7351b08..fc9b26e6ab4203 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -20,7 +20,7 @@ def plannerd_thread(sm=None, pm=None): lateral_planner = LateralPlanner(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlan'], poll=['radarState', 'modelV2']) if pm is None: diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index f0ae7da2b98c1e..c100a796b5ac11 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -34,6 +34,7 @@ PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True), PythonProcess("updated", "selfdrive.updated", enabled=not PC, persistent=True), PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True), + PythonProcess("mapd", "selfdrive.mapd.mapd"), ] managed_processes = {p.name: p for p in procs} diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py new file mode 100644 index 00000000000000..4367fcd1c0ccf8 --- /dev/null +++ b/selfdrive/mapd/lib/NodesData.py @@ -0,0 +1,273 @@ +import numpy as np +from enum import Enum +from .geo import DIRECTION, R + +_TURN_CURVATURE_THRESHOLD = 0.001 # 1/mts. A curvature over this value will generate a speed limit section. +_MAX_LAT_ACC = 1.5 # Maximum lateral acceleration in turns. + + +def vectors(points): + """Provides a array of vectors on cartesian space (x, y). + Each vector represents the path from a point in `points` to the next. + `points` must by a (N, 2) array of [lat, lon] pairs in radians. + """ + latA = points[:-1, 0] + latB = points[1:, 0] + delta = np.diff(points, axis=0) + dlon = delta[:, 1] + + x = np.sin(dlon) * np.cos(latB) + y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) + + return np.column_stack((x, y)) + + +def nodes_raw_data_array_for_wr(wr, drop_last=False): + """Provides an array of raw node data (id, lat, lon, speed_limit) for all nodes in way relation + """ + sl = wr.speed_limit if wr.speed_limit is not None else 0. + data = np.array(list(map(lambda n: (n.id, n.lat, n.lon, sl), wr.way.nodes)), dtype=float) + + # reverse the order if way direction is backwards + if wr.direction == DIRECTION.BACKWARD: + data = np.flip(data, axis=0) + + # drop last if requested + return data[:-1] if drop_last else data + + +def node_calculations(points): + """Provides node calculations based on an array of (lat, lon) points in radians. + points is a (N x 1) array where N >= 3 + """ + if len(points) < 3: + raise(IndexError) + + # Get the vector representation of node points in cartesian plane. + # (N-1, 2) array. Not including (0., 0.) + v = vectors(points) * R + + # Calculate the vector magnitudes (or distance) + # (N-1, 1) array. No distance for v[-1] + d = np.linalg.norm(v, axis=1) + + # Calculate angles between vectors when stack one after the other. + # https://math.stackexchange.com/questions/2610186/discrete-points-curvature-analysis + # (N-2, 1) array. v[0] and v[-1] have no angle + dot = np.sum(-v[:-1] * v[1:], axis=1) + a = np.arccos(dot / (d[:-1] * d[1:])) + + # Calculate the curvature from the circumcircle of a triangle + # https://www.mathopenref.com/trianglecircumcircle.html + # (N-2, 1) array. v[0] and v[-1] have no curvature + c = 2. * np.sin(a) / np.linalg.norm(v[:-1] + v[1:], axis=1) + + # Calculate the bearing (from true north clockwise) for every node. + # (N-1, 1) array. No bearing for v[-1] + b = np.arctan2(v[:, 0], v[:, 1]) + + # Pad the outputs to match the size of arrays to N + + # Add origin to vector space. (i.e first node in list) + v = np.concatenate(([[0., 0.]], v)) + # Provide distance to previous node and distance to next node + dp = np.concatenate(([0.], d)) + dn = np.concatenate((d, [0.])) + # Angles on edge nodes should be pi. i.e. a straight line. + a = np.concatenate(([[np.pi], a, [np.pi]])) + # Curvature on edges should be 0. i.e a straight line. + c = np.concatenate(([[0.], c, [0.]])) + # Bearing of last node should keep bearing from previous. + b = np.concatenate((b, [b[-1]])) + + return v, dp, dn, a, c, b + + +def speed_limits_for_curvatures_data(curv, dist): + """Provides the calculations for the speed limits from the curvatures array and distances, + by providing indexes to curvature sections and correspoinding speed limit values + """ + # Find where curvatures overshoot turn curvature threshold + overshoots = curv >= _TURN_CURVATURE_THRESHOLD + + # Speed section nodes are those that overshoot if a neighboring node also does. + overshoots = np.concatenate(([[0.], overshoots, [0.]])) + is_section = np.convolve(overshoots, np.ones(3), 'valid') >= 2 + + # Find the indixes where the region starts + is_section_ = np.concatenate(([False], is_section)) + idx_up = np.nonzero((is_section_[:-1] != is_section_[1:]) & is_section_[1:])[0] + + # Find the indexes where the sections end + is_section_ = np.concatenate((is_section, [False])) + idx_down = np.nonzero((is_section_[:-1] != is_section_[1:]) & is_section_[:-1])[0] + + # Find the maximum curvature in the sections + max_curvs = np.array([]) + for i in range(len(idx_up)): + if idx_up[i] < idx_down[i]: + max_curvs = np.append(max_curvs, np.amax(curv[idx_up[i]:idx_down[i]])) + else: + max_curvs = np.append(max_curvs, curv[idx_up[i]]) + + # Caclulate speed limit for confort on the section + speed_limits = np.sqrt(_MAX_LAT_ACC / max_curvs) + + # Stack data and return + return np.column_stack((idx_up, idx_down, speed_limits)) + + +class SpeedLimitSection(): + """And object representing a speed limited road section ahead. + provides the start and end distance and the speed limit value + """ + def __init__(self, start, end, value): + self.start = start + self.end = end + self.value = value + + def __repr__(self): + return f'from: {self.start}, to: {self.end}, limit: {self.value}' + + +class NodeDataIdx(Enum): + """Column index for data elements on NodesData underlying data store. + """ + node_id = 0 + lat = 1 + lon = 2 + speed_limit = 3 + x = 4 # x value of cartesian vector representing the section between last node and this node. + y = 5 # y value of cartesian vector representing the section between last node and this node. + dist_prev = 6 # distance to previous node. + dist_next = 7 # distance to next node + angle = 8 # angles between line segments coming into this node and leaving this node. + curvature = 9 # estimated curvature at this node. + bearing = 10 # bearing of the vector departing from this node. + + +class NodesData: + """Container for the list of node data from a ordered list of way relations to be used in a Route + """ + def __init__(self, way_relations): + self._nodes_data = np.array([]) + self._curvature_speed_sections_data = np.array([]) + + way_count = len(way_relations) + if way_count == 0: + return + + # We want all the nodes from the last way section + nodes_data = nodes_raw_data_array_for_wr(way_relations[-1]) + + # For the ways before the last in the route we want all the nodes but the last, as that one is the first on + # the next section. Collect them, append last way node data and concatenate the numpy arrays. + if way_count > 1: + wrs_data = tuple(map(lambda wr: nodes_raw_data_array_for_wr(wr, True), way_relations[:-1])) + wrs_data += (nodes_data,) + nodes_data = np.concatenate(wrs_data) + + # Get a subarray with lat, lon to compute the remaining node values. + lat_lon_array = nodes_data[:, [1, 2]] + points = np.radians(lat_lon_array) + # Ensure we have more than 3 points, if not calculations are not possible. + if len(points) < 3: + return + vect, dist_prev, dist_next, angle, curvature, bearing = node_calculations(points) + + # append calculations to nodes_data + # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, angle, curvature, bearing] + self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, angle, curvature, bearing)) + + # Store calculcations for curvature sections speed limits + # _curvature_speed_sections_data structure: [idx_up, idx_down, speed_limits] + dist = np.cumsum(dist_next, axis=0) + self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curvature, dist) + + @property + def count(self): + return len(self._nodes_data) + + def get(self, node_data_idx): + """Returns the array containing all the elements of a specific NodeDataIdx type. + """ + if len(self._nodes_data) == 0 or node_data_idx.value >= self._nodes_data.shape[1]: + return np.array([]) + + return self._nodes_data[:, node_data_idx.value] + + def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): + """Returns and array of SpeedLimitSection objects for the actual route ahead of current location + """ + if len(self._nodes_data) == 0 or ahead_idx is None: + return [] + + # Find the cumulative distances where speed limit changes. Build Speed limit sections for those. + dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) + dist = np.cumsum(dist, axis=0) + sl = self.get(NodeDataIdx.speed_limit)[ahead_idx - 1:] + sl_next = np.concatenate((sl[1:], [0.])) + + # Create a boolean mask where speed limit changes and filter values + sl_change = sl != sl_next + distances = dist[sl_change] + speed_limits = sl[sl_change] + + # Create speed limits sections combining all continious nodes that have same speed limit value. + start = 0. + limits_ahead = [] + for idx, end in enumerate(distances): + limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx])) + start = end + + return limits_ahead + + def curvatures_ahead(self, ahead_idx, distance_to_node_ahead): + """Provides a numpy array of ordered pairs by distance including the distance ahead and the curvature. + """ + if len(self._nodes_data) == 0 or ahead_idx is None: + return np.array([]) + + # Find the cumulative distances to nodes and its curvature + dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:-1])) + dist = np.cumsum(dist, axis=0) + curv = self.get(NodeDataIdx.curvature)[ahead_idx:] + + return np.column_stack((dist, curv)) + + def distance_to_end(self, ahead_idx, distance_to_node_ahead): + if len(self._nodes_data) == 0 or ahead_idx is None: + return None + + return np.sum(np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))) + + def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahead): + """Returns and array of SpeedLimitSection objects for the actual route ahead of current location for + speed limit sections due to curvatures in the road. + """ + if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None: + return [] + + # Find the cumulative distances from the current location + dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) + dist = np.cumsum(dist, axis=0) + + # Get indexes and limits from data and adjust to ahead_idx + idx_up = self._curvature_speed_sections_data[:, 0] - ahead_idx + idx_down = self._curvature_speed_sections_data[:, 1] - ahead_idx + speed_limits = self._curvature_speed_sections_data[:, 2] + + # Create speed limits sections + limits_ahead = [] + for i in range(len(idx_up)): + up_idx = int(idx_up[i]) + down_idx = int(idx_down[i]) + + if up_idx < 0: + if down_idx >= 0: + limits_ahead.append(SpeedLimitSection(0, dist[down_idx], speed_limits[i])) + continue + + limits_ahead.append(SpeedLimitSection(dist[up_idx], dist[down_idx], speed_limits[i])) + + return limits_ahead diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py new file mode 100644 index 00000000000000..a0deeb8e3abfea --- /dev/null +++ b/selfdrive/mapd/lib/Route.py @@ -0,0 +1,266 @@ +from .NodesData import NodesData, NodeDataIdx +import numpy as np + + +_DISTANCE_LIMIT_FOR_CURRENT_CURVATURE = 20. # mts +_SUBSTANTIAL_CURVATURE_THRESHOLD = 0.003 # 333 mts radius + + +class Route(): + """A set of consecutive way relations forming a default driving route. + """ + def __init__(self, current, way_relations, way_collection_id): + self.way_collection_id = way_collection_id + self._ordered_way_relations = [] + self._nodes_data = None + self._reset() + + # An active current way is needed to be able to build a route + if not current.active: + return + + # We need a ref or a name to build a route. + ref = current.ref + name = current.name + # TODO: consider allowing to build a route when no ref or name is available. + # be aware of the time taken to search for matching ways as we build the route. + if ref is None and name is None: + return + + # Reduce way relations to those matching the ref or name of the current one. + way_relations = list(filter(lambda wr: wr.has_name_or_ref(name, ref), way_relations)) + + # Build the ordered way relations list by recursively finding the next wr. + wr = current + while wr is not None: + self._ordered_way_relations.append(wr) + wr, way_relations = wr.next_wr(way_relations) + + # Build the node data from the ordered list of way relations + self._nodes_data = NodesData(self._ordered_way_relations) + + # Locate where we are in the route node list. + self._locate() + + def __repr__(self): + count = self._nodes_data.count if self._nodes_data is not None else None + return f'Route: {self.way_collection_id}, idx ahead: {self._ahead_idx} of {count}' + + def _reset(self): + self._limits_ahead = None + self._cuvature_limits_ahead = None + self._curvatures_ahead = None + self._ahead_idx = None + self._distance_to_node_ahead = None + + @property + def located(self): + return self._ahead_idx is not None + + def _locate(self): + """Will resolve the index in the nodes_data list for the node ahead of the current location. + It updates as well the distance from the current location to the node ahead. + """ + current = self.current_wr + if current is None: + return + + node_ahead_id = current.node_ahead.id + self._distance_to_node_ahead = current.distance_to_node_ahead + start_idx = self._ahead_idx if self._ahead_idx is not None else 1 + self._ahead_idx = None + + ids = self._nodes_data.get(NodeDataIdx.node_id) + for idx in range(start_idx, len(ids)): + if ids[idx] == node_ahead_id: + self._ahead_idx = idx + break + + @property + def valid(self): + return self.current_wr is not None + + @property + def current_wr(self): + return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None + + def update(self, location, bearing): + """Will update the route structure based on the given `location` and `bearing` assuming progress on the route + on the original direction. If direction has changed or active point on the route can not be found, the route + will become invalid. + """ + if len(self._ordered_way_relations) == 0 or location is None or bearing is None: + return + + # Skip if no update on location or bearing. + if self.current_wr.location == location and self.current_wr.bearing == bearing: + return + + # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route + # with the way relations remaining ahead. + for idx, wr in enumerate(self._ordered_way_relations): + active_direction = wr.direction + wr.update(location, bearing) + + if not wr.active: + continue + + if wr.direction == active_direction: + # We have now the current wr. Repopulate from here till the end and locate + self._ordered_way_relations = self._ordered_way_relations[idx:] + self._reset() + self._locate() + return + + # Driving direction on the route has changed. stop. + break + + # if we got here, there is no new active way relation or driving direction has changed. Reset. + self._reset() + + @property + def speed_limits_ahead(self): + """Returns and array of SpeedLimitSection objects for the actual route ahead of current location + """ + if self._limits_ahead is not None: + return self._limits_ahead + + if self._nodes_data is None or self._ahead_idx is None: + return [] + + self._limits_ahead = self._nodes_data.speed_limits_ahead(self._ahead_idx, self._distance_to_node_ahead) + return self._limits_ahead + + @property + def curvature_speed_limits_ahead(self): + """Returns and array of SpeedLimitSection objects for the actual route ahead of current location due to curvatures + """ + if self._cuvature_limits_ahead is not None: + return self._cuvature_limits_ahead + + if self._nodes_data is None or self._ahead_idx is None: + return [] + + self._cuvature_limits_ahead = self._nodes_data. \ + curvatures_speed_limit_sections_ahead(self._ahead_idx, self._distance_to_node_ahead) + + return self._cuvature_limits_ahead + + @property + def current_speed_limit(self): + if not self.located: + return None + + limits_ahead = self.speed_limits_ahead + if not len(limits_ahead) or limits_ahead[0].start != 0: + return None + + return limits_ahead[0].value + + @property + def current_curvature_speed_limit(self): + if not self.located: + return None + + limits_ahead = self.curvature_speed_limits_ahead + if not len(limits_ahead) or limits_ahead[0].start != 0: + return None + + return limits_ahead[0].value + + @property + def next_speed_limit_section(self): + if not self.located: + return None + + limits_ahead = self.speed_limits_ahead + if not len(limits_ahead): + return None + + # Find the first section that does not start in 0. i.e. the next section + for section in limits_ahead: + if section.start > 0: + return section + + return None + + @property + def next_curvature_speed_limit_section(self): + if not self.located: + return None + + limits_ahead = self.curvature_speed_limits_ahead + if not len(limits_ahead): + return None + + return limits_ahead[0] + + @property + def curvatures_ahead(self): + """Provides a list of ordered pairs by distance including the distance ahead and the curvature. + """ + if not self.located or self._nodes_data is None: + return None + + if self._curvatures_ahead is not None: + return self._curvatures_ahead + + self._curvatures_ahead = self._nodes_data.curvatures_ahead(self._ahead_idx, self._distance_to_node_ahead) + return self._curvatures_ahead + + @property + def immediate_curvature(self): + """Provides the highest curvature value in the immediate region ahead. + """ + if not self.located: + return None + + curvatures_ahead = self.curvatures_ahead + if not len(curvatures_ahead): + return None + + immediate_curvatures = curvatures_ahead[curvatures_ahead[:, 0] <= _DISTANCE_LIMIT_FOR_CURRENT_CURVATURE] + if not len(immediate_curvatures): + return None + + return np.max(immediate_curvatures[:, 1]) + + @property + def max_curvature_ahead(self): + """Provides the maximum curvature on route ahead + """ + if not self.located: + return None + + curvatures_ahead = self.curvatures_ahead + if not len(curvatures_ahead): + return None + + return np.max(curvatures_ahead[:, 1]) + + @property + def next_substantial_curvature(self): + """Provides the next substantial curvature and the distance to it. + """ + if not self.located: + return None + + curvatures_ahead = self.curvatures_ahead + if not len(curvatures_ahead): + return None + + filt = np.logical_and(curvatures_ahead[:, 0] > _DISTANCE_LIMIT_FOR_CURRENT_CURVATURE, + curvatures_ahead[:, 1] > _SUBSTANTIAL_CURVATURE_THRESHOLD) + substantial_curvatures_ahead = curvatures_ahead[filt] + + if not len(substantial_curvatures_ahead): + return None + + return substantial_curvatures_ahead[0, :] + + @property + def distance_to_end(self): + if not self.located: + return None + + return self._nodes_data.distance_to_end(self._ahead_idx, self._distance_to_node_ahead) diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py new file mode 100644 index 00000000000000..150cc186675cd0 --- /dev/null +++ b/selfdrive/mapd/lib/WayCollection.py @@ -0,0 +1,36 @@ +from .WayRelation import WayRelation +from .Route import Route +import uuid + + +class WayCollection(): + """A collection of WayRelations to use for maps data analysis. + """ + def __init__(self, ways): + self.id = uuid.uuid4() + self.way_relations = list(map(lambda way: WayRelation(way), ways)) + + def get_route(self, location, bearing): + """Provides the best route found in the way collection based on provided `location` and `bearing` + """ + if location is None or bearing is None: + return None + + # Update all way relations in collection to the provided location and bearing. + for wr in self.way_relations: + wr.update(location, bearing) + + # From those matching (i.e. active), select the one with minimum bearing delta between + # route segment and provided bearing + active_way_relations = list(filter(lambda wr: wr.active, self.way_relations)) + active_way_relations.sort(key=lambda wr: wr.active_bearing_delta) + if len(active_way_relations) == 0: + return None + + # Pick first relation as current adn reset location for the remaining located way relations + current = active_way_relations[0] + if len(active_way_relations) > 1: + for wr in active_way_relations[1:]: + wr.reset_location_variables() + + return Route(current, self.way_relations, self.id) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py new file mode 100644 index 00000000000000..294dbb2e29d466 --- /dev/null +++ b/selfdrive/mapd/lib/WayRelation.py @@ -0,0 +1,383 @@ +from .geo import DIRECTION, R +from selfdrive.config import Conversions as CV +from datetime import datetime +import numpy as np +import re + + +_ACCEPTABLE_BEARING_DELTA_V = [70., 50., 30., 10.] +_ACCEPTABLE_BEARING_DELTA_BP = [30., 100., 200., 300.] +_WAY_BBOX_PADING = 1.6e-06 # 10 mts of pading to bounding box. (expressed in radians) + +_COUNTRY_LIMITS_KPH = { + 'DE': { + 'urban': 50., + 'rural': 100., + 'motorway': 0., + 'living_street': 7., + 'bicycle_road': 30. + } +} + +_WD = { + 'Mo': 0, + 'Tu': 1, + 'We': 2, + 'Th': 3, + 'Fr': 4, + 'Sa': 5, + 'Su': 6 +} + +_ALL_WD = _WD.values() + + +def is_osm_time_condition_active(condition_string): + """ + Will indicate if a time condition for a restriction as described + @ https://wiki.openstreetmap.org/wiki/Conditional_restrictions + is active for the current date and time of day. + """ + now = datetime.now().astimezone() + today = now.date() + week_days = [] + + # Look for days of week matched and validate if today matches criteria. + dr = re.findall(r'(Mo|Tu|We|Th|Fr|Sa|Su[-,\s]*?)', condition_string) + + if len(dr) == 1: + week_days = [_WD[dr[0]]] + # If two or more matches condider it a range of days between 1st and 2nd element. + elif len(dr) > 1: + week_days = list(range(_WD[dr[0]], _WD[dr[1]] + 1)) + + # If valid week days list is not empy and today day is not in the list, then the time-date range is not active. + if len(week_days) > 0 and now.weekday() not in week_days: + return False + + # Look for time ranges on the day. No time range, means all day + tr = re.findall(r'([0-9]{1,2}:[0-9]{2})\s*?-\s*?([0-9]{1,2}:[0-9]{2})', condition_string) + + # if no time range but there were week days set, consider it active during the whole day + if len(tr) == 0: + return len(dr) > 0 + + # Search among time ranges matched, one where now time belongs too. If found range is active. + for times_tup in tr: + times = list(map(lambda tt: datetime. + combine(today, datetime.strptime(tt, '%H:%M').time().replace(tzinfo=now.tzinfo)), times_tup)) + if now >= times[0] and now <= times[1]: + return True + + return False + + +def speed_limit_for_osm_tag_limit_string(limit_string): + # https://wiki.openstreetmap.org/wiki/Key:maxspeed + if limit_string is None: + # When limit is set to 0. is considered not existing. + return 0. + + limit = 0. + # Look for matches of speed by default in kph, or in mph when explicitly noted. + v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', limit_string) + if v is not None: + conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS + limit = conv * float(v[1]) + + else: + # Look for matches of speed with country implicit values. + v = re.match(r'^\s*([A-Z]{2}):([a-z_]+):?([0-9]{1,3})?(\s+)?(mph)?\s*', limit_string) + + if v is not None: + if v[2] == "zone" and v[3] is not None: + conv = CV.MPH_TO_MS if v[5] is not None and v[5] == "mph" else CV.KPH_TO_MS + limit = conv * float(v[3]) + elif v[1] in _COUNTRY_LIMITS_KPH and v[2] in _COUNTRY_LIMITS_KPH[v[1]]: + limit = _COUNTRY_LIMITS_KPH[v[1]][v[2]] * CV.KPH_TO_MS + + return limit + + +def conditional_speed_limit_for_osm_tag_limit_string(limit_string): + if limit_string is None: + # When limit is set to 0. is considered not existing. + return 0. + + # Look for matches of the ` @ ()` format + v = re.match(r'^(.*)@\s*\((.*)\).*$', limit_string) + if v is None: + return 0. # No valid format match + + value = speed_limit_for_osm_tag_limit_string(v[1]) + if value == 0.: + return 0. # Invalid speed limit value + + # Look for date-time conditions separated by semicolon + v = re.findall(r'(?:;|^)([^;]*)', v[2]) + for datetime_condition in v: + if is_osm_time_condition_active(datetime_condition): + return value + + # If we get here, no current date-time conditon is active. + return 0. + + +def bearing_to_points(point, points): + """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each + one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud, + longitude pair in radians. + """ + delta = points - point + x = np.sin(delta[:, 1]) * np.cos(points[:, 0]) + y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1])) + return np.arctan2(x, y) + + +def distance_to_points(point, points): + """Calculate the distance of the vectors between `point` and each one of the entries in `points`. + Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians. + """ + delta = points - point + a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2 + c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a)) + return c * R + + +class WayRelation(): + """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle. + """ + def __init__(self, way, location=None, bearing=None): + self.way = way + self.reset_location_variables() + self.direction = DIRECTION.NONE + self.distance_to_node_ahead = 0. + self._speed_limit = None + + # Create a numpy array with nodes data to support calculations. + self._nodes_np = np.radians(np.array([[nd.lat, nd.lon] for nd in way.nodes], dtype=float)) + + # Define bounding box to ease the process of locating a node in a way. + # [[min_lat, min_lon], [max_lat, max_lon]] + self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING, + np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING)) + + if location is not None and bearing is not None: + self.update(location, bearing) + + def __repr__(self): + return f'(id: {self.id}, name: {self.name}, ref: {self.ref}, ahead: {self.ahead_idx}, \ + behind: {self.behind_idx}, {self.direction}, active: {self.active})' + + def reset_location_variables(self): + self.location = None + self.bearing = None + self.active = False + self.ahead_idx = None + self.behind_idx = None + self._active_bearing_delta = None + + @property + def id(self): + return self.way.id + + def update(self, location, bearing): + """Will update and validate the associated way with a given `location` and `bearing`. + Specifically it will find the nodes behind and ahead of the current location and bearing. + If no proper fit to the way geometry, the way relation is marked as invalid. + """ + self.reset_location_variables() + + # Ignore if location not in way bounding box + if not self.is_location_in_bbox(location): + return + + # Find where we are located in the way: + location_rad = np.radians(np.array(location)) + bearing_rad = np.radians(bearing) + + # - Get the distance and bearings from location to all nodes. + bearings = bearing_to_points(location_rad, self._nodes_np) + distances = distance_to_points(location_rad, self._nodes_np) + + # - Get absolute bearing delta to current driving bearing. + delta = np.abs(bearing_rad - bearings) + + # - Nodes are ahead if the cosine of the delta is positive + is_ahead = np.cos(delta) >= 0. + + # - Possible locations on the way are those where adjacent nodes change from ahead to behind or viceversa. + possible_idxs = np.nonzero(np.diff(is_ahead))[0] + + # - when no possible locations found, then the location is not in this way. + if len(possible_idxs) == 0: + return + + # - The smallest angle between bearing and the bearing of the way, is the sine of the delta. + # This value indicates how far are we from alignment with the way direction and will aid us in + # choosing a location when we have multiple candidates. + delta_abs = np.abs(np.sin(delta)) + + # - Get the deltas on nodes ahead and behind for the possible locations and pick the minimum as the delta + # to actual way bearing. + delta_to_way_bearings = np.min(np.row_stack((delta_abs[possible_idxs], delta_abs[possible_idxs + 1])), axis=0) + + # - Get the index where the delta to way bearing is minimum. That is the chosen location. + min_delta_idx = possible_idxs[np.argmin(delta_to_way_bearings)] + + # Populate location variables with result + if is_ahead[min_delta_idx]: + self.direction = DIRECTION.BACKWARD + self.ahead_idx = min_delta_idx + self.behind_idx = min_delta_idx + 1 + else: + self.direction = DIRECTION.FORWARD + self.ahead_idx = min_delta_idx + 1 + self.behind_idx = min_delta_idx + + self._active_bearing_delta = np.amin(delta_to_way_bearings) + self.distance_to_node_ahead = distances[self.ahead_idx] + self.active = True + self.location = location + self.bearing = bearing + self._speed_limit = None + + def update_direction_from_starting_node(self, start_node_id): + self._speed_limit = None + if self.way.nodes[0].id == start_node_id: + self.direction = DIRECTION.FORWARD + elif self.way.nodes[-1].id == start_node_id: + self.direction = DIRECTION.BACKWARD + else: + self.direction = DIRECTION.NONE + + def is_location_in_bbox(self, location): + """Indicates if a given location is contained in the bounding box surrounding the way. + self.bbox = [[min_lat, min_lon], [max_lat, max_lon]] + """ + radians = np.radians(np.array(location, dtype=float)) + is_g = np.greater_equal(radians, self.bbox[0, :]) + is_l = np.less_equal(radians, self.bbox[1, :]) + + return np.all(np.concatenate((is_g, is_l))) + + @property + def speed_limit(self): + if self._speed_limit is not None: + return self._speed_limit + + # Get string from corresponding tag, consider conditional limits first. + limit_string = self.way.tags.get("maxspeed:conditional") + if limit_string is None: + if self.direction == DIRECTION.FORWARD: + limit_string = self.way.tags.get("maxspeed:forward:conditional") + elif self.direction == DIRECTION.BACKWARD: + limit_string = self.way.tags.get("maxspeed:backward:conditional") + + limit = conditional_speed_limit_for_osm_tag_limit_string(limit_string) + + # When no conditional limit set, attempt to get from regular speed limit tags. + if limit == 0.: + limit_string = self.way.tags.get("maxspeed") + if limit_string is None: + if self.direction == DIRECTION.FORWARD: + limit_string = self.way.tags.get("maxspeed:forward") + elif self.direction == DIRECTION.BACKWARD: + limit_string = self.way.tags.get("maxspeed:backward") + + limit = speed_limit_for_osm_tag_limit_string(limit_string) + + self._speed_limit = limit + return self._speed_limit + + @property + def ref(self): + return self.way.tags.get("ref", None) + + @property + def name(self): + return self.way.tags.get("name", None) + + @property + def active_bearing_delta(self): + """Returns the delta between the current location bearing and the exact + bearing of the portion of way we are currentluy located at. + """ + return self._active_bearing_delta + + @property + def node_behind(self): + return self.way.nodes[self.behind_idx] if self.behind_idx is not None else None + + @property + def node_ahead(self): + return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None + + @property + def last_node(self): + """Returns the last node on the way considering the traveling direction + """ + if self.direction == DIRECTION.FORWARD: + return self.way.nodes[-1] + if self.direction == DIRECTION.BACKWARD: + return self.way.nodes[0] + return None + + def edge_on_node(self, node_id): + """Indicates if the associated way starts or ends in the node with `node_id` + """ + return self.way.nodes[0].id == node_id or self.way.nodes[-1].id == node_id + + def node_before_edge_coordinates(self, node_id): + """Returns the coordinates of the node before the edge node identifeid with `node_id` + """ + if self.way.nodes[0].id == node_id: + return np.array([self.way.nodes[1].lat, self.way.nodes[1].lon], dtype=float) + + if self.way.nodes[-1].id == node_id: + return np.array([self.way.nodes[-2].lat, self.way.nodes[-2].lon], dtype=float) + + return np.array([0., 0.]) + + def next_wr(self, way_relations): + """Returns a tuple with the next way relation (if any) based on `location` and `bearing` and + the `way_relations` list excluding the found next way relation. (to help with recursion) + """ + if self.direction not in [DIRECTION.FORWARD, DIRECTION.BACKWARD]: + return None, way_relations + + def continuation_factor(next_wr): + """Indicates how much the `next_wr` looks like a straight continuation of the current one. + A min value of `0` indicates the `next_wr` continues with the exact same bearing as current. + A max value of `2` indicates the `next_wr` continues in the complete oposite direction to current. + """ + ref_point = np.array([self.last_node.lat, self.last_node.lon], dtype=float) + adjacent_points = np.row_stack((self.node_before_edge_coordinates(self.last_node.id), + next_wr.node_before_edge_coordinates(self.last_node.id))) + bearings = bearing_to_points(np.radians(ref_point), np.radians(adjacent_points)) + delta = np.diff(bearings)[0] + return np.cos(delta) + 1 + + possible_next_wr = list(filter(lambda wr: wr.id != self.id and wr.edge_on_node(self.last_node.id), way_relations)) + possible_next_wr.sort(key=lambda wr: continuation_factor(wr)) + possibles = len(possible_next_wr) + + if possibles == 0: + return None, way_relations + + if possibles == 1 or (self.ref is None and self.name is None): + next_wr = possible_next_wr[0] + else: + next_wr = next((wr for wr in possible_next_wr if wr.has_name_or_ref(self.name, self.ref)), possible_next_wr[0]) + + next_wr.update_direction_from_starting_node(self.last_node.id) + updated_way_relations = list(filter(lambda wr: wr.id != next_wr.id, way_relations)) + + return next_wr, updated_way_relations + + def has_name_or_ref(self, name, ref): + if ref is not None and self.ref is not None and self.ref == ref: + return True + if name is not None and self.name is not None and self.name == name: + return True + return False diff --git a/selfdrive/mapd/lib/__init__.py b/selfdrive/mapd/lib/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py new file mode 100644 index 00000000000000..87137986fb9df7 --- /dev/null +++ b/selfdrive/mapd/lib/geo.py @@ -0,0 +1,179 @@ +from math import sin, cos, sqrt, atan2, radians, degrees +from enum import Enum + + +R = 6373000.0 # approximate radius of earth in mt +CURVATURE_OFFSET = 300 # mts. The distance offset for curvature calculation +MAX_DIST_FOR_CURVATURE = 500 # mts. Max distance between nodes for curvature calculation + + +def coord_to_rad(point): + """Tranform coordinates in degrees to radians + """ + return tuple(map(lambda p: radians(p), point)) + + +def distance(point_a, point_b): + """Calculate the distance in meters between two points expressed in coordinates in degrees (lat, lon) + """ + point_a_in_rad = coord_to_rad(point_a) + point_b_in_rad = coord_to_rad(point_b) + return _distance_from_rad(point_a_in_rad, point_b_in_rad) + + +def _distance_from_rad(point_a_in_rad, point_b_in_rad): + """Calculate the distance in meters between two points expressed in coordinates in radians (lat, lon) + """ + (latA, lonA) = point_a_in_rad + (latB, lonB) = point_b_in_rad + + dlon = lonB - lonA + dlat = latB - latA + + a = sin(dlat / 2)**2 + cos(latA) * cos(latB) * sin(dlon / 2)**2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + return R * c + + +def bearing(point_a, point_b): + """Calculate the angle in degrees between to true north and a line joining two points expresed + in coordinates in degrees (lat, lon) + """ + point_a_in_rad = coord_to_rad(point_a) + point_b_in_rad = coord_to_rad(point_b) + return _bearing_from_rad(point_a_in_rad, point_b_in_rad) + + +def xy(ref_point, point): + """Calculates the approximated x y cartesian coordinates for a given coordiante `point` (lat, lon in degrees) + in reference to a reference point `ref_point` + """ + point_a_in_rad = coord_to_rad(ref_point) + point_b_in_rad = coord_to_rad(point) + return _xy_from_rad(point_a_in_rad, point_b_in_rad) + + +def _x_y_bearing_from_rad(point_a_in_rad, point_b_in_rad): + """Calculates the approximated x y cartesian coordinates (in mts) and the bearing angle (in degrees) + for a given coordiante `point_b_in_rad` (lat, lon in radians) in reference to a + reference point `point_a_in_rad` (lat, lon in radians) + """ + (latA, lonA) = point_a_in_rad + (latB, lonB) = point_b_in_rad + + dlon = lonB - lonA + + x = sin(dlon) * cos(latB) + y = cos(latA) * sin(latB) - (sin(latA) * cos(latB) * cos(dlon)) + bearing = degrees(atan2(x, y)) + return x * R, y * R, (bearing + 360) % 360 + + +def _bearing_from_rad(point_a_in_rad, point_b_in_rad): + """Calculate the angle in degrees between to true north and a line joining two points expresed + in coordinates in radians (lat, lon) + """ + _, _, bearing = _x_y_bearing_from_rad(point_a_in_rad, point_b_in_rad) + return (bearing + 360) % 360 + + +def _xy_from_rad(point_a_in_rad, point_b_in_rad): + """Calculates the approximated x y cartesian coordinates for a given coordiante `point_b_in_rad` (lat, lon in radians) + in reference to a reference point `point_a_in_rad` (lat, lon in radians) + """ + x, y, _ = _x_y_bearing_from_rad(point_a_in_rad, point_b_in_rad) + return x, y + + +def distance_and_bearing(point_a, point_b): + """ Provides distance and bearing calucations between two points in a single method call. see `distance` and + `bearing` for details. + """ + point_a_in_rad = coord_to_rad(point_a) + point_b_in_rad = coord_to_rad(point_b) + + return _distance_from_rad(point_a_in_rad, point_b_in_rad), _bearing_from_rad(point_a_in_rad, point_b_in_rad) + + +def bearing_delta(bearing_a, bearing_b): + """Returns the angle difference in degrees between two bearing angles (in degrees) + """ + return (bearing_a - bearing_b + 180) % 360 - 180 + + +def absoule_delta_with_direction(delta): + """Takes a `bearing_delta` and provides its absolute value ignoring its direction. The direction is then + provided as an additional element on the result tuple. + If delta is between -90 and 90, direction is AHEAD, between 90 and 270 is BEHIND. + """ + delta_ahead = abs(bearing_delta(delta, 0.)) + delta_behind = abs(delta_ahead - 180) + + if delta_ahead < delta_behind: + return (delta_ahead, DIRECTION.AHEAD) + elif delta_ahead > delta_behind: + return (delta_behind, DIRECTION.BEHIND) + else: + return (delta_ahead, DIRECTION.NONE) + + +def three_point_curvature_alt(ref, prev, next): + # https://math.stackexchange.com/questions/2507540/numerical-way-to-solve-for-the-curvature-of-a-curve + # https://en.wikipedia.org/wiki/Heron%27s_formula + prev_r = (prev[0] - ref[0], prev[1] - ref[1]) + next_r = (next[0] - ref[0], next[1] - ref[1]) + + prev_ang = atan2(prev_r[0], prev_r[1]) + next_ang = atan2(next_r[0], next_r[1]) + a = CURVATURE_OFFSET + b = CURVATURE_OFFSET + + prev_n = (a * cos(prev_ang), a * sin(prev_ang)) + next_n = (b * cos(next_ang), b * sin(next_ang)) + + c = xy_distance(next_n, prev_n) + s = (a + b + c) / 2. + A = sqrt(s * (s - a) * (s - b) * (s - c)) + + return 4 * A / (a * b * c) + + +def three_point_tangent_angle(prev, ref, next): + """Angle (in readians) of the tangent line formed by three points in sequence `prev`, `ref`, `next` + """ + # https://www.math24.net/curvature-radius + prev_vec = (ref[0] - prev[0], ref[1] - prev[1]) + next_vec = (next[0] - ref[0], next[1] - ref[1]) + avg_vec = ((prev_vec[0] + next_vec[0]) / 2., (prev_vec[1] + next_vec[1]) / 2.) + + return atan2(avg_vec[1], avg_vec[0]) + + +def three_point_curvature(prev_xy, ref_xy, next_xy, prev_tan, ref_tan, next_tan): + """Aproximated curvature for a line joining 3 points with calculated tangent angles. Aproximation by + averaging the variation of the tangent angle over distance. + """ + prev_tan_delta = ref_tan - prev_tan if prev_tan is not None else 0 + prev_dist = min(xy_distance(prev_xy, ref_xy), MAX_DIST_FOR_CURVATURE) + next_tan_delta = next_tan - ref_tan if next_tan is not None else 0 + next_dist = min(xy_distance(next_xy, ref_xy), MAX_DIST_FOR_CURVATURE) + + prev_curv = prev_tan_delta / prev_dist + next_curv = next_tan_delta / next_dist + + return (prev_curv + next_curv) / 2. + + +def xy_distance(A, B): + """Distance between two point on a cartesian plane. + """ + return sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) + + +class DIRECTION(Enum): + NONE = 0 + AHEAD = 1 + BEHIND = 2 + FORWARD = 3 + BACKWARD = 4 diff --git a/selfdrive/mapd/lib/mock_data.py b/selfdrive/mapd/lib/mock_data.py new file mode 100644 index 00000000000000..ab5cd7e49c505e --- /dev/null +++ b/selfdrive/mapd/lib/mock_data.py @@ -0,0 +1,88 @@ +import numpy as np + + +class MockRoad(): + # Test data in degrees from this road: + # https://www.google.de/maps/@52.209263,13.8723137,13z + road1_points_grad = np.array([ + [52.1933703, 13.8723799], + [52.1939477, 13.8711273], + [52.1942004, 13.8705818], + [52.1945408, 13.8698496], + [52.1948447, 13.8691873], + [52.1950772, 13.8685726], + [52.1951168, 13.8684641], + [52.1956681, 13.8670323], + [52.1958716, 13.8664936], + [52.1964366, 13.8649875], + [52.1969283, 13.8636040], + [52.1970203, 13.8634430], + [52.1975486, 13.8626307], + [52.1976354, 13.8624971], + [52.1977827, 13.8621795], + [52.1978564, 13.8619220], + [52.1981843, 13.8604497], + [52.1982614, 13.8602140], + [52.1983351, 13.8600595], + [52.1992768, 13.8579824], + [52.1995107, 13.8574321], + [52.1995948, 13.8572604], + [52.1996818, 13.8571155], + [52.1998000, 13.8570029], + [52.2000659, 13.8568236], + [52.2003868, 13.8566005], + [52.2007182, 13.8564460], + [52.2008760, 13.8564117], + [52.2009865, 13.8564117], + [52.2011390, 13.8564202], + [52.2012267, 13.8564496], + [52.2012544, 13.8564577], + [52.2013179, 13.8564803], + [52.2020491, 13.8571756], + [52.2026014, 13.8576991], + [52.2027592, 13.8578879], + [52.2027960, 13.8579309], + [52.2028960, 13.8580939], + [52.2030170, 13.8583343], + [52.2036587, 13.8597076], + [52.2052946, 13.8633039], + [52.2064332, 13.8658435], + [52.2067856, 13.8666332], + [52.2068961, 13.8668477], + [52.2070777, 13.8670890], + [52.2073723, 13.8674409], + [52.2077457, 13.8679387], + [52.2083874, 13.8687455], + [52.2093341, 13.8699214], + [52.2099652, 13.8707540], + [52.2102282, 13.8712089], + [52.2104228, 13.8715694], + [52.2106122, 13.8718955], + [52.2107619, 13.8721756], + [52.2108695, 13.8723771], + [52.2110747, 13.8727610], + [52.2111514, 13.8729047], + [52.2114010, 13.8733718], + [52.2114694, 13.8735006], + [52.2115430, 13.8736636], + [52.2116086, 13.8737571], + [52.2116770, 13.8738172], + [52.2117611, 13.8738515], + [52.2118664, 13.8738566], + [52.2119322, 13.8738439], + [52.2121058, 13.8737924], + [52.2122583, 13.8737495], + [52.2123265, 13.8737260], + [52.2124213, 13.8736894], + [52.2127466, 13.8734888], + [52.2128263, 13.8734491], + [52.2131313, 13.8733117], + [52.2133943, 13.8731830], + [52.2136625, 13.8731057], + [52.2139465, 13.8730456], + [52.2143619, 13.8730113], + [52.2148773, 13.8729942], + [52.2152275, 13.8730325], + [52.2153110, 13.8730398], + [52.2157442, 13.8730848], + [52.2158833, 13.8731036]]) diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py new file mode 100644 index 00000000000000..22af6b571e26c9 --- /dev/null +++ b/selfdrive/mapd/lib/osm.py @@ -0,0 +1,26 @@ +import overpy + + +class OSM(): + def __init__(self): + self.api = overpy.Overpass() + + def fetch_road_ways_around_location(self, location, radius): + lat, lon = location + + # fetch all ways and nodes on this ways around location + around_str = f'{str(radius)},{str(lat)},{str(lon)}' + q = """ + way(around:""" + around_str + """) + [highway] + [highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape|service)$"]; + (._;>;); + out; + """ + try: + ways = self.api.query(q).ways + except Exception as e: + print(f'Exception while querying OSM:\n{e}') + ways = [] + + return ways diff --git a/selfdrive/mapd/lib/test_NodesData.py b/selfdrive/mapd/lib/test_NodesData.py new file mode 100644 index 00000000000000..ec4e07481c6e9f --- /dev/null +++ b/selfdrive/mapd/lib/test_NodesData.py @@ -0,0 +1,94 @@ +import unittest +import numpy as np +from numpy.testing import assert_array_almost_equal +from .mock_data import MockRoad +from .NodesData import vectors + + +class TestNodesData(unittest.TestCase): + def test_vectors(self): + points = np.radians(MockRoad.road1_points_grad) + expected = np.array([ + [-1.34011951e-05, 1.00776468e-05], + [-5.83610920e-06, 4.41046897e-06], + [-7.83348567e-06, 5.94114032e-06], + [-7.08560788e-06, 5.30408795e-06], + [-6.57632550e-06, 4.05791838e-06], + [-1.16077872e-06, 6.91151252e-07], + [-1.53178098e-05, 9.62215139e-06], + [-5.76314175e-06, 3.55176643e-06], + [-1.61124141e-05, 9.86127759e-06], + [-1.48006628e-05, 8.58192512e-06], + [-1.72237209e-06, 1.60570482e-06], + [-8.68985228e-06, 9.22062311e-06], + [-1.42922812e-06, 1.51494711e-06], + [-3.39761486e-06, 2.57087743e-06], + [-2.75467373e-06, 1.28631255e-06], + [-1.57501989e-05, 5.72309451e-06], + [-2.52143954e-06, 1.34565295e-06], + [-1.65278643e-06, 1.28630942e-06], + [-2.22196114e-05, 1.64360838e-05], + [-5.88675934e-06, 4.08234746e-06], + [-1.83673390e-06, 1.46782408e-06], + [-1.55004206e-06, 1.51843800e-06], + [-1.20451533e-06, 2.06298011e-06], + [-1.91801338e-06, 4.64083285e-06], + [-2.38653483e-06, 5.60076524e-06], + [-1.65269781e-06, 5.78402290e-06], + [-3.66908309e-07, 2.75412965e-06], + [0.00000000e+00, 1.92858882e-06], + [9.09242615e-08, 2.66162711e-06], + [3.14490354e-07, 1.53065382e-06], + [8.66452477e-08, 4.83456208e-07], + [2.41750593e-07, 1.10828411e-06], + [7.43745228e-06, 1.27618831e-05], + [5.59968054e-06, 9.63947367e-06], + [2.01951467e-06, 2.75413219e-06], + [4.59952643e-07, 6.42281301e-07], + [1.74353749e-06, 1.74533121e-06], + [2.57144338e-06, 2.11185266e-06], + [1.46893187e-05, 1.11999169e-05], + [3.84659229e-05, 2.85527952e-05], + [2.71627936e-05, 1.98727946e-05], + [8.44632540e-06, 6.15058628e-06], + [2.29420323e-06, 1.92859222e-06], + [2.58083439e-06, 3.16952222e-06], + [3.76373643e-06, 5.14174911e-06], + [5.32416098e-06, 6.51707770e-06], + [8.62890928e-06, 1.11998258e-05], + [1.25762497e-05, 1.65231340e-05], + [8.90452991e-06, 1.10148240e-05], + [4.86505726e-06, 4.59023120e-06], + [3.85545276e-06, 3.39642031e-06], + [3.48753893e-06, 3.30566145e-06], + [2.99557303e-06, 2.61276368e-06], + [2.15496788e-06, 1.87797727e-06], + [4.10564937e-06, 3.58142649e-06], + [1.53680853e-06, 1.33866906e-06], + [4.99540175e-06, 4.35635790e-06], + [1.37744970e-06, 1.19380643e-06], + [1.74319821e-06, 1.28456429e-06], + [9.99931238e-07, 1.14493663e-06], + [6.42735560e-07, 1.19380547e-06], + [3.66818436e-07, 1.46782199e-06], + [5.45413874e-08, 1.83783170e-06], + [-1.35818548e-07, 1.14842666e-06], + [-5.50758101e-07, 3.02989178e-06], + [-4.58785270e-07, 2.66162724e-06], + [-2.51315555e-07, 1.19031459e-06], + [-3.91409773e-07, 1.65457223e-06], + [-2.14525206e-06, 5.67755902e-06], + [-4.24558096e-07, 1.39102753e-06], + [-1.46936730e-06, 5.32325561e-06], + [-1.37632061e-06, 4.59021715e-06], + [-8.26642899e-07, 4.68097349e-06], + [-6.42702724e-07, 4.95673534e-06], + [-3.66796960e-07, 7.25009780e-06], + [-1.82861669e-07, 8.99542699e-06], + [4.09564134e-07, 6.11214315e-06], + [7.80629912e-08, 1.45734993e-06], + [4.81205526e-07, 7.56076647e-06], + [2.01036346e-07, 2.42775302e-06]]) + + v = vectors(points) + assert_array_almost_equal(v, expected) diff --git a/selfdrive/mapd/lib/test_geo.py b/selfdrive/mapd/lib/test_geo.py new file mode 100644 index 00000000000000..53a5c7acb71f06 --- /dev/null +++ b/selfdrive/mapd/lib/test_geo.py @@ -0,0 +1,223 @@ +import unittest +from decimal import Decimal +from math import pi, sqrt +from .geo import coord_to_rad, distance, bearing, xy, distance_and_bearing, bearing_delta, DIRECTION, \ + absoule_delta_with_direction, three_point_tangent_angle, three_point_curvature, xy_distance + + +class TestMapsdGeoLibrary(unittest.TestCase): + # 0. coord to rad point tuple conversion + def test_coord_to_rad(self): + points = [ + (0., 360.), + (Decimal(0.), Decimal(360.)), + (Decimal(180.), Decimal(540.)), + ] + expected = [ + (0., 2 * pi), + (0., 2 * pi), + (pi, 3 * pi), + ] + rad_tuples = list(map(lambda p: coord_to_rad(p), points)) + self.assertEqual(rad_tuples, expected) + + # 1. test distance calculation between two points in coordiantes. + def test_distance(self): + a = (Decimal(0.), Decimal(0.)) + b = (Decimal(0.1), Decimal(0.1)) + c = (Decimal(0.01), Decimal(0.01)) + d = (Decimal(-0.01), Decimal(-0.01)) + + dist1 = distance(a, b) + dist2 = distance(a, c) + dist3 = distance(b, c) + dist4 = distance(a, d) + + self.assertAlmostEqual(dist1, 15730, 0) + self.assertAlmostEqual(dist2, 1573, 0) + self.assertAlmostEqual(dist3, 14157, 0) + self.assertAlmostEqual(dist4, 1573, 0) + + # 2. Test bearing between two points + def test_bearing(self): + ref_point = (0., 0.) + points = [ + (0., 1.), + (0., -1.), + (-1., 0.), + (1., 0.), + ] + expected = [ + 90., + 270., + 180., + 0., + ] + bearings = list(map(lambda p: bearing(ref_point, p), points)) + self.assertEqual(bearings, expected) + + # 3. Test cartesian coordinates from lat lon coordinates + def test_xy(self): + ref_point = (1., 1.) + points = [ + (1., 1.01), + (1., 0.99), + (0.99, 1.), + (1.01, 1.), + ] + expected = [ + (1112., 0), + (-1112, 0), + (0, -1112), + (0, 1112), + ] + xys = list(map(lambda p: xy(ref_point, p), points)) + self._assertAlmostEqualListOfTuples(xys, expected) + + # 4. Test distance and bearing combined method + def test_distance_and_bearing(self): + a = (Decimal(0.), Decimal(0.)) + b = (Decimal(0.01), Decimal(0.01)) + dist, bearing = distance_and_bearing(a, b) + + self.assertAlmostEqual(dist, 1573, 0) + self.assertAlmostEqual(bearing, 45, 0) + + # 5. Test bearing delta + def test_bearing_delta(self): + a = 0 + b = 90 + c = 180 + d = 270 + + deltas = [ + bearing_delta(a, b), + bearing_delta(a, c), + bearing_delta(a, d), + bearing_delta(b, a), + bearing_delta(b, c), + bearing_delta(d, b), + ] + expected = [ + -90, + -180, + 90, + 90, + -90, + -180 + ] + + self.assertEqual(deltas, expected) + + # 6. Test absolute bearing delta with direction info + def test_absoule_delta_with_direction(self): + deltas = [ + 0, + 45, + -45, + 89, + -89, + 90, + -90, + 91, + -91, + 135, + -135, + 180, + 360 + ] + expected = [ + (0, DIRECTION.AHEAD), + (45, DIRECTION.AHEAD), + (45, DIRECTION.AHEAD), + (89, DIRECTION.AHEAD), + (89, DIRECTION.AHEAD), + (90, DIRECTION.NONE), + (90, DIRECTION.NONE), + (89, DIRECTION.BEHIND), + (89, DIRECTION.BEHIND), + (45, DIRECTION.BEHIND), + (45, DIRECTION.BEHIND), + (0, DIRECTION.BEHIND), + (0, DIRECTION.AHEAD), + ] + + d_and_d = list(map(lambda d: absoule_delta_with_direction(d), deltas)) + self.assertEqual(d_and_d, expected) + + # 7. Test tangent angle estimation from three points + def test_three_point_tangent_angle(self): + a = (0.99, 0.99) + b = (0.99, 1.01) + c = (1.01, 1.01) + d = (1.01, 0.99) + + angles = [ + three_point_tangent_angle(a, b, c), + three_point_tangent_angle(b, c, d), + three_point_tangent_angle(c, d, a), + three_point_tangent_angle(d, a, b), + ] + expected = [ + pi / 4., + -pi / 4., + -3 * pi / 4., + 3 * pi / 4., + ] + + self.assertEqual(angles, expected, 4) + + # 8. Test the curvature estimation from three points + def test_three_point_curvature(self): + data = [ + ((0., 10.), (2.5, 12.5), (5., 15.), 3 * pi / 8., pi / 4., pi / 8.), + ((0., 10.), (-2.5, 12.5), (-5., 15.), 5 * pi / 8., 3 * pi / 4., 7 * pi / 8.), + ((0., -10.), (-2.5, -12.5), (-5., -15.), 11 * pi / 8., 5 * pi / 4., 9 * pi / 8.), + ((0., -10.), (2.5, -12.5), (5., -15.), -3 * pi / 8., -pi / 4., -pi / 8.), + ] + + curvatures = list(map(lambda d: three_point_curvature(d[0], d[1], d[2], d[3], d[4], d[5]), data)) + expected = [ + -0.11107, + 0.11107, + -0.11107, + 0.11107, + ] + + self._assertAlmostEqualList(curvatures, expected, 3) + + # 9. Test cartesian distance + def test_xy_distance(self): + v = sqrt(50) + a = (v, v) + b = (-v, -v) + c = (-v, v) + + distances = [ + xy_distance(a, b), + xy_distance(a, c), + xy_distance(b, c), + xy_distance(c, b), + xy_distance(c, a), + xy_distance(b, a), + ] + expected = [ + 20, + 2 * v, + 2 * v, + 2 * v, + 2 * v, + 20 + ] + + self.assertEqual(distances, expected) + + # Helpers + def _assertAlmostEqualList(self, a, b, places=0): + for idx, el_a in enumerate(a): + self.assertAlmostEqual(el_a, b[idx], places) + + def _assertAlmostEqualListOfTuples(self, a, b, places=0): + for idx, el_a in enumerate(a): + for idy, el_el_a in enumerate(el_a): + self.assertAlmostEqual(el_el_a, b[idx][idy], places) diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100644 index 00000000000000..d5f26c6a78ca2f --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +import numpy as np +from time import strftime, gmtime +import cereal.messaging as messaging +from common.realtime import Ratekeeper +from selfdrive.mapd.lib.osm import OSM +from selfdrive.mapd.lib.geo import distance +from selfdrive.mapd.lib.WayCollection import WayCollection + + +QUERY_RADIUS = 3000 # mts +MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts +FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. + +_DEBUG = False + + +def _debug(msg): + if not _DEBUG: + return + print(msg) + + +class MapD(): + def __init__(self): + self.osm = OSM() + self.way_collection = None + self.route = None + self.last_gps_fix_timestamp = 0 + self.last_gps = None + self.lat = None + self.lon = None + self.bearing = None + self.accuracy = None + self.bearingAccuracy = None + self.gps_speed = 0. + self.last_fetch_location = None + self.last_route_update_fix_timestamp = 0 + self.last_publish_fix_timestamp = 0 + + @property + def location(self): + if self.lat is None or self.lon is None: + return None + return self.lat, self.lon + + def update_gps(self, sm): + sock = 'gpsLocationExternal' + if not sm.updated[sock] or not sm.valid[sock]: + return + + log = sm[sock] + self.last_gps = log + + # ignore the message if the fix is invalid + if log.flags % 2 == 0: + return + + self.last_gps_fix_timestamp = log.timestamp # Unix TS. Milliseconds since January 1, 1970. + self.lat = log.latitude + self.lon = log.longitude + self.bearing = log.bearingDeg + self.accuracy = log.accuracy + self.bearingAccuracy = log.bearingAccuracyDeg + self.gps_speed = log.speed + + _debug('Mapd: ********* Got GPS fix' + f'Pos: {self.lat}, {self.lon} +/- {self.accuracy} mts.\n' + f'Bearing: {self.bearing} +/- {self.bearingAccuracy} deg.\n' + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' + f'*******') + + def updated_osm_data(self): + if self.route is not None: + distance_to_end = self.route.distance_to_end + if distance_to_end is not None and distance_to_end >= MIN_DISTANCE_FOR_NEW_QUERY: + # do not query as long as we have a route with enough distance ahead. + return + + if self.location is None: + return + + if self.last_fetch_location is not None: + distance_since_last = distance(self.location, self.last_fetch_location) + if distance_since_last < QUERY_RADIUS - MIN_DISTANCE_FOR_NEW_QUERY: + # do not query if are still not close to the border of previous query area + return + + ways = self.osm.fetch_road_ways_around_location(self.location, QUERY_RADIUS) + self.way_collection = WayCollection(ways) + self.last_fetch_location = self.location + + _debug(f'Mapd: Updated map data @ {self.location} - got {len(ways)} ways') + + def update_route(self, sm): + if self.way_collection is None or self.location is None or self.bearing is None: + return + + if self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: + # No new fix since last update + return + + self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp + + # Create the route if not existent or if it was generated by an older way collection + if self.route is None or self.route.way_collection_id != self.way_collection.id: + self.route = self.way_collection.get_route(self.location, self.bearing) + _debug(f'Mapd *****: Route created: \n{self.route}\n********') + return + + # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start + # jumping and creating unnecesary loosing of the route. Since the route update timestamp has been updated + # a new liveMapData message will be published with the current values (which is desirable) + if self.gps_speed < FULL_STOP_MAX_SPEED: + _debug('Mapd *****: Route Not updated as car has Stopped ********') + return + + self.route.update(self.location, self.bearing) + if self.route.located: + _debug(f'Mapd *****: Route updated: \n{self.route}\n********') + return + + # if an old route did not mange to locate, attempt to regenerate form way collection. + self.route = self.way_collection.get_route(self.location, self.bearing) + _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') + + def publish(self, pm, sm): + # Ensure we have a route currently located + if self.route is None or not self.route.located: + return + + # Ensure we have a route update since last publish + if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp: + return + + self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp + + speed_limit = self.route.current_speed_limit + next_speed_limit_section = self.route.next_speed_limit_section + current_curvature = self.route.immediate_curvature + curvatures_ahead = self.route._curvatures_ahead + curvatures_ahead = np.array([]) if curvatures_ahead is None else curvatures_ahead + next_subst_curvature = self.route.next_substantial_curvature + + map_data_msg = messaging.new_message('liveMapDataDEPRECATED') + map_data_msg.valid = sm.all_alive_and_valid(service_list=['gpsLocationExternal']) + map_data_msg.liveMapDataDEPRECATED.lastGps = self.last_gps + map_data_msg.liveMapDataDEPRECATED.speedLimitValid = bool(speed_limit is not None) + map_data_msg.liveMapDataDEPRECATED.speedLimit = float(speed_limit if speed_limit is not None else 0.0) + map_data_msg.liveMapDataDEPRECATED.speedLimitAheadValid = bool(next_speed_limit_section is not None) + map_data_msg.liveMapDataDEPRECATED.speedLimitAhead = float(next_speed_limit_section.value + if next_speed_limit_section is not None else 0.0) + map_data_msg.liveMapDataDEPRECATED.speedLimitAheadDistance = float(next_speed_limit_section.start + if next_speed_limit_section is not None else 0.0) + map_data_msg.liveMapDataDEPRECATED.curvatureValid = bool(current_curvature is not None) + map_data_msg.liveMapDataDEPRECATED.curvature = float(current_curvature if current_curvature is not None else 0.0) + map_data_msg.liveMapDataDEPRECATED.roadCurvatureX = [float(c[0]) for c in curvatures_ahead] + map_data_msg.liveMapDataDEPRECATED.roadCurvature = [float(c[1]) for c in curvatures_ahead] + map_data_msg.liveMapDataDEPRECATED.distToTurn = float(next_subst_curvature[0] + if next_subst_curvature is not None else 0.0) + + pm.send('liveMapDataDEPRECATED', map_data_msg) + _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********') + + +# provides live map data information +def mapd_thread(sm=None, pm=None): + mapd = MapD() + rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz + + # *** setup messaging + if sm is None: + sm = messaging.SubMaster(['gpsLocationExternal']) + if pm is None: + pm = messaging.PubMaster(['liveMapDataDEPRECATED']) + + while True: + sm.update() + mapd.update_gps(sm) + mapd.updated_osm_data() + mapd.update_route(sm) + mapd.publish(pm, sm) + rk.keep_time() + + +def main(sm=None, pm=None): + mapd_thread(sm, pm) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/speed.py b/selfdrive/mapd/speed.py new file mode 100644 index 00000000000000..26d972d9d51c14 --- /dev/null +++ b/selfdrive/mapd/speed.py @@ -0,0 +1,76 @@ +from lib.osm import OSM +from lib.WayCollection import WayCollection +from decimal import Decimal +import sys +import csv + + +# TEST: python speed.py 52.273948132602584 13.91490391150784 1000 313 +# TEST: python speed.py 52.19538880646971 13.867764690138795 2000 305 +# STRESS: python speed.py 52.5094376 13.397043 3000 353.27514648437 + + +def csv_out_curvatures(name, curv_data): + with open(f'{name}.csv', 'w', newline='') as results_csv: + csv_writer = csv.writer(results_csv, delimiter=',') + csv_writer.writerow([ + 'X', 'Y', 'tan', 'cur' + ]) + for curv in curv_data: + csv_writer.writerow(curv) + + +if __name__ == '__main__': + location = (Decimal(sys.argv[1]), Decimal(sys.argv[2])) + location2 = Decimal(52.27791306120662), Decimal(13.90794088430264) + bearing = float(sys.argv[4]) + + # 1. Get ways around location + osm = OSM() + ways = osm.fetch_road_ways_around_location(location, float(sys.argv[3])) + + # 2. Create the collection + way_collection = WayCollection(ways) + + # 3. Find the current route + route = way_collection.get_route(location, bearing) + + # 4. Output + print('_____ GIVEN DIRECTION') + if route is None or not route.valid: + print('No valid routes found for given loaction and bearing.') + else: + print(f'Current way: {route.current_wr}') + print(f'Speed Limit: {route.current_wr.speed_limit}') + print(f'Route Ahead: {route}') + print(f'Current speed limit: {route.current_speed_limit}') + print(f'Next speed limit: {route.next_speed_limit_section}') + print(f'Curvature now: {route.immediate_curvature}') + print(f'Max Curvature: {route.max_curvature_ahead}') + print(f'Next Curvature: {route.next_substantial_curvature}') + print(f'Limits Ahead: {route.speed_limits_ahead}') + print(f'curvatures: {route.curvatures_ahead}') + print(f'Distance to end: {route.distance_to_end}') + # csv_out_curvatures('forward', route.curvatures) + + # 4. Update on the oposit direction for testing + route = way_collection.get_route(location, bearing - 180) + + # 5. Output in oposit direction + print(' ') + print('_____REVERSE DIRECTION') + if route is None or not route.valid: + print('No valid routes found for given loaction and bearing.') + else: + print(f'Current way: {route.current_wr}') + print(f'Speed Limit: {route.current_wr.speed_limit}') + print(f'Route Ahead: {route}') + print(f'Current speed limit: {route.current_speed_limit}') + print(f'Next speed limit: {route.next_speed_limit_section}') + print(f'Curvature now: {route.immediate_curvature}') + print(f'Max Curvature: {route.max_curvature_ahead}') + print(f'Next Curvature: {route.next_substantial_curvature}') + print(f'Limits Ahead: {route.speed_limits_ahead}') + print(f'curvatures: {route.curvatures_ahead}') + print(f'Distance to end: {route.distance_to_end}') + # csv_out_curvatures('backward', route.curvatures) \ No newline at end of file From 98b259c55701f4e658aaea9fdd3b2c9928f8fe98 Mon Sep 17 00:00:00 2001 From: alfhern Date: Fri, 30 Apr 2021 13:09:11 +0200 Subject: [PATCH 11/32] LiveMapData: Updated logic for current way selection --- selfdrive/mapd/lib/NodesData.py | 18 +------- selfdrive/mapd/lib/WayCollection.py | 39 ++++++++++++++---- selfdrive/mapd/lib/WayRelation.py | 64 +++++++++++++++++++++-------- selfdrive/mapd/lib/geo.py | 17 ++++++++ 4 files changed, 96 insertions(+), 42 deletions(-) diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py index 4367fcd1c0ccf8..2d9069d71dbbf9 100644 --- a/selfdrive/mapd/lib/NodesData.py +++ b/selfdrive/mapd/lib/NodesData.py @@ -1,27 +1,11 @@ import numpy as np from enum import Enum -from .geo import DIRECTION, R +from .geo import DIRECTION, R, vectors _TURN_CURVATURE_THRESHOLD = 0.001 # 1/mts. A curvature over this value will generate a speed limit section. _MAX_LAT_ACC = 1.5 # Maximum lateral acceleration in turns. -def vectors(points): - """Provides a array of vectors on cartesian space (x, y). - Each vector represents the path from a point in `points` to the next. - `points` must by a (N, 2) array of [lat, lon] pairs in radians. - """ - latA = points[:-1, 0] - latB = points[1:, 0] - delta = np.diff(points, axis=0) - dlon = delta[:, 1] - - x = np.sin(dlon) * np.cos(latB) - y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) - - return np.column_stack((x, y)) - - def nodes_raw_data_array_for_wr(wr, drop_last=False): """Provides an array of raw node data (id, lat, lon, speed_limit) for all nodes in way relation """ diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py index 150cc186675cd0..69164af279e2c6 100644 --- a/selfdrive/mapd/lib/WayCollection.py +++ b/selfdrive/mapd/lib/WayCollection.py @@ -3,6 +3,9 @@ import uuid +_ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475 # sin(pi/4) | 45 degrees acceptable bearing delta + + class WayCollection(): """A collection of WayRelations to use for maps data analysis. """ @@ -20,17 +23,39 @@ def get_route(self, location, bearing): for wr in self.way_relations: wr.update(location, bearing) - # From those matching (i.e. active), select the one with minimum bearing delta between - # route segment and provided bearing + # Get the way relations where a match was found. i.e. those now marked as active. active_way_relations = list(filter(lambda wr: wr.active, self.way_relations)) - active_way_relations.sort(key=lambda wr: wr.active_bearing_delta) + + # If no active, then we could not find a current way to build a route. if len(active_way_relations) == 0: return None - # Pick first relation as current adn reset location for the remaining located way relations - current = active_way_relations[0] - if len(active_way_relations) > 1: - for wr in active_way_relations[1:]: + # If only one active, then pick it as current. + if len(active_way_relations) == 1: + current = active_way_relations[0] + + # If more than one is active, filter out any active way relation where the bearing delta indicator is too high. + else: + wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND, + active_way_relations)) + + # If delta bearing indicator is too high for all, then use as current the one that has the shorter one. + if len(wr_acceptable_bearing) == 0: + active_way_relations.sort(key=lambda wr: wr.active_bearing_delta) + current = active_way_relations[0] + + # If only one with acceptable bearing, use it. + elif len(wr_acceptable_bearing) == 1: + current = wr_acceptable_bearing[0] + + # If more than one with acceptable bearing, then now choose the closest one to the way + else: + wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way) + current = wr_acceptable_bearing[0] + + # Reset location for the remaining located way relations + for wr in active_way_relations: + if wr.id != current.id: wr.reset_location_variables() return Route(current, self.way_relations, self.id) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index 294dbb2e29d466..1ca84be53a8edb 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -1,12 +1,10 @@ -from .geo import DIRECTION, R +from .geo import DIRECTION, R, vectors from selfdrive.config import Conversions as CV from datetime import datetime import numpy as np import re -_ACCEPTABLE_BEARING_DELTA_V = [70., 50., 30., 10.] -_ACCEPTABLE_BEARING_DELTA_BP = [30., 100., 200., 300.] _WAY_BBOX_PADING = 1.6e-06 # 10 mts of pading to bounding box. (expressed in radians) _COUNTRY_LIMITS_KPH = { @@ -176,6 +174,7 @@ def reset_location_variables(self): self.ahead_idx = None self.behind_idx = None self._active_bearing_delta = None + self._distance_to_way = None @property def id(self): @@ -192,18 +191,17 @@ def update(self, location, bearing): if not self.is_location_in_bbox(location): return - # Find where we are located in the way: location_rad = np.radians(np.array(location)) bearing_rad = np.radians(bearing) - # - Get the distance and bearings from location to all nodes. + # - Get the distance and bearings from location to all nodes. (N) bearings = bearing_to_points(location_rad, self._nodes_np) distances = distance_to_points(location_rad, self._nodes_np) - # - Get absolute bearing delta to current driving bearing. + # - Get absolute bearing delta to current driving bearing. (N) delta = np.abs(bearing_rad - bearings) - # - Nodes are ahead if the cosine of the delta is positive + # - Nodes are ahead if the cosine of the delta is positive (N) is_ahead = np.cos(delta) >= 0. # - Possible locations on the way are those where adjacent nodes change from ahead to behind or viceversa. @@ -213,17 +211,40 @@ def update(self, location, bearing): if len(possible_idxs) == 0: return - # - The smallest angle between bearing and the bearing of the way, is the sine of the delta. - # This value indicates how far are we from alignment with the way direction and will aid us in - # choosing a location when we have multiple candidates. - delta_abs = np.abs(np.sin(delta)) + # - Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2) + v = vectors(self._nodes_np) * R - # - Get the deltas on nodes ahead and behind for the possible locations and pick the minimum as the delta - # to actual way bearing. - delta_to_way_bearings = np.min(np.row_stack((delta_abs[possible_idxs], delta_abs[possible_idxs + 1])), axis=0) + # - Calculate the vector magnitudes (or distance) between nodes. (N-1) + d = np.linalg.norm(v, axis=1) - # - Get the index where the delta to way bearing is minimum. That is the chosen location. - min_delta_idx = possible_idxs[np.argmin(delta_to_way_bearings)] + # - Find then angle formed between the vectors from the current location to consecutive nodes. This is the + # value of the difference in the bearings of the vectors. + teta = np.diff(bearings) + + # - When two consecutive nodes will be ahead and behind, they will form a triangle with the current location. + # We find the closest distance to the way by solving the ara of the triangle and finding the height (h). + # We must use the abolute value of the sin of the angle in the formula, which is equivalent to ensure we + # are considering the smallest of the two angles formed between the two vectors. + # https://www.mathsisfun.com/algebra/trig-area-triangle-without-right-angle.html + h = distances[:-1] * distances[1:] * np.abs(np.sin(teta)) / d + + # - Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1) + bw = np.arctan2(v[:, 0], v[:, 1]) + + # - Calculate the delta between driving bearing and way bearings. (N-1) + bw_delta = bw - bearing_rad + + # - The absolut value of the sin of `bw_delta` indicates how close the bearings match independent of direction. + # We will use this value along the distance to the way to aid on way selection. (N-1) + abs_sin_bw_delta = np.abs(np.sin(bw_delta)) + + # - Get the delta to way bearing indicators and the distance to the way for the possible locations. + abs_sin_bw_delta_possible = abs_sin_bw_delta[possible_idxs] + h_possible = h[possible_idxs] + + # - Get the index where the distance to the way is minimum. That is the chosen location. + min_h_possible_idx = np.argmin(h_possible) + min_delta_idx = possible_idxs[min_h_possible_idx] # Populate location variables with result if is_ahead[min_delta_idx]: @@ -235,7 +256,8 @@ def update(self, location, bearing): self.ahead_idx = min_delta_idx + 1 self.behind_idx = min_delta_idx - self._active_bearing_delta = np.amin(delta_to_way_bearings) + self._distance_to_way = h[min_delta_idx] + self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx] self.distance_to_node_ahead = distances[self.ahead_idx] self.active = True self.location = location @@ -300,11 +322,17 @@ def name(self): @property def active_bearing_delta(self): - """Returns the delta between the current location bearing and the exact + """Returns the sine of the delta between the current location bearing and the exact bearing of the portion of way we are currentluy located at. """ return self._active_bearing_delta + @property + def distance_to_way(self): + """Returns the perpendicular (i.e. minimum) distance between current location and the way + """ + return self._distance_to_way + @property def node_behind(self): return self.way.nodes[self.behind_idx] if self.behind_idx is not None else None diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py index 87137986fb9df7..a02377d1b97db4 100644 --- a/selfdrive/mapd/lib/geo.py +++ b/selfdrive/mapd/lib/geo.py @@ -1,5 +1,6 @@ from math import sin, cos, sqrt, atan2, radians, degrees from enum import Enum +import numpy as np R = 6373000.0 # approximate radius of earth in mt @@ -7,6 +8,22 @@ MAX_DIST_FOR_CURVATURE = 500 # mts. Max distance between nodes for curvature calculation +def vectors(points): + """Provides a array of vectors on cartesian space (x, y). + Each vector represents the path from a point in `points` to the next. + `points` must by a (N, 2) array of [lat, lon] pairs in radians. + """ + latA = points[:-1, 0] + latB = points[1:, 0] + delta = np.diff(points, axis=0) + dlon = delta[:, 1] + + x = np.sin(dlon) * np.cos(latB) + y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) + + return np.column_stack((x, y)) + + def coord_to_rad(point): """Tranform coordinates in degrees to radians """ From 374dad8e1b0cbdbf479872eddef424e712070731 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 16 Mar 2021 16:29:26 +0100 Subject: [PATCH 12/32] Debug --- selfdrive/mapd/lib/osm.py | 3 ++- selfdrive/mapd/mapd.py | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py index 22af6b571e26c9..c1c005f2151765 100644 --- a/selfdrive/mapd/lib/osm.py +++ b/selfdrive/mapd/lib/osm.py @@ -3,7 +3,8 @@ class OSM(): def __init__(self): - self.api = overpy.Overpass() + # self.api = overpy.Overpass() + self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter') def fetch_road_ways_around_location(self, location, radius): lat, lon = location diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index d5f26c6a78ca2f..31df7e80478bcb 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -12,7 +12,7 @@ MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. -_DEBUG = False +_DEBUG = True def _debug(msg): From 173c1fada3746078e3b5ca02d9831f73e367acb4 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 9 Mar 2021 13:59:08 +0100 Subject: [PATCH 13/32] Speed Limit Controller to make use of Live Map Data --- selfdrive/controls/controlsd.py | 2 +- .../controls/lib/longitudinal_planner.py | 3 +- .../controls/lib/speed_limit_controller.py | 145 ++++++++++++++++-- selfdrive/controls/plannerd.py | 2 +- 4 files changed, 139 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8c83a8e04c7dda..49f758754a4cf5 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -519,7 +519,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.v_cruise_kph) - controlsState.speedLimit = float(CS.cruiseState.speedLimit) + controlsState.speedLimit = float(self.sm['longitudinalPlan'].speedLimit) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 99e44fb84701e9..1dd3f12581cb84 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -171,7 +171,7 @@ def update(self, sm, CP): # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) # update speed limit solution calculation. - self.speed_limit_controller.update(enabled, self.v_acc_start, self.a_acc_start, sm['carState'], + self.speed_limit_controller.update(enabled, self.v_acc_start, self.a_acc_start, sm, v_cruise_setpoint, accel_limits_turns, jerk_limits, self.events) else: starting = long_control_state == LongCtrlState.starting @@ -241,6 +241,7 @@ def publish(self, sm, pm): longitudinalPlan.decelForTurnDEPRECATED = bool(self.turn_controller.is_active) longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state + longitudinalPlan.speedLimit = float(self.speed_limit_controller.speed_limit) longitudinalPlan.eventsDEPRECATED = self.events.to_msg() longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py index 61800ce4ba171a..6f4b9786636b4a 100644 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -1,4 +1,6 @@ import numpy as np +import time +from enum import Enum from cereal import log, car from common.params import Params from common.realtime import sec_since_boot @@ -11,13 +13,24 @@ _MIN_ADAPTING_BRAKE_ACC = -1.5 # Minimum acceleration allowed when adapting to lower speed limit. _MIN_ADAPTING_BRAKE_JERK = -1.0 # Minimum jerk allowed when adapting to lower speed limit. _SPEED_OFFSET_TH = -3.0 # m/s Maximum offset between speed limit and current speed for adapting state. -_LIMIT_ADAPT_TIME = 5.0 # Ideal time (s) to adapt to lower speed limit. i.e. braking. +_LIMIT_ADAPT_TIME_PER_MS = 1.8 # Ideal adapt time(s) to lower speed limit. i.e. braking for every m/s of speed delta. +_MIN_LIMIT_ADAPT_TIME = 5. # s, Minimum time to provide for adapting logic. _MAX_SPEED_OFFSET_DELTA = 1.0 # m/s Maximum delta for speed limit changes. +_MAX_MAP_DATA_AGE = 10.0 # s Maximum time to hold to map data, then consider it invalid. + SpeedLimitControlState = log.ControlsState.SpeedLimitControlState EventName = car.CarEvent.EventName +_DEBUG = False + + +def _debug(msg): + if not _DEBUG: + return + print(msg) + def _description_for_state(speed_limit_control_state): if speed_limit_control_state == SpeedLimitControlState.inactive: @@ -30,6 +43,98 @@ def _description_for_state(speed_limit_control_state): return 'ACTIVE' +class SpeedLimitResolver(): + class Key(Enum): + car_state = 'car_state' + map_data = 'map_data' + + class Policy(Enum): + car_state_only = 0 + map_data_only = 1 + car_state_priority = 2 + map_data_priority = 3 + combined = 4 + + def __init__(self, v_ref, current_speed_limit, sm, policy=Policy.map_data_priority): + self._results = {} + self._v_ref = v_ref # Reference speed for calculation of time to next speed limit in map data. + self._current_speed_limit = current_speed_limit + self._sm = sm + self._policy = policy + self.speed_limit = 0. + + def resolve(self): + self._get_from_car_state() + self._get_from_map_data() + self._consolidate() + + def _get_from_car_state(self): + self._results[SpeedLimitResolver.Key.car_state] = self._sm['carState'].cruiseState.speedLimit + + def _get_from_map_data(self): + self._results[SpeedLimitResolver.Key.map_data] = 0. + + # Ignore if no live map data + sock = 'liveMapDataDEPRECATED' + if self._sm.logMonoTime[sock] is None: + _debug('SL: No map data for speed limit') + return + + # Load limits from map_data + map_data = self._sm[sock] + speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.0 + + # Calculate the age of the gps fix. Ignore if too old. + gps_fix_age = time.time() - map_data.lastGps.timestamp * 1e-3 + if gps_fix_age > _MAX_MAP_DATA_AGE: + _debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') + return + + # Estimate the time left to reach new speed limit ahead (if any) and use it if we are close + # enough while traveling when the speed limit is being reduced or set for the first time. + if map_data.speedLimitAheadValid and self._v_ref > 0: + next_speed_limit = map_data.speedLimitAhead + if self._current_speed_limit == 0 or next_speed_limit <= self._current_speed_limit: + next_speed_limit_time = (map_data.speedLimitAheadDistance / self._v_ref) - gps_fix_age + if next_speed_limit_time <= max(_LIMIT_ADAPT_TIME_PER_MS * (self._v_ref - next_speed_limit), + _MIN_LIMIT_ADAPT_TIME): + speed_limit = next_speed_limit + + # Populate results + self._results[SpeedLimitResolver.Key.map_data] = speed_limit + + def _consolidate(self): + values = [] + + if self._policy == SpeedLimitResolver.Policy.car_state_only or \ + self._policy == SpeedLimitResolver.Policy.car_state_priority or \ + self._policy == SpeedLimitResolver.Policy.combined: + values.append(self._results[SpeedLimitResolver.Key.car_state]) + + if self._policy == SpeedLimitResolver.Policy.map_data_only or \ + self._policy == SpeedLimitResolver.Policy.map_data_priority or \ + self._policy == SpeedLimitResolver.Policy.combined: + values.append(self._results[SpeedLimitResolver.Key.map_data]) + + if max(values) == 0.: + if self._policy == SpeedLimitResolver.Policy.car_state_priority: + values.append(self._results[SpeedLimitResolver.Key.map_data]) + + elif self._policy == SpeedLimitResolver.Policy.map_data_priority: + values.append(self._results[SpeedLimitResolver.Key.car_state]) + + # Get all non-zero values and set the minimum if any, otherwise 0. + values = np.array(values) + values = values[values > 0.] + + if len(values) > 0: + self.speed_limit = np.amin(values) + else: + self.speed_limit = 0. + + _debug(f'SL: *** Speed Limit set: {self.speed_limit}') + + class SpeedLimitController(): def __init__(self, CP): self._params = Params() @@ -44,6 +149,7 @@ def __init__(self, CP): self._adapting_jerk_limits = [_MIN_ADAPTING_BRAKE_JERK, 1.0] self._v_ego = 0.0 self._a_ego = 0.0 + self._v_adapting = 0.0 self._v_offset = 0.0 self._v_cruise_setpoint = 0.0 self._v_cruise_setpoint_prev = 0.0 @@ -58,6 +164,7 @@ def __init__(self, CP): self._state = SpeedLimitControlState.inactive self._state_prev = SpeedLimitControlState.inactive self._adapting_cycles = 0 + self._adapting_time = 0. self.v_limit = 0.0 self.a_limit = 0.0 @@ -70,14 +177,19 @@ def state(self): @state.setter def state(self, value): if value != self._state: - print(f'Speed Limit Controller state: {_description_for_state(value)}') + _debug(f'Speed Limit Controller state: {_description_for_state(value)}') + if value == SpeedLimitControlState.adapting: self._adapting_cycles = 0 # Reset adapting state cycle count when entereing state. + # Adapting time must be calculated at the moment we enter adapting state. + self._adapting_time = abs(_LIMIT_ADAPT_TIME_PER_MS * self._v_offset) + elif value == SpeedLimitControlState.tempInactive: # Make sure speed limit is set to `set` value, this will have the effect # of canceling delayed increase limit, if pending. self._speed_limit = self._speed_limit_set self._speed_limit_prev = self._speed_limit + self._state = value @property @@ -85,15 +197,19 @@ def is_active(self): return self.state > SpeedLimitControlState.tempInactive @property - def speed_limit(self): + def speed_limit_offseted(self): return self._speed_limit * (1.0 + self._speed_limit_perc_offset / 100.0) + @property + def speed_limit(self): + return self._speed_limit + def _update_params(self): time = sec_since_boot() if time > self._last_params_update + 5.0: self._speed_limit_perc_offset = float(self._params.get("SpeedLimitPercOffset")) self._is_enabled = self._params.get("SpeedLimitControl", encoding='utf8') == "1" - print(f'Updated Speed limit params. enabled: {self._is_enabled}, \ + _debug(f'Updated Speed limit params. enabled: {self._is_enabled}, \ perc_offset: {self._speed_limit_perc_offset:.1f}') self._last_params_update = time @@ -111,7 +227,7 @@ def _update_calculations(self): elif time > self._last_speed_limit_set_change_ts + _WAIT_TIME_LIMIT_RISE: self._speed_limit = self._speed_limit_set # Update current velocity offset (error) - self._v_offset = self.speed_limit - self._v_ego + self._v_offset = self.speed_limit_offseted - self._v_ego # Update change tracking variables self._speed_limit_changed = self._speed_limit != self._speed_limit_prev self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev @@ -170,8 +286,8 @@ def _update_solution(self): # adapting elif self.state == SpeedLimitControlState.adapting: # Calculate to adapt speed on target time. - adapting_time = max(_LIMIT_ADAPT_TIME - self._adapting_cycles * _LON_MPC_STEP, 1.0) # min adapt time 1 sec. - a_target = (self.speed_limit - self._v_ego) / adapting_time + adapting_time = max(self._adapting_time - self._adapting_cycles * _LON_MPC_STEP, 1.0) # min adapt time 1 sec. + a_target = (self.speed_limit_offseted - self._v_ego) / adapting_time # smooth out acceleration using jerk limits. j_limits = np.array(self._adapting_jerk_limits) a_limits = self._a_ego + j_limits * _LON_MPC_STEP @@ -183,7 +299,7 @@ def _update_solution(self): # active elif self.state == SpeedLimitControlState.active: # Calculate following same cruise logic in planner.py - self.v_limit, self.a_limit = speed_smoother(self._v_ego, self._a_ego, self.speed_limit, + self.v_limit, self.a_limit = speed_smoother(self._v_ego, self._a_ego, self.speed_limit_offseted, self._active_accel_limits[1], self._active_accel_limits[0], self._active_jerk_limits[1], self._active_jerk_limits[0], _LON_MPC_STEP) @@ -202,11 +318,20 @@ def _update_events(self, events): elif self._speed_limit_set_change < 0: events.add(EventName.speedLimitDecrease) - def update(self, enabled, v_ego, a_ego, CS, v_cruise_setpoint, accel_limits, jerk_limits, events=Events()): + def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, accel_limits, jerk_limits, + events=Events()): self._op_enabled = enabled self._v_ego = v_ego self._a_ego = a_ego - self._speed_limit_set = CS.cruiseState.speedLimit + + # velocity before adapting should folow v_ego while not in adapting state. + if self.state != SpeedLimitControlState.adapting: + self._v_adapting = self._v_ego + + resolver = SpeedLimitResolver(self._v_adapting, self.speed_limit, sm) + resolver.resolve() + self._speed_limit_set = resolver.speed_limit + self._v_cruise_setpoint = v_cruise_setpoint self._active_accel_limits = accel_limits self._active_jerk_limits = jerk_limits diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index fc9b26e6ab4203..3f49185d991ab7 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -20,7 +20,7 @@ def plannerd_thread(sm=None, pm=None): lateral_planner = LateralPlanner(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlan'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlan', 'liveMapDataDEPRECATED'], poll=['radarState', 'modelV2']) if pm is None: From bb48171769b7c45a8da172b965dd449edad833db Mon Sep 17 00:00:00 2001 From: alfhern Date: Thu, 29 Apr 2021 13:25:16 +0200 Subject: [PATCH 14/32] SpeedLimitController: Fix to take poper care of adapting to lower speed lmits ahead provided by mapdata --- .../controls/lib/speed_limit_controller.py | 83 +++++++++++-------- 1 file changed, 50 insertions(+), 33 deletions(-) diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py index 6f4b9786636b4a..d9df570d878c87 100644 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -10,10 +10,11 @@ _LON_MPC_STEP = 0.2 # Time stemp of longitudinal control (5 Hz) _WAIT_TIME_LIMIT_RISE = 2.0 # Waiting time before raising the speed limit. -_MIN_ADAPTING_BRAKE_ACC = -1.5 # Minimum acceleration allowed when adapting to lower speed limit. -_MIN_ADAPTING_BRAKE_JERK = -1.0 # Minimum jerk allowed when adapting to lower speed limit. -_SPEED_OFFSET_TH = -3.0 # m/s Maximum offset between speed limit and current speed for adapting state. -_LIMIT_ADAPT_TIME_PER_MS = 1.8 # Ideal adapt time(s) to lower speed limit. i.e. braking for every m/s of speed delta. +_MIN_ADAPTING_BRAKE_ACC = -1. # Minimum acceleration allowed when adapting to lower speed limit. +_MIN_ADAPTING_BRAKE_JERK = -0.5 # Minimum jerk allowed when adapting to lower speed limit. +_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. + +_LIMIT_ADAPT_TIME_PER_MS = 1. # Ideal adapt time(s) to lower speed limit. i.e. braking for every m/s of speed delta. _MIN_LIMIT_ADAPT_TIME = 5. # s, Minimum time to provide for adapting logic. _MAX_SPEED_OFFSET_DELTA = 1.0 # m/s Maximum delta for speed limit changes. @@ -55,52 +56,75 @@ class Policy(Enum): map_data_priority = 3 combined = 4 - def __init__(self, v_ref, current_speed_limit, sm, policy=Policy.map_data_priority): + def __init__(self, policy=Policy.map_data_priority): self._results = {} - self._v_ref = v_ref # Reference speed for calculation of time to next speed limit in map data. - self._current_speed_limit = current_speed_limit - self._sm = sm + self._v_ego = 0. + self._current_speed_limit = 0. self._policy = policy + self._next_speed_limit_prev = 0. self.speed_limit = 0. - def resolve(self): + def resolve(self, v_ego, current_speed_limit, sm): + self._v_ego = v_ego + self._current_speed_limit = current_speed_limit + self._sm = sm + self._get_from_car_state() self._get_from_map_data() self._consolidate() + return self.speed_limit + def _get_from_car_state(self): self._results[SpeedLimitResolver.Key.car_state] = self._sm['carState'].cruiseState.speedLimit def _get_from_map_data(self): - self._results[SpeedLimitResolver.Key.map_data] = 0. - # Ignore if no live map data sock = 'liveMapDataDEPRECATED' if self._sm.logMonoTime[sock] is None: + self._results[SpeedLimitResolver.Key.map_data] = 0. _debug('SL: No map data for speed limit') return # Load limits from map_data map_data = self._sm[sock] - speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.0 + speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. + next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. # Calculate the age of the gps fix. Ignore if too old. gps_fix_age = time.time() - map_data.lastGps.timestamp * 1e-3 if gps_fix_age > _MAX_MAP_DATA_AGE: + self._results[SpeedLimitResolver.Key.map_data] = 0. _debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') return - # Estimate the time left to reach new speed limit ahead (if any) and use it if we are close - # enough while traveling when the speed limit is being reduced or set for the first time. - if map_data.speedLimitAheadValid and self._v_ref > 0: - next_speed_limit = map_data.speedLimitAhead - if self._current_speed_limit == 0 or next_speed_limit <= self._current_speed_limit: - next_speed_limit_time = (map_data.speedLimitAheadDistance / self._v_ref) - gps_fix_age - if next_speed_limit_time <= max(_LIMIT_ADAPT_TIME_PER_MS * (self._v_ref - next_speed_limit), - _MIN_LIMIT_ADAPT_TIME): - speed_limit = next_speed_limit - - # Populate results + # When we have no ahead speed limit to consider or it is greater than current speed limit + # or car has stopped, then provide current value and reset tracking. + if next_speed_limit == 0. or self._v_ego == 0. or next_speed_limit > self._current_speed_limit: + self._results[SpeedLimitResolver.Key.map_data] = speed_limit + self._next_speed_limit_prev = 0. + return + + # When we have a next_speed_limit value that has not changed from a provided next speed limit value + # in previous resolutions, we keep providing it. + if next_speed_limit == self._next_speed_limit_prev: + self._results[SpeedLimitResolver.Key.map_data] = next_speed_limit + return + + # Reset tracking + self._next_speed_limit_prev = 0. + + # Calculate the time to the next speed limit and the adapt (braking) + next_speed_limit_time = (map_data.speedLimitAheadDistance / self._v_ego) - gps_fix_age + adapt_time = _LIMIT_ADAPT_TIME_PER_MS * (self._v_ego - next_speed_limit) + + # When we detect we are close enough, we provide the next limit value and track it. + if next_speed_limit_time <= adapt_time: + self._results[SpeedLimitResolver.Key.map_data] = next_speed_limit + self._next_speed_limit_prev = next_speed_limit + return + + # Otherwise we just provide the map datae speed limit. self._results[SpeedLimitResolver.Key.map_data] = speed_limit def _consolidate(self): @@ -138,6 +162,7 @@ def _consolidate(self): class SpeedLimitController(): def __init__(self, CP): self._params = Params() + self._resolver = SpeedLimitResolver() self._last_params_update = 0.0 self._is_metric = self._params.get("IsMetric", encoding='utf8') == "1" self._is_enabled = self._params.get("SpeedLimitControl", encoding='utf8') == "1" @@ -149,7 +174,6 @@ def __init__(self, CP): self._adapting_jerk_limits = [_MIN_ADAPTING_BRAKE_JERK, 1.0] self._v_ego = 0.0 self._a_ego = 0.0 - self._v_adapting = 0.0 self._v_offset = 0.0 self._v_cruise_setpoint = 0.0 self._v_cruise_setpoint_prev = 0.0 @@ -189,7 +213,7 @@ def state(self, value): # of canceling delayed increase limit, if pending. self._speed_limit = self._speed_limit_set self._speed_limit_prev = self._speed_limit - + self._state = value @property @@ -324,14 +348,7 @@ def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, accel_limits, jer self._v_ego = v_ego self._a_ego = a_ego - # velocity before adapting should folow v_ego while not in adapting state. - if self.state != SpeedLimitControlState.adapting: - self._v_adapting = self._v_ego - - resolver = SpeedLimitResolver(self._v_adapting, self.speed_limit, sm) - resolver.resolve() - self._speed_limit_set = resolver.speed_limit - + self._speed_limit_set = self._resolver.resolve(v_ego, self.speed_limit, sm) self._v_cruise_setpoint = v_cruise_setpoint self._active_accel_limits = accel_limits self._active_jerk_limits = jerk_limits From 6299c2d2f48260cc5baf0d07974b0c819e09c5e0 Mon Sep 17 00:00:00 2001 From: alfhern Date: Fri, 30 Apr 2021 15:25:11 +0200 Subject: [PATCH 15/32] SpeedLimitControl: Improvements to active/inactive/temp_inactive logic --- selfdrive/controls/lib/speed_limit_controller.py | 9 +++++---- selfdrive/controls/lib/turn_controller.py | 1 - selfdrive/ui/paint.cc | 2 +- selfdrive/ui/ui.hpp | 5 ----- 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py index d9df570d878c87..bf3c09e9c093c7 100644 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -7,6 +7,9 @@ from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.events import Events + +_PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates. + _LON_MPC_STEP = 0.2 # Time stemp of longitudinal control (5 Hz) _WAIT_TIME_LIMIT_RISE = 2.0 # Waiting time before raising the speed limit. @@ -230,11 +233,9 @@ def speed_limit(self): def _update_params(self): time = sec_since_boot() - if time > self._last_params_update + 5.0: - self._speed_limit_perc_offset = float(self._params.get("SpeedLimitPercOffset")) + if time > self._last_params_update + _PARAMS_UPDATE_PERIOD: self._is_enabled = self._params.get("SpeedLimitControl", encoding='utf8') == "1" - _debug(f'Updated Speed limit params. enabled: {self._is_enabled}, \ - perc_offset: {self._speed_limit_perc_offset:.1f}') + _debug(f'Updated Speed limit params. enabled: {self._is_enabled}') self._last_params_update = time def _update_calculations(self): diff --git a/selfdrive/controls/lib/turn_controller.py b/selfdrive/controls/lib/turn_controller.py index d02a81dcf17ad0..0e1a78be686e5d 100644 --- a/selfdrive/controls/lib/turn_controller.py +++ b/selfdrive/controls/lib/turn_controller.py @@ -5,7 +5,6 @@ from common.params import Params from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV -from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE _LON_MPC_STEP = 0.2 # Time stemp of longitudinal control (5 Hz) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index c00886deda2261..ee9cf23f0f731e 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -241,7 +241,7 @@ static void ui_draw_vision_speedlimit(UIState *s) { const float speed_offset = (s->scene.is_metric ? speedLimitOffset * 3.6 : speedLimitOffset * 2.2369363) + 0.5; auto speedLimitControlState = s->scene.controls_state.getSpeedLimitControlState(); - const bool force_active = s->scene.speed_limit_control_enabled && seconds_since_boot() < s->scene.last_speed_limit_sign_tap + 5.0; + const bool force_active = s->scene.speed_limit_control_enabled && seconds_since_boot() < s->scene.last_speed_limit_sign_tap + 2.0; const bool inactive = !force_active && (!s->scene.speed_limit_control_enabled || speedLimitControlState == cereal::ControlsState::SpeedLimitControlState::INACTIVE); const bool temp_inactive = !force_active && (s->scene.speed_limit_control_enabled && speedLimitControlState == cereal::ControlsState::SpeedLimitControlState::TEMP_INACTIVE); const int ring_alpha = inactive ? 100 : 255; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 861f0b17ea02c2..bf148aeb05d834 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -173,11 +173,6 @@ typedef struct UIState { // device state bool awake; - // speed limit controll state - bool speed_limit_control_enabled; - float speed_limit_perc_offset; - double last_speed_limit_sign_tap; - bool sidebar_collapsed; Rect video_rect, viz_rect; float car_space_transform[6]; From 740521485fc89a561ee0515c9dfbd50024064f2f Mon Sep 17 00:00:00 2001 From: alfhern Date: Wed, 28 Apr 2021 09:14:06 +0200 Subject: [PATCH 16/32] TurnController: Change turn controller curvature evaluation range to 20-150 --- selfdrive/controls/lib/turn_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/turn_controller.py b/selfdrive/controls/lib/turn_controller.py index 0e1a78be686e5d..18a1723bae1e39 100644 --- a/selfdrive/controls/lib/turn_controller.py +++ b/selfdrive/controls/lib/turn_controller.py @@ -22,8 +22,8 @@ _LEAVING_ACC = 0.0 # Allowed acceleration when leaving the turn. _EVAL_STEP = 5. # evaluate curvature every 5mts -_EVAL_START = 0. # start evaluating 0 mts ahead -_EVAL_LENGHT = 195. # evaluate curvature for 130mts +_EVAL_START = 20. # start evaluating 0 mts ahead +_EVAL_LENGHT = 150. # evaluate curvature for 150mts _EVAL_RANGE = np.arange(_EVAL_START, _EVAL_LENGHT, _EVAL_STEP) _MAX_JERK_ACC_INCREASE = 0.5 # Maximum jerk allowed when increasing acceleration. From ac49a9a3cfdf207c37c120cef3fd3a83f65f6af0 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 4 May 2021 14:24:59 +0200 Subject: [PATCH 17/32] TurnController: UI display of turn controller state --- selfdrive/controls/controlsd.py | 3 +- .../controls/lib/longitudinal_planner.py | 3 +- selfdrive/controls/lib/turn_controller.py | 71 +++++++++---------- selfdrive/ui/paint.cc | 18 +++-- selfdrive/ui/ui.hpp | 7 ++ 5 files changed, 56 insertions(+), 46 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 49f758754a4cf5..5ea7bb310b1498 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -525,11 +525,12 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.ufAccelCmd = float(self.LoC.pid.f) controlsState.vTargetLead = float(v_acc) controlsState.aTarget = float(a_acc) - controlsState.decelForModelDEPRECATED = self.sm['longitudinalPlan'].decelForTurnDEPRECATED controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter + controlsState.turnControllerState = self.sm['longitudinalPlan'].turnControllerState + controlsState.turnAcc = float(self.sm['longitudinalPlan'].turnAcc) controlsState.speedLimitControlState = self.sm['longitudinalPlan'].speedLimitControlState if self.CP.steerControlType == car.CarParams.SteerControlType.angle: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1dd3f12581cb84..c9a3c825c614bb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -239,7 +239,8 @@ def publish(self, sm, pm): longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource longitudinalPlan.fcw = self.fcw - longitudinalPlan.decelForTurnDEPRECATED = bool(self.turn_controller.is_active) + longitudinalPlan.turnControllerState = self.turn_controller.state + longitudinalPlan.turnAcc = float(self.turn_controller.a_turn) longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state longitudinalPlan.speedLimit = float(self.speed_limit_controller.speed_limit) longitudinalPlan.eventsDEPRECATED = self.events.to_msg() diff --git a/selfdrive/controls/lib/turn_controller.py b/selfdrive/controls/lib/turn_controller.py index 18a1723bae1e39..71f11abd6df8bd 100644 --- a/selfdrive/controls/lib/turn_controller.py +++ b/selfdrive/controls/lib/turn_controller.py @@ -1,6 +1,7 @@ import numpy as np import math from enum import Enum +from cereal import log from common.numpy_fast import interp from common.params import Params from common.realtime import sec_since_boot @@ -43,6 +44,8 @@ _TURNING_ACC_V = [0.5, -0.2, -0.4] # acc value _TURNING_ACC_BP = [1., 2., 3.] # absolute value of current lat acc +TurnControllerState = log.ControlsState.TurnControllerState + def eval_curvature(poly, x_vals): """ @@ -70,22 +73,15 @@ def lat_acc(curv): return np.vectorize(lat_acc)(x_curv) -class TurnState(Enum): - DISABLED = 1 - ENTERING = 2 - TURNING = 3 - LEAVING = 4 - - @property - def description(self): - if self == TurnState.DISABLED: - return 'DISABLED' - if self == TurnState.ENTERING: - return 'ENTERING' - if self == TurnState.TURNING: - return 'TURNING' - if self == TurnState.LEAVING: - return 'LEAVING' +def _description_for_state(turn_controller_state): + if turn_controller_state == TurnControllerState.disabled: + return 'DISABLED' + if turn_controller_state == TurnControllerState.entering: + return 'ENTERING' + if turn_controller_state == TurnControllerState.turning: + return 'TURNING' + if turn_controller_state == TurnControllerState.leaving: + return 'LEAVING' class TurnController(): @@ -99,13 +95,13 @@ def __init__(self, CP): self._last_params_update = 0.0 self._v_cruise_setpoint = 0.0 self._v_ego = 0.0 - self._state = TurnState.DISABLED + self._state = TurnControllerState.disabled self._reset() @property def v_turn_future(self): - return float(self._v_turn_future) if self.state != TurnState.DISABLED else self._v_cruise_setpoint + return float(self._v_turn_future) if self.state != TurnControllerState.disabled else self._v_cruise_setpoint @property def state(self): @@ -113,13 +109,13 @@ def state(self): @property def is_active(self): - return self._state != TurnState.DISABLED + return self._state != TurnControllerState.disabled @state.setter def state(self, value): if value != self._state: - print(f'TurnController state: {value.description}') - if value == TurnState.DISABLED: + print(f'TurnController state: {_description_for_state(value)}') + if value == TurnControllerState.disabled: self._reset() self._state = value @@ -149,8 +145,7 @@ def _update_calculations(self): path_poly = np.array([0., 0., 0., 0.]) pred_curvatures = eval_curvature(path_poly, _EVAL_RANGE) - max_pred_curvature_idx = np.argmax(pred_curvatures) - self._max_pred_curvature = pred_curvatures[max_pred_curvature_idx] + self._max_pred_curvature = np.amax(pred_curvatures) self._max_pred_lat_acc = self._v_ego**2 * self._max_pred_curvature a_lat_reg_max = interp(self._v_ego, _A_LAT_REG_MAX_BP, _A_LAT_REG_MAX_V) @@ -167,11 +162,11 @@ def _state_transition(self): # In any case, if system is disabled or the feature is disabeld or min braking param has been # set to non negative value, disable. if not self._op_enabled or not self._is_enabled or self._min_braking_acc >= 0.0: - self.state = TurnState.DISABLED + self.state = TurnControllerState.disabled return # DISABLED - if self.state == TurnState.DISABLED: + if self.state == TurnControllerState.disabled: # Do not enter a turn control cycle if speed is low. if self._v_ego <= _MIN_V: pass @@ -179,35 +174,35 @@ def _state_transition(self): # acceleration is predicted, then move to Entering turn state. elif self._max_pred_curvature >= _ENTERING_PRED_CURVATURE_TH \ and self._max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH: - self.state = TurnState.ENTERING + self.state = TurnControllerState.entering # ENTERING - elif self.state == TurnState.ENTERING: + elif self.state == TurnControllerState.entering: # Transition to Turning if current curvature over threshold. if self._current_curvature >= _TURNING_CURVATURE_TH: - self.state = TurnState.TURNING + self.state = TurnControllerState.turning # Abort if road straightens. elif self._max_pred_curvature < _ABORT_ENTERING_CURVATURE_TH: - self.state = TurnState.DISABLED + self.state = TurnControllerState.disabled # TURNING - elif self.state == TurnState.TURNING: + elif self.state == TurnControllerState.turning: # Transition to Leaving if current curvature under threshold. if self._current_curvature < _LEAVING_CURVATURE_TH: - self.state = TurnState.LEAVING + self.state = TurnControllerState.leaving # LEAVING - elif self.state == TurnState.LEAVING: + elif self.state == TurnControllerState.leaving: # Transition back to Turning if current curvature over threshold. if self._current_curvature >= _TURNING_CURVATURE_TH: - self.state = TurnState.TURNING + self.state = TurnControllerState.turning elif self._current_curvature < _FINISH_CURVATURE_TH: - self.state = TurnState.DISABLED + self.state = TurnControllerState.disabled def _update_solution(self): # Calculate target acceleration based on turn state. # DISABLED - if self.state == TurnState.DISABLED: + if self.state == TurnControllerState.disabled: a_target = self._a_ego # ENTERING - elif self.state == TurnState.ENTERING: + elif self.state == TurnControllerState.entering: entering_smooth_decel = interp(self._max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V) print(f'Overshooting {self._lat_acc_overshoot_ahead}, _entering_smooth_decel {entering_smooth_decel:.2f}') if self._lat_acc_overshoot_ahead: @@ -215,11 +210,11 @@ def _update_solution(self): else: a_target = entering_smooth_decel # TURNING - elif self.state == TurnState.TURNING: + elif self.state == TurnControllerState.turning: current_lat_acc = self._current_curvature * self._v_ego**2 a_target = interp(current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V) # LEAVING - elif self.state == TurnState.LEAVING: + elif self.state == TurnControllerState.leaving: a_target = _LEAVING_ACC # smooth out acceleration using jerk limits. diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index ee9cf23f0f731e..3dc9ea4f832d59 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -262,12 +262,18 @@ static void ui_draw_vision_speed(UIState *s) { } static void ui_draw_vision_event(UIState *s) { - if (s->scene.controls_state.getDecelForModelDEPRECATED() && s->scene.controls_state.getEnabled()) { - // draw winding road sign - const int img_turn_size = 96; - const int img_turn_x = s->viz_rect.right() - img_turn_size - bdr_s; - const int img_turn_y = s->viz_rect.y + (bdr_s * 1.5); - ui_draw_image(s, {img_turn_x, img_turn_y, img_turn_size, img_turn_size}, "trafficSign_turn", 1.0f); + auto turnControllerState = s->scene.controls_state.getTurnControllerState(); + if (turnControllerState > cereal::ControlsState::TurnControllerState::DISABLED && s->scene.controls_state.getEnabled()) { + // draw a rectangle with colors indicating the state with the value of the acceleration inside. + const int size = 184; + const Rect rect = {s->viz_rect.right() - size - bdr_s, int(s->viz_rect.y + (bdr_s * 1.5)), size, size}; + ui_fill_rect(s->vg, rect, COLOR_BLACK_ALPHA(100), 30.); + ui_draw_rect(s->vg, rect, tcs_colors[turnControllerState], 10, 20.); + const float turnAcc = s->scene.controls_state.getTurnAcc(); + char acc_str[16]; + snprintf(acc_str, sizeof(acc_str), "%.2f", turnAcc); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + ui_draw_text(s, rect.centerX(), rect.centerY(), acc_str, 48, COLOR_WHITE, "sans-bold"); } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index bf148aeb05d834..9645804d7dc88a 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -84,6 +84,13 @@ static std::map bg_colors = { {STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)}, }; +static std::map tcs_colors = { + {cereal::ControlsState::TurnControllerState::DISABLED, nvgRGBA(0x0, 0x0, 0x0, 0xff)}, + {cereal::ControlsState::TurnControllerState::ENTERING, COLOR_RED}, + {cereal::ControlsState::TurnControllerState::TURNING, COLOR_YELLOW}, + {cereal::ControlsState::TurnControllerState::LEAVING, nvgRGBA(0x17, 0x86, 0x44, 0xf1)}, +}; + typedef struct { float x, y; } vertex_data; From f4bf30ea0be6e85fc22a62cc66085142b36ff323 Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 26 Apr 2021 14:10:36 +0200 Subject: [PATCH 18/32] TurnSpeedController: Calculate turn speed sections based on spline interpolation of route path --- selfdrive/mapd/lib/NodesData.py | 126 ++++++++++++++++---------------- 1 file changed, 64 insertions(+), 62 deletions(-) diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py index 2d9069d71dbbf9..5c14988d293933 100644 --- a/selfdrive/mapd/lib/NodesData.py +++ b/selfdrive/mapd/lib/NodesData.py @@ -1,9 +1,11 @@ import numpy as np +from scipy import interpolate from enum import Enum from .geo import DIRECTION, R, vectors -_TURN_CURVATURE_THRESHOLD = 0.001 # 1/mts. A curvature over this value will generate a speed limit section. -_MAX_LAT_ACC = 1.5 # Maximum lateral acceleration in turns. +_TURN_CURVATURE_THRESHOLD = 0.002 # 1/mts. A curvature over this value will generate a speed limit section. +_MAX_LAT_ACC = 2.0 # Maximum lateral acceleration in turns. +_SPLINE_EVAL_STEP = 10 # mts for spline evaluation for curvature calculation def nodes_raw_data_array_for_wr(wr, drop_last=False): @@ -35,70 +37,79 @@ def node_calculations(points): # (N-1, 1) array. No distance for v[-1] d = np.linalg.norm(v, axis=1) - # Calculate angles between vectors when stack one after the other. - # https://math.stackexchange.com/questions/2610186/discrete-points-curvature-analysis - # (N-2, 1) array. v[0] and v[-1] have no angle - dot = np.sum(-v[:-1] * v[1:], axis=1) - a = np.arccos(dot / (d[:-1] * d[1:])) - - # Calculate the curvature from the circumcircle of a triangle - # https://www.mathopenref.com/trianglecircumcircle.html - # (N-2, 1) array. v[0] and v[-1] have no curvature - c = 2. * np.sin(a) / np.linalg.norm(v[:-1] + v[1:], axis=1) - # Calculate the bearing (from true north clockwise) for every node. # (N-1, 1) array. No bearing for v[-1] b = np.arctan2(v[:, 0], v[:, 1]) - # Pad the outputs to match the size of arrays to N - # Add origin to vector space. (i.e first node in list) v = np.concatenate(([[0., 0.]], v)) + # Provide distance to previous node and distance to next node dp = np.concatenate(([0.], d)) dn = np.concatenate((d, [0.])) - # Angles on edge nodes should be pi. i.e. a straight line. - a = np.concatenate(([[np.pi], a, [np.pi]])) - # Curvature on edges should be 0. i.e a straight line. - c = np.concatenate(([[0.], c, [0.]])) + # Bearing of last node should keep bearing from previous. b = np.concatenate((b, [b[-1]])) - return v, dp, dn, a, c, b + return v, dp, dn, b + + +def spline_curvature_calculations(vect, dist_prev): + """Provides an array of curvatures and its distances by applying a spline interpolation + to the path described by the nodes data. + """ + # create cumulative arrays for distance traveled and vector (x, y) + ds = np.cumsum(dist_prev, axis=0) + vs = np.cumsum(vect, axis=0) + + # spline interpolation + tck, u = interpolate.splprep([vs[:, 0], vs[:, 1]]) + + # evaluate every _SPLINE_EVAL_STEP mts. + n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u)) + unew = np.arange(0, n + 1) / n + + # get derivatives + d1 = interpolate.splev(unew, tck, der=1) + d2 = interpolate.splev(unew, tck, der=2) + + # calculate curvatures + num = d1[0] * d2[1] - d1[1] * d2[0] + den = (d1[0]**2 + d1[1]**2)**(1.5) + curv = np.abs(num / den) + curv_ds = unew * ds[-1] + + return curv, curv_ds def speed_limits_for_curvatures_data(curv, dist): """Provides the calculations for the speed limits from the curvatures array and distances, - by providing indexes to curvature sections and correspoinding speed limit values + by providing distances to curvature sections and correspoinding speed limit values """ - # Find where curvatures overshoot turn curvature threshold - overshoots = curv >= _TURN_CURVATURE_THRESHOLD - - # Speed section nodes are those that overshoot if a neighboring node also does. - overshoots = np.concatenate(([[0.], overshoots, [0.]])) - is_section = np.convolve(overshoots, np.ones(3), 'valid') >= 2 + # Find where curvatures overshoot turn curvature threshold and define as section + is_section = curv >= _TURN_CURVATURE_THRESHOLD # Find the indixes where the region starts is_section_ = np.concatenate(([False], is_section)) - idx_up = np.nonzero((is_section_[:-1] != is_section_[1:]) & is_section_[1:])[0] + idx_start = np.nonzero((is_section_[:-1] != is_section_[1:]) & is_section_[1:])[0] # Find the indexes where the sections end is_section_ = np.concatenate((is_section, [False])) - idx_down = np.nonzero((is_section_[:-1] != is_section_[1:]) & is_section_[:-1])[0] + idx_stop = np.nonzero((is_section_[:-1] != is_section_[1:]) & is_section_[:-1])[0] # Find the maximum curvature in the sections max_curvs = np.array([]) - for i in range(len(idx_up)): - if idx_up[i] < idx_down[i]: - max_curvs = np.append(max_curvs, np.amax(curv[idx_up[i]:idx_down[i]])) + for i in range(len(idx_start)): + if idx_start[i] < idx_stop[i]: + max_curvs = np.append(max_curvs, np.amax(curv[idx_start[i]:idx_stop[i]])) else: - max_curvs = np.append(max_curvs, curv[idx_up[i]]) + max_curvs = np.append(max_curvs, curv[idx_start[i]]) # Caclulate speed limit for confort on the section speed_limits = np.sqrt(_MAX_LAT_ACC / max_curvs) # Stack data and return - return np.column_stack((idx_up, idx_down, speed_limits)) + return np.column_stack((dist[idx_start], dist[idx_stop], speed_limits)) class SpeedLimitSection(): @@ -125,9 +136,7 @@ class NodeDataIdx(Enum): y = 5 # y value of cartesian vector representing the section between last node and this node. dist_prev = 6 # distance to previous node. dist_next = 7 # distance to next node - angle = 8 # angles between line segments coming into this node and leaving this node. - curvature = 9 # estimated curvature at this node. - bearing = 10 # bearing of the vector departing from this node. + bearing = 8 # bearing of the vector departing from this node. class NodesData: @@ -157,16 +166,17 @@ def __init__(self, way_relations): # Ensure we have more than 3 points, if not calculations are not possible. if len(points) < 3: return - vect, dist_prev, dist_next, angle, curvature, bearing = node_calculations(points) + vect, dist_prev, dist_next, bearing = node_calculations(points) # append calculations to nodes_data - # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, angle, curvature, bearing] - self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, angle, curvature, bearing)) + # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, bearing] + self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, bearing)) - # Store calculcations for curvature sections speed limits - # _curvature_speed_sections_data structure: [idx_up, idx_down, speed_limits] - dist = np.cumsum(dist_next, axis=0) - self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curvature, dist) + # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process. + # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits] + if len(vect) > 3: + curv, curv_ds = spline_curvature_calculations(vect, dist_prev) + self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curv, curv_ds) @property def count(self): @@ -232,26 +242,18 @@ def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahea if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None: return [] - # Find the cumulative distances from the current location - dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])) - dist = np.cumsum(dist, axis=0) - - # Get indexes and limits from data and adjust to ahead_idx - idx_up = self._curvature_speed_sections_data[:, 0] - ahead_idx - idx_down = self._curvature_speed_sections_data[:, 1] - ahead_idx - speed_limits = self._curvature_speed_sections_data[:, 2] + # Find the current distance traveled so far on the route. + dist_curr = np.cumsum(self.get(NodeDataIdx.dist_next)[:ahead_idx])[-1] - distance_to_node_ahead - # Create speed limits sections - limits_ahead = [] - for i in range(len(idx_up)): - up_idx = int(idx_up[i]) - down_idx = int(idx_down[i]) + # Filter the sections to get only those where the stop distance is ahead of current. + sec_filter = self._curvature_speed_sections_data[:, 1] > dist_curr + data = self._curvature_speed_sections_data[sec_filter] - if up_idx < 0: - if down_idx >= 0: - limits_ahead.append(SpeedLimitSection(0, dist[down_idx], speed_limits[i])) - continue + # Offset distances to current distance. + data[:, 0] -= dist_curr + data[:, 1] -= dist_curr - limits_ahead.append(SpeedLimitSection(dist[up_idx], dist[down_idx], speed_limits[i])) + # Create speed limits sections + limits_ahead = [SpeedLimitSection(max(0., d[0]), d[1], d[2]) for d in data] return limits_ahead From c77852fe0835f3e4af4c3dd3c5f065ecad068d58 Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 26 Apr 2021 19:37:05 +0200 Subject: [PATCH 19/32] TurnSpeedController: Implementation --- common/params_pyx.pyx | 1 + selfdrive/controls/controlsd.py | 2 + .../controls/lib/longitudinal_planner.py | 19 +- .../controls/lib/speed_limit_controller.py | 7 +- .../controls/lib/turn_speed_controller.py | 220 ++++++++++++++++++ selfdrive/controls/plannerd.py | 2 +- selfdrive/manager/manager.py | 3 +- selfdrive/mapd/lib/Route.py | 4 +- selfdrive/mapd/mapd.py | 49 ++-- selfdrive/ui/paint.cc | 18 ++ selfdrive/ui/qt/offroad/settings.cc | 6 + 11 files changed, 299 insertions(+), 32 deletions(-) create mode 100644 selfdrive/controls/lib/turn_speed_controller.py diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index fa304dece0dd59..76b34ba14b1744 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -73,6 +73,7 @@ keys = { b"Timezone": [TxType.PERSISTENT], b"TrainingVersion": [TxType.PERSISTENT], b"TurnVisionControl": [TxType.PERSISTENT], + b"TurnSpeedControl": [TxType.PERSISTENT], b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], b"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START], b"Version": [TxType.PERSISTENT], diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5ea7bb310b1498..0c51ff1950cca5 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -532,6 +532,8 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.turnControllerState = self.sm['longitudinalPlan'].turnControllerState controlsState.turnAcc = float(self.sm['longitudinalPlan'].turnAcc) controlsState.speedLimitControlState = self.sm['longitudinalPlan'].speedLimitControlState + controlsState.turnSpeed = float(self.sm['longitudinalPlan'].turnSpeed) + controlsState.turnSpeedControlState = self.sm['longitudinalPlan'].turnSpeedControlState if self.CP.steerControlType == car.CarParams.SteerControlType.angle: controlsState.lateralControlState.angleState = lac_log diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c9a3c825c614bb..9684edf738ff39 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,6 +16,7 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.turn_controller import TurnController from selfdrive.controls.lib.speed_limit_controller import SpeedLimitController +from selfdrive.controls.lib.turn_speed_controller import TurnSpeedController LON_MPC_STEP = 0.2 # first step is 0.2s @@ -67,7 +68,8 @@ def __init__(self, CP): self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.turn_controller = TurnController(CP) - self.speed_limit_controller = SpeedLimitController(CP) + self.speed_limit_controller = SpeedLimitController() + self.turn_speed_controller = TurnSpeedController() self.v_acc_start = 0.0 self.a_acc_start = 0.0 @@ -102,6 +104,8 @@ def choose_solution(self, v_cruise_setpoint, enabled): solutions['turn'] = self.turn_controller.v_turn if self.speed_limit_controller.is_active: solutions['limit'] = self.speed_limit_controller.v_limit + if self.turn_speed_controller.is_active: + solutions['turnlimit'] = self.turn_speed_controller.v_turn_limit slowest = min(solutions, key=solutions.get) @@ -122,12 +126,17 @@ def choose_solution(self, v_cruise_setpoint, enabled): elif slowest == 'limit': self.v_acc = self.speed_limit_controller.v_limit self.a_acc = self.speed_limit_controller.a_limit + elif slowest == 'turnlimit': + self.v_acc = self.turn_speed_controller.v_turn_limit + self.a_acc = self.turn_speed_controller.a_turn_limit self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) if self.turn_controller.is_active: self.v_acc_future = min(self.v_acc_future, self.turn_controller.v_turn_future) if self.speed_limit_controller.is_active: self.v_acc_future = min(self.v_acc_future, self.speed_limit_controller.v_limit_future) + if self.turn_speed_controller.is_active: + self.v_acc_future = min(self.v_acc_future, self.turn_speed_controller.v_turn_limit_future) def update(self, sm, CP): """Gets called when new radarState is available""" @@ -173,6 +182,9 @@ def update(self, sm, CP): # update speed limit solution calculation. self.speed_limit_controller.update(enabled, self.v_acc_start, self.a_acc_start, sm, v_cruise_setpoint, accel_limits_turns, jerk_limits, self.events) + # update turn speed solution calculation. + self.turn_speed_controller.update(enabled, self.v_acc_start, self.a_acc_start, sm, accel_limits_turns, + jerk_limits) else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) @@ -185,6 +197,7 @@ def update(self, sm, CP): self.v_cruise = reset_speed self.a_cruise = reset_accel self.speed_limit_controller.deactivate() # Deactivate speed limit controller to provide no solution. + self.turn_speed_controller.deactivate() # Deactivate turn speed controller to provide no solution. self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) @@ -241,6 +254,10 @@ def publish(self, sm, pm): longitudinalPlan.turnControllerState = self.turn_controller.state longitudinalPlan.turnAcc = float(self.turn_controller.a_turn) + + longitudinalPlan.turnSpeedControlState = self.turn_speed_controller.state + longitudinalPlan.turnSpeed = float(self.turn_speed_controller.speed_limit) + longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state longitudinalPlan.speedLimit = float(self.speed_limit_controller.speed_limit) longitudinalPlan.eventsDEPRECATED = self.events.to_msg() diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py index bf3c09e9c093c7..f342e579f14f22 100644 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -83,7 +83,7 @@ def _get_from_car_state(self): def _get_from_map_data(self): # Ignore if no live map data - sock = 'liveMapDataDEPRECATED' + sock = 'liveMapData' if self._sm.logMonoTime[sock] is None: self._results[SpeedLimitResolver.Key.map_data] = 0. _debug('SL: No map data for speed limit') @@ -95,7 +95,7 @@ def _get_from_map_data(self): next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. # Calculate the age of the gps fix. Ignore if too old. - gps_fix_age = time.time() - map_data.lastGps.timestamp * 1e-3 + gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 if gps_fix_age > _MAX_MAP_DATA_AGE: self._results[SpeedLimitResolver.Key.map_data] = 0. _debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}') @@ -163,14 +163,13 @@ def _consolidate(self): class SpeedLimitController(): - def __init__(self, CP): + def __init__(self): self._params = Params() self._resolver = SpeedLimitResolver() self._last_params_update = 0.0 self._is_metric = self._params.get("IsMetric", encoding='utf8') == "1" self._is_enabled = self._params.get("SpeedLimitControl", encoding='utf8') == "1" self._speed_limit_perc_offset = float(self._params.get("SpeedLimitPercOffset")) - self._CP = CP self._op_enabled = False self._active_jerk_limits = [0.0, 0.0] self._active_accel_limits = [0.0, 0.0] diff --git a/selfdrive/controls/lib/turn_speed_controller.py b/selfdrive/controls/lib/turn_speed_controller.py new file mode 100644 index 00000000000000..4d089ee6e2a8cb --- /dev/null +++ b/selfdrive/controls/lib/turn_speed_controller.py @@ -0,0 +1,220 @@ +import numpy as np +import time +from common.params import Params +from cereal import log +from common.realtime import sec_since_boot +from selfdrive.controls.lib.speed_smoother import speed_smoother + +_LON_MPC_STEP = 0.2 # Time stemp of longitudinal control (5 Hz) + +_MIN_ADAPTING_BRAKE_ACC = -1.5 # Minimum acceleration allowed when adapting to lower speed limit. +_MIN_ADAPTING_BRAKE_JERK = -1.0 # Minimum jerk allowed when adapting to lower speed limit. +_SPEED_OFFSET_TH = -3.0 # m/s Maximum offset between speed limit and current speed for adapting state. +_LIMIT_ADAPT_TIME_PER_MS = 1.0 # Ideal adapt time(s) to lower speed limit. i.e. braking for every m/s of speed delta. +_MIN_LIMIT_ADAPT_TIME = 5. # s, Minimum time to provide for adapting logic. +_MIN_SPEED_LIMIT = 11. # m/s, Minimum speed limit to provide as solution. + +_MAX_MAP_DATA_AGE = 10.0 # s Maximum time to hold to map data, then consider it invalid. + +_DEBUG = False + +TurnSpeedControlState = log.ControlsState.SpeedLimitControlState + + +def _debug(msg): + if not _DEBUG: + return + print(msg) + + +def _description_for_state(turn_speed_control_state): + if turn_speed_control_state == TurnSpeedControlState.inactive: + return 'INACTIVE' + if turn_speed_control_state == TurnSpeedControlState.adapting: + return 'ADAPTING' + if turn_speed_control_state == TurnSpeedControlState.active: + return 'ACTIVE' + + +class TurnSpeedController(): + def __init__(self): + self._params = Params() + self._last_params_update = 0.0 + self._is_enabled = self._params.get("TurnSpeedControl", encoding='utf8') == "1" + self._op_enabled = False + self._active_jerk_limits = [0.0, 0.0] + self._active_accel_limits = [0.0, 0.0] + self._adapting_jerk_limits = [_MIN_ADAPTING_BRAKE_JERK, 1.0] + self._v_ego = 0.0 + self._a_ego = 0.0 + + self._v_offset = 0.0 + self._speed_limit = 0.0 + self._state = TurnSpeedControlState.inactive + + self._next_speed_limit_prev = 0. + self._adapting_cycles = 0 + self._adapting_time = 0. + + self.v_turn_limit = 0.0 + self.a_turn_limit = 0.0 + self.v_turn_limit_future = 0.0 + + @property + def state(self): + return self._state + + @state.setter + def state(self, value): + if value != self._state: + _debug(f'Turn Speed Controller state: {_description_for_state(value)}') + + if value == TurnSpeedControlState.adapting: + self._adapting_cycles = 0 # Reset adapting state cycle count when entereing state. + # Adapting time must be calculated at the moment we enter adapting state. + self._adapting_time = abs(_LIMIT_ADAPT_TIME_PER_MS * self._v_offset) + + self._state = value + + @property + def is_active(self): + return self.state > TurnSpeedControlState.tempInactive + + @property + def speed_limit(self): + return max(self._speed_limit, _MIN_SPEED_LIMIT) if self._speed_limit > 0. else 0. + + def _get_limit_from_map_data(self, sm): + # Ignore if no live map data + sock = 'liveMapData' + if sm.logMonoTime[sock] is None: + _debug('TS: No map data for speed limit') + return 0. + + # Load limits from map_data + map_data = sm[sock] + speed_limit = 0. + next_speed_limit = map_data.turnSpeedLimitAhead if map_data.turnSpeedLimitAheadValid else 0. + + # Calculate the age of the gps fix. Ignore if too old. + gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3 + if gps_fix_age > _MAX_MAP_DATA_AGE: + _debug(f'TS: Ignoring map data as is too old. Age: {gps_fix_age}') + return 0. + + # Ensure current speed limit is considered only if we are inside the section. + if map_data.turnSpeedLimitValid and self._v_ego > 0.: + speed_limit_end_time = (map_data.turnSpeedLimitEndDistance / self._v_ego) - gps_fix_age + if speed_limit_end_time > 0.: + speed_limit = map_data.turnSpeedLimit + + # When we have no ahead speed limit to consider or it is greater than current speed limit + # or car has stopped, then provide current value and reset tracking. + if next_speed_limit == 0. or self._v_ego == 0. or (speed_limit > 0 and next_speed_limit > speed_limit): + self._next_speed_limit_prev = 0. + return speed_limit + + # When we have a next_speed_limit value that has not changed from a provided next speed limit value + # in previous resolutions, we keep providing it. + if next_speed_limit == self._next_speed_limit_prev: + return next_speed_limit + + # Reset tracking + self._next_speed_limit_prev = 0. + + # Calculate the time to the next speed limit and the adapt (braking) + next_speed_limit_time = (map_data.turnSpeedLimitAheadDistance / self._v_ego) - gps_fix_age + adapt_time = _LIMIT_ADAPT_TIME_PER_MS * (self._v_ego - max(next_speed_limit, _MIN_SPEED_LIMIT)) + + # When we detect we are close enough, we provide the next limit value and track it. + if next_speed_limit_time <= adapt_time: + self._next_speed_limit_prev = next_speed_limit + return next_speed_limit + + # Otherwise we just provide the calculated speed_limit + return speed_limit + + def _update_params(self): + time = sec_since_boot() + if time > self._last_params_update + 5.0: + self._is_enabled = self._params.get("TurnSpeedControl", encoding='utf8') == "1" + self._last_params_update = time + + def _update_calculations(self): + # Update current velocity offset (error) + self._v_offset = self.speed_limit - self._v_ego + + def _state_transition(self): + # In any case, if op is disabled, or speed limit control is disabled + # or the reported speed limit is 0, deactivate. + if not self._op_enabled or not self._is_enabled or self.speed_limit == 0.: + self.state = TurnSpeedControlState.inactive + return + + # inactive + if self.state == TurnSpeedControlState.inactive: + # If the limit speed offset is negative (i.e. reduce speed) and lower than threshold + # we go to adapting state to quickly reduce speed, otherwise we go directly to active + if self._v_offset < _SPEED_OFFSET_TH: + self.state = TurnSpeedControlState.adapting + else: + self.state = TurnSpeedControlState.active + # adapting + elif self.state == TurnSpeedControlState.adapting: + self._adapting_cycles += 1 + # Go to active once the speed offset is over threshold. + if self._v_offset >= _SPEED_OFFSET_TH: + self.state = TurnSpeedControlState.active + # active + elif self.state == TurnSpeedControlState.active: + # Go to adapting if the speed offset goes below threshold. + if self._v_offset < _SPEED_OFFSET_TH: + self.state = TurnSpeedControlState.adapting + + def _update_solution(self): + # inactive + if self.state == TurnSpeedControlState.inactive: + # Preserve values + self.v_turn_limit = self._v_ego + self.a_turn_limit = self._a_ego + self.v_turn_limit_future = self._v_ego + # adapting + elif self.state == TurnSpeedControlState.adapting: + # Calculate to adapt speed on target time. + adapting_time = max(self._adapting_time - self._adapting_cycles * _LON_MPC_STEP, 1.0) # min adapt time 1 sec. + a_target = (self.speed_limit - self._v_ego) / adapting_time + # smooth out acceleration using jerk limits. + j_limits = np.array(self._adapting_jerk_limits) + a_limits = self._a_ego + j_limits * _LON_MPC_STEP + a_target = max(min(a_target, a_limits[1]), a_limits[0]) + # calculate the solution values + self.a_turn_limit = max(a_target, _MIN_ADAPTING_BRAKE_ACC) # acceleration in next Longitudinal control step. + self.v_turn_limit = self._v_ego + self.a_turn_limit * _LON_MPC_STEP # speed in next Longitudinal control step. + self.v_turn_limit_future = max(self._v_ego + self.a_turn_limit * 4., self.speed_limit) # speed in 4 seconds. + # active + elif self.state == TurnSpeedControlState.active: + # Calculate following same cruise logic in planner.py + self.v_turn_limit, self.a_turn_limit = \ + speed_smoother(self._v_ego, self._a_ego, self.speed_limit, self._active_accel_limits[1], + self._active_accel_limits[0], self._active_jerk_limits[1], self._active_jerk_limits[0], + _LON_MPC_STEP) + self.v_turn_limit = max(self.v_turn_limit, 0.) + self.v_turn_limit_future = self.speed_limit + + def update(self, enabled, v_ego, a_ego, sm, accel_limits, jerk_limits): + self._op_enabled = enabled + self._v_ego = v_ego + self._a_ego = a_ego + self._active_accel_limits = accel_limits + self._active_jerk_limits = jerk_limits + + # Get the speed limit from Map Data + self._speed_limit = self._get_limit_from_map_data(sm) + + self._update_params() + self._update_calculations() + self._state_transition() + self._update_solution() + + def deactivate(self): + self.state = TurnSpeedControlState.inactive diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 3f49185d991ab7..c0ab34edce80dc 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -20,7 +20,7 @@ def plannerd_thread(sm=None, pm=None): lateral_planner = LateralPlanner(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlan', 'liveMapDataDEPRECATED'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'lateralPlan', 'liveMapData'], poll=['radarState', 'modelV2']) if pm is None: diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index efcf2d7e0e170d..7b257ed44489f3 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -39,9 +39,10 @@ def manager_init(): ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("MaxDecelerationForTurns", "-3.0"), ("OpenpilotEnabledToggle", "1"), - ("TurnVisionControl", "1"), ("SpeedLimitControl", "1"), ("SpeedLimitPercOffset", "10.0"), + ("TurnSpeedControl", "1"), + ("TurnVisionControl", "1"), ("VisionRadarToggle", "0"), ("IsDriverViewEnabled", "0"), ] diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index a0deeb8e3abfea..197a5ab7924a2c 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -158,7 +158,7 @@ def current_speed_limit(self): return limits_ahead[0].value @property - def current_curvature_speed_limit(self): + def current_curvature_speed_limit_section(self): if not self.located: return None @@ -166,7 +166,7 @@ def current_curvature_speed_limit(self): if not len(limits_ahead) or limits_ahead[0].start != 0: return None - return limits_ahead[0].value + return limits_ahead[0] @property def next_speed_limit_section(self): diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 31df7e80478bcb..337300bdf4c882 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import numpy as np from time import strftime, gmtime import cereal.messaging as messaging from common.realtime import Ratekeeper @@ -137,29 +136,33 @@ def publish(self, pm, sm): speed_limit = self.route.current_speed_limit next_speed_limit_section = self.route.next_speed_limit_section - current_curvature = self.route.immediate_curvature - curvatures_ahead = self.route._curvatures_ahead - curvatures_ahead = np.array([]) if curvatures_ahead is None else curvatures_ahead - next_subst_curvature = self.route.next_substantial_curvature + turn_speed_limit_section = self.route.current_curvature_speed_limit_section + next_turn_speed_limit_section = self.route.next_curvature_speed_limit_section - map_data_msg = messaging.new_message('liveMapDataDEPRECATED') + map_data_msg = messaging.new_message('liveMapData') map_data_msg.valid = sm.all_alive_and_valid(service_list=['gpsLocationExternal']) - map_data_msg.liveMapDataDEPRECATED.lastGps = self.last_gps - map_data_msg.liveMapDataDEPRECATED.speedLimitValid = bool(speed_limit is not None) - map_data_msg.liveMapDataDEPRECATED.speedLimit = float(speed_limit if speed_limit is not None else 0.0) - map_data_msg.liveMapDataDEPRECATED.speedLimitAheadValid = bool(next_speed_limit_section is not None) - map_data_msg.liveMapDataDEPRECATED.speedLimitAhead = float(next_speed_limit_section.value - if next_speed_limit_section is not None else 0.0) - map_data_msg.liveMapDataDEPRECATED.speedLimitAheadDistance = float(next_speed_limit_section.start - if next_speed_limit_section is not None else 0.0) - map_data_msg.liveMapDataDEPRECATED.curvatureValid = bool(current_curvature is not None) - map_data_msg.liveMapDataDEPRECATED.curvature = float(current_curvature if current_curvature is not None else 0.0) - map_data_msg.liveMapDataDEPRECATED.roadCurvatureX = [float(c[0]) for c in curvatures_ahead] - map_data_msg.liveMapDataDEPRECATED.roadCurvature = [float(c[1]) for c in curvatures_ahead] - map_data_msg.liveMapDataDEPRECATED.distToTurn = float(next_subst_curvature[0] - if next_subst_curvature is not None else 0.0) - - pm.send('liveMapDataDEPRECATED', map_data_msg) + + map_data_msg.liveMapData.lastGpsTimestamp = self.last_gps.timestamp + map_data_msg.liveMapData.speedLimitValid = bool(speed_limit is not None) + map_data_msg.liveMapData.speedLimit = float(speed_limit if speed_limit is not None else 0.0) + map_data_msg.liveMapData.speedLimitAheadValid = bool(next_speed_limit_section is not None) + map_data_msg.liveMapData.speedLimitAhead = float(next_speed_limit_section.value + if next_speed_limit_section is not None else 0.0) + map_data_msg.liveMapData.speedLimitAheadDistance = float(next_speed_limit_section.start + if next_speed_limit_section is not None else 0.0) + + map_data_msg.liveMapData.turnSpeedLimitValid = bool(turn_speed_limit_section is not None) + map_data_msg.liveMapData.turnSpeedLimit = float(turn_speed_limit_section.value + if turn_speed_limit_section is not None else 0.0) + map_data_msg.liveMapData.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end + if turn_speed_limit_section is not None else 0.0) + map_data_msg.liveMapData.turnSpeedLimitAheadValid = bool(next_turn_speed_limit_section is not None) + map_data_msg.liveMapData.turnSpeedLimitAhead = float(next_turn_speed_limit_section.value + if next_turn_speed_limit_section is not None else 0.0) + map_data_msg.liveMapData.turnSpeedLimitAheadDistance = float(next_turn_speed_limit_section.start + if next_turn_speed_limit_section is not None else 0.0) + + pm.send('liveMapData', map_data_msg) _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********') @@ -172,7 +175,7 @@ def mapd_thread(sm=None, pm=None): if sm is None: sm = messaging.SubMaster(['gpsLocationExternal']) if pm is None: - pm = messaging.PubMaster(['liveMapDataDEPRECATED']) + pm = messaging.PubMaster(['liveMapData']) while True: sm.update() diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 3dc9ea4f832d59..edbc0eb93546e2 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -253,6 +253,23 @@ static void ui_draw_vision_speedlimit(UIState *s) { } } +static void ui_draw_vision_turnspeed(UIState *s) { + const float turnSpeed = s->scene.controls_state.getTurnSpeed(); + + if (turnSpeed > 0.0 && s->scene.controls_state.getEnabled()) { + const int viz_maxspeed_h = 202; + const float sign_center_x = s->viz_rect.right() - bdr_s * 4 - speed_sgn_r * 3; + const float sign_center_y = s->viz_rect.y + viz_maxspeed_h / 2; + const float speed = (s->scene.is_metric ? turnSpeed * 3.6 : turnSpeed * 2.2369363) + 0.5; + + auto turnSpeedControlState = s->scene.controls_state.getTurnSpeedControlState(); + const bool inactive = turnSpeedControlState == cereal::ControlsState::SpeedLimitControlState::INACTIVE; + const int ring_alpha = inactive ? 100 : 255; + + ui_draw_speed_sign(s, sign_center_x, sign_center_y, speed_sgn_r, speed, 0, "sans-bold", ring_alpha, ring_alpha); + } +} + static void ui_draw_vision_speed(UIState *s) { const float speed = std::max(0.0, s->scene.car_state.getVEgo() * (s->scene.is_metric ? 3.6 : 2.2369363)); const std::string speed_str = std::to_string((int)std::nearbyint(speed)); @@ -355,6 +372,7 @@ static void ui_draw_vision_header(UIState *s) { ui_draw_vision_maxspeed(s); ui_draw_vision_speedlimit(s); ui_draw_vision_speed(s); + ui_draw_vision_turnspeed(s); ui_draw_vision_event(s); } diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 79f1c97f82d0bf..59a4225ba9ffef 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -78,6 +78,12 @@ QWidget * toggles_panel() { "Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits.", "../assets/offroad/icon_speed_limit.png" )); + toggles_list->addWidget(horizontal_line()); + toggles_list->addWidget(new ParamControl("TurnSpeedControl", + "Enable Map Data Turn Control", + "Use curvature info from map data to define speed limits to take turns ahead", + "../assets/offroad/icon_openpilot.png" + )); bool record_lock = Params().read_db_bool("RecordFrontLock"); record_toggle->setEnabled(!record_lock); From a4d443fd1e084e92218531600c228ed2327bbe20 Mon Sep 17 00:00:00 2001 From: alfhern Date: Fri, 7 May 2021 15:38:05 +0200 Subject: [PATCH 20/32] Turn Speed Controller: Update route building logic to use way index by edge nodes ids. --- selfdrive/mapd/lib/Route.py | 56 +++++--- selfdrive/mapd/lib/WayCollection.py | 10 +- selfdrive/mapd/lib/WayRelation.py | 110 ++++------------ selfdrive/mapd/lib/geo.py | 174 ++++++------------------ selfdrive/mapd/lib/test_geo.py | 196 +--------------------------- 5 files changed, 114 insertions(+), 432 deletions(-) diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index 197a5ab7924a2c..ddf3aac0643ffa 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -1,4 +1,5 @@ from .NodesData import NodesData, NodeDataIdx +from .geo import ref_vectors, R import numpy as np @@ -9,7 +10,7 @@ class Route(): """A set of consecutive way relations forming a default driving route. """ - def __init__(self, current, way_relations, way_collection_id): + def __init__(self, current, wr_index, way_collection_id): self.way_collection_id = way_collection_id self._ordered_way_relations = [] self._nodes_data = None @@ -19,22 +20,47 @@ def __init__(self, current, way_relations, way_collection_id): if not current.active: return - # We need a ref or a name to build a route. - ref = current.ref - name = current.name - # TODO: consider allowing to build a route when no ref or name is available. - # be aware of the time taken to search for matching ways as we build the route. - if ref is None and name is None: - return + # Build the route by finding iteratavely the best matching ways continuing after the end of the + # current (last_wr) way. Use the index to find the continuation posibilities on each iteration. + last_wr = current + while True: + # - Append current element to the route list of ordered way relations. + self._ordered_way_relations.append(last_wr) + + # Get the id of the node at the end of the way. + last_node_id = last_wr.last_node.id + + # Get the way relations that share the end node id from the index + way_relations = wr_index[last_node_id] + + # if no more way_relations than last_wr, we got to the end. + if len(way_relations) == 1: + break + + # Get the coordinates for the edge node + ref_point = last_wr.last_node_coordinates + + # Get the array of coordinaes for the nodes following edge node on each of the common way relations. + points = np.array(list(map(lambda wr: wr.node_before_edge_coordinates(last_node_id), way_relations))) + + # Get the vectors in cartesian plane for the end sections of each way. + v = ref_vectors(ref_point, points) * R + + # - Calculate the bearing (from true north clockwise) for every end section of each way. + b = np.arctan2(v[:, 0], v[:, 1]) + + # - Find index of las_wr section and calculate deltas of bearings to the other sections. + last_wr_idx = way_relations.index(last_wr) + b_ref = b[last_wr_idx] + delta = b - b_ref - # Reduce way relations to those matching the ref or name of the current one. - way_relations = list(filter(lambda wr: wr.has_name_or_ref(name, ref), way_relations)) + # - The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent + # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi and -pi. + best_idx = np.argmin(np.cos(delta)) - # Build the ordered way relations list by recursively finding the next wr. - wr = current - while wr is not None: - self._ordered_way_relations.append(wr) - wr, way_relations = wr.next_wr(way_relations) + # - Select next way and update its direction before continuing to next iteration. + last_wr = way_relations[best_idx] + last_wr.update_direction_from_starting_node(last_node_id) # Build the node data from the ordered list of way relations self._nodes_data = NodesData(self._ordered_way_relations) diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py index 69164af279e2c6..7def068128b49c 100644 --- a/selfdrive/mapd/lib/WayCollection.py +++ b/selfdrive/mapd/lib/WayCollection.py @@ -13,6 +13,12 @@ def __init__(self, ways): self.id = uuid.uuid4() self.way_relations = list(map(lambda way: WayRelation(way), ways)) + # Create the index by edge node ids. + self.wr_index = {} + for wr in self.way_relations: + for node_id in wr.edge_nodes_ids: + self.wr_index[node_id] = self.wr_index.get(node_id, []) + [wr] + def get_route(self, location, bearing): """Provides the best route found in the way collection based on provided `location` and `bearing` """ @@ -36,7 +42,7 @@ def get_route(self, location, bearing): # If more than one is active, filter out any active way relation where the bearing delta indicator is too high. else: - wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND, + wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND, active_way_relations)) # If delta bearing indicator is too high for all, then use as current the one that has the shorter one. @@ -58,4 +64,4 @@ def get_route(self, location, bearing): if wr.id != current.id: wr.reset_location_variables() - return Route(current, self.way_relations, self.id) + return Route(current, self.wr_index, self.id) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index 1ca84be53a8edb..f0b8f581e60f7c 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -1,4 +1,4 @@ -from .geo import DIRECTION, R, vectors +from .geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points from selfdrive.config import Conversions as CV from datetime import datetime import numpy as np @@ -121,27 +121,6 @@ def conditional_speed_limit_for_osm_tag_limit_string(limit_string): return 0. -def bearing_to_points(point, points): - """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each - one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud, - longitude pair in radians. - """ - delta = points - point - x = np.sin(delta[:, 1]) * np.cos(points[:, 0]) - y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1])) - return np.arctan2(x, y) - - -def distance_to_points(point, points): - """Calculate the distance of the vectors between `point` and each one of the entries in `points`. - Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians. - """ - delta = points - point - a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2 - c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a)) - return c * R - - class WayRelation(): """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle. """ @@ -160,12 +139,19 @@ def __init__(self, way, location=None, bearing=None): self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING, np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING)) + # Get the edge nodes ids. + self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id] + if location is not None and bearing is not None: self.update(location, bearing) def __repr__(self): - return f'(id: {self.id}, name: {self.name}, ref: {self.ref}, ahead: {self.ahead_idx}, \ - behind: {self.behind_idx}, {self.direction}, active: {self.active})' + return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})' + + def __eq__(self, other): + if isinstance(other, WayRelation): + return self.id == other.id + return False def reset_location_variables(self): self.location = None @@ -266,9 +252,9 @@ def update(self, location, bearing): def update_direction_from_starting_node(self, start_node_id): self._speed_limit = None - if self.way.nodes[0].id == start_node_id: + if self.edge_nodes_ids[0] == start_node_id: self.direction = DIRECTION.FORWARD - elif self.way.nodes[-1].id == start_node_id: + elif self.edge_nodes_ids[-1] == start_node_id: self.direction = DIRECTION.BACKWARD else: self.direction = DIRECTION.NONE @@ -312,14 +298,6 @@ def speed_limit(self): self._speed_limit = limit return self._speed_limit - @property - def ref(self): - return self.way.tags.get("ref", None) - - @property - def name(self): - return self.way.tags.get("name", None) - @property def active_bearing_delta(self): """Returns the sine of the delta between the current location bearing and the exact @@ -351,61 +329,23 @@ def last_node(self): return self.way.nodes[0] return None - def edge_on_node(self, node_id): - """Indicates if the associated way starts or ends in the node with `node_id` + @property + def last_node_coordinates(self): + """Returns the coordinates for the last node on the way considering the traveling direction. (in radians) """ - return self.way.nodes[0].id == node_id or self.way.nodes[-1].id == node_id + if self.direction == DIRECTION.FORWARD: + return self._nodes_np[-1] + if self.direction == DIRECTION.BACKWARD: + return self._nodes_np[0] + return None def node_before_edge_coordinates(self, node_id): - """Returns the coordinates of the node before the edge node identifeid with `node_id` + """Returns the coordinates of the node before the edge node identifeid with `node_id`. (in radians) """ - if self.way.nodes[0].id == node_id: - return np.array([self.way.nodes[1].lat, self.way.nodes[1].lon], dtype=float) + if self.edge_nodes_ids[0] == node_id: + return self._nodes_np[1] - if self.way.nodes[-1].id == node_id: - return np.array([self.way.nodes[-2].lat, self.way.nodes[-2].lon], dtype=float) + if self.edge_nodes_ids[-1] == node_id: + return self._nodes_np[-2] return np.array([0., 0.]) - - def next_wr(self, way_relations): - """Returns a tuple with the next way relation (if any) based on `location` and `bearing` and - the `way_relations` list excluding the found next way relation. (to help with recursion) - """ - if self.direction not in [DIRECTION.FORWARD, DIRECTION.BACKWARD]: - return None, way_relations - - def continuation_factor(next_wr): - """Indicates how much the `next_wr` looks like a straight continuation of the current one. - A min value of `0` indicates the `next_wr` continues with the exact same bearing as current. - A max value of `2` indicates the `next_wr` continues in the complete oposite direction to current. - """ - ref_point = np.array([self.last_node.lat, self.last_node.lon], dtype=float) - adjacent_points = np.row_stack((self.node_before_edge_coordinates(self.last_node.id), - next_wr.node_before_edge_coordinates(self.last_node.id))) - bearings = bearing_to_points(np.radians(ref_point), np.radians(adjacent_points)) - delta = np.diff(bearings)[0] - return np.cos(delta) + 1 - - possible_next_wr = list(filter(lambda wr: wr.id != self.id and wr.edge_on_node(self.last_node.id), way_relations)) - possible_next_wr.sort(key=lambda wr: continuation_factor(wr)) - possibles = len(possible_next_wr) - - if possibles == 0: - return None, way_relations - - if possibles == 1 or (self.ref is None and self.name is None): - next_wr = possible_next_wr[0] - else: - next_wr = next((wr for wr in possible_next_wr if wr.has_name_or_ref(self.name, self.ref)), possible_next_wr[0]) - - next_wr.update_direction_from_starting_node(self.last_node.id) - updated_way_relations = list(filter(lambda wr: wr.id != next_wr.id, way_relations)) - - return next_wr, updated_way_relations - - def has_name_or_ref(self, name, ref): - if ref is not None and self.ref is not None and self.ref == ref: - return True - if name is not None and self.name is not None and self.name == name: - return True - return False diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py index a02377d1b97db4..344d43f600aa02 100644 --- a/selfdrive/mapd/lib/geo.py +++ b/selfdrive/mapd/lib/geo.py @@ -1,4 +1,4 @@ -from math import sin, cos, sqrt, atan2, radians, degrees +from math import sin, cos, sqrt, atan2, radians from enum import Enum import numpy as np @@ -24,6 +24,43 @@ def vectors(points): return np.column_stack((x, y)) +def ref_vectors(ref, points): + """Provides a array of vectors on cartesian space (x, y). + Each vector represents the path from ref to a point in `points`. + `points` must by a (N, 2) array of [lat, lon] pairs in radians. + """ + latA = ref[0] + latB = points[:, 0] + delta = points - ref + dlon = delta[:, 1] + + x = np.sin(dlon) * np.cos(latB) + y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon)) + + return np.column_stack((x, y)) + + +def bearing_to_points(point, points): + """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each + one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud, + longitude pair in radians. + """ + delta = points - point + x = np.sin(delta[:, 1]) * np.cos(points[:, 0]) + y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1])) + return np.arctan2(x, y) + + +def distance_to_points(point, points): + """Calculate the distance of the vectors between `point` and each one of the entries in `points`. + Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians. + """ + delta = points - point + a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2 + c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a)) + return c * R + + def coord_to_rad(point): """Tranform coordinates in degrees to radians """ @@ -53,141 +90,6 @@ def _distance_from_rad(point_a_in_rad, point_b_in_rad): return R * c -def bearing(point_a, point_b): - """Calculate the angle in degrees between to true north and a line joining two points expresed - in coordinates in degrees (lat, lon) - """ - point_a_in_rad = coord_to_rad(point_a) - point_b_in_rad = coord_to_rad(point_b) - return _bearing_from_rad(point_a_in_rad, point_b_in_rad) - - -def xy(ref_point, point): - """Calculates the approximated x y cartesian coordinates for a given coordiante `point` (lat, lon in degrees) - in reference to a reference point `ref_point` - """ - point_a_in_rad = coord_to_rad(ref_point) - point_b_in_rad = coord_to_rad(point) - return _xy_from_rad(point_a_in_rad, point_b_in_rad) - - -def _x_y_bearing_from_rad(point_a_in_rad, point_b_in_rad): - """Calculates the approximated x y cartesian coordinates (in mts) and the bearing angle (in degrees) - for a given coordiante `point_b_in_rad` (lat, lon in radians) in reference to a - reference point `point_a_in_rad` (lat, lon in radians) - """ - (latA, lonA) = point_a_in_rad - (latB, lonB) = point_b_in_rad - - dlon = lonB - lonA - - x = sin(dlon) * cos(latB) - y = cos(latA) * sin(latB) - (sin(latA) * cos(latB) * cos(dlon)) - bearing = degrees(atan2(x, y)) - return x * R, y * R, (bearing + 360) % 360 - - -def _bearing_from_rad(point_a_in_rad, point_b_in_rad): - """Calculate the angle in degrees between to true north and a line joining two points expresed - in coordinates in radians (lat, lon) - """ - _, _, bearing = _x_y_bearing_from_rad(point_a_in_rad, point_b_in_rad) - return (bearing + 360) % 360 - - -def _xy_from_rad(point_a_in_rad, point_b_in_rad): - """Calculates the approximated x y cartesian coordinates for a given coordiante `point_b_in_rad` (lat, lon in radians) - in reference to a reference point `point_a_in_rad` (lat, lon in radians) - """ - x, y, _ = _x_y_bearing_from_rad(point_a_in_rad, point_b_in_rad) - return x, y - - -def distance_and_bearing(point_a, point_b): - """ Provides distance and bearing calucations between two points in a single method call. see `distance` and - `bearing` for details. - """ - point_a_in_rad = coord_to_rad(point_a) - point_b_in_rad = coord_to_rad(point_b) - - return _distance_from_rad(point_a_in_rad, point_b_in_rad), _bearing_from_rad(point_a_in_rad, point_b_in_rad) - - -def bearing_delta(bearing_a, bearing_b): - """Returns the angle difference in degrees between two bearing angles (in degrees) - """ - return (bearing_a - bearing_b + 180) % 360 - 180 - - -def absoule_delta_with_direction(delta): - """Takes a `bearing_delta` and provides its absolute value ignoring its direction. The direction is then - provided as an additional element on the result tuple. - If delta is between -90 and 90, direction is AHEAD, between 90 and 270 is BEHIND. - """ - delta_ahead = abs(bearing_delta(delta, 0.)) - delta_behind = abs(delta_ahead - 180) - - if delta_ahead < delta_behind: - return (delta_ahead, DIRECTION.AHEAD) - elif delta_ahead > delta_behind: - return (delta_behind, DIRECTION.BEHIND) - else: - return (delta_ahead, DIRECTION.NONE) - - -def three_point_curvature_alt(ref, prev, next): - # https://math.stackexchange.com/questions/2507540/numerical-way-to-solve-for-the-curvature-of-a-curve - # https://en.wikipedia.org/wiki/Heron%27s_formula - prev_r = (prev[0] - ref[0], prev[1] - ref[1]) - next_r = (next[0] - ref[0], next[1] - ref[1]) - - prev_ang = atan2(prev_r[0], prev_r[1]) - next_ang = atan2(next_r[0], next_r[1]) - a = CURVATURE_OFFSET - b = CURVATURE_OFFSET - - prev_n = (a * cos(prev_ang), a * sin(prev_ang)) - next_n = (b * cos(next_ang), b * sin(next_ang)) - - c = xy_distance(next_n, prev_n) - s = (a + b + c) / 2. - A = sqrt(s * (s - a) * (s - b) * (s - c)) - - return 4 * A / (a * b * c) - - -def three_point_tangent_angle(prev, ref, next): - """Angle (in readians) of the tangent line formed by three points in sequence `prev`, `ref`, `next` - """ - # https://www.math24.net/curvature-radius - prev_vec = (ref[0] - prev[0], ref[1] - prev[1]) - next_vec = (next[0] - ref[0], next[1] - ref[1]) - avg_vec = ((prev_vec[0] + next_vec[0]) / 2., (prev_vec[1] + next_vec[1]) / 2.) - - return atan2(avg_vec[1], avg_vec[0]) - - -def three_point_curvature(prev_xy, ref_xy, next_xy, prev_tan, ref_tan, next_tan): - """Aproximated curvature for a line joining 3 points with calculated tangent angles. Aproximation by - averaging the variation of the tangent angle over distance. - """ - prev_tan_delta = ref_tan - prev_tan if prev_tan is not None else 0 - prev_dist = min(xy_distance(prev_xy, ref_xy), MAX_DIST_FOR_CURVATURE) - next_tan_delta = next_tan - ref_tan if next_tan is not None else 0 - next_dist = min(xy_distance(next_xy, ref_xy), MAX_DIST_FOR_CURVATURE) - - prev_curv = prev_tan_delta / prev_dist - next_curv = next_tan_delta / next_dist - - return (prev_curv + next_curv) / 2. - - -def xy_distance(A, B): - """Distance between two point on a cartesian plane. - """ - return sqrt((A[0] - B[0])**2 + (A[1] - B[1])**2) - - class DIRECTION(Enum): NONE = 0 AHEAD = 1 diff --git a/selfdrive/mapd/lib/test_geo.py b/selfdrive/mapd/lib/test_geo.py index 53a5c7acb71f06..f035cf0fe640e9 100644 --- a/selfdrive/mapd/lib/test_geo.py +++ b/selfdrive/mapd/lib/test_geo.py @@ -1,8 +1,7 @@ import unittest from decimal import Decimal -from math import pi, sqrt -from .geo import coord_to_rad, distance, bearing, xy, distance_and_bearing, bearing_delta, DIRECTION, \ - absoule_delta_with_direction, three_point_tangent_angle, three_point_curvature, xy_distance +from math import pi +from .geo import coord_to_rad class TestMapsdGeoLibrary(unittest.TestCase): @@ -21,197 +20,6 @@ def test_coord_to_rad(self): rad_tuples = list(map(lambda p: coord_to_rad(p), points)) self.assertEqual(rad_tuples, expected) - # 1. test distance calculation between two points in coordiantes. - def test_distance(self): - a = (Decimal(0.), Decimal(0.)) - b = (Decimal(0.1), Decimal(0.1)) - c = (Decimal(0.01), Decimal(0.01)) - d = (Decimal(-0.01), Decimal(-0.01)) - - dist1 = distance(a, b) - dist2 = distance(a, c) - dist3 = distance(b, c) - dist4 = distance(a, d) - - self.assertAlmostEqual(dist1, 15730, 0) - self.assertAlmostEqual(dist2, 1573, 0) - self.assertAlmostEqual(dist3, 14157, 0) - self.assertAlmostEqual(dist4, 1573, 0) - - # 2. Test bearing between two points - def test_bearing(self): - ref_point = (0., 0.) - points = [ - (0., 1.), - (0., -1.), - (-1., 0.), - (1., 0.), - ] - expected = [ - 90., - 270., - 180., - 0., - ] - bearings = list(map(lambda p: bearing(ref_point, p), points)) - self.assertEqual(bearings, expected) - - # 3. Test cartesian coordinates from lat lon coordinates - def test_xy(self): - ref_point = (1., 1.) - points = [ - (1., 1.01), - (1., 0.99), - (0.99, 1.), - (1.01, 1.), - ] - expected = [ - (1112., 0), - (-1112, 0), - (0, -1112), - (0, 1112), - ] - xys = list(map(lambda p: xy(ref_point, p), points)) - self._assertAlmostEqualListOfTuples(xys, expected) - - # 4. Test distance and bearing combined method - def test_distance_and_bearing(self): - a = (Decimal(0.), Decimal(0.)) - b = (Decimal(0.01), Decimal(0.01)) - dist, bearing = distance_and_bearing(a, b) - - self.assertAlmostEqual(dist, 1573, 0) - self.assertAlmostEqual(bearing, 45, 0) - - # 5. Test bearing delta - def test_bearing_delta(self): - a = 0 - b = 90 - c = 180 - d = 270 - - deltas = [ - bearing_delta(a, b), - bearing_delta(a, c), - bearing_delta(a, d), - bearing_delta(b, a), - bearing_delta(b, c), - bearing_delta(d, b), - ] - expected = [ - -90, - -180, - 90, - 90, - -90, - -180 - ] - - self.assertEqual(deltas, expected) - - # 6. Test absolute bearing delta with direction info - def test_absoule_delta_with_direction(self): - deltas = [ - 0, - 45, - -45, - 89, - -89, - 90, - -90, - 91, - -91, - 135, - -135, - 180, - 360 - ] - expected = [ - (0, DIRECTION.AHEAD), - (45, DIRECTION.AHEAD), - (45, DIRECTION.AHEAD), - (89, DIRECTION.AHEAD), - (89, DIRECTION.AHEAD), - (90, DIRECTION.NONE), - (90, DIRECTION.NONE), - (89, DIRECTION.BEHIND), - (89, DIRECTION.BEHIND), - (45, DIRECTION.BEHIND), - (45, DIRECTION.BEHIND), - (0, DIRECTION.BEHIND), - (0, DIRECTION.AHEAD), - ] - - d_and_d = list(map(lambda d: absoule_delta_with_direction(d), deltas)) - self.assertEqual(d_and_d, expected) - - # 7. Test tangent angle estimation from three points - def test_three_point_tangent_angle(self): - a = (0.99, 0.99) - b = (0.99, 1.01) - c = (1.01, 1.01) - d = (1.01, 0.99) - - angles = [ - three_point_tangent_angle(a, b, c), - three_point_tangent_angle(b, c, d), - three_point_tangent_angle(c, d, a), - three_point_tangent_angle(d, a, b), - ] - expected = [ - pi / 4., - -pi / 4., - -3 * pi / 4., - 3 * pi / 4., - ] - - self.assertEqual(angles, expected, 4) - - # 8. Test the curvature estimation from three points - def test_three_point_curvature(self): - data = [ - ((0., 10.), (2.5, 12.5), (5., 15.), 3 * pi / 8., pi / 4., pi / 8.), - ((0., 10.), (-2.5, 12.5), (-5., 15.), 5 * pi / 8., 3 * pi / 4., 7 * pi / 8.), - ((0., -10.), (-2.5, -12.5), (-5., -15.), 11 * pi / 8., 5 * pi / 4., 9 * pi / 8.), - ((0., -10.), (2.5, -12.5), (5., -15.), -3 * pi / 8., -pi / 4., -pi / 8.), - ] - - curvatures = list(map(lambda d: three_point_curvature(d[0], d[1], d[2], d[3], d[4], d[5]), data)) - expected = [ - -0.11107, - 0.11107, - -0.11107, - 0.11107, - ] - - self._assertAlmostEqualList(curvatures, expected, 3) - - # 9. Test cartesian distance - def test_xy_distance(self): - v = sqrt(50) - a = (v, v) - b = (-v, -v) - c = (-v, v) - - distances = [ - xy_distance(a, b), - xy_distance(a, c), - xy_distance(b, c), - xy_distance(c, b), - xy_distance(c, a), - xy_distance(b, a), - ] - expected = [ - 20, - 2 * v, - 2 * v, - 2 * v, - 2 * v, - 20 - ] - - self.assertEqual(distances, expected) - # Helpers def _assertAlmostEqualList(self, a, b, places=0): for idx, el_a in enumerate(a): From 86d860e61edf7327918cb1ecece3cb98c7cb30d2 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 11 May 2021 09:36:36 +0200 Subject: [PATCH 21/32] LiveMapData: fix issue with going on wrong direction --- selfdrive/mapd/lib/Route.py | 22 +++++++++++++++++++--- selfdrive/mapd/lib/WayCollection.py | 26 +++++++++++--------------- selfdrive/mapd/lib/WayRelation.py | 8 ++++++++ 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index ddf3aac0643ffa..34144a15243344 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -1,6 +1,7 @@ from .NodesData import NodesData, NodeDataIdx from .geo import ref_vectors, R import numpy as np +from itertools import compress _DISTANCE_LIMIT_FOR_CURRENT_CURVATURE = 20. # mts @@ -40,7 +41,7 @@ def __init__(self, current, wr_index, way_collection_id): # Get the coordinates for the edge node ref_point = last_wr.last_node_coordinates - # Get the array of coordinaes for the nodes following edge node on each of the common way relations. + # Get the array of coordinates for the nodes following edge node on each of the common way relations. points = np.array(list(map(lambda wr: wr.node_before_edge_coordinates(last_node_id), way_relations))) # Get the vectors in cartesian plane for the end sections of each way. @@ -54,13 +55,28 @@ def __init__(self, current, wr_index, way_collection_id): b_ref = b[last_wr_idx] delta = b - b_ref + # - Update the direction of the possible continuation ways excluding the last_wr + for idx, wr in enumerate(way_relations): + if idx != last_wr_idx: + wr.update_direction_from_starting_node(last_node_id) + + # - Filter the possible continuation way relations: + # - exclude last_wr + # - exclude all way relations that are prohibited due to traffic direction. + mask = [idx != last_wr_idx and not wr.is_prohibited for idx, wr in enumerate(way_relations)] + way_relations = list(compress(way_relations, mask)) + delta = delta[mask] + + # if no options left, we got to the end. + if len(way_relations) == 0: + break + # - The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi and -pi. best_idx = np.argmin(np.cos(delta)) - # - Select next way and update its direction before continuing to next iteration. + # - Select next way. last_wr = way_relations[best_idx] - last_wr.update_direction_from_starting_node(last_node_id) # Build the node data from the ordered list of way relations self._nodes_data = NodesData(self._ordered_way_relations) diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py index 7def068128b49c..773e194f9becdc 100644 --- a/selfdrive/mapd/lib/WayCollection.py +++ b/selfdrive/mapd/lib/WayCollection.py @@ -29,26 +29,27 @@ def get_route(self, location, bearing): for wr in self.way_relations: wr.update(location, bearing) - # Get the way relations where a match was found. i.e. those now marked as active. - active_way_relations = list(filter(lambda wr: wr.active, self.way_relations)) + # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of + # travel is valid. + valid_way_relations = list(filter(lambda wr: wr.active and not wr.is_prohibited, self.way_relations)) # If no active, then we could not find a current way to build a route. - if len(active_way_relations) == 0: + if len(valid_way_relations) == 0: return None - # If only one active, then pick it as current. - if len(active_way_relations) == 1: - current = active_way_relations[0] + # If only one valid, then pick it as current. + if len(valid_way_relations) == 1: + current = valid_way_relations[0] - # If more than one is active, filter out any active way relation where the bearing delta indicator is too high. + # If more than one is valid, filter out any valid way relation where the bearing delta indicator is too high. else: wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND, - active_way_relations)) + valid_way_relations)) # If delta bearing indicator is too high for all, then use as current the one that has the shorter one. if len(wr_acceptable_bearing) == 0: - active_way_relations.sort(key=lambda wr: wr.active_bearing_delta) - current = active_way_relations[0] + valid_way_relations.sort(key=lambda wr: wr.active_bearing_delta) + current = valid_way_relations[0] # If only one with acceptable bearing, use it. elif len(wr_acceptable_bearing) == 1: @@ -59,9 +60,4 @@ def get_route(self, location, bearing): wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way) current = wr_acceptable_bearing[0] - # Reset location for the remaining located way relations - for wr in active_way_relations: - if wr.id != current.id: - wr.reset_location_variables() - return Route(current, self.wr_index, self.id) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index f0b8f581e60f7c..6e35e88d8a8003 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -305,6 +305,14 @@ def active_bearing_delta(self): """ return self._active_bearing_delta + @property + def is_one_way(self): + return self.way.tags.get("oneway") in ['yes'] or self.way.tags.get("highway") in ["motorway"] + + @property + def is_prohibited(self): + return self.is_one_way and self.direction == DIRECTION.BACKWARD + @property def distance_to_way(self): """Returns the perpendicular (i.e. minimum) distance between current location and the way From 0a4712d43b23e7e0c0991b8bcd1c6ed2a56de1b6 Mon Sep 17 00:00:00 2001 From: alfhern Date: Wed, 12 May 2021 16:38:53 +0200 Subject: [PATCH 22/32] LiveMapData: Fix loop route issue --- selfdrive/mapd/lib/Route.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index 34144a15243344..ac0972e0568ff7 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -25,6 +25,11 @@ def __init__(self, current, wr_index, way_collection_id): # current (last_wr) way. Use the index to find the continuation posibilities on each iteration. last_wr = current while True: + # - Make sure the last_wr is not the same as the first one in the ordered list as to prevent circle routes + # to loop forever. + if len(self._ordered_way_relations) > 0 and last_wr.id == self._ordered_way_relations[0].id: + break + # - Append current element to the route list of ordered way relations. self._ordered_way_relations.append(last_wr) From 78afb67a16e914935c60ec217b947599d130f459 Mon Sep 17 00:00:00 2001 From: alfhern Date: Wed, 12 May 2021 10:41:04 +0200 Subject: [PATCH 23/32] LiveMapData: Query OSM without blocking mapd execution --- selfdrive/mapd/mapd.py | 103 +++++++++++++++++++++++++++++------------ 1 file changed, 74 insertions(+), 29 deletions(-) diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 337300bdf4c882..9e1fdde6fb839d 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import threading from time import strftime, gmtime import cereal.messaging as messaging from common.realtime import Ratekeeper @@ -20,6 +21,13 @@ def _debug(msg): print(msg) +def excepthook(args): + _debug(f'MapD: Threading exception:\n{args}') + + +threading.excepthook = excepthook + + class MapD(): def __init__(self): self.osm = OSM() @@ -36,6 +44,8 @@ def __init__(self): self.last_fetch_location = None self.last_route_update_fix_timestamp = 0 self.last_publish_fix_timestamp = 0 + self._query_thread = None + self._lock = threading.Lock() @property def location(self): @@ -69,6 +79,36 @@ def update_gps(self, sm): f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' f'*******') + def _query_osm_not_blocking(self): + def query(osm, location, radius): + _debug(f'Mapd: Start query for OSM map data at {location}') + ways = osm.fetch_road_ways_around_location(location, radius) + _debug(f'Mapd: Query to OSM finished with {len(ways)} ways') + + # Only issue an update if we received some ways. Otherwise it is most likely a conectivity issue. + # Will retry on next loop. + if len(ways) > 0: + new_way_collection = WayCollection(ways) + + # Use the lock to update the way_collection as it might be being used to update the route. + self._lock.acquire() + try: + _debug('Mapd: Locking to write results from osm') + self.way_collection = new_way_collection + self.last_fetch_location = location + _debug(f'Mapd: Updated map data @ {location} - got {len(ways)} ways') + + finally: + self._lock.release() + _debug('Mapd: Releasing Lock to write results from osm') + + # Ignore if we have a query thread already running. + if self._query_thread is not None and self._query_thread.is_alive(): + return + + self._query_thread = threading.Thread(target=query, args=(self.osm, self.location, QUERY_RADIUS)) + self._query_thread.start() + def updated_osm_data(self): if self.route is not None: distance_to_end = self.route.distance_to_end @@ -85,43 +125,48 @@ def updated_osm_data(self): # do not query if are still not close to the border of previous query area return - ways = self.osm.fetch_road_ways_around_location(self.location, QUERY_RADIUS) - self.way_collection = WayCollection(ways) - self.last_fetch_location = self.location - - _debug(f'Mapd: Updated map data @ {self.location} - got {len(ways)} ways') + self._query_osm_not_blocking() def update_route(self, sm): - if self.way_collection is None or self.location is None or self.bearing is None: - return + # We use the lock when updating the route, as it reads `way_collection` which can ben updated by + # a new query result from the _query_thread. + _debug('Mapd: Locking to update route') + self._lock.acquire() + try: + if self.way_collection is None or self.location is None or self.bearing is None: + return - if self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: - # No new fix since last update - return + if self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: + # No new fix since last update + return - self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp + self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp - # Create the route if not existent or if it was generated by an older way collection - if self.route is None or self.route.way_collection_id != self.way_collection.id: - self.route = self.way_collection.get_route(self.location, self.bearing) - _debug(f'Mapd *****: Route created: \n{self.route}\n********') - return + # Create the route if not existent or if it was generated by an older way collection + if self.route is None or self.route.way_collection_id != self.way_collection.id: + self.route = self.way_collection.get_route(self.location, self.bearing) + _debug(f'Mapd *****: Route created: \n{self.route}\n********') + return - # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start - # jumping and creating unnecesary loosing of the route. Since the route update timestamp has been updated - # a new liveMapData message will be published with the current values (which is desirable) - if self.gps_speed < FULL_STOP_MAX_SPEED: - _debug('Mapd *****: Route Not updated as car has Stopped ********') - return + # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start + # jumping and creating unnecesary loosing of the route. Since the route update timestamp has been updated + # a new liveMapData message will be published with the current values (which is desirable) + if self.gps_speed < FULL_STOP_MAX_SPEED: + _debug('Mapd *****: Route Not updated as car has Stopped ********') + return - self.route.update(self.location, self.bearing) - if self.route.located: - _debug(f'Mapd *****: Route updated: \n{self.route}\n********') - return + self.route.update(self.location, self.bearing) + if self.route.located: + _debug(f'Mapd *****: Route updated: \n{self.route}\n********') + return + + # if an old route did not mange to locate, attempt to regenerate form way collection. + self.route = self.way_collection.get_route(self.location, self.bearing) + _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') - # if an old route did not mange to locate, attempt to regenerate form way collection. - self.route = self.way_collection.get_route(self.location, self.bearing) - _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') + finally: + self._lock.release() + _debug('Mapd: Releasing Lock to update route') def publish(self, pm, sm): # Ensure we have a route currently located From c0ae833d46f49a04ab6bfddeb86457e3bdb17334 Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 17 May 2021 13:19:17 +0200 Subject: [PATCH 24/32] LiveMapData: Prevent route continuation with sharp turns close to query area border --- selfdrive/mapd/config.py | 2 ++ selfdrive/mapd/lib/Route.py | 47 ++++++++++++++++++++--------- selfdrive/mapd/lib/WayCollection.py | 11 +++++-- selfdrive/mapd/lib/osm.py | 4 +-- selfdrive/mapd/mapd.py | 10 +++--- 5 files changed, 50 insertions(+), 24 deletions(-) create mode 100644 selfdrive/mapd/config.py diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py new file mode 100644 index 00000000000000..08ce5834add52a --- /dev/null +++ b/selfdrive/mapd/config.py @@ -0,0 +1,2 @@ +QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. +MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query. \ No newline at end of file diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index ac0972e0568ff7..6060c3e63218ac 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -1,17 +1,28 @@ from .NodesData import NodesData, NodeDataIdx -from .geo import ref_vectors, R -import numpy as np +from selfdrive.mapd.config import QUERY_RADIUS +from .geo import ref_vectors, R, distance_to_points from itertools import compress +import numpy as np _DISTANCE_LIMIT_FOR_CURRENT_CURVATURE = 20. # mts _SUBSTANTIAL_CURVATURE_THRESHOLD = 0.003 # 333 mts radius +_MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420 # bearing delta at route edge must be 180 +/- 70 degrees. +_MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distance to edge of query radius. class Route(): """A set of consecutive way relations forming a default driving route. """ - def __init__(self, current, wr_index, way_collection_id): + def __init__(self, current, wr_index, way_collection_id, query_center): + """Create a Route object from a given `wr_index` (Way relation index) + + Args: + current (WayRelation): The Way Relation that is currently located. It must be active. + wr_index (Dict(NodeId, [WayRelation])): The index of WayRelations by node id of an edge node. + way_collection_id (UUID): The id of the Way Collection that created this Route. + query_center (Numpy Array): lat, lon] numpy array in radians indicating the center of the data query. + """ self.way_collection_id = way_collection_id self._ordered_way_relations = [] self._nodes_data = None @@ -33,23 +44,19 @@ def __init__(self, current, wr_index, way_collection_id): # - Append current element to the route list of ordered way relations. self._ordered_way_relations.append(last_wr) - # Get the id of the node at the end of the way. + # - Get the id of the node at the end of the way and the fetch the way relations that share the end node id from + # the index. last_node_id = last_wr.last_node.id - - # Get the way relations that share the end node id from the index way_relations = wr_index[last_node_id] - # if no more way_relations than last_wr, we got to the end. + # - If no more way_relations than last_wr, we got to the end. if len(way_relations) == 1: break - # Get the coordinates for the edge node + # - Get the coordinates for the edge node and build the array of coordinates for the nodes before the edge node + # on each of the common way relations, then get the vectors in cartesian plane for the end sections of each way. ref_point = last_wr.last_node_coordinates - - # Get the array of coordinates for the nodes following edge node on each of the common way relations. points = np.array(list(map(lambda wr: wr.node_before_edge_coordinates(last_node_id), way_relations))) - - # Get the vectors in cartesian plane for the end sections of each way. v = ref_vectors(ref_point, points) * R # - Calculate the bearing (from true north clockwise) for every end section of each way. @@ -60,12 +67,13 @@ def __init__(self, current, wr_index, way_collection_id): b_ref = b[last_wr_idx] delta = b - b_ref - # - Update the direction of the possible continuation ways excluding the last_wr + # - Update the direction of the possible route continuation ways (excluding the last_wr) as starting + # from last_node_id for idx, wr in enumerate(way_relations): if idx != last_wr_idx: wr.update_direction_from_starting_node(last_node_id) - # - Filter the possible continuation way relations: + # - Filter the possible route continuation way relations: # - exclude last_wr # - exclude all way relations that are prohibited due to traffic direction. mask = [idx != last_wr_idx and not wr.is_prohibited for idx, wr in enumerate(way_relations)] @@ -78,7 +86,16 @@ def __init__(self, current, wr_index, way_collection_id): # - The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi and -pi. - best_idx = np.argmin(np.cos(delta)) + cos_delta = np.cos(delta) + best_idx = np.argmin(cos_delta) + + # - Make sure to not select as route continuation a way that turns too much if we are close to the border of + # map data queried. This is to avoid building a route that takes a sharp turn just because we do not have the + # data for the way that actually continues straight. + if cos_delta[best_idx] > _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE: + dist_to_center = distance_to_points(query_center, np.array([ref_point]))[0] + if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE: + break # - Select next way. last_wr = way_relations[best_idx] diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py index 773e194f9becdc..d416e2e9fce8f6 100644 --- a/selfdrive/mapd/lib/WayCollection.py +++ b/selfdrive/mapd/lib/WayCollection.py @@ -9,9 +9,16 @@ class WayCollection(): """A collection of WayRelations to use for maps data analysis. """ - def __init__(self, ways): + def __init__(self, ways, query_center): + """Creates a WayCollection with a set of OSM way objects. + + Args: + ways (Array): Collection of Way objects fetched from OSM in a radius around `query_center` + query_center (Numpy Array): [lat, lon] numpy array in radians indicating the center of the data query. + """ self.id = uuid.uuid4() self.way_relations = list(map(lambda way: WayRelation(way), ways)) + self.query_center = query_center # Create the index by edge node ids. self.wr_index = {} @@ -60,4 +67,4 @@ def get_route(self, location, bearing): wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way) current = wr_acceptable_bearing[0] - return Route(current, self.wr_index, self.id) + return Route(current, self.wr_index, self.id, self.query_center) diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py index c1c005f2151765..75f4737a900438 100644 --- a/selfdrive/mapd/lib/osm.py +++ b/selfdrive/mapd/lib/osm.py @@ -6,9 +6,7 @@ def __init__(self): # self.api = overpy.Overpass() self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter') - def fetch_road_ways_around_location(self, location, radius): - lat, lon = location - + def fetch_road_ways_around_location(self, lat, lon, radius): # fetch all ways and nodes on this ways around location around_str = f'{str(radius)},{str(lat)},{str(lon)}' q = """ diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 9e1fdde6fb839d..0544db660445dc 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -1,15 +1,15 @@ #!/usr/bin/env python3 import threading +import numpy as np from time import strftime, gmtime import cereal.messaging as messaging from common.realtime import Ratekeeper from selfdrive.mapd.lib.osm import OSM from selfdrive.mapd.lib.geo import distance from selfdrive.mapd.lib.WayCollection import WayCollection +from .config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY -QUERY_RADIUS = 3000 # mts -MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. _DEBUG = True @@ -82,13 +82,15 @@ def update_gps(self, sm): def _query_osm_not_blocking(self): def query(osm, location, radius): _debug(f'Mapd: Start query for OSM map data at {location}') - ways = osm.fetch_road_ways_around_location(location, radius) + lat, lon = location + ways = osm.fetch_road_ways_around_location(lat, lon, radius) _debug(f'Mapd: Query to OSM finished with {len(ways)} ways') # Only issue an update if we received some ways. Otherwise it is most likely a conectivity issue. # Will retry on next loop. if len(ways) > 0: - new_way_collection = WayCollection(ways) + location_rad = np.radians(np.array([lat, lon])) + new_way_collection = WayCollection(ways, location_rad) # Use the lock to update the way_collection as it might be being used to update the route. self._lock.acquire() From 6670874ada385e1cf50650a2e3c9def6e3cd710f Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 17 May 2021 14:29:25 +0200 Subject: [PATCH 25/32] LiveMapData: Use location as numpy array in radians alrady from mapd onwards --- selfdrive/mapd/lib/Route.py | 10 +++---- selfdrive/mapd/lib/WayCollection.py | 8 +++--- selfdrive/mapd/lib/WayRelation.py | 24 ++++++++-------- selfdrive/mapd/mapd.py | 44 +++++++++++++---------------- 4 files changed, 39 insertions(+), 47 deletions(-) diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index 6060c3e63218ac..e4d9279e82b8f9 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -148,23 +148,23 @@ def valid(self): def current_wr(self): return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None - def update(self, location, bearing): - """Will update the route structure based on the given `location` and `bearing` assuming progress on the route + def update(self, location_rad, bearing): + """Will update the route structure based on the given `location_rad` and `bearing` assuming progress on the route on the original direction. If direction has changed or active point on the route can not be found, the route will become invalid. """ - if len(self._ordered_way_relations) == 0 or location is None or bearing is None: + if len(self._ordered_way_relations) == 0 or location_rad is None or bearing is None: return # Skip if no update on location or bearing. - if self.current_wr.location == location and self.current_wr.bearing == bearing: + if self.current_wr.location_rad == location_rad and self.current_wr.bearing == bearing: return # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route # with the way relations remaining ahead. for idx, wr in enumerate(self._ordered_way_relations): active_direction = wr.direction - wr.update(location, bearing) + wr.update(location_rad, bearing) if not wr.active: continue diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py index d416e2e9fce8f6..d81e8c28ee03a1 100644 --- a/selfdrive/mapd/lib/WayCollection.py +++ b/selfdrive/mapd/lib/WayCollection.py @@ -26,15 +26,15 @@ def __init__(self, ways, query_center): for node_id in wr.edge_nodes_ids: self.wr_index[node_id] = self.wr_index.get(node_id, []) + [wr] - def get_route(self, location, bearing): - """Provides the best route found in the way collection based on provided `location` and `bearing` + def get_route(self, location_rad, bearing): + """Provides the best route found in the way collection based on provided `location_rad` and `bearing` """ - if location is None or bearing is None: + if location_rad is None or bearing is None: return None # Update all way relations in collection to the provided location and bearing. for wr in self.way_relations: - wr.update(location, bearing) + wr.update(location_rad, bearing) # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of # travel is valid. diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index 6e35e88d8a8003..0de434293071a0 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -124,7 +124,7 @@ def conditional_speed_limit_for_osm_tag_limit_string(limit_string): class WayRelation(): """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle. """ - def __init__(self, way, location=None, bearing=None): + def __init__(self, way, location_rad=None, bearing=None): self.way = way self.reset_location_variables() self.direction = DIRECTION.NONE @@ -142,8 +142,8 @@ def __init__(self, way, location=None, bearing=None): # Get the edge nodes ids. self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id] - if location is not None and bearing is not None: - self.update(location, bearing) + if location_rad is not None and bearing is not None: + self.update(location_rad, bearing) def __repr__(self): return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})' @@ -154,7 +154,7 @@ def __eq__(self, other): return False def reset_location_variables(self): - self.location = None + self.location_rad = None self.bearing = None self.active = False self.ahead_idx = None @@ -166,18 +166,17 @@ def reset_location_variables(self): def id(self): return self.way.id - def update(self, location, bearing): - """Will update and validate the associated way with a given `location` and `bearing`. + def update(self, location_rad, bearing): + """Will update and validate the associated way with a given `location_rad` and `bearing`. Specifically it will find the nodes behind and ahead of the current location and bearing. If no proper fit to the way geometry, the way relation is marked as invalid. """ self.reset_location_variables() # Ignore if location not in way bounding box - if not self.is_location_in_bbox(location): + if not self.is_location_in_bbox(location_rad): return - location_rad = np.radians(np.array(location)) bearing_rad = np.radians(bearing) # - Get the distance and bearings from location to all nodes. (N) @@ -246,7 +245,7 @@ def update(self, location, bearing): self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx] self.distance_to_node_ahead = distances[self.ahead_idx] self.active = True - self.location = location + self.location_rad = location_rad self.bearing = bearing self._speed_limit = None @@ -259,13 +258,12 @@ def update_direction_from_starting_node(self, start_node_id): else: self.direction = DIRECTION.NONE - def is_location_in_bbox(self, location): + def is_location_in_bbox(self, location_rad): """Indicates if a given location is contained in the bounding box surrounding the way. self.bbox = [[min_lat, min_lon], [max_lat, max_lon]] """ - radians = np.radians(np.array(location, dtype=float)) - is_g = np.greater_equal(radians, self.bbox[0, :]) - is_l = np.less_equal(radians, self.bbox[1, :]) + is_g = np.greater_equal(location_rad, self.bbox[0, :]) + is_l = np.less_equal(location_rad, self.bbox[1, :]) return np.all(np.concatenate((is_g, is_l))) diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 0544db660445dc..99912e691d67c1 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -5,7 +5,7 @@ import cereal.messaging as messaging from common.realtime import Ratekeeper from selfdrive.mapd.lib.osm import OSM -from selfdrive.mapd.lib.geo import distance +from selfdrive.mapd.lib.geo import distance_to_points from selfdrive.mapd.lib.WayCollection import WayCollection from .config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY @@ -35,8 +35,8 @@ def __init__(self): self.route = None self.last_gps_fix_timestamp = 0 self.last_gps = None - self.lat = None - self.lon = None + self.location_deg = None # The current location in degrees. + self.location_rad = None # The current location in radians as a Numpy array. self.bearing = None self.accuracy = None self.bearingAccuracy = None @@ -47,12 +47,6 @@ def __init__(self): self._query_thread = None self._lock = threading.Lock() - @property - def location(self): - if self.lat is None or self.lon is None: - return None - return self.lat, self.lon - def update_gps(self, sm): sock = 'gpsLocationExternal' if not sm.updated[sock] or not sm.valid[sock]: @@ -66,30 +60,29 @@ def update_gps(self, sm): return self.last_gps_fix_timestamp = log.timestamp # Unix TS. Milliseconds since January 1, 1970. - self.lat = log.latitude - self.lon = log.longitude + self.location_rad = np.array([log.latitude, log.longitude], dtype=float) + self.location_deg = (log.latitude, log.longitude) self.bearing = log.bearingDeg self.accuracy = log.accuracy self.bearingAccuracy = log.bearingAccuracyDeg self.gps_speed = log.speed _debug('Mapd: ********* Got GPS fix' - f'Pos: {self.lat}, {self.lon} +/- {self.accuracy} mts.\n' + f'Pos: {self.location_deg} +/- {self.accuracy} mts.\n' f'Bearing: {self.bearing} +/- {self.bearingAccuracy} deg.\n' f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' f'*******') def _query_osm_not_blocking(self): - def query(osm, location, radius): - _debug(f'Mapd: Start query for OSM map data at {location}') - lat, lon = location + def query(osm, location_deg, location_rad, radius): + _debug(f'Mapd: Start query for OSM map data at {location_deg}') + lat, lon = location_deg ways = osm.fetch_road_ways_around_location(lat, lon, radius) _debug(f'Mapd: Query to OSM finished with {len(ways)} ways') # Only issue an update if we received some ways. Otherwise it is most likely a conectivity issue. # Will retry on next loop. if len(ways) > 0: - location_rad = np.radians(np.array([lat, lon])) new_way_collection = WayCollection(ways, location_rad) # Use the lock to update the way_collection as it might be being used to update the route. @@ -97,8 +90,8 @@ def query(osm, location, radius): try: _debug('Mapd: Locking to write results from osm') self.way_collection = new_way_collection - self.last_fetch_location = location - _debug(f'Mapd: Updated map data @ {location} - got {len(ways)} ways') + self.last_fetch_location = location_rad + _debug(f'Mapd: Updated map data @ {location_deg} - got {len(ways)} ways') finally: self._lock.release() @@ -108,7 +101,8 @@ def query(osm, location, radius): if self._query_thread is not None and self._query_thread.is_alive(): return - self._query_thread = threading.Thread(target=query, args=(self.osm, self.location, QUERY_RADIUS)) + self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad, + QUERY_RADIUS)) self._query_thread.start() def updated_osm_data(self): @@ -118,11 +112,11 @@ def updated_osm_data(self): # do not query as long as we have a route with enough distance ahead. return - if self.location is None: + if self.location_rad is None: return if self.last_fetch_location is not None: - distance_since_last = distance(self.location, self.last_fetch_location) + distance_since_last = distance_to_points(self.last_fetch_location, np.array([self.location_rad]))[0] if distance_since_last < QUERY_RADIUS - MIN_DISTANCE_FOR_NEW_QUERY: # do not query if are still not close to the border of previous query area return @@ -135,7 +129,7 @@ def update_route(self, sm): _debug('Mapd: Locking to update route') self._lock.acquire() try: - if self.way_collection is None or self.location is None or self.bearing is None: + if self.way_collection is None or self.location_rad is None or self.bearing is None: return if self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: @@ -146,7 +140,7 @@ def update_route(self, sm): # Create the route if not existent or if it was generated by an older way collection if self.route is None or self.route.way_collection_id != self.way_collection.id: - self.route = self.way_collection.get_route(self.location, self.bearing) + self.route = self.way_collection.get_route(self.location_rad, self.bearing) _debug(f'Mapd *****: Route created: \n{self.route}\n********') return @@ -157,13 +151,13 @@ def update_route(self, sm): _debug('Mapd *****: Route Not updated as car has Stopped ********') return - self.route.update(self.location, self.bearing) + self.route.update(self.location_rad, self.bearing) if self.route.located: _debug(f'Mapd *****: Route updated: \n{self.route}\n********') return # if an old route did not mange to locate, attempt to regenerate form way collection. - self.route = self.way_collection.get_route(self.location, self.bearing) + self.route = self.way_collection.get_route(self.location_rad, self.bearing) _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') finally: From 281f6db6b4924ec8813077dc67a1dc53e9b28e35 Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 17 May 2021 15:09:41 +0200 Subject: [PATCH 26/32] LiveMapData: Use bearing in radians alrady from mapd onwards --- selfdrive/mapd/lib/Route.py | 12 ++++++------ selfdrive/mapd/lib/WayCollection.py | 8 ++++---- selfdrive/mapd/lib/WayRelation.py | 10 ++++------ selfdrive/mapd/mapd.py | 20 ++++++++------------ 4 files changed, 22 insertions(+), 28 deletions(-) diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index e4d9279e82b8f9..eadad30f04e332 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -148,23 +148,23 @@ def valid(self): def current_wr(self): return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None - def update(self, location_rad, bearing): - """Will update the route structure based on the given `location_rad` and `bearing` assuming progress on the route - on the original direction. If direction has changed or active point on the route can not be found, the route + def update(self, location_rad, bearing_rad): + """Will update the route structure based on the given `location_rad` and `bearing_rad` assuming progress on the + route on the original direction. If direction has changed or active point on the route can not be found, the route will become invalid. """ - if len(self._ordered_way_relations) == 0 or location_rad is None or bearing is None: + if len(self._ordered_way_relations) == 0 or location_rad is None or bearing_rad is None: return # Skip if no update on location or bearing. - if self.current_wr.location_rad == location_rad and self.current_wr.bearing == bearing: + if self.current_wr.location_rad == location_rad and self.current_wr.bearing_rad == bearing_rad: return # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route # with the way relations remaining ahead. for idx, wr in enumerate(self._ordered_way_relations): active_direction = wr.direction - wr.update(location_rad, bearing) + wr.update(location_rad, bearing_rad) if not wr.active: continue diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py index d81e8c28ee03a1..44515a4050da23 100644 --- a/selfdrive/mapd/lib/WayCollection.py +++ b/selfdrive/mapd/lib/WayCollection.py @@ -26,15 +26,15 @@ def __init__(self, ways, query_center): for node_id in wr.edge_nodes_ids: self.wr_index[node_id] = self.wr_index.get(node_id, []) + [wr] - def get_route(self, location_rad, bearing): - """Provides the best route found in the way collection based on provided `location_rad` and `bearing` + def get_route(self, location_rad, bearing_rad): + """Provides the best route found in the way collection based on provided `location_rad` and `bearing_rad` """ - if location_rad is None or bearing is None: + if location_rad is None or bearing_rad is None: return None # Update all way relations in collection to the provided location and bearing. for wr in self.way_relations: - wr.update(location_rad, bearing) + wr.update(location_rad, bearing_rad) # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of # travel is valid. diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index 0de434293071a0..5d82b4dabfc777 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -155,7 +155,7 @@ def __eq__(self, other): def reset_location_variables(self): self.location_rad = None - self.bearing = None + self.bearing_rad = None self.active = False self.ahead_idx = None self.behind_idx = None @@ -166,8 +166,8 @@ def reset_location_variables(self): def id(self): return self.way.id - def update(self, location_rad, bearing): - """Will update and validate the associated way with a given `location_rad` and `bearing`. + def update(self, location_rad, bearing_rad): + """Will update and validate the associated way with a given `location_rad` and `bearing_rad`. Specifically it will find the nodes behind and ahead of the current location and bearing. If no proper fit to the way geometry, the way relation is marked as invalid. """ @@ -177,8 +177,6 @@ def update(self, location_rad, bearing): if not self.is_location_in_bbox(location_rad): return - bearing_rad = np.radians(bearing) - # - Get the distance and bearings from location to all nodes. (N) bearings = bearing_to_points(location_rad, self._nodes_np) distances = distance_to_points(location_rad, self._nodes_np) @@ -246,7 +244,7 @@ def update(self, location_rad, bearing): self.distance_to_node_ahead = distances[self.ahead_idx] self.active = True self.location_rad = location_rad - self.bearing = bearing + self.bearing_rad = bearing_rad self._speed_limit = None def update_direction_from_starting_node(self, start_node_id): diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 99912e691d67c1..3c6e2f225893a6 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -37,9 +37,7 @@ def __init__(self): self.last_gps = None self.location_deg = None # The current location in degrees. self.location_rad = None # The current location in radians as a Numpy array. - self.bearing = None - self.accuracy = None - self.bearingAccuracy = None + self.bearing_rad = None self.gps_speed = 0. self.last_fetch_location = None self.last_route_update_fix_timestamp = 0 @@ -62,14 +60,12 @@ def update_gps(self, sm): self.last_gps_fix_timestamp = log.timestamp # Unix TS. Milliseconds since January 1, 1970. self.location_rad = np.array([log.latitude, log.longitude], dtype=float) self.location_deg = (log.latitude, log.longitude) - self.bearing = log.bearingDeg - self.accuracy = log.accuracy - self.bearingAccuracy = log.bearingAccuracyDeg + self.bearing_rad = np.radians(log.bearingDeg, dtype=float) self.gps_speed = log.speed _debug('Mapd: ********* Got GPS fix' - f'Pos: {self.location_deg} +/- {self.accuracy} mts.\n' - f'Bearing: {self.bearing} +/- {self.bearingAccuracy} deg.\n' + f'Pos: {self.location_deg} +/- {log.accuracy} mts.\n' + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg} deg.\n' f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}' f'*******') @@ -129,7 +125,7 @@ def update_route(self, sm): _debug('Mapd: Locking to update route') self._lock.acquire() try: - if self.way_collection is None or self.location_rad is None or self.bearing is None: + if self.way_collection is None or self.location_rad is None or self.bearing_rad is None: return if self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp: @@ -140,7 +136,7 @@ def update_route(self, sm): # Create the route if not existent or if it was generated by an older way collection if self.route is None or self.route.way_collection_id != self.way_collection.id: - self.route = self.way_collection.get_route(self.location_rad, self.bearing) + self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad) _debug(f'Mapd *****: Route created: \n{self.route}\n********') return @@ -151,13 +147,13 @@ def update_route(self, sm): _debug('Mapd *****: Route Not updated as car has Stopped ********') return - self.route.update(self.location_rad, self.bearing) + self.route.update(self.location_rad, self.bearing_rad) if self.route.located: _debug(f'Mapd *****: Route updated: \n{self.route}\n********') return # if an old route did not mange to locate, attempt to regenerate form way collection. - self.route = self.way_collection.get_route(self.location_rad, self.bearing) + self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad) _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********') finally: From 98a15fc895403ec9ae0caf62aa200276c3796fdb Mon Sep 17 00:00:00 2001 From: alfhern Date: Wed, 19 May 2021 08:55:46 +0200 Subject: [PATCH 27/32] LiveMapData: Location and Bearing in radiasn corrections --- selfdrive/mapd/lib/Route.py | 2 +- selfdrive/mapd/mapd.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index eadad30f04e332..38f1ddc48c2fd3 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -157,7 +157,7 @@ def update(self, location_rad, bearing_rad): return # Skip if no update on location or bearing. - if self.current_wr.location_rad == location_rad and self.current_wr.bearing_rad == bearing_rad: + if np.array_equal(self.current_wr.location_rad, location_rad) and self.current_wr.bearing_rad == bearing_rad: return # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 3c6e2f225893a6..d55f5caaae51f5 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -58,7 +58,7 @@ def update_gps(self, sm): return self.last_gps_fix_timestamp = log.timestamp # Unix TS. Milliseconds since January 1, 1970. - self.location_rad = np.array([log.latitude, log.longitude], dtype=float) + self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float)) self.location_deg = (log.latitude, log.longitude) self.bearing_rad = np.radians(log.bearingDeg, dtype=float) self.gps_speed = log.speed From 2d7727abd63fb53a0edb85230f79f4f7ce3e6046 Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 17 May 2021 16:10:35 +0200 Subject: [PATCH 28/32] LiveMapData: Clenaup unused code --- selfdrive/mapd/config.py | 5 +- selfdrive/mapd/lib/NodesData.py | 13 ------ selfdrive/mapd/lib/Route.py | 71 +---------------------------- selfdrive/mapd/lib/WayRelation.py | 6 --- selfdrive/mapd/lib/geo.py | 30 ------------ selfdrive/mapd/lib/test_geo.py | 4 +- selfdrive/mapd/mapd.py | 4 +- selfdrive/mapd/speed.py | 76 ------------------------------- 8 files changed, 8 insertions(+), 201 deletions(-) delete mode 100644 selfdrive/mapd/speed.py diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py index 08ce5834add52a..48b7b3790db20f 100644 --- a/selfdrive/mapd/config.py +++ b/selfdrive/mapd/config.py @@ -1,2 +1,5 @@ +# Map query config + QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries. -MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query. \ No newline at end of file +MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query. +FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py index 5c14988d293933..6da892c81c8d7d 100644 --- a/selfdrive/mapd/lib/NodesData.py +++ b/selfdrive/mapd/lib/NodesData.py @@ -216,19 +216,6 @@ def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead): return limits_ahead - def curvatures_ahead(self, ahead_idx, distance_to_node_ahead): - """Provides a numpy array of ordered pairs by distance including the distance ahead and the curvature. - """ - if len(self._nodes_data) == 0 or ahead_idx is None: - return np.array([]) - - # Find the cumulative distances to nodes and its curvature - dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:-1])) - dist = np.cumsum(dist, axis=0) - curv = self.get(NodeDataIdx.curvature)[ahead_idx:] - - return np.column_stack((dist, curv)) - def distance_to_end(self, ahead_idx, distance_to_node_ahead): if len(self._nodes_data) == 0 or ahead_idx is None: return None diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py index 38f1ddc48c2fd3..08ab0861d9676f 100644 --- a/selfdrive/mapd/lib/Route.py +++ b/selfdrive/mapd/lib/Route.py @@ -5,8 +5,6 @@ import numpy as np -_DISTANCE_LIMIT_FOR_CURRENT_CURVATURE = 20. # mts -_SUBSTANTIAL_CURVATURE_THRESHOLD = 0.003 # 333 mts radius _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420 # bearing delta at route edge must be 180 +/- 70 degrees. _MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distance to edge of query radius. @@ -44,7 +42,7 @@ def __init__(self, current, wr_index, way_collection_id, query_center): # - Append current element to the route list of ordered way relations. self._ordered_way_relations.append(last_wr) - # - Get the id of the node at the end of the way and the fetch the way relations that share the end node id from + # - Get the id of the node at the end of the way and the fetch the way relations that share the end node id from # the index. last_node_id = last_wr.last_node.id way_relations = wr_index[last_node_id] @@ -140,10 +138,6 @@ def _locate(self): self._ahead_idx = idx break - @property - def valid(self): - return self.current_wr is not None - @property def current_wr(self): return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None @@ -259,69 +253,6 @@ def next_curvature_speed_limit_section(self): return limits_ahead[0] - @property - def curvatures_ahead(self): - """Provides a list of ordered pairs by distance including the distance ahead and the curvature. - """ - if not self.located or self._nodes_data is None: - return None - - if self._curvatures_ahead is not None: - return self._curvatures_ahead - - self._curvatures_ahead = self._nodes_data.curvatures_ahead(self._ahead_idx, self._distance_to_node_ahead) - return self._curvatures_ahead - - @property - def immediate_curvature(self): - """Provides the highest curvature value in the immediate region ahead. - """ - if not self.located: - return None - - curvatures_ahead = self.curvatures_ahead - if not len(curvatures_ahead): - return None - - immediate_curvatures = curvatures_ahead[curvatures_ahead[:, 0] <= _DISTANCE_LIMIT_FOR_CURRENT_CURVATURE] - if not len(immediate_curvatures): - return None - - return np.max(immediate_curvatures[:, 1]) - - @property - def max_curvature_ahead(self): - """Provides the maximum curvature on route ahead - """ - if not self.located: - return None - - curvatures_ahead = self.curvatures_ahead - if not len(curvatures_ahead): - return None - - return np.max(curvatures_ahead[:, 1]) - - @property - def next_substantial_curvature(self): - """Provides the next substantial curvature and the distance to it. - """ - if not self.located: - return None - - curvatures_ahead = self.curvatures_ahead - if not len(curvatures_ahead): - return None - - filt = np.logical_and(curvatures_ahead[:, 0] > _DISTANCE_LIMIT_FOR_CURRENT_CURVATURE, - curvatures_ahead[:, 1] > _SUBSTANTIAL_CURVATURE_THRESHOLD) - substantial_curvatures_ahead = curvatures_ahead[filt] - - if not len(substantial_curvatures_ahead): - return None - - return substantial_curvatures_ahead[0, :] - @property def distance_to_end(self): if not self.located: diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index 5d82b4dabfc777..d15b5301f080d4 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -27,8 +27,6 @@ 'Su': 6 } -_ALL_WD = _WD.values() - def is_osm_time_condition_active(condition_string): """ @@ -315,10 +313,6 @@ def distance_to_way(self): """ return self._distance_to_way - @property - def node_behind(self): - return self.way.nodes[self.behind_idx] if self.behind_idx is not None else None - @property def node_ahead(self): return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py index 344d43f600aa02..55a29af9fc64cd 100644 --- a/selfdrive/mapd/lib/geo.py +++ b/selfdrive/mapd/lib/geo.py @@ -1,4 +1,3 @@ -from math import sin, cos, sqrt, atan2, radians from enum import Enum import numpy as np @@ -61,35 +60,6 @@ def distance_to_points(point, points): return c * R -def coord_to_rad(point): - """Tranform coordinates in degrees to radians - """ - return tuple(map(lambda p: radians(p), point)) - - -def distance(point_a, point_b): - """Calculate the distance in meters between two points expressed in coordinates in degrees (lat, lon) - """ - point_a_in_rad = coord_to_rad(point_a) - point_b_in_rad = coord_to_rad(point_b) - return _distance_from_rad(point_a_in_rad, point_b_in_rad) - - -def _distance_from_rad(point_a_in_rad, point_b_in_rad): - """Calculate the distance in meters between two points expressed in coordinates in radians (lat, lon) - """ - (latA, lonA) = point_a_in_rad - (latB, lonB) = point_b_in_rad - - dlon = lonB - lonA - dlat = latB - latA - - a = sin(dlat / 2)**2 + cos(latA) * cos(latB) * sin(dlon / 2)**2 - c = 2 * atan2(sqrt(a), sqrt(1 - a)) - - return R * c - - class DIRECTION(Enum): NONE = 0 AHEAD = 1 diff --git a/selfdrive/mapd/lib/test_geo.py b/selfdrive/mapd/lib/test_geo.py index f035cf0fe640e9..3a14da47824e3f 100644 --- a/selfdrive/mapd/lib/test_geo.py +++ b/selfdrive/mapd/lib/test_geo.py @@ -1,7 +1,7 @@ import unittest from decimal import Decimal from math import pi -from .geo import coord_to_rad +import numpy as np class TestMapsdGeoLibrary(unittest.TestCase): @@ -17,7 +17,7 @@ def test_coord_to_rad(self): (0., 2 * pi), (pi, 3 * pi), ] - rad_tuples = list(map(lambda p: coord_to_rad(p), points)) + rad_tuples = list(map(lambda p: np.radians(p), points)) self.assertEqual(rad_tuples, expected) # Helpers diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index d55f5caaae51f5..ccd48e8bf71821 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -7,11 +7,9 @@ from selfdrive.mapd.lib.osm import OSM from selfdrive.mapd.lib.geo import distance_to_points from selfdrive.mapd.lib.WayCollection import WayCollection -from .config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY +from .config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED -FULL_STOP_MAX_SPEED = 1.39 # m/s Max speed for considering car is stopped. - _DEBUG = True diff --git a/selfdrive/mapd/speed.py b/selfdrive/mapd/speed.py deleted file mode 100644 index 26d972d9d51c14..00000000000000 --- a/selfdrive/mapd/speed.py +++ /dev/null @@ -1,76 +0,0 @@ -from lib.osm import OSM -from lib.WayCollection import WayCollection -from decimal import Decimal -import sys -import csv - - -# TEST: python speed.py 52.273948132602584 13.91490391150784 1000 313 -# TEST: python speed.py 52.19538880646971 13.867764690138795 2000 305 -# STRESS: python speed.py 52.5094376 13.397043 3000 353.27514648437 - - -def csv_out_curvatures(name, curv_data): - with open(f'{name}.csv', 'w', newline='') as results_csv: - csv_writer = csv.writer(results_csv, delimiter=',') - csv_writer.writerow([ - 'X', 'Y', 'tan', 'cur' - ]) - for curv in curv_data: - csv_writer.writerow(curv) - - -if __name__ == '__main__': - location = (Decimal(sys.argv[1]), Decimal(sys.argv[2])) - location2 = Decimal(52.27791306120662), Decimal(13.90794088430264) - bearing = float(sys.argv[4]) - - # 1. Get ways around location - osm = OSM() - ways = osm.fetch_road_ways_around_location(location, float(sys.argv[3])) - - # 2. Create the collection - way_collection = WayCollection(ways) - - # 3. Find the current route - route = way_collection.get_route(location, bearing) - - # 4. Output - print('_____ GIVEN DIRECTION') - if route is None or not route.valid: - print('No valid routes found for given loaction and bearing.') - else: - print(f'Current way: {route.current_wr}') - print(f'Speed Limit: {route.current_wr.speed_limit}') - print(f'Route Ahead: {route}') - print(f'Current speed limit: {route.current_speed_limit}') - print(f'Next speed limit: {route.next_speed_limit_section}') - print(f'Curvature now: {route.immediate_curvature}') - print(f'Max Curvature: {route.max_curvature_ahead}') - print(f'Next Curvature: {route.next_substantial_curvature}') - print(f'Limits Ahead: {route.speed_limits_ahead}') - print(f'curvatures: {route.curvatures_ahead}') - print(f'Distance to end: {route.distance_to_end}') - # csv_out_curvatures('forward', route.curvatures) - - # 4. Update on the oposit direction for testing - route = way_collection.get_route(location, bearing - 180) - - # 5. Output in oposit direction - print(' ') - print('_____REVERSE DIRECTION') - if route is None or not route.valid: - print('No valid routes found for given loaction and bearing.') - else: - print(f'Current way: {route.current_wr}') - print(f'Speed Limit: {route.current_wr.speed_limit}') - print(f'Route Ahead: {route}') - print(f'Current speed limit: {route.current_speed_limit}') - print(f'Next speed limit: {route.next_speed_limit_section}') - print(f'Curvature now: {route.immediate_curvature}') - print(f'Max Curvature: {route.max_curvature_ahead}') - print(f'Next Curvature: {route.next_substantial_curvature}') - print(f'Limits Ahead: {route.speed_limits_ahead}') - print(f'curvatures: {route.curvatures_ahead}') - print(f'Distance to end: {route.distance_to_end}') - # csv_out_curvatures('backward', route.curvatures) \ No newline at end of file From e227ba57d775182936434b90719c897db20e9296 Mon Sep 17 00:00:00 2001 From: alfhern Date: Mon, 17 May 2021 16:30:24 +0200 Subject: [PATCH 29/32] LiveMapData: Calculate vectors, distances between nodes and way section bearings only once in WayRelation --- selfdrive/mapd/lib/WayRelation.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py index d15b5301f080d4..314c17b0f073b0 100644 --- a/selfdrive/mapd/lib/WayRelation.py +++ b/selfdrive/mapd/lib/WayRelation.py @@ -132,6 +132,15 @@ def __init__(self, way, location_rad=None, bearing=None): # Create a numpy array with nodes data to support calculations. self._nodes_np = np.radians(np.array([[nd.lat, nd.lon] for nd in way.nodes], dtype=float)) + # Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2) + v = vectors(self._nodes_np) * R + + # Calculate the vector magnitudes (or distance) between nodes. (N-1) + self._way_distances = np.linalg.norm(v, axis=1) + + # Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1) + self._way_bearings = np.arctan2(v[:, 0], v[:, 1]) + # Define bounding box to ease the process of locating a node in a way. # [[min_lat, min_lon], [max_lat, max_lon]] self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING, @@ -192,28 +201,19 @@ def update(self, location_rad, bearing_rad): if len(possible_idxs) == 0: return - # - Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2) - v = vectors(self._nodes_np) * R - - # - Calculate the vector magnitudes (or distance) between nodes. (N-1) - d = np.linalg.norm(v, axis=1) - # - Find then angle formed between the vectors from the current location to consecutive nodes. This is the # value of the difference in the bearings of the vectors. teta = np.diff(bearings) # - When two consecutive nodes will be ahead and behind, they will form a triangle with the current location. - # We find the closest distance to the way by solving the ara of the triangle and finding the height (h). + # We find the closest distance to the way by solving the area of the triangle and finding the height (h). # We must use the abolute value of the sin of the angle in the formula, which is equivalent to ensure we # are considering the smallest of the two angles formed between the two vectors. # https://www.mathsisfun.com/algebra/trig-area-triangle-without-right-angle.html - h = distances[:-1] * distances[1:] * np.abs(np.sin(teta)) / d - - # - Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1) - bw = np.arctan2(v[:, 0], v[:, 1]) + h = distances[:-1] * distances[1:] * np.abs(np.sin(teta)) / self._way_distances # - Calculate the delta between driving bearing and way bearings. (N-1) - bw_delta = bw - bearing_rad + bw_delta = self._way_bearings - bearing_rad # - The absolut value of the sin of `bw_delta` indicates how close the bearings match independent of direction. # We will use this value along the distance to the way to aid on way selection. (N-1) From 4ace864bb65ce5c6fed82f92c7ce1949e989ee90 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 25 May 2021 12:57:03 +0200 Subject: [PATCH 30/32] install opspline automatically in comma2 devices --- installer/custom/install_gfortran.sh | 34 + installer/custom/termux-elf-cleaner/COPYING | 674 ++++++++++++++++++ installer/custom/termux-elf-cleaner/Makefile | 16 + installer/custom/termux-elf-cleaner/README.md | 39 + installer/custom/termux-elf-cleaner/elf.h | 211 ++++++ .../termux-elf-cleaner/termux-elf-cleaner.cpp | 191 +++++ launch_chffrplus.sh | 2 +- selfdrive/manager/custom_dep.py | 55 ++ 8 files changed, 1221 insertions(+), 1 deletion(-) create mode 100644 installer/custom/install_gfortran.sh create mode 100644 installer/custom/termux-elf-cleaner/COPYING create mode 100644 installer/custom/termux-elf-cleaner/Makefile create mode 100644 installer/custom/termux-elf-cleaner/README.md create mode 100644 installer/custom/termux-elf-cleaner/elf.h create mode 100644 installer/custom/termux-elf-cleaner/termux-elf-cleaner.cpp create mode 100755 selfdrive/manager/custom_dep.py diff --git a/installer/custom/install_gfortran.sh b/installer/custom/install_gfortran.sh new file mode 100644 index 00000000000000..fa2641a338fd3f --- /dev/null +++ b/installer/custom/install_gfortran.sh @@ -0,0 +1,34 @@ +#!/data/data/com.termux/files/usr/bin/sh +# Get some needed tools. coreutils for mkdir command, gnugp for the signing key, and apt-transport-https to actually connect to the repo +apt-get update +apt-get --assume-yes upgrade +apt-get --assume-yes install coreutils gnupg + +# Make the sources.list.d directory +mkdir -p $PREFIX/etc/apt/sources.list.d + +# Write the needed source file +echo "deb https://its-pointless.github.io/files/24 termux extras" > $PREFIX/etc/apt/sources.list.d/pointless.list + +# Add signing key from https://its-pointless.github.io/pointless.gpg +curl -sL https://its-pointless.github.io/pointless.gpg | apt-key add - + +# Update apt +apt update + +# install gfortran +apt install gcc-8 -y +setupclang-gfort-8 + +# Elf cleaner is needed to remove a DT_ENTRY warning that prints out when gfortran -v is called to get +# its version number and this breaks the pip installation script when fortran is used. + +# Build elf cleaner +SCRIPTPATH="$( cd -- "$(dirname "$0")" >/dev/null 2>&1 ; pwd -P )" +ELFCLEANERPATH=$SCRIPTPATH/termux-elf-cleaner/ +cd $ELFCLEANERPATH +make + +# Perform elf cleaner on gfortran +./termux-elf-cleaner $(which gfortran) + diff --git a/installer/custom/termux-elf-cleaner/COPYING b/installer/custom/termux-elf-cleaner/COPYING new file mode 100644 index 00000000000000..9cecc1d4669ee8 --- /dev/null +++ b/installer/custom/termux-elf-cleaner/COPYING @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + {one line to give the program's name and a brief idea of what it does.} + Copyright (C) {year} {name of author} + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + {project} Copyright (C) {year} {fullname} + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/installer/custom/termux-elf-cleaner/Makefile b/installer/custom/termux-elf-cleaner/Makefile new file mode 100644 index 00000000000000..71138bee18282c --- /dev/null +++ b/installer/custom/termux-elf-cleaner/Makefile @@ -0,0 +1,16 @@ +CXXFLAGS += -std=c++11 -Wall -Wextra -pedantic +PREFIX ?= /usr/local + +termux-elf-cleaner: termux-elf-cleaner.cpp + +clean: + rm -f termux-elf-cleaner + +install: termux-elf-cleaner + mkdir -p $(PREFIX)/bin + install termux-elf-cleaner $(PREFIX)/bin/termux-elf-cleaner + +uninstall: + rm -f $(PREFIX)/bin/termux-elf-cleaner + +.PHONY: clean install uninstall diff --git a/installer/custom/termux-elf-cleaner/README.md b/installer/custom/termux-elf-cleaner/README.md new file mode 100644 index 00000000000000..f72d056dca82cc --- /dev/null +++ b/installer/custom/termux-elf-cleaner/README.md @@ -0,0 +1,39 @@ +# termux-elf-cleaner +Utility for Android ELF files to remove unused parts that the linker warns about. + +## Description +When loading ELF files, the Android linker warns about unsupported dynamic section entries with warnings such as: + + WARNING: linker: /data/data/org.kost.nmap.android.networkmapper/bin/nmap: unused DT entry: type 0x6ffffffe arg 0x8a7d4 + WARNING: linker: /data/data/org.kost.nmap.android.networkmapper/bin/nmap: unused DT entry: type 0x6fffffff arg 0x3 + +This utility strips away the following dynamic section entries: + +- `DT_RPATH` - not supported in any Android version. +- `DT_RUNPATH` - supported from Android 7.0. +- `DT_VERDEF` - supported from Android 6.0. +- `DT_VERDEFNUM` - supported from Android 6.0. +- `DT_VERNEEDED` - supported from Android 6.0. +- `DT_VERNEEDNUM` - supported from Android 6.0. +- `DT_VERSYM` - supported from Android 6.0. + +It also removes the three ELF sections of type: + +- `SHT_GNU_verdef` +- `SHT_GNU_verneed` +- `SHT_GNU_versym` + +## Usage +```sh +usage: termux-elf-cleaner + +Processes ELF files to remove unsupported section types and +dynamic section entries which the Android linker warns about. +``` + +## Author +Fredrik Fornwall ([@fornwall](https://github.com/fornwall)). + +## License + +SPDX-License-Identifier: [GPL-3.0-or-later](https://spdx.org/licenses/GPL-3.0-or-later.html) diff --git a/installer/custom/termux-elf-cleaner/elf.h b/installer/custom/termux-elf-cleaner/elf.h new file mode 100644 index 00000000000000..447616ae29b2b8 --- /dev/null +++ b/installer/custom/termux-elf-cleaner/elf.h @@ -0,0 +1,211 @@ +#ifndef ELF_H_INCLUDED +#define ELF_H_INCLUDED + +#include + +/* Type for a 16-bit quantity. */ +typedef uint16_t Elf32_Half; +typedef uint16_t Elf64_Half; + +/* Types for signed and unsigned 32-bit quantities. */ +typedef uint32_t Elf32_Word; +typedef int32_t Elf32_Sword; +typedef uint32_t Elf64_Word; +typedef int32_t Elf64_Sword; + +/* Types for signed and unsigned 64-bit quantities. */ +typedef uint64_t Elf32_Xword; +typedef int64_t Elf32_Sxword; +typedef uint64_t Elf64_Xword; +typedef int64_t Elf64_Sxword; + +/* Type of addresses. */ +typedef uint32_t Elf32_Addr; +typedef uint64_t Elf64_Addr; + +/* Type of file offsets. */ +typedef uint32_t Elf32_Off; +typedef uint64_t Elf64_Off; + +/* Type for section indices, which are 16-bit quantities. */ +typedef uint16_t Elf32_Section; +typedef uint16_t Elf64_Section; + +/* Type for version symbol information. */ +typedef Elf32_Half Elf32_Versym; +typedef Elf64_Half Elf64_Versym; + + +/* The ELF file header. This appears at the start of every ELF file. */ +typedef struct { + unsigned char e_ident[16]; /* Magic number and other info */ + Elf32_Half e_type; /* Object file type */ + Elf32_Half e_machine; /* Architecture */ + Elf32_Word e_version; /* Object file version */ + Elf32_Addr e_entry; /* Entry point virtual address */ + Elf32_Off e_phoff; /* Program header table (usually follows elf header directly) file offset */ + Elf32_Off e_shoff; /* Section header table (at end of file) file offset */ + Elf32_Word e_flags; /* Processor-specific flags */ + Elf32_Half e_ehsize; /* ELF header size in bytes */ + Elf32_Half e_phentsize; /* Program header table entry size */ + Elf32_Half e_phnum; /* Program header table entry count */ + Elf32_Half e_shentsize; /* Section header table entry size */ + Elf32_Half e_shnum; /* Section header table entry count */ + Elf32_Half e_shstrndx; /* Section header string table index */ +} Elf32_Ehdr; +typedef struct { + unsigned char e_ident[16]; /* Magic number and other info */ + Elf64_Half e_type; /* Object file type */ + Elf64_Half e_machine; /* Architecture */ + Elf64_Word e_version; /* Object file version */ + Elf64_Addr e_entry; /* Entry point virtual address */ + Elf64_Off e_phoff; /* Program header table file offset */ + Elf64_Off e_shoff; /* Section header table file offset */ + Elf64_Word e_flags; /* Processor-specific flags */ + Elf64_Half e_ehsize; /* ELF header size in bytes */ + Elf64_Half e_phentsize; /* Program header table entry size */ + Elf64_Half e_phnum; /* Program header table entry count */ + Elf64_Half e_shentsize; /* Section header table entry size */ + Elf64_Half e_shnum; /* Section header table entry count */ + Elf64_Half e_shstrndx; /* Section header string table index */ +} Elf64_Ehdr; + +/* Section header entry. The number of section entries in the file are determined by the "e_shnum" field of the ELF header.*/ +typedef struct { + Elf32_Word sh_name; /* Section name (string tbl index) */ + Elf32_Word sh_type; /* Section type */ + Elf32_Word sh_flags; /* Section flags */ + Elf32_Addr sh_addr; /* Section virtual addr at execution */ + Elf32_Off sh_offset; /* Section file offset */ + Elf32_Word sh_size; /* Section size in bytes */ + Elf32_Word sh_link; /* Link to another section */ + Elf32_Word sh_info; /* Additional section information */ + Elf32_Word sh_addralign; /* Section alignment */ + Elf32_Word sh_entsize; /* Entry size if section holds table */ +} Elf32_Shdr; +typedef struct { + Elf64_Word sh_name; /* Section name (string tbl index) */ + Elf64_Word sh_type; /* Section type */ + Elf64_Xword sh_flags; /* Section flags */ + Elf64_Addr sh_addr; /* Section virtual addr at execution */ + Elf64_Off sh_offset; /* Section file offset */ + Elf64_Xword sh_size; /* Section size in bytes */ + Elf64_Word sh_link; /* Link to another section */ + Elf64_Word sh_info; /* Additional section information */ + Elf64_Xword sh_addralign; /* Section alignment */ + Elf64_Xword sh_entsize; /* Entry size if section holds table */ +} Elf64_Shdr; + +/* Legal values for sh_type (section type). */ +#define SHT_NULL 0 /* Section header table entry unused */ +#define SHT_PROGBITS 1 /* Program data */ +#define SHT_SYMTAB 2 /* Symbol table */ +#define SHT_STRTAB 3 /* String table */ +#define SHT_RELA 4 /* Relocation entries with addends */ +#define SHT_HASH 5 /* Symbol hash table */ +#define SHT_DYNAMIC 6 /* Dynamic linking information. Contains Elf32_Dyn/Elf64_Dyn entries. */ +#define SHT_NOTE 7 /* Notes */ +#define SHT_NOBITS 8 /* Program space with no data (bss) */ +#define SHT_REL 9 /* Relocation entries, no addends */ +#define SHT_SHLIB 10 /* Reserved */ +#define SHT_DYNSYM 11 /* Dynamic linker symbol table */ +#define SHT_INIT_ARRAY 14 /* Array of constructors */ +#define SHT_FINI_ARRAY 15 /* Array of destructors */ +#define SHT_PREINIT_ARRAY 16 /* Array of pre-constructors */ +#define SHT_GROUP 17 /* Section group */ +#define SHT_SYMTAB_SHNDX 18 /* Extended section indeces */ +#define SHT_NUM 19 /* Number of defined types. */ +#define SHT_LOOS 0x60000000 /* Start OS-specific. */ +#define SHT_GNU_ATTRIBUTES 0x6ffffff5 /* Object attributes. */ +#define SHT_GNU_HASH 0x6ffffff6 /* GNU-style hash table. */ +#define SHT_GNU_LIBLIST 0x6ffffff7 /* Prelink library list */ +#define SHT_CHECKSUM 0x6ffffff8 /* Checksum for DSO content. */ +#define SHT_LOSUNW 0x6ffffffa /* Sun-specific low bound. */ +#define SHT_SUNW_move 0x6ffffffa +#define SHT_SUNW_COMDAT 0x6ffffffb +#define SHT_SUNW_syminfo 0x6ffffffc +#define SHT_GNU_verdef 0x6ffffffd /* Version definition section. */ +#define SHT_GNU_verneed 0x6ffffffe /* Version needs section. */ +#define SHT_GNU_versym 0x6fffffff /* Version symbol table. */ +#define SHT_HISUNW 0x6fffffff /* Sun-specific high bound. */ +#define SHT_HIOS 0x6fffffff /* End OS-specific type */ +#define SHT_LOPROC 0x70000000 /* Start of processor-specific */ +#define SHT_HIPROC 0x7fffffff /* End of processor-specific */ +#define SHT_LOUSER 0x80000000 /* Start of application-specific */ +#define SHT_HIUSER 0x8fffffff /* End of application-specific */ + +/* Dynamic section entry. */ +typedef struct { + Elf32_Sword d_tag; /* Dynamic entry type */ + union { Elf32_Word d_val; Elf32_Addr d_ptr; } d_un; /* Integer or address value */ +} Elf32_Dyn; +typedef struct { + Elf64_Sxword d_tag; /* Dynamic entry type */ + union { Elf64_Xword d_val; Elf64_Addr d_ptr; } d_un; /* Integer or address value */ +} Elf64_Dyn; + +/* Legal values for d_tag (dynamic entry type). */ +#define DT_NULL 0 /* Marks end of dynamic section */ +#define DT_NEEDED 1 /* Name of needed library */ +#define DT_PLTRELSZ 2 /* Size in bytes of PLT relocs */ +#define DT_PLTGOT 3 /* Processor defined value */ +#define DT_HASH 4 /* Address of symbol hash table */ +#define DT_STRTAB 5 /* Address of string table */ +#define DT_SYMTAB 6 /* Address of symbol table */ +#define DT_RELA 7 /* Address of Rela relocs */ +#define DT_RELASZ 8 /* Total size of Rela relocs */ +#define DT_RELAENT 9 /* Size of one Rela reloc */ +#define DT_STRSZ 10 /* Size of string table */ +#define DT_SYMENT 11 /* Size of one symbol table entry */ +#define DT_INIT 12 /* Address of init function */ +#define DT_FINI 13 /* Address of termination function */ +#define DT_SONAME 14 /* Name of shared object */ +#define DT_RPATH 15 /* Library search path (deprecated) */ +#define DT_SYMBOLIC 16 /* Start symbol search here */ +#define DT_REL 17 /* Address of Rel relocs */ +#define DT_RELSZ 18 /* Total size of Rel relocs */ +#define DT_RELENT 19 /* Size of one Rel reloc */ +#define DT_PLTREL 20 /* Type of reloc in PLT */ +#define DT_DEBUG 21 /* For debugging; unspecified */ +#define DT_TEXTREL 22 /* Reloc might modify .text */ +#define DT_JMPREL 23 /* Address of PLT relocs */ +#define DT_BIND_NOW 24 /* Process relocations of object */ +#define DT_INIT_ARRAY 25 /* Array with addresses of init fct */ +#define DT_FINI_ARRAY 26 /* Array with addresses of fini fct */ +#define DT_INIT_ARRAYSZ 27 /* Size in bytes of DT_INIT_ARRAY */ +#define DT_FINI_ARRAYSZ 28 /* Size in bytes of DT_FINI_ARRAY */ +#define DT_RUNPATH 29 /* Library search path */ +#define DT_FLAGS 30 /* Flags for the object being loaded */ +#define DT_ENCODING 32 /* Start of encoded range */ +#define DT_PREINIT_ARRAY 32 /* Array with addresses of preinit fct*/ +#define DT_PREINIT_ARRAYSZ 33 /* size in bytes of DT_PREINIT_ARRAY */ +#define DT_NUM 34 /* Number used */ +#define DT_LOOS 0x6000000d /* Start of OS-specific */ +#define DT_HIOS 0x6ffff000 /* End of OS-specific */ +#define DT_VERDEF 0x6ffffffc +#define DT_VERDEFNUM 0x6ffffffd +#define DT_LOPROC 0x70000000 /* Start of processor-specific */ +#define DT_HIPROC 0x7fffffff /* End of processor-specific */ + + +/* Symbol table entry. */ +typedef struct { + Elf32_Word st_name; /* Symbol name (string tbl index) */ + Elf32_Addr st_value; /* Symbol value */ + Elf32_Word st_size; /* Symbol size */ + unsigned char st_info; /* Symbol type and binding */ + unsigned char st_other; /* Symbol visibility */ + Elf32_Section st_shndx; /* Section index */ +} Elf32_Sym; + +typedef struct { + Elf64_Word st_name; /* Symbol name (string tbl index) */ + unsigned char st_info; /* Symbol type and binding */ + unsigned char st_other; /* Symbol visibility */ + Elf64_Section st_shndx; /* Section index */ + Elf64_Addr st_value; /* Symbol value */ + Elf64_Xword st_size; /* Symbol size */ +} Elf64_Sym; + + +#endif diff --git a/installer/custom/termux-elf-cleaner/termux-elf-cleaner.cpp b/installer/custom/termux-elf-cleaner/termux-elf-cleaner.cpp new file mode 100644 index 00000000000000..97742768e9dc34 --- /dev/null +++ b/installer/custom/termux-elf-cleaner/termux-elf-cleaner.cpp @@ -0,0 +1,191 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef __ANDROID_API__ +#define __ANDROID_API__ 21 +#endif + +// Include a local elf.h copy as not all platforms have it. +#include "elf.h" + +#define DT_GNU_HASH 0x6ffffef5 +#define DT_VERSYM 0x6ffffff0 +#define DT_FLAGS_1 0x6ffffffb +#define DT_VERNEEDED 0x6ffffffe +#define DT_VERNEEDNUM 0x6fffffff + +#define DF_1_NOW 0x00000001 /* Set RTLD_NOW for this object. */ +#define DF_1_GLOBAL 0x00000002 /* Set RTLD_GLOBAL for this object. */ +#define DF_1_NODELETE 0x00000008 /* Set RTLD_NODELETE for this object.*/ + +#if __ANDROID_API__ < 23 +#define SUPPORTED_DT_FLAGS_1 (DF_1_NOW | DF_1_GLOBAL) +#else +// The supported DT_FLAGS_1 values as of Android 6.0. +#define SUPPORTED_DT_FLAGS_1 (DF_1_NOW | DF_1_GLOBAL | DF_1_NODELETE) +#endif + +template +bool process_elf(uint8_t* bytes, size_t elf_file_size, char const* file_name) +{ + if (sizeof(ElfSectionHeaderType) > elf_file_size) { + fprintf(stderr, "termux-elf-cleaner: Elf header for '%s' would end at %zu but file size only %zu\n", file_name, sizeof(ElfSectionHeaderType), elf_file_size); + return false; + } + ElfHeaderType* elf_hdr = reinterpret_cast(bytes); + + size_t last_section_header_byte = elf_hdr->e_shoff + sizeof(ElfSectionHeaderType) * elf_hdr->e_shnum; + if (last_section_header_byte > elf_file_size) { + fprintf(stderr, "termux-elf-cleaner: Section header for '%s' would end at %zu but file size only %zu\n", file_name, last_section_header_byte, elf_file_size); + return false; + } + ElfSectionHeaderType* section_header_table = reinterpret_cast(bytes + elf_hdr->e_shoff); + + for (unsigned int i = 1; i < elf_hdr->e_shnum; i++) { + ElfSectionHeaderType* section_header_entry = section_header_table + i; + if (section_header_entry->sh_type == SHT_DYNAMIC) { + size_t const last_dynamic_section_byte = section_header_entry->sh_offset + section_header_entry->sh_size; + if (last_dynamic_section_byte > elf_file_size) { + fprintf(stderr, "termux-elf-cleaner: Dynamic section for '%s' would end at %zu but file size only %zu\n", file_name, last_dynamic_section_byte, elf_file_size); + return false; + } + + size_t const dynamic_section_entries = section_header_entry->sh_size / sizeof(ElfDynamicSectionEntryType); + ElfDynamicSectionEntryType* const dynamic_section = + reinterpret_cast(bytes + section_header_entry->sh_offset); + + unsigned int last_nonnull_entry_idx = 0; + for (unsigned int j = dynamic_section_entries - 1; j > 0; j--) { + ElfDynamicSectionEntryType* dynamic_section_entry = dynamic_section + j; + if (dynamic_section_entry->d_tag != DT_NULL) { + last_nonnull_entry_idx = j; + break; + } + } + + for (unsigned int j = 0; j < dynamic_section_entries; j++) { + ElfDynamicSectionEntryType* dynamic_section_entry = dynamic_section + j; + char const* removed_name = nullptr; + switch (dynamic_section_entry->d_tag) { +#if __ANDROID_API__ <= 21 + case DT_GNU_HASH: removed_name = "DT_GNU_HASH"; break; +#endif +#if __ANDROID_API__ < 23 + case DT_VERSYM: removed_name = "DT_VERSYM"; break; + case DT_VERNEEDED: removed_name = "DT_VERNEEDED"; break; + case DT_VERNEEDNUM: removed_name = "DT_VERNEEDNUM"; break; + case DT_VERDEF: removed_name = "DT_VERDEF"; break; + case DT_VERDEFNUM: removed_name = "DT_VERDEFNUM"; break; +#endif + case DT_RPATH: removed_name = "DT_RPATH"; break; +#if __ANDROID_API__ < 24 + case DT_RUNPATH: removed_name = "DT_RUNPATH"; break; +#endif + } + if (removed_name != nullptr) { + printf("termux-elf-cleaner: Removing the %s dynamic section entry from '%s'\n", removed_name, file_name); + // Tag the entry with DT_NULL and put it last: + dynamic_section_entry->d_tag = DT_NULL; + // Decrease j to process new entry index: + std::swap(dynamic_section[j--], dynamic_section[last_nonnull_entry_idx--]); + } else if (dynamic_section_entry->d_tag == DT_FLAGS_1) { + // Remove unsupported DF_1_* flags to avoid linker warnings. + decltype(dynamic_section_entry->d_un.d_val) orig_d_val = + dynamic_section_entry->d_un.d_val; + decltype(dynamic_section_entry->d_un.d_val) new_d_val = + (orig_d_val & SUPPORTED_DT_FLAGS_1); + if (new_d_val != orig_d_val) { + printf("termux-elf-cleaner: Replacing unsupported DF_1_* flags %llu with %llu in '%s'\n", + (unsigned long long) orig_d_val, + (unsigned long long) new_d_val, + file_name); + dynamic_section_entry->d_un.d_val = new_d_val; + } + } + } + } +#if __ANDROID_API__ < 23 + else if (section_header_entry->sh_type == SHT_GNU_verdef || + section_header_entry->sh_type == SHT_GNU_verneed || + section_header_entry->sh_type == SHT_GNU_versym) { + printf("termux-elf-cleaner: Removing version section from '%s'\n", file_name); + section_header_entry->sh_type = SHT_NULL; + } +#endif + } + return true; +} + + +int main(int argc, char const** argv) +{ + if (argc < 2 || (argc == 2 && strcmp(argv[1], "-h")==0)) { + fprintf(stderr, "usage: %s \n", argv[0]); + fprintf(stderr, "\nProcesses ELF files to remove unsupported section types\n" + "and dynamic section entries which the Android linker (API %d)\nwarns about.\n", + __ANDROID_API__); + return 1; + } + + for (int i = 1; i < argc; i++) { + char const* file_name = argv[i]; + int fd = open(file_name, O_RDWR); + if (fd < 0) { + char* error_message; + if (asprintf(&error_message, "open(\"%s\")", file_name) == -1) error_message = (char*) "open()"; + perror(error_message); + return 1; + } + + struct stat st; + if (fstat(fd, &st) < 0) { perror("fstat()"); return 1; } + + if (st.st_size < (long long) sizeof(Elf32_Ehdr)) { + close(fd); + continue; + } + + void* mem = mmap(0, st.st_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + if (mem == MAP_FAILED) { perror("mmap()"); return 1; } + + uint8_t* bytes = reinterpret_cast(mem); + if (!(bytes[0] == 0x7F && bytes[1] == 'E' && bytes[2] == 'L' && bytes[3] == 'F')) { + // Not the ELF magic number. + munmap(mem, st.st_size); + close(fd); + continue; + } + + if (bytes[/*EI_DATA*/5] != 1) { + fprintf(stderr, "termux-elf-cleaner: Not little endianness in '%s'\n", file_name); + munmap(mem, st.st_size); + close(fd); + continue; + } + + uint8_t const bit_value = bytes[/*EI_CLASS*/4]; + if (bit_value == 1) { + if (!process_elf(bytes, st.st_size, file_name)) return 1; + } else if (bit_value == 2) { + if (!process_elf(bytes, st.st_size, file_name)) return 1; + } else { + printf("termux-elf-cleaner: Incorrect bit value %d in '%s'\n", bit_value, file_name); + return 1; + } + + if (msync(mem, st.st_size, MS_SYNC) < 0) { perror("msync()"); return 1; } + + munmap(mem, st.st_size); + close(fd); + } + return 0; +} diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index c9605cbab3fcef..1ffb3afae5c9cf 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -221,7 +221,7 @@ function launch { # start manager cd selfdrive/manager - ./build.py && ./manager.py + ./custom_dep.py && ./build.py && ./manager.py # if broken, keep on screen error while true; do sleep 1; done diff --git a/selfdrive/manager/custom_dep.py b/selfdrive/manager/custom_dep.py new file mode 100755 index 00000000000000..1f6a26c6d6c070 --- /dev/null +++ b/selfdrive/manager/custom_dep.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +import os +import sys +from urllib.request import urlopen +import subprocess +import importlib.util + +# NOTE: Do NOT import anything here that needs be built (e.g. params) +from common.basedir import BASEDIR +from common.spinner import Spinner + +OPSPLINE_SPEC = importlib.util.find_spec('opspline') +TOTAL_PIP_STEPS = 24 +MAX_BUILD_PROGRESS = 100 + + +def wait_for_internet_connection(): + while True: + try: + _ = urlopen('https://www.google.com/', timeout=10) + return + except Exception: + pass + + +def install_dep(spinner): + wait_for_internet_connection() + + # mount system rw so apt and pip can do its thing + subprocess.check_call(['mount', '-o', 'rw,remount', '/system']) + + # Run preparation script for pip installation + subprocess.check_call(['sh', './install_gfortran.sh'], cwd=os.path.join(BASEDIR, 'installer/custom/')) + + # install pip from git + package = 'git+https://github.com/move-fast/opspline.git@master' + # pip = subprocess.check_call([sys.executable, "-m", "pip", "install", "-v", package], stderr=subprocess.PIPE) + pip = subprocess.Popen([sys.executable, "-m", "pip", "install", "-v", package], stdout=subprocess.PIPE) + + # Read progress from pip and update spinner + steps = 0 + while True: + output = pip.stdout.readline() + if pip.poll() is not None: + break + if output: + steps += 1 + spinner.update_progress(MAX_BUILD_PROGRESS * min(1., steps / TOTAL_PIP_STEPS), 100.) + print(output.decode('utf8', 'replace')) + + +if __name__ == "__main__" and OPSPLINE_SPEC is None: + spinner = Spinner() + spinner.update_progress(0, 100) + install_dep(spinner) From dd9c4aa37d5d8ab5dc8f7b5a08c127565d8e73d3 Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 25 May 2021 14:34:12 +0200 Subject: [PATCH 31/32] use opspline instead of scipy.interpolate --- selfdrive/mapd/lib/NodesData.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py index 6da892c81c8d7d..0eea798d37894c 100644 --- a/selfdrive/mapd/lib/NodesData.py +++ b/selfdrive/mapd/lib/NodesData.py @@ -1,5 +1,5 @@ import numpy as np -from scipy import interpolate +from opspline import splev, splprep from enum import Enum from .geo import DIRECTION, R, vectors @@ -63,15 +63,15 @@ def spline_curvature_calculations(vect, dist_prev): vs = np.cumsum(vect, axis=0) # spline interpolation - tck, u = interpolate.splprep([vs[:, 0], vs[:, 1]]) + tck, u = splprep([vs[:, 0], vs[:, 1]]) # evaluate every _SPLINE_EVAL_STEP mts. n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u)) unew = np.arange(0, n + 1) / n # get derivatives - d1 = interpolate.splev(unew, tck, der=1) - d2 = interpolate.splev(unew, tck, der=2) + d1 = splev(unew, tck, der=1) + d2 = splev(unew, tck, der=2) # calculate curvatures num = d1[0] * d2[1] - d1[1] * d2[0] From 3b3bc83366b0cf6f18221a252a947c0cd9a6212f Mon Sep 17 00:00:00 2001 From: alfhern Date: Tue, 25 May 2021 16:23:36 +0200 Subject: [PATCH 32/32] Add opspline to pipfile --- Pipfile | 1 + 1 file changed, 1 insertion(+) diff --git a/Pipfile b/Pipfile index 47254a1b557f92..87952d9f2777bf 100644 --- a/Pipfile +++ b/Pipfile @@ -113,6 +113,7 @@ onnx = "*" onnxruntime = "*" timezonefinder = "*" sentry-sdk = "*" +opspline = {git = "https://github.com/move-fast/opspline.git", editable = true, ref = "master"} [requires] python_version = "3.8"