From 2b69ba4208a85c5315af86ccf441061c436ba531 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 12 Dec 2020 15:49:08 -0700 Subject: [PATCH] mapd (#81) * mapd * do not clean the cache * exe * try this * smart speed. * Revert "smart speed." This reverts commit 19c7fb308fdc5063fc37004fc1281bb03a663d89. * planener crash * miss no_curvature_speed * 10mph * uidebug * Revert "uidebug" This reverts commit fbaf01f7fae9c30dc7cf335a510020e3268942b3. * mapd ui (#82) * bordershifter * smartspeed variables, sm logic, and img * speedlimit ui elements * add back smartspeed * more speedlimit * fixes * map_size * missing assets * Smartspeed (#83) * bordershifter * smartspeed variables, sm logic, and img * speedlimit ui elements * add back smartspeed * more speedlimit * fixes * map_size * missing assets * missing param Co-authored-by: Comma Device --- cereal/log.capnp | 10 +- cereal/service_list.yaml | 5 +- common/params_pyx.pyx | 4 + .../assets/img_trafficSign_speedahead.png | Bin 0 -> 30278 bytes selfdrive/controls/lib/planner.py | 86 ++ selfdrive/controls/plannerd.py | 2 +- selfdrive/manager.py | 24 +- selfdrive/mapd/README.md | 104 +++ selfdrive/mapd/__init__.py | 1 + selfdrive/mapd/default_speeds.json | 111 +++ selfdrive/mapd/default_speeds_generator.py | 257 ++++++ selfdrive/mapd/mapd.py | 600 ++++++++++++++ selfdrive/mapd/mapd_helpers.py | 768 ++++++++++++++++++ selfdrive/ui/paint.cc | 138 +++- selfdrive/ui/paint_dp.cc | 10 +- selfdrive/ui/ui.cc | 13 +- selfdrive/ui/ui.hpp | 16 +- 17 files changed, 2113 insertions(+), 36 deletions(-) create mode 100644 selfdrive/assets/img_trafficSign_speedahead.png create mode 100644 selfdrive/mapd/README.md create mode 100644 selfdrive/mapd/__init__.py create mode 100644 selfdrive/mapd/default_speeds.json create mode 100644 selfdrive/mapd/default_speeds_generator.py create mode 100755 selfdrive/mapd/mapd.py create mode 100755 selfdrive/mapd/mapd_helpers.py diff --git a/cereal/log.capnp b/cereal/log.capnp index 6128f788164d29..bc2d5de6f236bb 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1811,6 +1811,13 @@ struct GPSPlannerPlan { xLookahead @7 :Float32; } +struct LiveTrafficData { + speedLimitValid @0 :Bool; + speedLimit @1 :Float32; + speedAdvisoryValid @2 :Bool; + speedAdvisory @3 :Float32; +} + struct TrafficEvent @0xacfa74a094e62626 { type @0 :Type; distance @1 :Float32; @@ -2142,6 +2149,7 @@ struct Event { frontEncodeIdx @76 :EncodeIndex; # driver facing camera wideEncodeIdx @77 :EncodeIndex; dragonConf @78 :DragonConf; + liveTrafficData @79:LiveTrafficData; } } @@ -2221,4 +2229,4 @@ struct DragonConf { dpDischargingAt @72 :UInt8; dpIsUpdating @73 :Bool; dpTimebombAssist @74 :Bool; -} \ No newline at end of file +} diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index b95ce6251e635d..c0af1611f48d1a 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -67,7 +67,7 @@ frontEncodeIdx: [8061, true, 5.] # should be 20fps on tici orbFeaturesSummary: [8062, true, 0.] driverState: [8063, true, 5., 1] liveParameters: [8064, true, 20., 2] -liveMapData: [8065, true, 0.] +liveMapData: [8065, false, 0.] cameraOdometry: [8066, true, 20., 5] pathPlan: [8067, true, 20., 2] kalmanOdometry: [8068, true, 0.] @@ -87,6 +87,9 @@ testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] testJoystick: [8056, false, 0.] +liveTrafficData: [8208, false, 100.] + + # 8080 is reserved for slave testing daemon # 8762 is reserved for logserver diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index cb1753a04d18a9..af147697d3a01d 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -45,7 +45,10 @@ keys = { b"LastAthenaPingTime": [TxType.PERSISTENT], b"LastUpdateTime": [TxType.PERSISTENT], b"LastUpdateException": [TxType.PERSISTENT], + b"LimitSetSpeed": [TxType.PERSISTENT], + b"LimitSetSpeedNeural": [TxType.PERSISTENT], b"LiveParameters": [TxType.PERSISTENT], + b"LongitudinalControl": [TxType.PERSISTENT], b"OpenpilotEnabledToggle": [TxType.PERSISTENT], b"LaneChangeEnabled": [TxType.PERSISTENT], b"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], @@ -56,6 +59,7 @@ keys = { b"ReleaseNotes": [TxType.PERSISTENT], b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], b"SubscriberInfo": [TxType.PERSISTENT], + b"SpeedLimitOffset": [TxType.PERSISTENT], b"TermsVersion": [TxType.PERSISTENT], b"TrainingVersion": [TxType.PERSISTENT], b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], diff --git a/selfdrive/assets/img_trafficSign_speedahead.png b/selfdrive/assets/img_trafficSign_speedahead.png new file mode 100644 index 0000000000000000000000000000000000000000..8357ff8a9112fc7e630869f19868bbe9df2e96e5 GIT binary patch literal 30278 zcma&NcQ~BUw>B({7$z7cY6OvpL5j|4W=m{dq2uANM34-WG z7bF-Z`i$XwMGMHFc5>`~o>D>nM|uR3<@At;vDEDW7RR^(G;q>Ad*4)ay}f2Rvl)c>wb< z@^J9+xAw9pd1me8?jz)G@53b~BqAhkrvU>-_U+PAQ#O8XiOZs>yVhN^R`%Y2i4Ot- zbEA|N)h&YZiRkh@?fz?3SclZQ)jCAb&t#>-l_ z(~atkH6u}NpVn4I2aRWwIB+bf{NFPb53BuXGmen~``CvSl@sTa{o(WEJ_Jbtrd?IRr zzgiy}{DF9g9kAsWNQRv0i+Fu-KATzF=Uu@*g$5&mC8qU}-+$#Sw+PxR*6Q9_6nhE@ zjp&VeCRmHhaM|ZwBOtlDt^!hde%x!Pcub_z7_t+%$!RX@QNKXi%)diag)V^v86%~J z(XQzw?}e4!IPORCT5xAAytpH$GKL9lp3^yQ;Qr!{blm};px8eO*H$$qQL0GiMWOU*0$X;e$LegN((AHs zX(|7fjG*}3&>i=9qnmldB5^w!rGFQ2khiZ_tA5<)xsm}T&x;)0#AL3Y;U^Bu-qW3a*a|1>Cp@1GF@fSC zfMRDup1mAm$&@NHpA?Vb&5%8!DfDj?u%0chL0i&e&t^PJY)1Jq>Ok$cotcG`ADPm{1G`fJ0Ob}k^(=lEdKYqBscj&`4PU8 zW~5Oei9#6;tn61%;_Bx@-H%$D(4R`gWQ}$Gg!f%VYPj|uR-`bER&eLLJ z#C}m3x##_0Qb9)0qt4ylzN*itSL9&9t@r-68c^ViQI1e49bOdIvuCQtgK_uPcNRr% zeV{ev`;gl6__Li2SefrFlI-!o=}E~(pVjzcm#wxYZMD88XIZNG4Sx_sh3?aoJ#!5% zirMG+ZW_aDT0zF2WR?y>4BHTum?4U?$D$6!OWia}#39cQeUtP|+hX*OTEgBv2Xp zonm#<-J(EI}DqAcUV6hRs7fK94URY;T_e6IVh zM=f3Cq)`rk&{tmMoeH*C> z6r{%@rs+EqQcL4_eHo<)e55$-(em`MPIf=p6DVlA*b1W^Es^BkPK(G}U{1?OATQOo~sKmc~@#7VDc|uWlfQ^rO^S1id6QXfT zhRqJe^&xZp&xmjALV)Va>%iyH4G-c*$1jl~o&k}1LA^H->pt=awFcz|f+JMk!ireZ zq^k>!92({HeXGkMmjfPFfxiQA53&XQcE?tSvpY5E?rsqWzWI?IbXueR`gSMpvg#v>^2!zia;+8ijj5xfX&pwR01=RC8@Q9r}U((#?N zCLI2=9~*N#7`Ou#rp)U3>zfw5QX4rUcYyFBS7u_)UG(KUsF2gOdGr1(F4zqGC(jx0 z(~t!2E(%=Lo?Qu+1y2EDNSRKQ707Zsuj9IE$|0@lz^1KWJ zI!hUKhGc#Bi+ormtj!)2aMjMKjoQ&Y43ZAa>aa7;h*_qLis*G&Pm)sczqjqJ%omB_ zuQ;`HxBD_kNrX=d&o(y=Mzsv7AR|)EmiKSX{)s$g71LZaZp03v^j*~uagZO-x)1?Z zAa-MlLjM>x6i}W-eCyu*E?S#Jp7+*A5no)QxUZki*#!HY>^dmflGm7WRLe!m9t)tL zCE2uBl4Z=6#f@)|s<_@lyj2;e6r)Z(2~Un0P5FXj&AkOl@tVv@Ms*?lU<1@t>kfTL z!ywfn0EYZ;Z!cE^FRw|T-n1Ge; zkuSIG^PAssebp+Ek=XRQ`KhOoo5qQp71JJ#I{uv7M#?)J^~^kBJNGZwa*lunwRy@? zt#thvxye+cOA6oUja&2*jG(X8j%<_CGY=60U`D#9bTHMmLY@0s&#xPAe~H_Ym`g>y zvJ)oWh@(UrRS~utu+OauFPWzCHsl1TKfc1L>raz+PYUIc9CHJ-C@LPbrDdx&nO!*c z**A4JR9mK6SZ9Il2KG*5%jkOK(qR_YhxEjttz?Z8m7n`lgX==e#56mEwQ1Y?I#)-i z0=YlPPhlr~OK2_{^~$gRwCAQ@Bng0zegtig&2|%nKMi$=>DMCY=IJ8!ScH|vID*kz zcjv-TT)_{Sx0YG%NA|uRdUV8-^m}V5Bux!r{qvLJrf|b<)VbR2*m4TaQHySZ9u%ZI zdx>HvjsD6z2HxN9OChPii?5yDHIh^rih5*k@y_|9xPBD^&|GwvD(>DQ!;HeCyVb&< zssSY5|MP83d&FbxRg;SkZ-Z4rhol&;Jg##1BeXj8&P zXLXvkLCSpbg(JEJBDaIp7kwJAR$38EymS~Ox%4do@Cr$Vx9x7K-?W75&}=6`fO4qg zgF)l@na^@z)?os>@6nC|11PWV>o3Ol(1Z&9fM zB}sFt_P#4KXLKO$(CRWPJzJ>+<9L?5#>k^L9z!ZQpI)sET)A)1%60r;qLa3-@zeR6 zh6tLU79E6dw-I?2Ur%!~0BC1K>cIq!6hq^t90wwg#V|(dL8QESxlsx~pY0F(@O(_b zxRejiwgdWr6pPVM{cE`!N|Odwt$`oG?G8cz&YCV8$7CS{ZYvi_L~+eo^Ug%*5(3XYkIY22>9}4~{H$I^<|< zwvE7BKBU5_xwmKY_|5GNp#>uShvsGr;;Lk;f#TV2(ql6-GY`^dZ&akwJuM7)jiJdS zl_s=Eof}r-U?k_vnE30qqJfT%&SbDyVnlWD+1{#wTnaON()IS*dB4o9%X5;RkxV^V z0AgEU=OT}!o7mcNKbYMPHE9NN!Dfx092^`0F`v`YU}rrxcNka(mP+ZC=pEB6gK*+w zGc)nd2y?TaXPWs~PR|!iJ?YsV!8|n@&#W!~>~Ei+Z99y{=0>4!yu|&v!#-(aV>6td zpnC1}hVCwNQ8HaYo#GN<{#=2nVAst_dRs1o=cm#vl4AWrMM;G9d#%rPPgYs#mw0cz zcgI_uZ-k{A>7B$sVFkuHVUPP5jHc*nJv*ov^9UoD8EY+bh-E*warOxQyOJJe%3`u} zmuGxRi((>kwgrr$bFsJoC{5KXK5@AyqhIQ(Y}s5uqPsoBIdsjthysIN&4of6|MTY)+Y@;O`X-n)d) z+^R2%wchZ4`uHfdg!6QPqS=#bS54cry~bgA{JJn!>U=Nf{QZGvHjuq8*WYY62GG65 z8j+3yVLb!q$VB+)`zii?>6djxk?2qcyI;yJMb{wo_IX?DE9QtK{f##NX0o^-I*rJz2~YZI1jM6W%P?&v%b5T-^g7|)Q_ z9O3Ky`rF}mX*qt!iqspwOY7Ug&{yC&olJ>OcZ>>)^HihRHu{Gv$sm$`n@P4ajs9iIOYf0C)%VCWmshH{Zs&}a@14D*H)Ua#*{+ZFbFdjk zP56AjGwnOanWo4hsk2Z#kxh?zu@I5A~`twY6#t}YGI)dZsm&^&4)kT_gcbX zOL&MH<0PG{@Xn^F&oSX|-@VVn1sd$0HOPLHIW!WX?7GzdQ$A1$IayPbFv~*QBeQf( z@^(YO4e)h)>j2(Q)sE78%vY<0X+Gf6C#ItyHQ1(mB-iCMh8BO1&7FqBtIi9q&GSfY zH*PENl!8t9ORY7~H@bl89+kA2oyUDS;{M2h_!F?8jSSzHas3xky;a*%JLBr!>V&e| zpXP7uW)xnsnX*v8)^0HgMwXu|Kf{BUob=eqlk6_(Qt)u?`Y-n+td>7u>Y73qn1ibT zCyF@koL7ltYBl>}7gO^2v*?j2{DU)Xei`>6cMEjNeB>^s+@jB0SH(#NCx(;Qj^oRX zGI&#%T5JM9F^g3q$=Ob{Vww`<`mkvg^2K)B3u^Yjq{yc`4rIuLOd*HC?6TGVbRMt@ zaV_k6z&E4X%K%XJoZ+{t-^W9anKI*nBL zYPMQFVemllA;S9Yz8t|dkaut5(&WKT=d{}#^MdUBiS9PFj>Ol}#jxP$Wd1G2AHIRi z9gxkc0FpP4%)JN1*EJOJ$<7GGZ;PZ0JttI`>^J=9LuzG|AlHN@V7t|xUu{NFu$IGV zmjR@x&9#LqOiLp+eLE;x?a##tt5E$ath*e=tykZ*-csaY-#MX69-DYZJGVf{E zoNReLKr9@ zCFSKp2fd1CBBSD2p36O~-0INj5gqp|;6(Z;-X&-QRbupX>b-vYrLG2^&hB2vy*wol zpSU~A?$KoZ7bTHlBtYGJt<&46qpdo*e0${ullrBN+^)QC^zv3?O=A@1(!*kwqI@PnQ@gHdj+)q*n0JG&Gu$+ngtq1Ohg4Uy!CiOHz8LjD zVKcKHd&)zuA%_bmv;OlBLb%_9ApTEbhy6*q2)G=c2yaL}vVP zWmHGM^q#pEH9sXq({um30j)!DCy&ZB!kzNxMV-~;Rsx?@zb>Rt3#iOg8_`G6&`fM= zFX$>YCDKTWgUG&KzLglb%VD|s9@oIHUXk`3#jy)m?}4McoC?A!t%+?fWnd0A-Vg2;t(J}7iT>>~`6M^i zFGH11DnbuFTY3)o#6I7gtZeqJyCevxQ8oor3!Y}rS^bgqsG|LH`v>8=8QU-y>KxpF zuJsl#&FdE%l~a}<41tJJkAOA2^;3WSRXnB@lKmz^xWS*+EU(MIu0R<3`>Jw-O*a)o zHB0;D4LSyfxTL3fk-`du%_@ht8CrG*3LbNERZZQ%!BlE1_I~5BpOA&s+5G_vt#Xe&L=}(q}2tzro@^?Nn5%Q7<(XIlo*CQ%Bp_ z?231ZEirgfB@W+!-;TE4O`sFHzN~uv897sH^w$^Z63GT-dzT07DLW48M*%fn?Nbs( zY-2aPSX8kAJz2+ZX5)=@R3FF9OQkqypH=V3zs>40S z*sgG$CgpaBeo0Ss7YXd9+j?wbVUN<$*6!z2?O(#*QAek5@%1eG>nRe&nl;Fz$)xiY zNi&a67JTO$6jJF;VGjed17l^g8LPkgUf`K`gd7McnmQX!q6~;M?NVXS2n+fZv{6=@ z0cT)f*z4s&KexYWBQ*kMT8c9eYSbXhEmb6i-0g{9tQX`4a;N;+UoFh+z_@IQGO;pQ zjBJUrx{cpLXvwmXHAP|J;-^U)C3=6z7(uKM1@^Rg;+DTXh@W>gUHRI^#)fDtf1C1t z{aQAbzi`~Og@kw&*5+mQ`WaF1YBl=p&4BU3SUR5SxS4IV5*+u(8cuy~3yK?2otkSw zZd(SdTxD&KjR|nD1~Ki198NiA+DDvlBf6oJK<6Z1TIi10wXi0?Hv~&UqS@9sMNB1{@b5cr{Zev136Iuy%|4qvU0 z)E9An>z7QhxEKYV$QuO|4TpuiTKDo4N#hpCykcS6{fZDC^zjx7BE%Qhb|`n=RDb8? zO5@F;4;p{U2fJS|rJ$Iajf8K%L2Y>Cy=;q(`8cw~i_YCzh;V9oL{;AU?+u^|T^ym6 z>pLrhyKy9Ir3Q%e)5((&Fo&h#@4AC0z|%&Sp$?moz>}5H`kVR}7#|P{d;{>>;9%y+$oD1GMqbN;Wl1T_D$ezjTK;i^Wb`6wQyr3WLWS7mDm z+Z7hSLYZU1rD-r3!5^epVok;HwKUM(BJB%R2@pX6UKW&S(d+OH7ZE7(D#N3q?x%2N z8>^vw@4^k2%G{{#NSU`J>#2exTb?QJMx>3@LvhS9AoF?j8yOsX)UR)c-+=p%;p~VO&RCHkiCB*bK(;vKKiLMDk>moBfrCSN)8bnmd4OG&ICkC}CRr+*gT~8SjTxOw%lW=d zz&JeH#WC9E@SYTYalhtn;T#YqWLZ6fC!`2S!2U^+8%4bJX^)24`(c-Mmjyf-q_h)O z3UTYPB4iQg&bAGU$v2*14IZL4s9#r9%1q)P>O7l044ZJR?-pV&EgD51kU6y53%Z9` zf8EEzn}xcCOFIGCw3?YV@Kc-!>oBdQ0iB(12UUz+T5to3@tC8bGvaobWY{@=qP673 zi_1}d?sH8~Lx2R|L~_>WjcKD_Sp#9KaqNwr!l${ABa6q6AAdaio7$Q-KPeNgV;sd6 z>OtOIRMcDg*|;tG@X&9WA~<~ecXTUICs+-uzGc+KZ4>Pmoi@rBGI2W+SVX6lUC1B@E6p4TIN*WqW zeSLh=+@jo)TH0E|TaPz;oWic)jhFO(KxTHEHc=Rzuk{fA1Z%V>nwiP}!o6G#38#EE2A1bWlv|Bu1qg zO^l}H)ZO5th`3B`I8vx1it8Vp;H#xT5ohmhVrt>7t@}S8jokyvCUayO;l7vuB;{3v z(8Iztn4Y%mv&zN(!aoK4OYx@X+@g)%@0z_U1Y zP>*Z($H?=j#|xj|vuZIHmypPiF_0Wbp6^Qjb`b*-nDd3ZuEa=|_8A&1Sw=AzYzT;b_LhR7?c-t z3{2Rvofrgm6!DV3&9HiqHqaPxx!6>^RLGhQQm|LNM7u}m9=p-@WTkDbnTbNK&Xe2} zA&U|jS=oVM#nUU<4pnxdSoO^PKKYU_H}+2V2H82$I-8eN%Q2m$Nl$Y9v2Kzhid%u- zFOA2YbvixcrNNXT9L>x!*~Fb0%Zje$mWew4=${_s>phyh{&D6WmlxDL-00iyOnATP zU-9Z$NhG@E$P|p8S zP^Y3Suc?0coAaO_n@t_Nm=MGi`J-=*d}6 zCmKjkGtM~l@5n=u14&YprI1J_M~5N3!*G5h_A1tCTtOSDxw`Gih@&~@#bi`2A0`mo z-XQ|w29V8yBn9W!3|$dr1u!@$e0Z}bfnCQCC6z~={`!S@Fee4_LB1-%^ZW5dnQnG= zwopgA81>?ijBfH&DQqxJq_djA&Bg&6XaBv~3Ld6#cDy{$;)+F3x*TU!kYcMHs0Z_| zkN6P}E@C1KVa;7_=|$=GVNk5xvdm)nEG4!W#I&?CZsw8RWv3Um^6=CzWV6wH67Tu_ zz-%6$QqSa4(d!@@oTSG11Wp*~apfA$!@&Vt-pO5f#!DKTN|YHH4#Duf7s%D3880IZ`2ebIWGvQe@#!%n?m;dl$5fGk<{e@bVD`M zi_Tmqo|8nuDxX&P5jVP+-Hi%+7OXUH9mM20FDrVW&yw z)}f9>YJ9)IeNh#xVSui$U|`7{8nlgu=l^->n+(GvT3*;7_9(uBDzWhD8s$ z4l2#xK7!Nr7mxhV^d17%Uq%p`1{CqfW&|ON?H;Zi%4kco+v+}w$L+NJ1y?kGuyClp zeDy(CmcHw%Z@sUbXt2kdCoISZ?5LPg3lmS*eiQYybjTe;69lz(WTQGvaKW4f zgT@o5|2lea}q5Y%@nc9=Mq!2(@LUx9Xuk({jpx=;9{xmPT zR)~9}>kIo}=1HJKl(1^-F-*ka(`c)Otq`4Zg&TR%7aAEdbc2}+48YevGvw;NFHy^H z|9z#k{T!x&PAH~zQt;a`rm6w^OPM~#h<-9E2iLPjAUtKnvQbd$yX80LS%p13-M^d9zx#^Pd~6h$&k z3L^yJv~|xct`l@7anXRJFntj}fq~C0KZD1gUig#{!dju)B1GKnZS57d53@@xDG^>) z5#0^Xs1tP}95={(eSLYK=2~V72#llbm7irWeft9>E8{*Y z!N-fy4)e;HO29#(1)WTZySG|DX7ns9dBYY6g$kn^$&@Z{E5yppKmA9xBzUC|cr#7J z7Lf&I1c~d`m?8%lC(EC5Zt_7lP+?Fme)LI(oBthh3B+rae`^Fcxc!=ZJxkKVQhm4$zhlnTb(a$<7-by-3u7dWQ?RIeX1%RUVU`1S z4ZyO9T^QP!G_K(iMg(+-!Mcl1m_FT9f+L&PxF+!W9)=($W1Vd6z0xE`{C7iB7N&8i zM@;)4YyB;wu40D1rS9L*L~Y+?(@pUSbK8{OVOiaX%UISIHZ1GPH||EYP1~a;BV!L1 zjXBlvn4DSPyer=rC4Hyn8y)(F{kh`XzC>gM^Nnt9?_Hf#L2qm%O*IT98YaRcHk9R* z)TSErX_UL{ZoAJ5ciz{cpG?^sT&o{9GC%K7f^>GUE%H+X4HS!|(3Ce`IXBe*zJQ-C zF}MEK0hhYJT@HBC@4C0lc|KW9w}E!^WdIkjya;q{?Bhn0gywJW(!j92;bq$Za=71z z1>9~!trclb8rN7`>5iQ*mb~I8PppKSb%I@6+(3RDDQ9VAWCtk^;bl_?B>n0sP8PqJ zn~diVQdwK{Ubm5^xP*{Tyt6n53QnCvo@-gvbe8#K?FaYnewRl*(6@aW->@U#im~%( zzpn{*%nm5TA5?_RnlVo&j5t7D!CdRzniIS!Hfe6&pFrb>6leLZ!=d44>G$oaaGclD z;QL7%Zu0W-A>)5C+$iu=zvy8%NBGO@R4jsbyI1QU_A|Ys7KpB*LquDc&(jiL|TM_;G*l9k1LAb4sZpD2k505xW5Io9Lzu z9$f8AKH$Rl5!ONtIl-^XXG{WbWK8DJUBEd{Q)ZqTW~<;~Fr|`(tGsn(3@Ih`$QAeL z>TfgE5g6-`wmB#xkm*gcKY_dD{E8qD791~QdiVc~4EU*HH!-%MQ-6a&7$_GyeHuEB za-#->!=BMI#6}tEIy|k^mmiF1cIZYMh`(SOgGYb?F?PRbdd!V-Tid%6AG^m&8Pe zQ3#i6J42GeHfs@_qHk{-z|Dl}BUdf&4OedAQ-MGmw?0*M@?HMJ$&7c+KUvlpMcO>j z?xkzE+Ifkj4@%cOadb#(Y(%da&)x&e zIEcHb1Eyl}9u%q51NLhr9u{nnO=?JZ%+0Kk#6LA8XF|_x@Rxf`Ogo3I%EJ&k+ks0GFmQ&XGN9GmYGh?$3mY-&7|$A2HUUy^jeIX zzO_{Z5$q?QOYVKL$u6b;EF><24>X=(al4CV(tx#`gV@)48prAh^h{d)i7=Bq6r>$9 z5y&iPIw~6rfW<5Gy~^-K$*u5i$W%ngEz(TRC-7m9dc08T)M0*h9i;$JpSP+O90}aD zFeJwSg+Bz%Ve(y$qw=uy+^DGwVrqjSXAFKoI$jFw%KM6B2y=d-cwSUr7#R?7G~=!3 z-N5Yj1(m+7$=qlAy&g>(WuH$EJB~G4O*8%5d^9Jzpn;T{WaNs->&n4? zhey}>dhZ)h3gFMGM|4H~6;;h~ALzN_pU!tsZ2~2dT+7fC`zoE*y{v6J^1i0O-}%=s zvNR_%Oz0KccFZb0?0xccT+IZc@jmi!Npdn8osAAj`{o*Cpmza6!Vgv&;#mCG)@1sS ze{Vuu$M*k-(FR4&O<2s|SB6w6oBZa!8hCRF&qT2#fJTt5Ke&MoOBN=1P0^yknti)l zH1D|>g+DdV9lJ(ls(CW)w@D59{zFH(Q@)W`VWpyG9X6ApUFXMwcy@|Q6xW`GryJ65 z^h&6(mrB_=SGf31p-JO5MkGXR1NLkFE}S66oosoBZ0$PfYiWsi4pTno-A7V4?yg>oEJ3(=if^PXp|FM z&o~QiZjrKI&Xh|Q7XH~;W5#b-I?EIv>h|}6e7}BTU?-IT$aTWPe}H|aDbl~y#yFAP z*xAJ;h)9KJyBR-aZ$JwK_x5jR%FjZq?*Iq;@DE{9Y>tE}-WEesop+5i?qE`swXVN? zJgT+M6;&lVnaDK~Xkhgk*l~bF)@$WOQR#@*KfDe1Ih`8M0&(<5q7MO6S`YA=-nzV^pwBOsj+{k1pdwQOpcg;8YD(rnnO>6snu(IL}>3j>nw=`7Z1VS)~sc-5qb>v^v zTXDA?y1;f`3Gm7=pvM-Xt~Q0|YN~P^l&EykXiOiE^Sf;U=im3UKADu=KlBT+9qQ1& z>4c)3sI7YiLe{v|<_b#5`<<+$uE{+<glsm$i?^(J6bq$L2MZyP&D?m6N3f|-?_W&Ym%zC}|pV;8813@7* zZnU38I^XDy*-h$~+`%21LmR_2v$Dd&TXX;QJe=T^L=u$)9HEvK#0eme2?yO9-lu^ZBf0Vee10 z&^}}fRErhPyii2|E~Olikx09isCMxtkvhlN4cqP4k9Sj71yclffiE$zfqwq$8uua}-$fZg5O$kh-P^kg$o5L61J<$BP;ZnKj|&$upFW*Pq6jtvzgE72T& zux+Gs`GIx17}(WNNc>53+Hefh{St>CANY5vwcjq{nQngKhT9)t%)@tl!;UGav=a$gvrI$Qr`6v);3M z-;ME76)?IS$c=0K!_+yZuFMxH!x_9eo^0=xdk9s(N`9NVx}=MksFgBsM%Emdas_LW zRJ7nr(lIn0vA;mvYAVgYUO&R$eEvfGkVAx$c3&^lSpAEnJHoCgT$y zyWCt9NX>Ln4t+z3<~*>*_e33N*JT@O0{5ZD%vd7v_eJLEq5qvXoP;0^5J=9@voc>; zQ*`3mB(C`~4h*&oKmVyU&9TLk8zltX#Txz|LYBl@-_o4`#F9&MbttXFE5jMVmR z07Y;XP1DHjD2ZBa0~^Ot>U5`b?Cl++FWD;I%1ePe?G!;#T?g_A49+rJTk_=WT)&rM0H+D|`Ty&W071@K+<58USKHOW7pg0q&#! z5t^KdbieyvsE8w}buI@~C4+B$I+fCSZ9GQU$7#CrO9l1L6`JG*~d}OU5}KQGTOuB04EL)TFBLcc!>~q6^5ypJOn_+6Nkzi zHv#lY#6l35|4ikTrwl9MKHd-{Aq$YLcg%6D&nXi(GNYIp76Le*RO*)5)R$9O>m zl;Avf-h2O$4!JJ>^_@AM>^1%C1yx%4(IC+nu&da@71C)t^7e~Wi2}P;Dl0xk)?mdD z7ywjGgc=BNl^hDt2qnW6X|_DvfkJv{DlgJa=D;}Pa|l_gzQw#i^-$E~b@s`bo| zkm5}HUOJ_bR9L)Bdy^LAQ+N{N{Wh#j@aI1zEzT_a_ia_@$h0nQkUO$u2Qf8uzP!|QCC@6 zStUITBkv}C%J;F&Caj0Y@p%{ZV1u8J&;B3TS_^AxH@(cZrJo#q6AH<=O+LXZKqZK- zY*bQ)Gf?v)t}+;aOT|mclxaN45foC(@uGnmRj$oK2(px#4rFAchLkXHIaBJE>w`f& z+qf&c=j^x!@_{T#*VTo~JXk#gYx42gZC5jc#42)6dKf=KxziClX9E#9bnN1o4qro9 z1@AtnYJr#5;l51_9adAi0*o3ltDvasQgxwE5!SE1e|r<~F?$hz)g+&3k_!R!NkJ&Q z@EeBgT}b&z_MyGoG=PQ1RtGY5I5e*pQ-sYPgrgl&HL$PZxlXqNBNj4PR#7!K*lWnK zNq*KS>hUcv)YFC`NhTM>Z3WxL#f9Rx1s&*Xo?Uc>+nX(nV%5x8*7;>uN!4#w#(ET zs%^G!qlRyEk!X>JUDA!aNYww~^M*f#nWHI!X8jN{;raka$D(vgn$~VtgB=Rrg`;w- z6Siqk>CW<~eNd1rd44#?W>$hy-L3twAnZ#`t9dT$Q1gT^T~!iaSfH;`VE;BKV+N@h-nN0DF$CkI6X(sn`DKEBAmmk}@|b zzI*(e!8eZS!q+@`bV)NQZ;}#PIYbuj!TW@%#GjZ*%=oFfwPV7~iN-WAc9_|+3c6Vt zB4l)BGjJ#Mz@X5N(Yg4Z8!7f=AwtZNpuW-IJDr6iV$0iZQC56%ky~NJQ61fLKjuPr z=jz+~uBeXoOfzZ(JGlVt@RfX)6m`JcfzOCPKTf~Hg#e$Ci*i*`h2-PbKiBZjwARrv z12hB%{)(73{G*i*TK{>8OB1-JJn}ubfZ$>^auH!){D+B9Lz)?0^&t&6bO3QRfqo!j zu+Qx;yv6Z9Gz+JzlNqyq1p!$8%e(bkwfx|VqIA>!!Dqx+(f{x(s;OK%|E`F61VzmW zm0Q6ZQCtVnoIm*Yu1r)&@sRTKrszKU+=c;eHSA$bem8C4$uaz3!^R2A|Mwt+qJu+C zIQIVxp_SJC$>#Xkg&h+ALkszN7ahLhK0k@EkMsLjVHKiKhkh8`S)|1q{vSQYNI!eC z_01~`6UQo?P4M`!wFt=PE=g0N7Fc5kF^=ay!iqrzeu#ZJ#W8Rk%@kPt*)YkW6)L5l zN3@p0ZxC_|39|97@~=(#&1yt_t2^p9}NEKNx)Up&jYrV{XgQm zj>WTzg%Q*S${YWqmFlaMIHCM^$72r!RWFBAGfd9V-T`qYn79AAzqq!U~B0lz2KBgJu|M(o8 zA)N=G2cx+F?xQOIPRs?vr~KLZ(tq(`1^_2+BNK}zytpu>GFysZ!PR56*`@C3t6Ns) zbpPluAD_ZSlJ4~_0-BGlslLMVBz*whA?8K@J8k}3d~$({QH+35xO7WNbBe7RjQ>Yy zF*;awZ%D_%CqNS5Zl?EPdn&~A=@rpN#b3@$4y;z!q1 zp>nF<@9xADj_6JNe}6rY9rTkb7}cC}@4r0Z-_$W~_nJRgoQxJJ&|KE)0w2bUR$pw? z8~?Tk19Bx`=C*kb-~cy@UKS|-Bkb^!_U+Z_)hQgDKyT9d(^Z&${GX-yU-Gjdg$Of*zoRrgzxHKN>D0AdHsBqj6k_w zn<-jAyYV~H&1Ce&85DWI{?9%)ZSrH56Gtwzdq_`?DKFP0?k0ta1p~0~#RYk#In2ey zC1H?6i}!A413Md=w(NY^dB_Nci_gQuLlS$5$UdVmUOKo#-j{oQqX_bypPzpWxZmS{ zP}@pO7&j?yXRZd2=xLF&yZevH%i$8Cr@Orl0oc%qQWl$ZH!LQ-1@3Dte{op{4{&v@ zQ@XcN2aBTk%kxzVLgEO{wubg-&d{{sH7|C&I%wSEpVNdah zX=e%o?3HyQq`YDMZq+rV*D>F@)HlYa?W#Fb_s1DjbdqaC0Inl6bXRdI_jP8f==JdJ!W~YO-dj4}P;$rlLV|jqQ0Ke5@fZYU2 zu?TjTBmBXJPLKuguI6d|G!@dm!+HSkN=%mv@akzG>H58rY9)jRGx_G}#*1CBawgoA z2>{LhPpBic{SLblyy^smb{E(fk3JukWU1-&Kg4=n;shx3RTk_8_y5>CsBG9}8`6PFdRdDWT>4Fn_vWrVNE z6g%eJAw6hrXdbu)*on1F`d>D+qi;tYKO>mjRE4MO;vVl^2gM6i^!*zD+ItnVQuohO z?Y+>2P31Tl#JZ!$!xh_Ve5_i2#{w58-}mx~6B*YxEZYRn zgl3vT?A{r!{zc~}0z1YRu!-&fLkpLFGuR;tYx=oL)=y4#g#WD*kTd$Q3UjLM_{gKICJapSoP)D}B^817B5iR~xgP_4_HY%1=m@*4Nh7HWHiGFD?MvN(6lK2y3^L!(*t2-i(^`5-O6& zMHv>tKNrgu+|;`~Nkhl$0y4 zp4Fv8_H&<3z*i(=#g^9yMIK$N(B{O}2X`X78u!uMfBO}sD_I}nqzwP< zeL8Oec+-BgsY#)bS3l-~DT|f5yL*YXd-1OXD(e1es(H2S8vEWIN!UQu@+huvjcEzH zUe5i(8BBS&+8@3GJwQr+T4dPdl{Jx`EUq}oX;bT(A~m`@C3;Og>#x!;5I1qqnFqNN2h{_f|ZA{Ejl*(T>z{%jtHkWvO=~BQ%st4Movm0$jNd*7V!_9H%zf zcF0&KA3{QJdT~Yeurs z+4PSW-zyG$x`<#y0SXsHaB#=RV`JD|aGmo{v%8zrkKGf8{UJndi|hz0=TC|k8&xB{ z@+j$Xq}~)GA_WAQ%_gMxbB0i1eP3C;H-B0@wkY?0I*X;C1F+9|Jb1gWGa1SDtg~j- zL?4*@EPvzTFQS;8uT{WHg3dxWa^JSY`|>bscaclI1x1O&Bf!6g7Em<$AU-9KmN9=g#02`N*MSUgXn*pLID;4FBsv(kiwnnj`-^9*w z>CKvOe|gghyEEARE>MSZyZ3msnDc>B-KpmuCtu9!!7eeP4Kl2Enj?i7ZdYx7^;Q-E+dNV(k~6(yZ%x||$mI;NK~Zu03e}3j&pv}E zOV9lj#?-8F!oKft9Vm8{mzQsa)8%BA0OKbbS;8g|T=@-M?wb zL{#k9dpRy27L*nYMSzZKEQWKHsxKdQXv~T70E?qY(`uD!eO+K07#1JP=hS#J`etZAd@rLaMJ>u8)xpJUEgLe2cM@QJv_Xl?NWJm; zOYS?e2Yv{D(PyvEM|cOoT3+K%E{1oQu^ioqi)XaJbDpMD{p(?K5bO`+6PRA}BBhy$ zKqQ{@!fc{eNKaFU_|~t^QSXkFG5T&6*uLLykZgkdqDJk-90(5kFr`$XsNvoE_mesz z8wH=RvyNR#U0R8AEdpUv{O^vC+ZZ<4?s_8-d86|J1tIMYo^r$V9m!(II{l6j*DMJ) z-Qz2gutT*teRAkw@koK_q)1_`pI+rBN>JwANR7{SGq{>4q~{4}6Nl+t5MM(?x2RFBhs! zZ8O_+qKAE;h@Ix_iI|VFMz1i|jxXHl`2PJnXI^dj(OVc07R&F*I2T#!sKp}T17+~V z^wm!2tlrS!@S@ZA-LG5gE^HI${7r%94^&vpeD%S;1sfT8y!_E%kT;!u!;=oFcBZJS zx1s5TMbBz4Z#)N7&EZJ!MaEWojY0i^m7=!xOKR1131Wr(>p8mpIY0jOqyeD_ncx+_ z$el&x7m%tyH=bS^SK2;(4(3XuJml-Qsm^Mx8WvX-sw9f{?lwgNV&?z*?II&do(U8? zG)wN18T&c(%oT*d7hxdzx8iP7%-2|tC-1LjSlo?MHd@{Ty!So(XLXAKs5`Utei7BF z!`A42(7Z@5`yqPSyS0rpphIH>xcYmFCGhrc}jtW7EqmR4hK zjMVS;q_%0jg6|1z-Tl{DO7#7ah7Rdl{fgz+D(@uFS-hPoOit(gOO2E@Mf~I$70hCo z&}l-Uh6T0d%k*`4ez@qEaSrTYv7NR<^`2W(z)KP~&yH5Jgxq5w72~5EQ1cy9^|)rp z+c%~BtJ9ugvqmx~NS(Ipiog8B;&)nH2*aW~B=&^%DIz*MR2V(4$l&n$>1_&8nV(dW zKaMfk?i~g@(m7o(H12PKgAOSBAp;?h&LSYURq@a9t47GwdI52@%(rHs>K>h_-zWcI zHz_DreCEZIxjN2!g1hP^^;|}1O_QE!1)Ep&lsTKV^ok174pQz`V#C-s)93y`8D7p* zIm7`K=2S#B$a1!KraL3vE^0l3N%&i(4og~}t@P3GRHE0Xyl=&`{*1>9JGR{nFo+L^ zl`(l8ic2R5`l>%r`*E{K1M-8-Jkg4In|kozL1K=^YvqPH_oE2)6=f)F$AqgN;dl7} z?=g*`DC~TAu;-c1ss%J)z;jsYoYOSTrqSZ@kjt-Na8z?j;v7z$^$hLkWNBstX@dn;BTW0a*{47~ws|u^x=ET~Gj$C)fM(F?% z8|umB9d$-JYB4f;b1dRt%c|s6k>j>LFl(#@2Z9NyiS$JS@x5X$;a@{P;4@{TH+M~O z;CE`hfW?DDJzBqn)pz?eRzJ9xMu;n5n>}lE`+G?bjA0wX)`g2dT8l|gEuN?)dQ@0d zK}$uji2m~fps5a4h*E{RA`^a!0e{lt%;JGC|ExgsXn|-kEUf+D4~#>YE(4rafw2@VMp|O8P3?}y$E6TofhJjp~aknd%jRd{$gRf#r`9l`*=>l zqOXnFYri049<~E$^j&rIO} zSLKSE6abJd2G_C1%Ur}q89^qlY9#jXBLaxEKgxD0(e}Sq=1CRn(>p>Kb^yhu2>#N2BbRJ1I53oBt@u1~A z`roHDBFT?=+H;d$EUt#i0)YUlu%+_*^Tz#>()@IV!*t+lTEAQd;ipqKO}_q~7vll+ zdX@$5E!D~^4wD9+qr3Rm6puPl;4()9teycZnTgmShIiQHK-?jr+4k|W{5;b}N%T=R z^aVS%G$d$J+awc73^#gF5pZ(_KJ84iCh$6+rI~ZoO=PGrYPOb)sxFrS>wm-sptRmr zVV>r`7HoyIOzwzTSW)wRuIyraI_ty{CuJb7{HaKXB~v8qFPea?dYE(;8YOx?sgZoC z>6VItK1a^;s)=p=`$r!gNFyMW+POO;$RJeiJq3Kq=!V1#--U8-r6J53)KLny9cB0& ztLW@Id*^gV9p=aS?n+UrG$KXk7O`VUesrfQa@&cgCxzU%RY2bqA?iqX)a`3L$I{RF zugBYW0T&}I;_&5gdVq9Q?v)+fKIHQtm-&izVTpGlXDu_2KK~X6c3yNyW+lCkk|hh; z0qvSQE@fD}r0`g+nY+XaD431^Iv}k^H=Oa>N_u+_n`0m}CmxtN)jf0NoOY${I%;w6 z;Ma6CM{fbdGAN%2>20eaOft9!uw-_WcY>*kepYpRwuuc$EjSnaYrqBUtBK}r$r)ks zl5+2LV2Dy$O-PY+%a(i7K~51e^l|E_!H;g-xaaWGCPw6^&^6}IijZk|-7-_gRBrROI$PnFL zW8m!iAC2U;qY3Xbl1Ac_m){sjM;HIDZSF3|ctc0ZRg?_``6{GNH5D5~t$#`;*=K+2 z-btWIw@;Pv;*U)V0SOuO5c~mnni-&kCy@d~<-0WfKD!faf8cDcd+pPQ!ZCu)g;#pF z`CUVwJlVQ)NVzU<6V)#Ul!i?-*W!7_4Tt^jq3^Yop}{o#rK^b!{2ffZMbPt;Vj z7z5sco9krj*LNCHmXXkdiN{Hgh3imD+T*3}e(Jj|DEs=-COV;HSAl&=IEma%dB6PDSZ8Fe8OrFD+>g5Ub-Tzj!=$~5{oK1EdL*d ze^rG{;zUPv5b>+WpnO$-K$-;!!i#A&#FN_^Ds#py9;}yC$EFRWXn@j6V>7zjZIvuiY>Gk9ALsI#{OsVsz`*>m8qMS0 z$NlyIm*RIO&ZMqmWi9{Wb`ReJFCHM}_wa?hlmqoNYtgdBvdypR-sX{NKZNNB(I}gr zbwP|obnG&SXhUyzcQQJ54nM~?^T45_HBV~U&|3Cmc+2C`F_?}e*`GWG*)v=6#k^6!;nE? zBtXk}>gDL0q4fSwwZSqz$!qzrm_7lsSxR1yy;4BjxoeeM? zjS>i>q(yc->IEbmKy0B<6L`{ifjw00zJC#H_h(c~Af4Y8x4Hfi@i~s~Xa1AT7?)uf z>ryKg)ES#(T`&2*#F(Z>x&}O9W!lc&7ZFJr$?wa!l*S6q_AP2*;2Gu@Ba5eH ztSiRJ=Pz$urJ0|fCvUIAoC+|vA|S^PTzs#V80ShG)~&)UV#}xaY10nP&tEXz8lf02 zj%a`+2p0X^Iev9l1Ik9YT^DLn3M%G3 zWM&CckKLLGEQXc>ub5PN6l^6%Oqg;R`t1ReBNs-8hMcqV1*S-`;3@36&Z0-3KtICd zGU$YWz;L*m+=~wh0`j0h(oUG<>Pm2lw5ne7GHc&gm!}Mq42=sPqHfF&2gUkpgY3;J zUAR=knKlJk+P+-F428&3vCF26%i#f&AeA<_8Xlc{d~D2!8OIqrUAm$cR{@eF?qVPg zb8e<2AOMsUyYrk57)z-R5G9&qQ*tKhiHeCx2`C8UC+2%>F8#dU`BvWIaO**r_@lJY zJhWn$?a}nU;7Ui+q%1eke99g= zK)#(4uJU2gH4tt=_bnQMWY|@{x6>o4l-D$Xjh2Z7fvFGlXX2HtYD*m_S~WjXVFsa+I>FU16a!XjW;)@lIA>k|4?|`o zFfghSEs=Wh^VT}yr!qmLL~sNuj=jUfb{6*LvO(I)o2+%Qsz&qIW=Y4lR(>;gHaS4X z?tcnGV`Frv3dacrK?8ZCfkM^T{yR}ptq~EWWlvaauNi4tR=FQ9F?xWHyQ!2m19e#t z^0@(VRTO9Qy(3bIX-$zOeR$HVrslD+Aj_wLZp;;&5XiQjeaCerAR__Ys{LMte=%A< zC5F8j?c1G?fO1BGwj@T2xSX2dP000VeFgZWxi*i;rMyIAKnHR(&^0P%B$ESD=pk zrySQu1}gu6DZo+YZ+wi*l?cV*Z>dFvUOc7meS;~N-XsU!QsbDF8EsLHv9bh*<8;jc zsC6P5PCN{a20sfiHdF@_5`@d|RpXt=b3W^a;VCYm61?l`XRmH|Mzv;toO=XPo~I-) z2b2sTY@t8C1jqPDYC=o+HdOE${=$M}qfiyW2Q-z=CeM@~YJ+K)t<%!@_c8fX-rGE8YEH&n!(~>hy1n4EaZRy~nmYe7P^ z9C_??Mt?`$2dmN2=WaSDGQq2$qIIVhbOCSk;|HbP6z#xMz@qhc8*wzGT6C2^YN!Qe zv_N3)0orHNaN2L-#d+mc=Y@4vGEVIGIX_{Zc~4jy6{&H|JOC}ZGo@s?#bMnuS5@H_ zyKxPq?KQP>6*Y3|UtE`VFEIOKx*B{6O zjic6`)1)Y}ZdQz(EZ-8I^jQwahOlbh>fMh_QjdzL4J5?x`EiqCir7UQ`8=ao&Ca-; zPU30`D2Tk$oXtHf`TPmM#jN+0&h+v#enuHEfXEKD44&llXQToGN^|~WYO+S|vUujd zo|+a0r(DxGvxKKce=U0Xx=dNP((~Wi{taO^ZakkiFiAC1I;UhN9P!>&2upC%;O(gGu-ts%8mpxtS z%D7)VB5ICHVKH|t(=;p`Op6>N-9L{@Q`!k#B%JXa!~b-%S+GU$iQLWrqF4vYgqpGx z6Bs~ot+c7|xstGc013vQsOO*h?0l)NGuKdhMwvFcvU=Lv*$Im(d;X*_;d`kD>FoN- zR%3KvU5)Tf-x6b~gWZ)s2;s4SnEz9sZUFz+8Uc|!>ijDeZdNb8{$6t{6?XM@SSWzkLn%7W_= z`e;4hgJqV&F$e0F^%SvW<~W(S1qKnyCvH_gZ9D2ppJ!>a94Pg@sd|UB{%`!~Zj&TU zh;MP93wv~{zicZOWz(pO4qPf-jwZ9<2*D#rFsoY)L5hsaCnFvcx13iZ*_bWeL(_xw z{nOYO{V718aA&<+ zZu{`VSv^kig^ILjhyp%$_}MGXZQ@sERwefir9la3eD26m_%&L6C$?Q$Er4+DVlmNi zJF3wn!3z_<584IoHrv;fVfhpCITy!}%v21og^>`Op=3?y8+6CJ-LU_k0P6*h(?wT? zdq%bOAuRuZmRxwSWV7hCxm17rWRjzUy}g34P@drP8;-q)<=*5CC8LxeMs_j+ ze69N?$NZbB=mW0=^w)mrNfN6wZV0{fIF@*{_xtxOJrl296{ggPCL0FeMEoi{Lj;i^6$PE9OmDp&}yFj+v2@PfeY zwhX`+T?pRchmSo5Q3X)dRX_*c)dIyc=x0YQN^y^QA86~XBroUHhJAg6q7-VGc_ov(KQS4 zwQv~8L1dzNK_*&hElmRdz8eVSkSv}BHw?u=;l7xQ1p1O0RLK@L--x+aW4{+wq1g%S z4@mvdSGy!7UWVmI{o=DqT@?Dd)_?l}{h8bPi|fzfA?&#h4Q8gV#Y=oVgXu2#!P$3) zSmitcG2*e%J6yMO1B6OH#EC>S+cGf<$gN2(2QJW!=t*NJUZNH-Y0LuYJ&5=?{1b%l zs7Q$g4Fn8%MU|8LcXED3@Wz{b5a4ESZ?-@F-pF|eEGz!vpEfIqwYa}Ar0q_%$eWC4 zcrX8YIYX=}VQjMfVu5wU67CfIExP?u>xU#kMdTtx=sRsc^z3x7ssv&SQ4z8c%TC&u z4qE<9A(#U9p$35@sSKx`f?S77ifJY$IXqFmBl4Mx@2H z`0vw>c3jGUy~x8`zU4+$>{MsVu=K?KZct;|49^B1*qPlHJD5@Rpq#meJ7M#=oq zNej(vdIH%-77u>3%@>}7UQlUHbnl|#@kFt*JlIizaE2622gDz+NAUg>%sL}42ECd% z)$WsI@A30$XPReh3nkGW?$dAPLV}h~2Qw5Gz2$ux!k^l4 z?b3#JERr!z&zHzfo>iF68{d1Q0~+zq=$U>Kiw1uJhmRwHc#B6#KDT@43P>VYerhj~ zOP4Hm;c=_bocZ|Ex1$`?*{z#ezX+M)MbMv?dCDu+Syv?MPHy%Xf(}@9%!2Yf2dTAb zJnJW2wYUb*tdIQO!rM38v?DMB?ed=`u>FlLCTYjHfu+Ea2)@qJ+yz7g-)HmZePTT~QanpIsEG1p;geVY5Ab=%6Isz1Ewb z1$F6+ovLTCFmGKpmZeo`K<9t#kzXsb0mHD#PTT@!B;S9G8Nq>WIG>g%%^oI+V7|He(iCR z!XD#K_M~ZfGU6#v@nXXvE;XnwqX^Ct!>NJ0R^nTChZ8HKh)$COtBG&%n#E2wI!Q>) zkAV(MH_{hU0`4}wKGS)q^FO%z512*L!%;VBN;eH zeC!UcICjsV@rAG99{>1*9V2v|?~~1VHL2dp82@<_%)#|Rif&N7qjkDt;rXJw@jQ_w z#gI2WA@O^m24#+Z?{l;vab?b~|GY0q6(Nt^#iGf<8l#zE(Jpm!(6w)d=l7uObzYB( z2vVDwOi-X49`Ye@m~4hRx$b` z_)P3hk5tAk#9*EJx+GBVzFv!UiGiaqb(~i#2Mc6j3+IF zTblt#Rnc85wP$O)(py@XZxQ%V}IgNuq|) z0RQo&-7O0zk64N;-a0LUdFTD$`m~9>iq^se+YCDMI$!)Oh$ulD2X3HdIAZH*NdB`1 zrv&Lo^97k#CEpv8K{dW3KEgpc)d##?HoxSk?`sye@MO(@<6N7X7}VJ$tlZB9^p&%P z?^+t#_B$&t-&5W}t19s&d6Imi zK7RwhMc*^9&j2i>aNpmFdk>e2+glym9t_-$hlzc|5Vd1O;4Dmc1c zDctV|8;Sl}QjELkng&(IlsoKC+{`sHTd8uS^t>4b1~O*q*#y_u9Kz;J}HdkNFI} zqQFwt{N2oLjo>aF8_TrDF(tZr@SM0oz)kxtMCHb5QcUK|{vSQp@}FYYIat5~hnE}R zXaN{FJ6=VM>eTop7fI7Y5Cf)_{hi64Bv`b}YVe%JYH@;?hcHxK01`oRoIw^H$OpBD z2WQjc4Fmr|pIh-~giqS5$)f+_3%?l}D-0Ze3iAEDd6$m}cI?(Fz$(g9ACL`E$g!x4hyRByy6s(7S)hpir~y;w*h$u_m~!yU8$-NevLHBE z`%?!Nnc3eKpJMm~9vChjOEJ`h;OG}S?fvJ{O))c+s<{6%k6@~o&31tu@T@QLuxHK$ z!vCAuBFyEQAMxK^XIJyuCuGrJ+ysLx>MZu%InpZavif6L5LQ<0+cvypzYv309>%KT zC-F^TFL6~J|1m1CplQ09P21L0x>L&j3qDuizHft$pikA17uRM0T`z2TbymcFPaDT9 z_W3{KMf~~XcW|QF-wN(L)S?Oez$o4`q&&7z;;%tz1rB}jkR{e#Q)Inl&M6ngnmbOZ zJbV7lwDHTu{G$t^U#uJ2KkshFoD)iV;6FmN<$Z}1WMauNWe5!WCK%wT2K#rIEGZFjOpS)u4fRJX7+<}-5KGp5>mzMo0_7R;b z#T-6{6LjA+r*1y7uREzF3y8q1JYJt$i{zdM=)fy5PipB094a-1xs0n)P7Khuwmkm8MbWEQFcge;W{w#0+bFQaQ9l}U53N+YC4BZ?7yR}Eo zf9<$Pt7O1%@si7sTl{^%uHNJ~CshA~ahWDafmkO%BmJea8gl|h$uk|~pd!p=_#TRB zd%xai)&@NMOaC;4PAgm`+p<-|Rr&RY*A%=x@%|2Mve!?*>vqY3*-SP#Q0WL)%r_BX zzA(4;^p-lM->+WN9d4`~B|1F=y?R|js$DvJAg@AA*lWEo3rrPt05__0U8}_xe~MBD zv^%ncCtGk4*AtIRrc^|3WU#-ZhB1k;DE_?q;=Xz}Z4_8f0I5h5kh&S=zwB?zl2U=i zVd{f&HXkEbuNmd6`K)Z6Lb`M?&N*CH!SVp-dc9R#E-2>|gvZMl)pdm{7P9^v6aP|_ zJ(E*NI~BN4I?L0$H=Q;>wD5+VbPGCNmV;LczA}5OLIRtI%+CeON>|dpVE&gkK}|P4 z6OUOhR|^h@mTJbL$=>Ccng#G$CQ9(uR&9U0)yS{lkwt%lQ4R5x5LqeV4_Wj!Eaiv) eaP>p`t`B!Lp{865OBr~+;wj0i$>m{8{r?}v@~#{J literal 0 HcmV?d00001 diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 8ea8c4a7b38c7c..83c35592ee1728 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -4,6 +4,9 @@ from common.params import Params from common.numpy_fast import interp +from datetime import datetime +import time + import cereal.messaging as messaging from cereal import car from common.realtime import sec_since_boot @@ -15,7 +18,11 @@ from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +offset = 4.47 +osm = True + MAX_SPEED = 255.0 +NO_CURVATURE_SPEED = 90.0 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 @@ -179,6 +186,66 @@ def update(self, sm, pm, CP, VM, PP): 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 + v_speedlimit = NO_CURVATURE_SPEED + v_curvature_map = NO_CURVATURE_SPEED + v_speedlimit_ahead = NO_CURVATURE_SPEED + now = datetime.now() + try: + if sm['liveMapData'].speedLimitValid and osm and self.osm and (sm['liveMapData'].lastGps.timestamp -time.mktime(now.timetuple()) * 1000) < 10000 and (smart_speed or smart_speed_max_vego > v_ego): + speed_limit = sm['liveMapData'].speedLimit + if speed_limit is not None: + v_speedlimit = speed_limit + # offset is in percentage,. + if v_ego > offset_limit: + v_speedlimit = v_speedlimit * (1. + self.offset/100.0) + if v_speedlimit > fixed_offset: + v_speedlimit = v_speedlimit + fixed_offset + else: + speed_limit = None + if sm['liveMapData'].speedLimitAheadValid and osm and self.osm and sm['liveMapData'].speedLimitAheadDistance < speed_ahead_distance and (sm['liveMapData'].lastGps.timestamp -time.mktime(now.timetuple()) * 1000) < 10000 and (smart_speed or smart_speed_max_vego > v_ego): + distanceatlowlimit = 50 + if sm['liveMapData'].speedLimitAhead < 21/3.6: + distanceatlowlimit = speed_ahead_distance = (v_ego - sm['liveMapData'].speedLimitAhead)*3.6*2 + if distanceatlowlimit < 50: + distanceatlowlimit = 0 + distanceatlowlimit = min(distanceatlowlimit,100) + speed_ahead_distance = (v_ego - sm['liveMapData'].speedLimitAhead)*3.6*5 + speed_ahead_distance = min(speed_ahead_distance,300) + speed_ahead_distance = max(speed_ahead_distance,50) + if speed_limit is not None and sm['liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm['liveMapData'].speedLimitAhead + (speed_limit - sm['liveMapData'].speedLimitAhead)*sm['liveMapData'].speedLimitAheadDistance/speed_ahead_distance: + speed_limit_ahead = sm['liveMapData'].speedLimitAhead + (speed_limit - sm['liveMapData'].speedLimitAhead)*(sm['liveMapData'].speedLimitAheadDistance - distanceatlowlimit)/(speed_ahead_distance - distanceatlowlimit) + else: + speed_limit_ahead = sm['liveMapData'].speedLimitAhead + if speed_limit_ahead is not None: + v_speedlimit_ahead = speed_limit_ahead + if v_ego > offset_limit: + v_speedlimit_ahead = v_speedlimit_ahead * (1. + self.offset/100.0) + if v_speedlimit_ahead > fixed_offset: + v_speedlimit_ahead = v_speedlimit_ahead + fixed_offset + if sm['liveMapData'].curvatureValid and sm['liveMapData'].distToTurn < speed_ahead_distance and osm and self.osm and (sm['liveMapData'].lastGps.timestamp -time.mktime(now.timetuple()) * 1000) < 10000: + curvature = abs(sm['liveMapData'].curvature) + radius = 1/max(1e-4, curvature) * curvature_factor + if gas_button_status == 1: + radius = radius * 2.0 + elif gas_button_status == 2: + radius = radius * 1.0 + else: + radius = radius * 1.5 + if radius > 500: + c=0.9 # 0.9 at 1000m = 108 kph + elif radius > 250: + c = 3.5-13/2500*radius # 2.2 at 250m 84 kph + else: + c= 3.0 - 2/625 *radius # 3.0 at 15m 24 kph + v_curvature_map = math.sqrt(c*radius) + v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map) + except KeyError: + pass + + decel_for_turn = bool(v_curvature_map < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) + + speed_ahead_distance = 250 + # dp self.dp_profile = sm['dragonConf'].dpAccelProfile self.dp_slow_on_curve = sm['dragonConf'].dpSlowOnCurve @@ -217,6 +284,21 @@ def update(self, sm, pm, CP, VM, PP): accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1]) + if decel_for_turn and sm['liveMapData'].distToTurn < speed_ahead_distance and not following: + time_to_turn = max(1.0, sm['liveMapData'].distToTurn / max((v_ego + v_curvature_map)/2, 1.)) + required_decel = min(0, (v_curvature_map - v_ego) / time_to_turn) + accel_limits[0] = max(accel_limits[0], required_decel) + if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm['liveMapData'].speedLimitAheadDistance > 1.0 and not following: + required_decel = min(0, (v_speedlimit_ahead*v_speedlimit_ahead - v_ego*v_ego)/(sm['liveMapData'].speedLimitAheadDistance*2)) + required_decel = max(required_decel, -3.0) + decel_for_turn = True + accel_limits[0] = required_decel + accel_limits[1] = required_decel + self.a_acc_start = required_decel + v_speedlimit_ahead = v_ego + + v_cruise_setpoint = min([v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead]) + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits_turns[1], accel_limits_turns[0], @@ -290,6 +372,10 @@ def update(self, sm, pm, CP, VM, PP): plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource + plan_send.plan.vCurvature = float(v_curvature_map) + plan_send.plan.decelForTurn = bool(decel_for_turn) + plan_send.plan.mapValid = True + radar_valid = not (radar_dead or radar_fault) plan_send.plan.radarValid = bool(radar_valid) plan_send.plan.radarCanError = bool(radar_can_error) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 7e20fb8c0dfd07..2f1e9467622f9f 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,7 +23,7 @@ def plannerd_thread(sm=None, pm=None): VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData'], poll=['radarState', 'model']) if pm is None: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 857830d5ece91b..50f4975120b284 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -5,7 +5,7 @@ import fcntl import errno import signal -import shutil +#import shutil import subprocess import datetime import textwrap @@ -75,6 +75,9 @@ def unblock_stdout(): from common.spinner import Spinner from common.text_window import TextWindow +if not (os.system("python3 -m pip list | grep 'scipy' ") == 0): + os.system("cd /data/openpilot/installer/scipy_installer/ && ./scipy_installer") + import importlib import traceback from multiprocessing import Process @@ -128,9 +131,9 @@ def unblock_stdout(): for i in range(3, -1, -1): print("....%d" % i) time.sleep(1) - subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) - shutil.rmtree("/tmp/scons_cache", ignore_errors=True) - shutil.rmtree("/data/scons_cache", ignore_errors=True) + #subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) + #shutil.rmtree("/tmp/scons_cache", ignore_errors=True) + #shutil.rmtree("/data/scons_cache", ignore_errors=True) else: print("scons build failed after retry") sys.exit(1) @@ -198,6 +201,7 @@ def unblock_stdout(): "updated": "selfdrive.updated", "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), + "mapd": ("selfdrive/mapd", ["./mapd.py"]), "rtshield": "selfdrive.rtshield", "systemd": "selfdrive.dragonpilot.systemd", "appd": "selfdrive.dragonpilot.appd", @@ -248,6 +252,7 @@ def get_running(): 'paramsd', 'camerad', 'proclogd', + 'mapd', 'locationd', 'clocksd', 'gpxd', @@ -353,9 +358,10 @@ def prepare_managed_process(p): subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) except subprocess.CalledProcessError: # make clean if the build failed - cloudlog.warning("building %s failed, make clean" % (proc, )) - subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) + if proc[0] != 'selfdrive/mapd': + cloudlog.warning("building %s failed, make clean" % (proc, )) + subprocess.check_call(["make", "clean"], cwd=os.path.join(BASEDIR, proc[0])) + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, proc[0])) def join_process(process, timeout): @@ -574,6 +580,10 @@ def main(): ("HasCompletedSetup", "0"), ("IsUploadRawEnabled", "1"), ("IsLdwEnabled", "1"), + ("SpeedLimitOffset", "0"), + ("LongitudinalControl", "1"), + ("LimitSetSpeed", "1"), + ("LimitSetSpeedNeural", "1"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), diff --git a/selfdrive/mapd/README.md b/selfdrive/mapd/README.md new file mode 100644 index 00000000000000..f155db518376d0 --- /dev/null +++ b/selfdrive/mapd/README.md @@ -0,0 +1,104 @@ +Instructions to get cached offline maps: + +Android Root: just install Termux and install arch linux and intall docker + +Android Non Root: +Intall Termux from the play store +run +pkg install termux-tools proot util-linux net-tools openssh git coreutils wget + +wget https://raw.githubusercontent.com/xeffyr/termux-x-repository/master/enablerepo.sh +bash enablerepo.sh + +and + +pkg install qemu-system-x86_64 --fix-missing + +to intall qemu + +run + +termux-setup-storage + +to setup storage access + +mkdir termux-docker && cd termux-docker +git clone https://github.com/pwdonald/chromeos-qemu-docker + +cd storage/external-1 + +to get to the external storage + +pkg install qemu-utils + +qemu-img create -f qcow2 virtual_drive 16G + +curl -sL http://dl-cdn.alpinelinux.org/alpine/v3.7/releases/x86_64/alpine-virt-3.7.3-x86_64.iso -o alpine_x86_64.iso + +to download the alpine virutal distro + +qemu-system-x86_64 -nographic -m 512m -cdrom alpine_x86_64.iso -hda virtual_drive -boot d -net nic -net user + +to start the intallation. Use mbr sys + +run setup-alpine + +If you encounter internet related errors, use google dns withecho 'nameserver 8.8.8.8' > /etc/resolv.conf and run setup-alpine again. + +Once done, exit the virtual machine with Ctrl-A X. + +create a swap file. + +qemu-img create -f qcow2 virtual_swap 8G + +to run the VM use this command + +qemu-system-x86_64 -nographic -hda virtual_drive -boot c -net user,hostfwd=tcp::10022-:22,hostfwd=tcp::10080-:80,hostfwd=tcp::12345-:12345 -net nic -hdb virtual_swap -m 2048M -smp 3 + +This can take a few minutes to start up so just be patient + +apk add vim + +apk update + +vim /etc/apk/repositories +to enable community features + +apk add wget + +apk add docker + +in +/etc/update-extlinux.conf +add +default_kernel_opts="... cgroup_enable=memory swapaccount=1" +and +update-extlinux + +check +docker info +to see that everything is working correctly + +setup hdb as a linux swap partition with fdisk. That should give you 10GB of total RAM. + +service docker start + +use the https://github.com/wiktorn/Overpass-API + +docker pull wiktorn/overpass-api + +docker run \ +-e OVERPASS_META=yes \ +-e OVERPASS_MODE=init \ +-e OVERPASS_PLANET_URL=http://download.geofabrik.de/europe/germany/baden-wuerttemberg/tuebingen-regbez-latest.osm.bz2 \ +-e OVERPASS_DIFF_URL=http://download.geofabrik.de/europe/germany/baden-wuerttemberg/tuebingen-regbez-updates/ \ +-e OVERPASS_RULES_LOAD=10 \ +-v /big/docker/overpass_db/:/db \ +-p 12345:80 \ +-i -t \ +--name overpass_bw wiktorn/overpass-api + +docker start overpass_bw and docker stop overpass_bw can now start the docker process + +This is a 100MB file and lands up using about 6GB of RAM and HDD space after the inital install it only requires 1GB to actively runn + diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 00000000000000..8b137891791fe9 --- /dev/null +++ b/selfdrive/mapd/__init__.py @@ -0,0 +1 @@ + diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json new file mode 100644 index 00000000000000..a8db6088077d9c --- /dev/null +++ b/selfdrive/mapd/default_speeds.json @@ -0,0 +1,111 @@ +{ + "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped", + "AR:urban": "40", + "AR:urban:primary": "60", + "AR:urban:secondary": "60", + "AR:rural": "110", + "AT:urban": "50", + "AT:rural": "100", + "AT:trunk": "100", + "AT:motorway": "130", + "BE:urban": "50", + "BE-VLG:rural": "70", + "BE-WAL:rural": "90", + "BE:trunk": "120", + "BE:motorway": "120", + "CH:urban[1]": "50", + "CH:rural": "80", + "CH:trunk": "100", + "CH:motorway": "120", + "CZ:pedestrian_zone": "20", + "CZ:living_street": "20", + "CZ:urban": "50", + "CZ:urban_trunk": "80", + "CZ:urban_motorway": "80", + "CZ:rural": "90", + "CZ:trunk": "110", + "CZ:motorway": "130", + "DK:urban": "50", + "DK:rural": "80", + "DK:motorway": "130", + "DE:living_street": "7", + "DE:residential": "30", + "DE:urban": "50", + "DE:rural": "100", + "DE:trunk": "none", + "DE:motorway": "none", + "FI:urban": "50", + "FI:rural": "80", + "FI:trunk": "100", + "FI:motorway": "120", + "FR:urban": "50", + "FR:rural": "80", + "FR:trunk": "110", + "FR:motorway": "130", + "GR:urban": "50", + "GR:rural": "90", + "GR:trunk": "110", + "GR:motorway": "130", + "HU:urban": "50", + "HU:rural": "90", + "HU:trunk": "110", + "HU:motorway": "130", + "IT:urban": "50", + "IT:rural": "90", + "IT:trunk": "110", + "IT:motorway": "130", + "JP:national": "60", + "JP:motorway": "100", + "LT:living_street": "20", + "LT:urban": "50", + "LT:rural": "90", + "LT:trunk": "120", + "LT:motorway": "130", + "PL:living_street": "20", + "PL:urban": "50", + "PL:rural": "90", + "PL:trunk": "100", + "PL:motorway": "140", + "RO:urban": "50", + "RO:rural": "90", + "RO:trunk": "100", + "RO:motorway": "130", + "RU:living_street": "20", + "RU:urban": "60", + "RU:rural": "90", + "RU:motorway": "110", + "SK:urban": "50", + "SK:rural": "90", + "SK:trunk": "90", + "SK:motorway": "90", + "SI:urban": "50", + "SI:rural": "90", + "SI:trunk": "110", + "SI:motorway": "130", + "ES:living_street": "20", + "ES:urban": "50", + "ES:rural": "50", + "ES:trunk": "90", + "ES:motorway": "120", + "SE:urban": "50", + "SE:rural": "70", + "SE:trunk": "90", + "SE:motorway": "110", + "GB:nsl_restricted": "30 mph", + "GB:nsl_single": "60 mph", + "GB:nsl_dual": "70 mph", + "GB:motorway": "70 mph", + "UA:urban": "50", + "UA:rural": "90", + "UA:trunk": "110", + "UA:motorway": "130", + "UZ:living_street": "30", + "UZ:urban": "70", + "UZ:rural": "100", + "UZ:motorway": "110", + "ZA:trunk": "120", + "ZA:residential": "60", + "ZA:rural": "100", + "ZA:urban": "60", + "ZA:motorway": "120" +} diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py new file mode 100644 index 00000000000000..e1a906bf82af83 --- /dev/null +++ b/selfdrive/mapd/default_speeds_generator.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python +import json + +DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json" + +def main(filename = DEFAULT_OUTPUT_FILENAME): + countries = [] + + """ + -------------------------------------------------- + US - United State of America + -------------------------------------------------- + """ + US = Country("US") # First step, create the country using the ISO 3166 two letter code + countries.append(US) # Second step, add the country to countries list + + """ Default rules """ + # Third step, add some default rules for the country + # Speed limit rules are based on OpenStreetMaps (OSM) tags. + # The dictionary {...} defines the tag_name: value + # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied. + # The text at the end is the speed limit (use no unit for km/h) + # Rules apply in the order in which they are written for each country + # Rules for specific regions (states) take priority over country rules + # If you modify existing country rules, you must update all existing states without that rule to use the old rule + US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph + US.add_rule({"highway": "trunk"}, "55 mph") + US.add_rule({"highway": "primary"}, "55 mph") + US.add_rule({"highway": "secondary"}, "45 mph") + US.add_rule({"highway": "tertiary"}, "35 mph") + US.add_rule({"highway": "unclassified"}, "55 mph") + US.add_rule({"highway": "residential"}, "25 mph") + US.add_rule({"highway": "service"}, "25 mph") + US.add_rule({"highway": "motorway_link"}, "55 mph") + US.add_rule({"highway": "trunk_link"}, "55 mph") + US.add_rule({"highway": "primary_link"}, "55 mph") + US.add_rule({"highway": "secondary_link"}, "45 mph") + US.add_rule({"highway": "tertiary_link"}, "35 mph") + US.add_rule({"highway": "living_street"}, "15 mph") + + """ States """ + new_york = US.add_region("New York") # Fourth step, add a state/region to country + new_york.add_rule({"highway": "motorway"}, "65 mph") + new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules + new_york.add_rule({"highway": "secondary"}, "55 mph") + new_york.add_rule({"highway": "tertiary"}, "55 mph") + new_york.add_rule({"highway": "residential"}, "30 mph") + new_york.add_rule({"highway": "primary_link"}, "45 mph") + new_york.add_rule({"highway": "secondary_link"}, "55 mph") + new_york.add_rule({"highway": "tertiary_link"}, "55 mph") + new_york.add_rule({"highway": "living_street"}, "20 mph") + # All if not written by the state, the rules will default to the country rules + + #california = US.add_region("California") + # California uses only the default US rules + + michigan = US.add_region("Michigan") + michigan.add_rule({"highway": "motorway"}, "70 mph") + + oregon = US.add_region("Oregon") + oregon.add_rule({"highway": "motorway"}, "55 mph") + oregon.add_rule({"highway": "secondary"}, "35 mph") + oregon.add_rule({"highway": "tertiary"}, "30 mph") + oregon.add_rule({"highway": "service"}, "15 mph") + oregon.add_rule({"highway": "secondary_link"}, "35 mph") + oregon.add_rule({"highway": "tertiary_link"}, "30 mph") + + south_dakota = US.add_region("South Dakota") + south_dakota.add_rule({"highway": "motorway"}, "80 mph") + south_dakota.add_rule({"highway": "trunk"}, "70 mph") + south_dakota.add_rule({"highway": "primary"}, "65 mph") + south_dakota.add_rule({"highway": "trunk_link"}, "70 mph") + south_dakota.add_rule({"highway": "primary_link"}, "65 mph") + + wisconsin = US.add_region("Wisconsin") + wisconsin.add_rule({"highway": "trunk"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary"}, "45 mph") + wisconsin.add_rule({"highway": "unclassified"}, "35 mph") + wisconsin.add_rule({"highway": "trunk_link"}, "65 mph") + wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph") + + """ + -------------------------------------------------- + AU - Australia + -------------------------------------------------- + """ + AU = Country("AU") + countries.append(AU) + + """ Default rules """ + AU.add_rule({"highway": "motorway"}, "100") + AU.add_rule({"highway": "trunk"}, "80") + AU.add_rule({"highway": "primary"}, "80") + AU.add_rule({"highway": "secondary"}, "50") + AU.add_rule({"highway": "tertiary"}, "50") + AU.add_rule({"highway": "unclassified"}, "80") + AU.add_rule({"highway": "residential"}, "50") + AU.add_rule({"highway": "service"}, "40") + AU.add_rule({"highway": "motorway_link"}, "90") + AU.add_rule({"highway": "trunk_link"}, "80") + AU.add_rule({"highway": "primary_link"}, "80") + AU.add_rule({"highway": "secondary_link"}, "50") + AU.add_rule({"highway": "tertiary_link"}, "50") + AU.add_rule({"highway": "living_street"}, "30") + + """ + -------------------------------------------------- + CA - Canada + -------------------------------------------------- + """ + CA = Country("CA") + countries.append(CA) + + """ Default rules """ + CA.add_rule({"highway": "motorway"}, "100") + CA.add_rule({"highway": "trunk"}, "80") + CA.add_rule({"highway": "primary"}, "80") + CA.add_rule({"highway": "secondary"}, "50") + CA.add_rule({"highway": "tertiary"}, "50") + CA.add_rule({"highway": "unclassified"}, "80") + CA.add_rule({"highway": "residential"}, "40") + CA.add_rule({"highway": "service"}, "40") + CA.add_rule({"highway": "motorway_link"}, "90") + CA.add_rule({"highway": "trunk_link"}, "80") + CA.add_rule({"highway": "primary_link"}, "80") + CA.add_rule({"highway": "secondary_link"}, "50") + CA.add_rule({"highway": "tertiary_link"}, "50") + CA.add_rule({"highway": "living_street"}, "20") + + + """ + -------------------------------------------------- + DE - Germany + -------------------------------------------------- + """ + DE = Country("DE") + countries.append(DE) + + """ Default rules """ + DE.add_rule({"highway": "motorway"}, "none") + DE.add_rule({"highway": "living_street"}, "10") + DE.add_rule({"highway": "residential"}, "30") + DE.add_rule({"highway": "service"}, "10") + DE.add_rule({"zone:traffic": "DE:rural"}, "100") + DE.add_rule({"zone:traffic": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:30"}, "30") + DE.add_rule({"zone:maxspeed": "DE:urban"}, "50") + DE.add_rule({"zone:maxspeed": "DE:rural"}, "100") + DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none") + DE.add_rule({"bicycle_road": "yes"}, "30") + + """ + -------------------------------------------------- + ZA - South Africa + -------------------------------------------------- + """ + ZA = Country("ZA") + countries.append(ZA) + + """ Default rules """ + ZA.add_rule({"highway": "motorway"}, "120") + ZA.add_rule({"highway": "motorway_link"}, "120") + ZA.add_rule({"highway": "primary"}, "100") + ZA.add_rule({"highway": "secondary"}, "100") + ZA.add_rule({"highway": "residential"}, "60") + + """ + -------------------------------------------------- + EE - Estonia + -------------------------------------------------- + """ + EE = Country("EE") + countries.append(EE) + + """ Default rules """ + EE.add_rule({"highway": "motorway"}, "90") + EE.add_rule({"highway": "trunk"}, "90") + EE.add_rule({"highway": "primary"}, "90") + EE.add_rule({"highway": "secondary"}, "50") + EE.add_rule({"highway": "tertiary"}, "50") + EE.add_rule({"highway": "unclassified"}, "90") + EE.add_rule({"highway": "residential"}, "40") + EE.add_rule({"highway": "service"}, "40") + EE.add_rule({"highway": "motorway_link"}, "90") + EE.add_rule({"highway": "trunk_link"}, "70") + EE.add_rule({"highway": "primary_link"}, "70") + EE.add_rule({"highway": "secondary_link"}, "50") + EE.add_rule({"highway": "tertiary_link"}, "50") + EE.add_rule({"highway": "living_street"}, "20") + + + """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """ + """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """ + + # Final step + write_json(countries, filename) + +def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME): + out_dict = {} + for country in countries: + out_dict.update(country.jsonify()) + json_string = json.dumps(out_dict, indent=2) + with open(filename, "wb") as f: + f.write(json_string) + + +class Region(object): + ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"] + ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"] + def __init__(self, name): + self.name = name + self.rules = [] + + def add_rule(self, tag_conditions, speed): + new_rule = {} + if not isinstance(tag_conditions, dict): + raise TypeError("Rule tag conditions must be dictionary") + if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions): + raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS + if 'highway' in tag_conditions: + if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES: + raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"])) + new_rule['tags'] = tag_conditions + try: + new_rule['speed'] = str(speed) + except ValueError: + raise ValueError("Rule speed must be string") + self.rules.append(new_rule) + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = self.rules + return ret_dict + +class Country(Region): + ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZA","ZM","ZW"] + def __init__(self, ISO_3166_alpha_2): + Region.__init__(self, ISO_3166_alpha_2) + if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES: + raise ValueError("Not valid IOS 3166 country code") + self.regions = {} + + def add_region(self, name): + self.regions[name] = Region(name) + return self.regions[name] + + def jsonify(self): + ret_dict = {} + ret_dict[self.name] = {} + for r_name, region in self.regions.items(): + ret_dict[self.name].update(region.jsonify()) + ret_dict[self.name]['Default'] = self.rules + return ret_dict + + +if __name__ == '__main__': + main() diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100755 index 00000000000000..1bc7c3073ddc31 --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,600 @@ +#!/usr/bin/env python3 + +import time +import math +import overpy +import socket +import requests +import threading +import numpy as np +# setup logging +import logging +import logging.handlers +from scipy import spatial +import selfdrive.crash as crash +from common.params import Params +from collections import defaultdict +import cereal.messaging as messaging +#import cereal.messaging_arne as messaging_arne +from selfdrive.version import version, dirty +from common.transformations.coordinates import geodetic2ecef +from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points, rate_curvature_points + +#DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +#from selfdrive.mapd import default_speeds_generator +#default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE) + +# define LoggerThread class to implement logging functionality +class LoggerThread(threading.Thread): + def __init__(self, threadID, name): + threading.Thread.__init__(self) + self.threadID = threadID + self.name = name + self.logger = logging.getLogger(name) + h = logging.handlers.RotatingFileHandler(str(name)+'-Thread.log', 'a', 10*1024*1024, 5) + f = logging.Formatter('%(asctime)s %(processName)-10s %(name)s %(levelname)-8s %(message)s') + h.setFormatter(f) + self.logger.addHandler(h) + self.logger.setLevel(logging.CRITICAL) # set to logging.DEBUG to enable logging + # self.logger.setLevel(logging.DEBUG) # set to logging.CRITICAL to disable logging + + def save_gps_data(self, gps, osm_way_id): + try: + location = [gps.speed, gps.bearing, gps.latitude, gps.longitude, gps.altitude, gps.accuracy, time.time(), osm_way_id] + with open("/data/openpilot/selfdrive/data_collection/gps-data", "a") as f: + f.write("{}\n".format(location)) + except: + self.logger.error("Unable to write gps data to external file") + + def run(self): + pass # will be overridden in the child class + +class QueryThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): # sharedParams is dict of params shared between two threads + # invoke parent constructor https://stackoverflow.com/questions/2399307/how-to-invoke-the-super-constructor-in-python + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + # memorize some parameters + self.OVERPASS_API_LOCAL = "http://192.168.43.1:12345/api/interpreter" + socket.setdefaulttimeout(15) + self.distance_to_edge = 500 + self.OVERPASS_API_URL = "https://z.overpass-api.de/api/interpreter" + self.OVERPASS_API_URL2 = "https://lz4.overpass-api.de/api/interpreter" + self.OVERPASS_HEADERS = { + 'User-Agent': 'NEOS (comma.ai)', + 'Accept-Encoding': 'gzip' + } + self.prev_ecef = None + + def is_connected_to_local(self, timeout=3.0): + try: + requests.get(self.OVERPASS_API_LOCAL, timeout=timeout) + self.logger.debug("connection local active") + return True + except: + self.logger.error("No local server available.") + return False + + def is_connected_to_internet(self, timeout=1.0): + try: + requests.get(self.OVERPASS_API_URL, timeout=timeout) + self.logger.debug("connection 1 active") + return True + except: + self.logger.error("No internet connection available.") + return False + + def is_connected_to_internet2(self, timeout=1.0): + try: + requests.get(self.OVERPASS_API_URL2, timeout=timeout) + self.logger.debug("connection 2 active") + return True + except: + self.logger.error("No internet connection available.") + return False + + def build_way_query(self, lat, lon, heading, radius=50): + """Builds a query to find all highways within a given radius around a point""" + a = 111132.954*math.cos(float(lat)/180*3.141592) + b = 111132.954 - 559.822 * math.cos( 2 * float(lat)/180*3.141592) + 1.175 * math.cos( 4 * float(lat)/180*3.141592) + heading = math.radians(-heading + 90) + lat = lat+math.sin(heading)*radius/2/b + lon = lon+math.cos(heading)*radius/2/a + pos = " (around:%f,%f,%f)" % (radius, lat, lon) + lat_lon = "(%f,%f)" % (lat, lon) + q = """( + way + """ + pos + """ + [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; + >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"]; + convert area ::id = id(), admin_level = t['admin_level'], + name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out; + """ + self.logger.debug("build_way_query : %s" % str(q)) + return q, lat, lon + + def run(self): + self.logger.debug("run method started for thread %s" % self.name) + + # for now we follow old logic, will be optimized later + start = time.time() + radius = 3000 + while True: + if time.time() - start > 2.0: + print("Mapd QueryThread lagging by: %s" % str(time.time() - start - 1.0)) + if time.time() - start < 1.0: + time.sleep(0.1) + continue + else: + start = time.time() + + self.logger.debug("Starting after sleeping for 1 second ...") + last_gps = self.sharedParams.get('last_gps', None) + self.logger.debug("last_gps = %s" % str(last_gps)) + + if last_gps is not None: + fix_ok = last_gps.flags & 1 + if not fix_ok: + continue + else: + continue + + last_query_pos = self.sharedParams.get('last_query_pos', None) + if last_query_pos is not None: + cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + if self.prev_ecef is None: + self.prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude)) + + dist = np.linalg.norm(cur_ecef - self.prev_ecef) + if dist < radius - self.distance_to_edge: #updated when we are close to the edge of the downloaded circle + continue + self.logger.debug("parameters, cur_ecef = %s, prev_ecef = %s, dist=%s" % (str(cur_ecef), str(self.prev_ecef), str(dist))) + + if dist > radius: + query_lock = self.sharedParams.get('query_lock', None) + if query_lock is not None: + query_lock.acquire() + self.sharedParams['cache_valid'] = False + query_lock.release() + else: + self.logger.error("There is no query_lock") + + if last_gps is not None and last_gps.accuracy < 5.0: + q, lat, lon = self.build_way_query(last_gps.latitude, last_gps.longitude, last_gps.bearing, radius=radius) + try: + if self.is_connected_to_local(): + api = overpy.Overpass(url=self.OVERPASS_API_LOCAL) + api.timeout = 15.0 + self.distance_to_edge = radius * 3 / 8 + elif self.is_connected_to_internet(): + api = overpy.Overpass(url=self.OVERPASS_API_URL) + self.logger.error("Using origional Server") + self.distance_to_edge = radius/4 + elif self.is_connected_to_internet2(): + api = overpy.Overpass(url=self.OVERPASS_API_URL2) + api.timeout = 10.0 + self.logger.error("Using backup Server") + self.distance_to_edge = radius/4 + else: + continue + new_result = api.query(q) + self.logger.debug("new_result = %s" % str(new_result)) + # Build kd-tree + nodes = [] + real_nodes = [] + node_to_way = defaultdict(list) + location_info = {} + + for n in new_result.nodes: + nodes.append((float(n.lat), float(n.lon), 0)) + real_nodes.append(n) + + for way in new_result.ways: + for n in way.nodes: + node_to_way[n.id].append(way) + + for area in new_result.areas: + if area.tags.get('admin_level', '') == "2": + location_info['country'] = area.tags.get('ISO3166-1:alpha2', '') + elif area.tags.get('admin_level', '') == "4": + location_info['region'] = area.tags.get('name', '') + + nodes = np.asarray(nodes) + nodes = geodetic2ecef(nodes) + tree = spatial.KDTree(nodes) + self.logger.debug("query thread, ... %s %s" % (str(nodes), str(tree))) + + # write result + query_lock = self.sharedParams.get('query_lock', None) + if query_lock is not None: + query_lock.acquire() + last_gps_mod = last_gps.as_builder() + last_gps_mod.latitude = lat + last_gps_mod.longitude = lon + last_gps = last_gps_mod.as_reader() + self.sharedParams['last_query_result'] = new_result, tree, real_nodes, node_to_way, location_info + self.prev_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) + self.sharedParams['last_query_pos'] = last_gps + self.sharedParams['cache_valid'] = True + query_lock.release() + else: + self.logger.error("There is not query_lock") + + except Exception as e: + self.logger.error("ERROR :" + str(e)) + print(str(e)) + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['last_query_result'] = None + query_lock.release() + else: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['last_query_result'] = None + query_lock.release() + + self.logger.debug("end of one cycle in endless loop ...") + +class MapsdThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): + # invoke parent constructor + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + self.pm = messaging.PubMaster(['liveMapData']) + self.logger.debug("entered mapsd_thread, ... %s" % ( str(self.pm))) + def run(self): + self.logger.debug("Entered run method for thread :" + str(self.name)) + cur_way = None + curvature_valid = False + curvature = None + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + max_speed = None + max_speed_ahead = None + max_speed_ahead_dist = None + max_speed_prev = 0 + had_good_gps = False + start = time.time() + while True: + if time.time() - start > 0.2: + print("Mapd MapsdThread lagging by: %s" % str(time.time() - start - 0.1)) + if time.time() - start < 0.1: + time.sleep(0.01) + continue + else: + start = time.time() + self.logger.debug("starting new cycle in endless loop") + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + gps = self.sharedParams['last_gps'] + traffic_status = self.sharedParams['traffic_status'] + traffic_confidence = self.sharedParams['traffic_confidence'] + last_not_none_signal = self.sharedParams['last_not_none_signal'] + speedLimittraffic = self.sharedParams['speedLimittraffic'] + speedLimittrafficvalid = self.sharedParams['speedLimittrafficvalid'] + speedLimittrafficAdvisory = self.sharedParams['speedLimittrafficAdvisory'] + speedLimittrafficAdvisoryvalid = self.sharedParams['speedLimittrafficAdvisoryvalid'] + query_lock.release() + if gps is None: + continue + fix_ok = gps.flags & 1 + self.logger.debug("fix_ok = %s" % str(fix_ok)) + + if gps.accuracy > 2.5: + if gps.accuracy > 5.0: + if not speedLimittrafficvalid: + if had_good_gps: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['speedLimittrafficvalid'] = True + if max_speed is not None: + speedLimittraffic = max_speed * 3.6 + else: + speedLimittraffic = 130 + query_lock.release() + else: + fix_ok = False + had_good_gps = False + if not speedLimittrafficvalid and not had_good_gps: + fix_ok = False + elif not had_good_gps: + had_good_gps = True + if not fix_ok or self.sharedParams['last_query_result'] is None or not self.sharedParams['cache_valid']: + self.logger.debug("fix_ok %s" % fix_ok) + self.logger.error("Error in fix_ok logic") + cur_way = None + curvature = None + max_speed_ahead = None + max_speed_ahead_dist = None + curvature_valid = False + upcoming_curvature = 0. + dist_to_turn = 0. + road_points = None + map_valid = False + else: + map_valid = True + lat = gps.latitude + lon = gps.longitude + heading = gps.bearing + speed = gps.speed + + query_lock.acquire() + cur_way = Way.closest(self.sharedParams['last_query_result'], lat, lon, heading, cur_way) + query_lock.release() + + if cur_way is not None: + self.logger.debug("cur_way is not None ...") + pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) + if pnts is not None: + xs = pnts[:, 0] + ys = pnts[:, 1] + road_points = [float(x) for x in xs], [float(y) for y in ys] + + if speed < 5: + curvature_valid = False + if curvature_valid and pnts.shape[0] <= 3: + curvature_valid = False + else: + curvature_valid = False + upcoming_curvature = 0. + curvature = None + dist_to_turn = 0. + # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found + if curvature_valid: + # Compute the curvature for each point + with np.errstate(divide='ignore'): + circles = [circle_through_points(*p, direction=True) for p in zip(pnts, pnts[1:], pnts[2:])] + circles = np.asarray(circles) + radii = np.nan_to_num(circles[:, 2]) + radii[abs(radii) < 15.] = 10000 + + if cur_way.way.tags['highway'] == 'trunk' or cur_way.way.tags['highway'] == 'motorway_link': + radii = radii*1.6 # https://media.springernature.com/lw785/springer-static/image/chp%3A10.1007%2F978-3-658-01689-0_21/MediaObjects/298553_35_De_21_Fig65_HTML.gif + elif cur_way.way.tags['highway'] == 'motorway': + radii = radii*2.8 + + curvature = 1. / radii + rate = [rate_curvature_points(*p) for p in zip(pnts[1:], pnts[2:],curvature[0:],curvature[1:])] + rate = ([0] + rate) + + curvature = np.abs(curvature) + curvature = np.multiply(np.minimum(np.multiply(rate,4000)+0.7,1.1),curvature) + # Index of closest point + closest = np.argmin(np.linalg.norm(pnts, axis=1)) + dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close + + # Compute distance along path + dists = list() + dists.append(0) + for p, p_prev in zip(pnts, pnts[1:, :]): + dists.append(dists[-1] + np.linalg.norm(p - p_prev)) + dists = np.asarray(dists) + dists = dists - dists[closest] + dist_to_closest + dists = dists[1:-1] + + close_idx = np.logical_and(dists > 0, dists < 500) + dists = dists[close_idx] + curvature = curvature[close_idx] + + if len(curvature): + curvature = np.nan_to_num(curvature) + upcoming_curvature = np.amax(curvature) + dist_to_turn =np.amin(dists[np.logical_and(curvature >= upcoming_curvature, curvature <= upcoming_curvature)]) + else: + upcoming_curvature = 0. + dist_to_turn = 999 + + dat = messaging.new_message() + dat.init('liveMapData') + + last_gps = self.sharedParams.get('last_gps', None) + + if last_gps is not None: + dat.liveMapData.lastGps = last_gps + + if cur_way is not None: + dat.liveMapData.wayId = cur_way.id + self.sharedParams['osm_way_id'] = cur_way.id + # Speed limit + max_speed = cur_way.max_speed(heading) + max_speed_ahead = None + max_speed_ahead_dist = None + if max_speed is not None: + max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE, traffic_status, traffic_confidence, last_not_none_signal) + else: + max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(speed*1.1, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE, traffic_status, traffic_confidence, last_not_none_signal) + # TODO: anticipate T junctions and right and left hand turns based on indicator + + if max_speed_ahead is not None and max_speed_ahead_dist is not None: + dat.liveMapData.speedLimitAheadValid = True + dat.liveMapData.speedLimitAhead = float(max_speed_ahead) + dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist) + if max_speed is not None: + if abs(max_speed - max_speed_prev) > 0.1: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['speedLimittrafficvalid'] = False + query_lock.release() + max_speed_prev = max_speed + advisory_max_speed = cur_way.advisory_max_speed() + if speedLimittrafficAdvisoryvalid: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = speedLimittrafficAdvisory / 3.6 + else: + if advisory_max_speed is not None: + dat.liveMapData.speedAdvisoryValid = True + dat.liveMapData.speedAdvisory = advisory_max_speed + + # Curvature + dat.liveMapData.curvatureValid = curvature_valid + dat.liveMapData.curvature = float(upcoming_curvature) + dat.liveMapData.distToTurn = float(dist_to_turn) + if road_points is not None: + dat.liveMapData.roadX, dat.liveMapData.roadY = road_points + if curvature is not None: + dat.liveMapData.roadCurvatureX = [float(x) for x in dists] + dat.liveMapData.roadCurvature = [float(x) for x in curvature] + else: + self.sharedParams['osm_way_id'] = 0 + if self.sharedParams['speedLimittrafficvalid']: + if speedLimittraffic > 0.1: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = speedLimittraffic / 3.6 + map_valid = False + else: + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + self.sharedParams['speedLimittrafficvalid'] = False + query_lock.release() + else: + if max_speed is not None and map_valid: + dat.liveMapData.speedLimitValid = True + dat.liveMapData.speedLimit = max_speed + + dat.liveMapData.mapValid = map_valid + self.logger.debug("Sending ... liveMapData ... %s", str(dat)) + self.pm.send('liveMapData', dat) + +class MessagedGPSThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): + # invoke parent constructor + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + self.sm = messaging.SubMaster(['gpsLocationExternal']) + self.logger.debug("entered messagedGPS_thread, ... %s" % (str(self.sm))) + def run(self): + self.logger.debug("Entered run method for thread :" + str(self.name)) + gps = None + start = time.time() + while True: + if time.time() - start > 0.2: + print("Mapd MessagedGPSThread lagging by: %s" % str(time.time() - start - 0.1)) + if time.time() - start < 0.1: + time.sleep(0.01) + continue + else: + start = time.time() + self.logger.debug("starting new cycle in endless loop") + self.sm.update(0) + if self.sm.updated['gpsLocationExternal']: + gps = self.sm['gpsLocationExternal'] + self.save_gps_data(gps, self.sharedParams['osm_way_id']) + + query_lock = self.sharedParams.get('query_lock', None) + + query_lock.acquire() + self.sharedParams['last_gps'] = gps + query_lock.release() + self.logger.debug("setting last_gps to %s" % str(gps)) + +class MessagedThread(LoggerThread): + def __init__(self, threadID, name, sharedParams={}): + # invoke parent constructor + LoggerThread.__init__(self, threadID, name) + self.sharedParams = sharedParams + self.sm = messaging.SubMaster(['liveTrafficData'])#,'trafficModelEvent']) + #self.logger.debug("entered messageArned_thread, ... %s" % str(self.arne_sm)) + def run(self): + self.logger.debug("Entered run method for thread :" + str(self.name)) + last_not_none_signal = 'NONE' + last_not_none_signal_counter = 0 + traffic_confidence = 0 + traffic_status = 'NONE' + speedLimittraffic = 0 + speedLimittraffic_prev = 0 + speedLimittrafficAdvisoryvalid = False + speedLimittrafficAdvisory = 0 + start = time.time() + while True: + if time.time() - start > 0.2: + print("Mapd MessagedArneThread lagging by: %s" % str(time.time() - start - 0.1)) + if time.time() - start < 0.1: + time.sleep(0.01) + continue + else: + start = time.time() + self.logger.debug("starting new cycle in endless loop") + #self.arne_sm.update(0) + #if self.arne_sm.updated['trafficModelEvent']: + # traffic_status = self.arne_sm['trafficModelEvent'].status + #traffic_confidence = round(self.arne_sm['trafficModelEvent'].confidence * 100, 2) + #if traffic_confidence >= 50 and (traffic_status == 'GREEN' or traffic_status == 'SLOW'): + #last_not_none_signal = traffic_status + #last_not_none_signal_counter = 0 + #elif traffic_confidence >= 50 and traffic_status == 'NONE' and last_not_none_signal != 'NONE': + #if last_not_none_signal_counter < 25: + #last_not_none_signal_counter = last_not_none_signal_counter + 1 + #print("self.last_not_none_signal_counter") + #print(self.last_not_none_signal_counter) + #print("self.last_not_none_signal") + #print(self.last_not_none_signal) + #else: + #last_not_none_signal = 'NONE' + query_lock = self.sharedParams.get('query_lock', None) + query_lock.acquire() + speedLimittrafficvalid = self.sharedParams['speedLimittrafficvalid'] + query_lock.release() + traffic = self.sm['liveTrafficData'] + if traffic.speedLimitValid: + speedLimittraffic = traffic.speedLimit + if abs(speedLimittraffic_prev - speedLimittraffic) > 0.1: + speedLimittrafficvalid = True + speedLimittraffic_prev = speedLimittraffic + else: + speedLimittrafficvalid = False + if traffic.speedAdvisoryValid: + speedLimittrafficAdvisory = traffic.speedAdvisory + speedLimittrafficAdvisoryvalid = True + else: + speedLimittrafficAdvisoryvalid = False + + query_lock.acquire() + self.sharedParams['traffic_status'] = traffic_status + self.sharedParams['traffic_confidence'] = traffic_confidence + self.sharedParams['last_not_none_signal'] = last_not_none_signal + self.sharedParams['speedLimittraffic'] = speedLimittraffic + self.sharedParams['speedLimittrafficvalid'] = speedLimittrafficvalid + self.sharedParams['speedLimittrafficAdvisory'] = speedLimittrafficAdvisory + self.sharedParams['speedLimittrafficAdvisoryvalid'] = speedLimittrafficAdvisoryvalid + query_lock.release() + +def main(): + params = Params() + dongle_id = params.get("DongleId") + crash.bind_user(id=dongle_id) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.install() + + # setup shared parameters + last_gps = None + query_lock = threading.Lock() + last_query_result = None + last_query_pos = None + cache_valid = False + traffic_status = 'None' + traffic_confidence = 100 + last_not_none_signal = 'None' + speedLimittraffic = 0 + speedLimittrafficvalid = False + speedLimittrafficAdvisory = 0 + osm_way_id = 0 + speedLimittrafficAdvisoryvalid = False + sharedParams = {'last_gps' : last_gps, 'query_lock' : query_lock, 'last_query_result' : last_query_result, \ + 'last_query_pos' : last_query_pos, 'cache_valid' : cache_valid, 'traffic_status' : traffic_status, \ + 'traffic_confidence' : traffic_confidence, 'last_not_none_signal' : last_not_none_signal, \ + 'speedLimittraffic' : speedLimittraffic, 'speedLimittrafficvalid' : speedLimittrafficvalid, \ + 'speedLimittrafficAdvisory' : speedLimittrafficAdvisory, 'speedLimittrafficAdvisoryvalid' : speedLimittrafficAdvisoryvalid, 'osm_way_id' : osm_way_id} + + qt = QueryThread(1, "QueryThread", sharedParams=sharedParams) + mt = MapsdThread(2, "MapsdThread", sharedParams=sharedParams) + mggps = MessagedGPSThread(3, "MessagedGPSThread", sharedParams=sharedParams) + #mgarne = MessagedArneThread(4, "MessagedArneThread", sharedParams=sharedParams) + + qt.start() + mt.start() + mggps.start() + #mgarne.start() + +if __name__ == "__main__": + main() diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py new file mode 100755 index 00000000000000..ba5bafdcd2825f --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -0,0 +1,768 @@ +import math +import json +import numpy as np +from datetime import datetime +from common.basedir import BASEDIR +from common.op_params import opParams +from selfdrive.config import Conversions as CV +from common.transformations.coordinates import LocalCoord, geodetic2ecef + +LOOKAHEAD_TIME = 10. +MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME + +op_params = opParams() + +traffic_lights = op_params.get('traffic_lights') +traffic_lights_without_direction = op_params.get('traffic_lights_without_direction') +rolling_stop = op_params.get('rolling_stop') + +DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json" +DEFAULT_SPEEDS = {} +with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS = json.loads(f.read()) + +DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json" +DEFAULT_SPEEDS_BY_REGION = {} +with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f: + DEFAULT_SPEEDS_BY_REGION = json.loads(f.read()) + +def rate_curvature_points(p2,p3,curvature2,curvature3): + x2, y2, _ = p2 + x3, y3, _ = p3 + if abs(curvature3) > abs(curvature2): + return abs((curvature3-curvature2)/(np.sqrt((x3-x2)**2+(y3-y2)**2))) + else: + return 0 + +def distance(x0,y0,x1,y1,x2,y2): + return abs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / np.sqrt(np.square(x2-x1) + np.square(y2-y1)) + +def circle_through_points(p1, p2, p3, force=False, direction=False): + """Fits a circle through three points + Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm""" + x1, y1, _ = p1 + x2, y2, _ = p2 + x3, y3, _ = p3 + + A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2 + B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1) + C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2) + D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2) + try: + if abs((y3-y1)*x2-(x3-x1)*y2+x3*y1-y3*x1)/np.sqrt((y3-y1)**2+(x3-x1)**2) > 0.1 or force: + if direction: + if (x2-x1)*(y3-y1)-(y2-y1)*(x3-x1)>0: + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + else: + return (-B / (2 * A), - C / (2 * A), -np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + else: + return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) + else: + return (-B / (2 * A), - C / (2 * A), 10000) + except RuntimeWarning: + return x2, y2, 10000 + +def parse_speed_unit(max_speed): + """Converts a maxspeed string to m/s based on the unit present in the input. + OpenStreetMap defaults to kph if no unit is present. """ + + if not max_speed: + return None + + conversion = CV.KPH_TO_MS + if 'mph' in max_speed: + max_speed = max_speed.replace(' mph', '') + conversion = CV.MPH_TO_MS + try: + return float(max_speed) * conversion + except ValueError: + return None + +def parse_speed_tags(tags): + """Parses tags on a way to find the maxspeed string""" + max_speed = None + + if 'maxspeed' in tags: + max_speed = tags['maxspeed'] + + if 'maxspeed:conditional' in tags: + try: + weekday = True + max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') + if cond.find('wet') > -0.5: + cond = cond.replace('wet','') + cond = cond.replace(' ','') + weekday = False + #TODO Check if road is wet waybe if wipers are on. + cond = cond[1:-1] + + now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays + if cond.find('Mo-Fr') > -0.5: + cond = cond.replace('Mo-Fr','') + cond = cond.replace(' ','') + if now.weekday() > 4: + weekday = False + if cond.find('Mo-Su') > -0.5: + cond = cond.replace('Mo-Su','') + cond = cond.replace(' ','') + if cond.find('; SH off') > -0.5: + cond = cond.replace('; SH off','') + cond = cond.replace(' ','') + if cond.find('Oct-Apr') > -0.5: + if 4 > now.month > 10: + weekday = False + else: + max_speed = max_speed_cond + else: + start, end = cond.split('-') + starthour, startminute = start.split(':') + endhour, endminute = end.split(':') + start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + midnight = datetime.strptime("00:00", "%H:%M").replace(year=now.year, month=now.month, day=now.day) + end1 = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) + if int(endhour) + int(endminute)/60 < int(starthour) + int(startminute)/60: + end2 = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day+1) + if start <= now <= end2 or midnight <= now <= end1 and weekday: + max_speed = max_speed_cond + else: + if start <= now <= end1 and weekday: + max_speed = max_speed_cond + except ValueError: + pass + + if not max_speed and 'source:maxspeed' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None) + if not max_speed and 'maxspeed:type' in tags: + max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None) + + max_speed = parse_speed_unit(max_speed) + return max_speed + +def geocode_maxspeed(tags, location_info): + max_speed = None + try: + geocode_country = location_info.get('country', '') + geocode_region = location_info.get('region', '') + + country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {}) + country_defaults = country_rules.get('Default', []) + for rule in country_defaults: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching country + + region_rules = country_rules.get(geocode_region, []) + for rule in region_rules: + rule_valid = all( + tag_name in tags + and tags[tag_name] == value + for tag_name, value in rule['tags'].items() + ) + if rule_valid: + max_speed = rule['speed'] + break #stop searching region + except KeyError: + pass + max_speed = parse_speed_unit(max_speed) + return max_speed + +class Way: + def __init__(self, way, query_results): + self.id = way.id + self.way = way + self.query_results = query_results + + points = list() + + for node in self.way.get_nodes(resolve_missing=False): + points.append((float(node.lat), float(node.lon), 0.)) + + self.points = np.asarray(points) + + @classmethod + def closest(cls, query_results, lat, lon, heading, prev_way=None): + if query_results is None: + return None + else: + # if prev_way is not None and len(prev_way.way.nodes) < 10: + # if prev_way.on_way(lat, lon, heading): + # return prev_way + # else: + # way = prev_way.next_way(heading) + # if way is not None and way.on_way(lat, lon, heading): + # return way + + results, tree, real_nodes, node_to_way, location_info = query_results + + cur_pos = geodetic2ecef((lat, lon, 0)) + nodes = tree.query_ball_point(cur_pos, 150) + + # If no nodes within 150m, choose closest one + if not nodes: + nodes = [tree.query(cur_pos)[1]] + + ways = [] + for n in nodes: + real_node = real_nodes[n] + ways += node_to_way[real_node.id] + ways = set(ways) + + closest_way = None + best_score = None + for way in ways: + way = Way(way, query_results) + # don't consider backward facing roads + if 'oneway' in way.way.tags and way.way.tags['oneway'] == 'yes': + angle=heading - math.atan2(way.way.nodes[0].lon-way.way.nodes[-1].lon,way.way.nodes[0].lat-way.way.nodes[-1].lat)*180/3.14159265358979 - 180 + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + backwards = abs(angle) > 90 + if backwards: + continue + + points = way.points_in_car_frame(lat, lon, heading, True) + + on_way = way.on_way(lat, lon, heading, points) + if not on_way: + continue + + # Create mask of points in front and behind + x = points[:, 0] + y = points[:, 1] + angles = np.arctan2(y, x) + front = np.logical_and((-np.pi / 2) < angles, angles < (np.pi / 2)) + if all(front): + angles[angles==0] = np.pi + front = np.logical_and((-np.pi / 2) < angles,angles < (np.pi / 2)) + behind = np.logical_not(front) + + dists = np.linalg.norm(points, axis=1) + + # Get closest point behind the car + dists_behind = np.copy(dists) + dists_behind[front] = np.NaN + closest_behind = points[np.nanargmin(dists_behind)] + + # Get closest point in front of the car + dists_front = np.copy(dists) + dists_front[behind] = np.NaN + closest_front = points[np.nanargmin(dists_front)] + + # fit line: y = a*x + b + x1, y1, _ = closest_behind + x2, y2, _ = closest_front + a = (y2 - y1) / max((x2 - x1), 1e-5) + b = y1 - a * x1 + + # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error + # (A 20 degree heading offset results in an a of about 1/3) + score = abs(a) * (abs(b) + 1) * 3. + abs(b) + + # Prefer same type of road + if prev_way is not None: + if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): + score *= 0.5 + + if closest_way is None or score < best_score: + closest_way = way + best_score = score + + if best_score is None: + return None + + # Normal score is < 5 + if best_score > 50: + return None + + return closest_way + + def __str__(self): + return "%s %s" % (self.id, self.way.tags) + + def max_speed(self, heading): + """Extracts the (conditional) speed limit from a way""" + if not self.way: + return None + angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180 + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + backwards = abs(angle) > 90 + if backwards: + if 'maxspeed:backward' in self.way.tags: + max_speed = self.way.tags['maxspeed:backward'] + max_speed = parse_speed_unit(max_speed) + return max_speed + else: + if 'maxspeed:forward' in self.way.tags: + max_speed = self.way.tags['maxspeed:forward'] + max_speed = parse_speed_unit(max_speed) + return max_speed + + max_speed = parse_speed_tags(self.way.tags) + if not max_speed: + location_info = self.query_results[4] + max_speed = geocode_maxspeed(self.way.tags, location_info) + + return max_speed + + def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead, traffic_status, traffic_confidence, last_not_none_signal): + """Look ahead for a max speed""" + if not self.way: + return None + + speed_ahead = None + speed_ahead_dist = None + lookahead_ways = 5 + way = self + for i in range(lookahead_ways): + way_pts = way.points_in_car_frame(lat, lon, heading, True) + #print way_pts + # Check current lookahead distance + if way_pts[0,0] < 0 and way_pts[-1,0] < 0: + break + elif way_pts[0,0] < 0: + max_dist = np.linalg.norm(way_pts[-1, :]) + elif way_pts[-1,0] < 0: + max_dist = np.linalg.norm(way_pts[0, :]) + else: + max_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + + + if max_dist > 2 * lookahead: + #print "max_dist break" + break + try: + if way.way.tags['junction']=='roundabout' or way.way.tags['junction']=='circular': + latmin = 181 + lonmin = 181 + latmax = -181 + lonmax = -181 + for n in way.way.nodes: + lonmax = max(n.lon,lonmax) + lonmin = min(n.lon,lonmin) + latmax = max(n.lat,latmax) + latmin = min(n.lat,latmin) + if way.way.nodes[0].id == way.way.nodes[-1].id: + a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(lonmax-lonmin) + else: + if way.way.nodes[1].id == way.way.nodes[-1].id: + circle = [0,0,30] + else: + circle = circle_through_points([way.way.nodes[0].lat,way.way.nodes[0].lon,1], [way.way.nodes[1].lat,way.way.nodes[1].lon,1], [way.way.nodes[-1].lat,way.way.nodes[-1].lon,1],True) + a = 111132.954*math.cos(float(latmax+latmin)/360*3.141592)*float(circle[2])*2 + speed_ahead = np.sqrt(2.0*a) + min_dist = 999.9 + for w in way_pts: + min_dist = min(min_dist, float(np.linalg.norm(w))) + speed_ahead_dist = min_dist + break + except KeyError: + pass + angle=heading - math.atan2(way.way.nodes[0].lon-way.way.nodes[-1].lon,way.way.nodes[0].lat-way.way.nodes[-1].lat)*180/3.14159265358979 - 180 + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + backwards = abs(angle) > 90 + if backwards: + if 'maxspeed:backward' in way.way.tags: + spd = way.way.tags['maxspeed:backward'] + spd = parse_speed_unit(spd) + if spd is not None and spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + break + else: + if 'maxspeed:forward' in way.way.tags: + spd = way.way.tags['maxspeed:forward'] + spd = parse_speed_unit(spd) + if spd is not None and spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + break + if 'maxspeed' in way.way.tags: + spd = parse_speed_tags(way.way.tags) + #print "spd found" + #print spd + if not spd: + location_info = self.query_results[4] + spd = geocode_maxspeed(way.way.tags, location_info) + #print "spd is actually" + #print spd + if spd is not None and spd < current_speed_limit: + speed_ahead = spd + min_dist = min(np.linalg.norm(way_pts[1, :]),np.linalg.norm(way_pts[0, :]),np.linalg.norm(way_pts[-1, :])) + speed_ahead_dist = min_dist + #print "slower speed found" + #print min_dist + + break + way_pts = way.points_in_car_frame(lat, lon, heading, False) + #print(way_pts) + + try: + count = 0 + loop_must_break = False + for n in way.way.nodes: + if 'highway' in n.tags and (n.tags['highway']=='stop' or n.tags['highway']=='give_way' or n.tags['highway']=='mini_roundabout' or (n.tags['highway']=='traffic_signals' and traffic_lights)) and way_pts[count,0] > 0: + if traffic_status == 'DEAD': + pass + elif traffic_confidence >= 50 and n.tags['highway']=='traffic_signals' and (traffic_status == 'GREEN' or (traffic_status == 'NONE' and not last_not_none_signal == 'SLOW')): + break + #elif traffic_confidence >= 75 and traffic_status == 'SLOW' and n.tags['highway'] != 'motorway': + # speed_ahead = 0 + # speed_ahead_dist = 250 + # loop_must_break = True + # break + if 'direction' in n.tags: + if backwards and (n.tags['direction']=='backward' or n.tags['direction']=='both'): + #print("backward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 3.0) + #print(speed_ahead_dist) + speed_ahead = 7/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + elif not backwards and (n.tags['direction']=='forward' or n.tags['direction']=='both'): + #print("forward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 3.0) + #print(speed_ahead_dist) + speed_ahead = 7/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + try: + if int(n.tags['direction']) > -0.1 and int(n.tags['direction']) < 360.1: + #print(int(n.tags['direction'])) + direction = int(n.tags['direction']) - heading + if direction < -180: + direction = direction + 360 + if direction > 180: + direction = direction - 360 + if abs(direction) > 135: + speed_ahead_dist = max(0. , way_pts[count, 0] - 3.0) + #print(speed_ahead_dist) + speed_ahead = 7/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + except (KeyError, ValueError): + pass + elif 'traffic_signals:direction' in n.tags: + if backwards and (n.tags['traffic_signals:direction']=='backward' or n.tags['traffic_signals:direction']=='both'): + #print("backward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 6.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='traffic_signals': + speed_ahead = 0 + loop_must_break = True + break + elif not backwards and (n.tags['traffic_signals:direction']=='forward' or n.tags['traffic_signals:direction']=='both'): + #print("forward") + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 6.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='traffic_signals': + speed_ahead = 0 + loop_must_break = True + break + try: + if int(n.tags['traffic_signals:direction']) > -0.1 and int(n.tags['traffic_signals:direction']) < 360.1: + #print(int(n.tags['traffic_signals:direction'])) + direction = int(n.tags['traffic_signals:direction']) - heading + if direction < -180: + direction = direction + 360 + if direction > 180: + direction = direction - 360 + if abs(direction) > 135: + speed_ahead_dist = max(0. , way_pts[count, 0] - 6.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='traffic_signals': + speed_ahead = 0 + loop_must_break = True + break + except (KeyError, ValueError): + pass + else: + if n.tags['highway']=='mini_roundabout': + if way_pts[count, 0] > 0: + speed_ahead_dist = max(0. , way_pts[count, 0] - 5.0) + #print(speed_ahead_dist) + speed_ahead = 15/3.6 + loop_must_break = True + break + if way_pts[count, 0] > 0 and traffic_lights_without_direction: + #print("no direction") + speed_ahead_dist = max(0. , way_pts[count, 0] - 10.0) + #print(speed_ahead_dist) + speed_ahead = 5/3.6 + if n.tags['highway']=='stop': + if rolling_stop: + speed_ahead = 2.5 + else: + speed_ahead = 0 + loop_must_break = True + break + if 'railway' in n.tags and n.tags['railway']=='level_crossing': + if way_pts[count, 0] > 0 and traffic_confidence >= 50 and traffic_status == 'SLOW': + speed_ahead = 0 + speed_ahead_dist = max(0. , way_pts[count, 0] - 10.0) + loop_must_break = True + break + if 'traffic_calming' in n.tags: + if way_pts[count, 0] > 0: + if n.tags['traffic_calming']=='bump' or n.tags['traffic_calming']=='hump': + speed_ahead = 2.24 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break + elif n.tags['traffic_calming']=='chicane' or n.tags['traffic_calming']=='choker': + speed_ahead = 20/3.6 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break + elif n.tags['traffic_calming']=='yes': + speed_ahead = 40/3.6 + speed_ahead_dist = way_pts[count, 0] + loop_must_break = True + break + count += 1 + if loop_must_break: break + except (KeyError, IndexError, ValueError): + pass + # Find next way + way = way.next_way(heading) + if not way: + #print "no way break" + break + + return speed_ahead, speed_ahead_dist + + def advisory_max_speed(self): + if not self.way: + return None + + tags = self.way.tags + adv_speed = None + + if 'maxspeed:advisory' in tags: + adv_speed = tags['maxspeed:advisory'] + adv_speed = parse_speed_unit(adv_speed) + return adv_speed + + def on_way(self, lat, lon, heading, points = None): + #if len(self.way.nodes) < 10: + # maybe = False + # factor = max(111132.954*math.cos(float(lat)/180*3.141592), 111132.954 - 559.822 * math.cos( 2 * float(lat)/180*3.141592) + 1.175 * math.cos( 4 * float(lat)/180*3.141592)) + # for n in range(len(self.way.nodes)-1): + # if factor * distance(lat,lon,float(self.way.nodes[n].lat),float(self.way.nodes[n].lon),float(self.way.nodes[n+1].lat),float(self.way.nodes[n+1].lon)) < 10.0: + # maybe = True + # if not maybe: + # return False + if points is None: + points = self.points_in_car_frame(lat, lon, heading, True) + x = points[:, 0] + return np.min(x) <= 0. and np.max(x) > 0. + + def closest_point(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading, True) + i = np.argmin(np.linalg.norm(points, axis=1)) + return points[i] + + def distance_to_closest_node(self, lat, lon, heading, points=None): + if points is None: + points = self.points_in_car_frame(lat, lon, heading, True) + return np.min(np.linalg.norm(points, axis=1)) + + def points_in_car_frame(self, lat, lon, heading, flip): + lc = LocalCoord.from_geodetic([lat, lon, 0.]) + + # Build rotation matrix + heading = math.radians(-heading + 90) + c, s = np.cos(heading), np.sin(heading) + rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]]) + + # Convert to local coordinates + points_carframe = lc.geodetic2ned(self.points).T + + # Rotate with heading of car + points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T + + if points_carframe[-1,0] < points_carframe[0,0] and flip: + points_carframe = np.flipud(points_carframe) + + return points_carframe + + def next_way(self, heading): + results, tree, real_nodes, node_to_way, location_info = self.query_results + #print "way.id" + #print self.id + #print "node0.id" + #print self.way.nodes[0].id + #print "node-1.id" + #print self.way.nodes[-1].id + #print "heading" + #print heading + angle=heading - math.atan2(self.way.nodes[0].lon-self.way.nodes[-1].lon,self.way.nodes[0].lat-self.way.nodes[-1].lat)*180/3.14159265358979 - 180 + #print "angle before" + #print angle + if angle < -180: + angle = angle + 360 + if angle > 180: + angle = angle - 360 + #print "angle" + #print angle + backwards = abs(angle) > 90 + #print "backwards" + #print backwards + if backwards: + node = self.way.nodes[0] + else: + node = self.way.nodes[-1] + + ways = node_to_way[node.id] + + way = None + try: + # Simple heuristic to find next way + ways = [w for w in ways if w.id != self.id] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + if len(ways) == 2: + try: + if ways[0].tags['junction']=='roundabout' or ways[0].tags['junction']=='circular': + #print ("roundabout found") + way = Way(ways[0], self.query_results) + return way + except (KeyError, IndexError): + pass + try: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): + way = Way(ways[0], self.query_results) + return way + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): + way = Way(ways[1], self.query_results) + return way + except (KeyError, IndexError): + pass + ways = [w for w in ways if (w.nodes[0] == node or w.nodes[-1] == node)] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + # Filter on highway tag + acceptable_tags = list() + cur_tag = self.way.tags['highway'] + acceptable_tags.append(cur_tag) + if cur_tag == 'motorway_link': + acceptable_tags.append('motorway') + acceptable_tags.append('trunk') + acceptable_tags.append('primary') + ways = [w for w in ways if w.tags['highway'] in acceptable_tags] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + #print "only one way found" + return way + if len(ways) == 2: + try: + if ways[0].tags['junction']=='roundabout' or ways[0].tags['junction']=='circular': + #print ("roundabout found") + way = Way(ways[0], self.query_results) + return way + except (KeyError, IndexError): + pass + try: + if (ways[0].tags['oneway'] == 'yes') and (ways[1].tags['oneway'] == 'yes'): + if (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id) and not (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id): + way = Way(ways[0], self.query_results) + return way + elif (ways[0].nodes[0].id != node.id and ways[1].nodes[0].id == node.id) and not (ways[0].nodes[0].id == node.id and ways[1].nodes[0].id != node.id): + way = Way(ways[1], self.query_results) + return way + except (KeyError, IndexError): + pass + # Filter on number of lanes + cur_num_lanes = int(self.way.tags['lanes']) + if len(ways) > 1: + ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] + if len(ways_same_lanes) == 1: + ways = ways_same_lanes + if len(ways) > 1: + ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] + if len(ways) == 1: + way = Way(ways[0], self.query_results) + + except (KeyError, ValueError): + pass + + return way + + def get_lookahead(self, lat, lon, heading, lookahead): + pnts = None + way = self + valid = False + + for i in range(5): + # Get new points and append to list + new_pnts = way.points_in_car_frame(lat, lon, heading, True) + + try: + if way.way.tags['junction']=='roundabout' or way.way.tags['junction']=='circular': + break + except KeyError: + pass + if pnts is None: + pnts = new_pnts + valid = True + else: + new_pnts = np.delete(new_pnts,[0,0,0], axis=0) + pnts = np.vstack([pnts, new_pnts]) + + # Check current lookahead distance + max_dist = np.linalg.norm(pnts[-1, :]) + + if max_dist > 2 * lookahead: + break + + # Find next way + startid = way.way.nodes[0].id + endid = way.way.nodes[-1].id + way = way.next_way(heading) + if not way: + break + if not (way.way.nodes[0].id == startid or way.way.nodes[0].id == endid or way.way.nodes[-1].id == startid or way.way.nodes[-1].id == endid): + break + return pnts, valid diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 632cc2875fd85b..c0ee6f989f75cc 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1,3 +1,8 @@ +#include +#include +#include +#include +#include #include "ui.hpp" #include #include @@ -19,6 +24,8 @@ extern "C"{ #include "sidebar.hpp" #include "paint_dp.hpp" +int border_shifter = 20; + // TODO: this is also hardcoded in common/transformations/camera.py // TODO: choose based on frame input size @@ -295,36 +302,105 @@ static void ui_draw_vision_maxspeed(UIState *s) { char maxspeed_str[32]; float maxspeed = s->scene.controls_state.getVCruise(); int maxspeed_calc = maxspeed * 0.6225 + 0.5; + float speedlimit = s->scene.speedlimit; + int speedlim_calc = speedlimit * 2.2369363 + 0.5; if (s->is_metric) { maxspeed_calc = maxspeed + 0.5; + speedlim_calc = speedlimit * 3.6 + 0.5; } - + int speed_lim_off = speedlim_calc * (1 + s->speed_lim_off / 100.0); bool is_cruise_set = (maxspeed != 0 && maxspeed != SET_SPEED_NA); - + bool is_speedlim_valid = s->scene.speedlimit_valid; + bool is_set_over_limit = is_speedlim_valid && s->scene.controls_state.getEnabled() && + is_cruise_set && maxspeed_calc > (speedlim_calc + speed_lim_off); int viz_maxspeed_w = 184; int viz_maxspeed_h = 202; - int viz_maxspeed_x = s->scene.viz_rect.x + (bdr_s*2); - int viz_maxspeed_y = s->scene.viz_rect.y + (bdr_s*1.5); + int viz_maxspeed_x = (s->video_rect.x + (bdr_is*2)); + int viz_maxspeed_y = (s->video_rect.y + (bdr_is*1.5)); int viz_maxspeed_xo = 180; - - viz_maxspeed_xo = 0; + viz_maxspeed_w += viz_maxspeed_xo; + viz_maxspeed_x += viz_maxspeed_w - (viz_maxspeed_xo * 2); + //viz_maxspeed_xo = 0; // Draw Background - ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, COLOR_BLACK_ALPHA(100), 30); + ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, + is_set_over_limit ? nvgRGBA(218, 111, 37, 180) : COLOR_BLACK_ALPHA(100), 30); // Draw Border NVGcolor color = COLOR_WHITE_ALPHA(100); + if (is_set_over_limit) { + color = COLOR_OCHRE; + } else if (is_speedlim_valid) { + color = s->is_ego_over_limit ? COLOR_WHITE_ALPHA(20) : COLOR_WHITE; + } ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, color, 20, 10); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); const int text_x = viz_maxspeed_x + (viz_maxspeed_xo / 2) + (viz_maxspeed_w / 2); - ui_draw_text(s->vg, text_x, 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); + ui_draw_text(s->vg, text_x, 148-border_shifter, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); if (is_cruise_set) { snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc); - ui_draw_text(s->vg, text_x, 242, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s->vg, text_x, 242-border_shifter, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); } else { - ui_draw_text(s->vg, text_x, 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + ui_draw_text(s->vg, text_x, 242-border_shifter, "-", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + } +} + +static void ui_draw_vision_speedlimit(UIState *s) { + char speedlim_str[32]; + float speedlimit = s->scene.speedlimit; + int speedlim_calc = speedlimit * 2.2369363 + 0.5; + if (s->is_metric) { + speedlim_calc = speedlimit * 3.6 + 0.5; + } + + bool is_speedlim_valid = s->scene.speedlimit_valid; + float hysteresis_offset = 0.5; + if (s->is_ego_over_limit) { + hysteresis_offset = 0.0; + } + s->is_ego_over_limit = is_speedlim_valid && s->scene.controls_state.getVEgo() > (speedlimit + hysteresis_offset); + + int viz_speedlim_w = 180; + int viz_speedlim_h = 202; + int viz_speedlim_x = (s->video_rect.x + (bdr_is*2)); + int viz_speedlim_y = (s->video_rect.y + (bdr_is*1.5)); + if (!is_speedlim_valid) { + viz_speedlim_w -= 5; + viz_speedlim_h -= 10; + viz_speedlim_x += 9; + viz_speedlim_y += 5; + } + // Draw Background + NVGcolor color = COLOR_WHITE_ALPHA(100); + if (is_speedlim_valid && s->is_ego_over_limit) { + color = nvgRGBA(218, 111, 37, 180); + } else if (is_speedlim_valid) { + color = COLOR_WHITE; + } + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, color, is_speedlim_valid ? 30 : 15); + + // Draw Border + if (is_speedlim_valid) { + ui_draw_rect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, + s->is_ego_over_limit ? COLOR_OCHRE : COLOR_WHITE, 20, 10); + } + const float text_x = viz_speedlim_x + viz_speedlim_w / 2; + const float text_y = viz_speedlim_y + (is_speedlim_valid ? 50 : 45); + // Draw "Speed Limit" Text + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + color = is_speedlim_valid && s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y, "SMART", 50, color, s->font_sans_semibold); + ui_draw_text(s->vg, text_x + (is_speedlim_valid ? 6 : 0), text_y + 40, "SPEED", 50, color, s->font_sans_semibold); + + // Draw Speed Text + color = s->is_ego_over_limit ? COLOR_WHITE : COLOR_BLACK; + if (is_speedlim_valid) { + snprintf(speedlim_str, sizeof(speedlim_str), "%d", speedlim_calc); + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, 48*2.5, color, s->font_sans_bold); + } else { + ui_draw_text(s->vg, text_x, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", 42*2.5, color, s->font_sans_semibold); } } @@ -377,12 +453,23 @@ static void ui_draw_vision_speed(UIState *s) { static void ui_draw_vision_event(UIState *s) { const int viz_event_w = 220; - const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_s*2); - const int viz_event_y = s->scene.viz_rect.y + (bdr_s*1.5); - if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { + const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_is*2); + const int viz_event_y = s->scene.viz_rect.y + (bdr_is*1.5); + if (s->scene.speedlimitahead_valid && s->scene.speedlimitaheaddistance < 300 && s->scene.controls_state.getEnabled() && s->limit_set_speed) { + const int img_turn_size = 160; + const int img_turn_x = viz_event_x-(img_turn_size/4)+80; + const int img_turn_y = viz_event_y+bdr_s-25; + float img_turn_alpha = 1.0f; + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, img_turn_x, img_turn_y, + img_turn_size, img_turn_size, 0, s->img_speed, img_turn_alpha); + nvgRect(s->vg, img_turn_x, img_turn_y, img_turn_size, img_turn_size); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); + } else if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { // draw winding road sign - const int img_turn_size = 160*1.5; - ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); + const int img_turn_size = 160*1.5*0.82; + ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_is - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; @@ -394,9 +481,16 @@ static void ui_draw_vision_event(UIState *s) { } } +static void ui_draw_vision_map(UIState *s) { + const int map_size = 96; + const int map_x = (s->video_rect.x + (map_size * 3) + (bdr_s * 3)); + const int map_y = (s->scene.viz_rect.bottom() + ((footer_h - map_size) / 2)); + ui_draw_circle_image(s->vg, map_x, map_y, map_size, s->img_map, s->scene.map_valid); +} + static void ui_draw_vision_face(UIState *s) { const int face_size = 96; - const int face_x = (s->scene.viz_rect.x + face_size + (bdr_s * 2)); + const int face_x = (s->scene.viz_rect.x + face_size + (bdr_is * 2)); const int face_y = (s->scene.viz_rect.bottom() - footer_h + ((footer_h - face_size) / 2)); ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.dmonitoring_state.getFaceDetected()); } @@ -465,6 +559,7 @@ static void ui_draw_vision_header(UIState *s) { } if (s->scene.dpUiMaxSpeed) { ui_draw_vision_maxspeed(s); + ui_draw_vision_speedlimit(s); } if (s->scene.dpUiSpeed) { ui_draw_vision_speed(s); @@ -477,6 +572,7 @@ static void ui_draw_vision_header(UIState *s) { static void ui_draw_vision_footer(UIState *s) { if (s->scene.dpUiFace) { ui_draw_vision_face(s); + ui_draw_vision_map(s); } if ((int)s->scene.dpDynamicFollow > 0) { ui_draw_df_button(s); @@ -510,9 +606,9 @@ void ui_draw_vision_alert(UIState *s, cereal::ControlsState::AlertSize va_size, color.a *= s->alert_blinking_alpha; int alr_s = alert_size_map[va_size]; - const int alr_x = scene->viz_rect.x - bdr_s + 100; - const int alr_w = scene->viz_rect.w + (bdr_s*2) - 200; - const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s) - 100; + const int alr_x = scene->viz_rect.x - bdr_is + 100; + const int alr_w = scene->viz_rect.w + (bdr_is*2) - 200; + const int alr_h = alr_s+(va_size==cereal::ControlsState::AlertSize::NONE?0:bdr_is) - 100; const int alr_y = s->fb_h-alr_h - 100; ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, color, 20); @@ -702,6 +798,10 @@ void ui_nvg_init(UIState *s) { s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); assert(s->img_wheel != 0); + s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); + assert(s->img_map != 0); + s->img_speed = nvgCreateImage(s->vg, "../assets/img_trafficSign_speedahead.png", 1); + assert(s->img_speed != 0); s->img_turn = nvgCreateImage(s->vg, "../assets/img_trafficSign_turn.png", 1); assert(s->img_turn != 0); s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); diff --git a/selfdrive/ui/paint_dp.cc b/selfdrive/ui/paint_dp.cc index 1a0515699a617e..4fbf7c413ab687 100644 --- a/selfdrive/ui/paint_dp.cc +++ b/selfdrive/ui/paint_dp.cc @@ -354,12 +354,12 @@ void bb_ui_draw_measures_right(UIState *s, int bb_x, int bb_y, int bb_w ) { void ui_draw_bbui(UIState *s) { const int bb_dml_w = 184; - const int bb_dml_x = s->scene.viz_rect.x + (bdr_s*2); - const int bb_dml_y = s->scene.viz_rect.y + (bdr_s*1.5) + 220; + const int bb_dml_x = s->scene.viz_rect.x + (bdr_is*2); + const int bb_dml_y = s->scene.viz_rect.y + (bdr_is*1.5) + 220; const int bb_dmr_w = 184; - const int bb_dmr_x =s->scene.viz_rect.x + s->scene.viz_rect.w - bb_dmr_w - (bdr_s * 2); - const int bb_dmr_y = s->scene.viz_rect.y + (bdr_s*1.5) + 220; + const int bb_dmr_x =s->scene.viz_rect.x + s->scene.viz_rect.w - bb_dmr_w - (bdr_is * 2); + const int bb_dmr_y = s->scene.viz_rect.y + (bdr_is*1.5) + 220; bb_ui_draw_measures_right(s, bb_dml_x, bb_dml_y, bb_dml_w); bb_ui_draw_measures_left(s, bb_dmr_x, bb_dmr_y, bb_dmr_w); -} \ No newline at end of file +} diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2fb22f64f11980..0b2a110bacd8ad 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -25,7 +25,7 @@ int write_param_float(float param, const char* param_name, bool persistent_param } void ui_init(UIState *s) { - s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents", "dragonConf", "carState"}); @@ -204,6 +204,13 @@ void update_sockets(UIState *s) { if (sm.updated("thermal")) { scene.thermal = sm["thermal"].getThermal(); } + if (sm.updated("liveMapData")) { + scene.map_valid = sm["liveMapData"].getLiveMapData().getMapValid(); + scene.speedlimit = sm["liveMapData"].getLiveMapData().getSpeedLimit(); + scene.speedlimit_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitValid(); + scene.speedlimitahead_valid = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadValid(); + scene.speedlimitaheaddistance = sm["liveMapData"].getLiveMapData().getSpeedLimitAheadDistance(); + } if (sm.updated("ubloxGnss")) { auto data = sm["ubloxGnss"].getUbloxGnss(); if (data.which() == cereal::UbloxGnss::MEASUREMENT_REPORT) { @@ -332,6 +339,10 @@ void ui_update(UIState *s) { // Read params if ((s->sm)->frame % (5*UI_FREQ) == 0) { read_param(&s->is_metric, "IsMetric"); + } else if ((s->sm)->frame % (7*UI_FREQ) == 0) { + read_param(&s->speed_lim_off, "SpeedLimitOffset"); + } else if ((s->sm)->frame % (11*UI_FREQ) == 0) { + read_param(&s->limit_set_speed, "LimitSetSpeed"); } else if ((s->sm)->frame % (6*UI_FREQ) == 0) { int param_read = read_param(&s->last_athena_ping, "LastAthenaPingTime"); if (param_read != 0) { // Failed to read param diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index afeb64468f7268..18a743559861ac 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -34,6 +34,8 @@ #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 COLOR_OCHRE nvgRGBA(218, 111, 37, 255) +#define COLOR_OCHRE_ALPHA(x) nvgRGBA(218, 111, 37, x) #define UI_BUF_COUNT 4 @@ -48,7 +50,8 @@ typedef struct Rect { } Rect; const int sbr_w = 300; -const int bdr_s = 30; +const int bdr_s = 10; +const int bdr_is = 30; const int header_h = 420; const int footer_h = 280; const Rect settings_btn = {50, 35, 200, 117}; @@ -107,6 +110,12 @@ typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. bool world_objects_visible; + float speedlimit; + bool speedlimit_valid; + float speedlimitaheaddistance; + bool speedlimitahead_valid; + bool map_valid; + bool is_rhd; bool frontview; bool uilayout_sidebarcollapsed; @@ -219,6 +228,8 @@ typedef struct UIState { int img_battery; int img_battery_charging; int img_network[6]; + int img_map; + int img_speed; SubMaster *sm; @@ -250,6 +261,9 @@ typedef struct UIState { bool ignition; bool is_metric; bool longitudinal_control; + bool limit_set_speed; + bool is_ego_over_limit; + float speed_lim_off; uint64_t last_athena_ping; uint64_t started_frame;