From 317df4dcad451dc8c577e87683f97783b6375af4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:37:09 +0900 Subject: [PATCH] perf(out_of_lane): use intersection with other lanes instead of difference with ego lane (#8870) Signed-off-by: Maxime CLEMENT --- .../README.md | 12 +- .../docs/ego_lane.png | Bin 17530 -> 0 bytes .../docs/other_lanes.png | Bin 0 -> 21164 bytes .../src/calculate_slowdown_points.cpp | 5 +- .../src/debug.cpp | 105 ++++++++++------ .../src/footprint.cpp | 3 + .../src/lanelets_selection.cpp | 116 +++++++++++------- .../src/lanelets_selection.hpp | 12 +- .../src/out_of_lane_collisions.cpp | 56 ++++++--- .../src/out_of_lane_collisions.hpp | 2 +- .../src/out_of_lane_module.cpp | 17 +-- .../src/types.hpp | 10 +- 12 files changed, 216 insertions(+), 122 deletions(-) delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md index 0b6aa697fcbef..dfc88ef676838 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -27,18 +27,18 @@ The length of the trajectory used for generating the footprints is limited by th ![ego_footprints](./docs/footprints.png) -### 2. Ego lanelets +### 2. Other lanelets -In the second step, we calculate the lanelets followed by the ego trajectory. -We select all lanelets crossed by the trajectory linestring (sequence of trajectory points), as well as their preceding lanelets. +In the second step, we calculate the lanelets where collisions should be avoided. +We consider all lanelets around the ego vehicle that are not crossed by the trajectory linestring (sequence of trajectory points) or their preceding lanelets. -![ego_lane](./docs/ego_lane.png) +![other_lanes](./docs/other_lanes.png) -In the debug visualization the combination of all ego lanelets is shown as a blue polygon. +In the debug visualization, these other lanelets are shown as blue polygons. ### 3. Out of lane areas -Next, for each trajectory point, we create the corresponding out of lane areas by subtracting the ego lanelets (from step 2) from the trajectory point footprint (from step 1). +Next, for each trajectory point, we create the corresponding out of lane areas by intersection the other lanelets (from step 2) with the trajectory point footprint (from step 1). Each area is associated with the lanelets overlapped by the area and with the corresponding ego trajectory point. ![out_of_lane_areas](./docs/out_of_lane_areas.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png deleted file mode 100644 index 65cb73226465ae3de6c27b35562d35c57c5906b5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17530 zcmbt+cRZEh|MyWTp^y_2*($>jiv)`ZhCH&b_`CIsu_%ImkmZE};1`KvZ4+g`T z#Jd9ilKS1n1pK)9Nzk3I#)}d(@2BU>3%1CK>7_U#O+ELl0v+OTNZ=T{? z@hU3ivlg=zml|mQ&9?ol)S@{e_>6CC^fS-PuLN*16d~EGZ#OngtKQ=Hj0MF;hWrj8 z@|}`Jic3-~1--eKS8uei;pR9?!%YX z$G9VQ-WEiBBxj+aa`R|#`3!&m*K6-N_!0!p!J0K3hZ$S-T&?DstxX&*<*qh-rnA3BZ+!QBzXo5 zX41=ssYH@bhT82SoStpWw6F`R-b7b-7P6_A>vR>eVbJ7KK1)|9m}Kn&^exg0$M~5F z4;=l{ja%{dx0BZTxP&D9-U>fTB_}8EVh+HLj~`Y1*v^=3ru#hAu%q{4m1OPg_TI~v zFU>R~3Kq?CwH}76j+pGvlbSG(+PWV~6>L95*461O)F94Q3l2R#9)p)$K_9h>WiN>R z5^1fRYiiFIsp-tIH9dTQ_^9>E<7YI-qSqVRYXLYb@yC&okv+G^zQqKf5JKlyNcru>rIr`-=c$$^MVTDmKVX4}Jqgzx ze}1OP{<3n|?Y1a9D;R|gFV6zIZjxMHUXG38NK!J&C28=G%04hH*v28zF2!`Z+!Wt( zZeVeeskNS50Xt4B>($p49Ua}$((;Y0vgWO>c~b8U>JCRyYhp1~` zCZ9hk9PHuJHp2J(l%`hABW-y8a_q17Hl~{}Rhx+^;e2Uf%@>*-i@mCRCsdD8F~umi~FXrH`VYp^03KCge}b#6~&vZkAd0|_AQc!t(U?y zGyHkL`+l8s;~?(7|5lpo1%A_GwIHCL6v_KnoH(4uoM^e1v`y{+v*92cJV`pbS^6`85Qu#~Y zj^xzz8K$XebL(wsDMDZG&|Lp~w5RmFE;QUtjndJ&y1MNnpDI1ZT(c^Vy}zT}gfD9; z+va+c{13K%Biu4Y=pQC!Pk+H?S6r|j;s4-rjlKd?`DmfGibA0V{r#5zjv{{7_sJqx zU~G`-t6Y~vhFbqdq1rsR7l#vAWK!XLFTh;HX@w2VKI@TOehk?#`Vs!{@6{aG&R60d zTPD`aO&7lrr$nc}Pd3|{z4k6V54UDBoAEiXnFqsY8=aSWlh`qB`?VRN?}qprj4LB` z^T$q4PYaY&$jHg>%D!XlaWle3ys0juIbFUu#X+1WAdZ&m+s@ijl(bEMzH&}2^xXNQ zDyxnk(hfhXmm>(ly-6KP;zz)RTA9mRJ1;(oUDhqxY#FxG^g)WRN8 zCyJbtp5hM$!!@I${%uVs2&^K%5p!ZpO2t~CjTLX23Nv4`yVV@zaU&^FGLx=bBl>t zw>L+SOD^@<1X_@v!6g09-@heeUc=YU>%UptgRQ6uKL2h-(Eo?TU+-ZO^H!dHGhqie zVbHQ=o8!R#e(e0C5r^sH`l3g13Qt*={GUpb{c#qlD!%3aocV9tEufSGPr{x6u`7|6 z1zTxsf=y_tQ7*7Xqh}GG#{6meV@-!y_fGupN0ET7{5vY3I@0;;sg|~`wnPfwJ@eMh zhY|6a(!5pX(&R&ua$^jpUsWiSK9|w>?2XdwER2rkaB!R|ZvV46Otu4x>MCw=xVx%} z%_R|?&0sjKzxPx4<>uv<>NiZ76QkmtR)hxz24Xf(7LEfETFWP8YPPioZ{NP{sl)^J z93gbO#cgfYs^&se(yh>-e1Byq&qHu^(y`5ca{o)=py6qMj|Fd36)m3J(OrC_JQ(fp z=N+HqBfq(mJL=!+)(tC~TYUf8lWKf-qN1Oeoh5IjV%*GgeP=WhKb`NsIb&pKNTn*Y z`rOeMrZMatgadN`P3oJzV8^4;7 zFv-r@`*GJW)&WGf&7F7l7*G9xQ?Z&atCrC+ZNVJCts~B{v0ahZd!5q7am`Cgic92V z#_isO9lAc8b(a_nuUuU^yT=6F*{%%hxyM@yGp=8{%f`I8O=a(d--7V+Z+*Q5i@INE zlU{iA?FNmw&*3NZwC~&KZaVdE2Ulm^*^{nL*4@8Vn&NI_0i^Is+F9+Mm%uir1oH@E zzrNW<+c~4SyfSs)7~gsA^1?=(sNcRVVP$t9!HeC!z(_}LA{ezfsoRr<*STXav(o4SbvR6kVwPJtg(#PVDbDPy<>6K=aN z)T+-44+qxUP_fWh=h0O)R76W+Z*PCIl<w0NY;VlJ6~1_n?A z1G;-&`{elcQbLI4( z@F=DxMO_`8fs~MnH5H?qtZ|*!VXzko=roDKxq$)yi=CV{dD5J~sh}G~f`WpF*!zyn zPTLl0s8*`;`DY|oV43B{mFw&FY!S(Zh#AhO-zMvk6exf8>}&-mqmOIHvM7Qga_C)k zrx!jA9nPrvZ6isCPBu4<+s-RRE7&qsoKIF-f*1<@6?7Gq6#zY}GTupNKv zK^~3o_gKf-a|Hjo{z-bf;{NXH$WJRe6bP8dbuBHeEiKu7BHc#}ep_do7u+c)A|&_Z z>Qb7h)M7l8H`XwfmF4AG;v(qFm1<6D9xtU_tj(UUThbrT9GrXkonJ(7rB>v|h0WWK zZ6BRJ-Z#m>{|LtBl!-qdwazIg9y(|}z@%_poGca>v*Tkhm?K7w5&q5DHm>Y!b?BP| zgVlapJuH>UgrUC?jydvtZnCEMEbr){TWqZj8qF4QgL#TpzisOCQ!Sjx3N7Xd!FJ4v z$JbM+%;@C$hIjqE8{+CL)phhbGmZXI!#s|6{eRyOa%gtnCLHP!&P3(L-6425&~h^I z`77}F0j>JKhz`H~{j2sgTg24c`)nu=2^5%sfPKEfX6q1Z?#DgJMEVfu#`$NWTd#F1 zF@y}dm1;;??b6Z1TN#{2jr<)!A<){7=R9dpPl8(VY3o~BI0aS1_x5!CckUTm#zKPV zNMP~rB}z_d^WX04ix5*7boY+vc?;dqj(rlu$K23+KGWhozq;D3qkDm@vM(cC&@~%* zVKYJ0#SG?td)>P4?BqaogfTZ(@>oG!Xo*6dvF@85NG>}I$K%lwBpD`kQDOFtnk)Wt+t>YI(=VI$(a z&R=SkYFF9NSJlsWDcjv=9rSh!s_0o3o+Zm*exB!~Y?p2~`d6UprioYc#W|W?SI}|h z)CGG@rQyR3y02PmXBkr}F`PTA+d&5Q&Gq#xg0)BUkq3^`UM=d&!^5Qu8f>IO*~JF6 z;V<0lv5SgN#4&9LGtcJ{XIonr8%w$?L+@usO<9wa%rvFj+=o zWT64$RUk+E^2Nh|fBTuw*4!J9dDjDwKKq@ClGxkX?e2Bn6a1>r2l8wQlrfdF_{_Y? zeT6#=pcD|q-gTsYQgtA+xJ0Y*jMyI`EU!+H;=6vq$@9Q_)OL@)zz2%7+$SEa+k+eV z8{riGQ+b-7joQ%oO>louko4u@XnUMV6JL28;i@8YF2&JFCo0>t))JH`Z_<70eR_!1 zi&qN(OXUBX8rIX-L6yemwn@95>hFZb_?=NeHOA&y?7gl%A7Py`7R-B*h0S@g3P&o*E% zIkpB1Gq6D~U#<=1$shT&>T%pruhQ#V-T|Iweq?wVD6M15UUJ3S=?6>w#)`E@$%0KN zWuHqrnDLIK@j1nIJ1zYBr6ZY>!l;PZGpjn>pLA4P?LK>4R$lIV&@|iPx_ZYS)GSzR zHx0t~c=zvU>FJs0?#fV)wF(XhgfNn!7ViU(rEc`M#bQvQ6{+PgD|eLYR)(7Dm5#=z z^f#XzYy!uGmHts(hP*h&lIaE)dCVT?)i*ua8b%o4 zqYJT5E_{G8fMsqnF#t7s+p~#NYw(B@-+nT?q7n}{4_Kr7=7Y@*<<}9fJH3@g_-w~! z$d7As&dSUOm?GQ3LL!P-pwKfnx?j2qZkW|g z)jQanjcs~DBk40%Kn(9R|EQ@22d)rkhVvGxA$O-dcxsvgtVd^z!+}% zjogG1XO_X;^>0BpPR~^fZTz;r-B{0fwU6dU7wVHef6L}6U&4u27=R<*&Bps}f1tk> zunUzKB85Gf%pw*Z@3cb;^;?~n$jr+$Jn6FUrwii_W>QZ?u0F)CS`~!lYdKr1W2Zc^ zW~?(MVh*eCY?8^zbO;%gWsrVHFR{BnI_otW73flT=e}J%Uuy%ImhUZ|7+m0a6^g$l z2UV4qKbovN8s45M8r*vL%I`cr_oIwf`Npp`hKKprvjeB#cS9%`t_37gdWcXwMK~q2@EP@y|yy6qNa&pH}L`U56 zMD+uHRCE~?70A*?G~i}Y;P%H#=eVM-_Vz0zzDG+b^Q)R$RaH&yn<#%&o@GzfW2xFk zv2^FHAR@`R8&?&-9o}wfzWt}rGM^Y^xy7Uh+OLUVBmAC+_d>rtr|K4Q>e8R6`Qh|Y z%is>DvMs;yjh?NkHvb$NLvGxV`WG%PnqN%J%@>aMK%G=kUao3K1_|7)F(v46*>$;ld{ghQ)E7K55V>p8DdnFh#Rr&D+2I_iwp{GA{ua zvbB!}MO7s7&zZZyy)TVz%zN3Ii@BLq26#e))WV~3M_+b7;g4!ZF&w2D<>~FssaaWB zc7ho_7QIW(c|*dYq7iE2uO*y*&oOCkbtBHJJkrBBe;j!h-n^Wc`$T1=Z^^kpLve(E zh8}Lw@s(Yij-c&yTiGuq_a&yH>%=q0r03co9laNDu|0e;GC~eh??3qSeNq`&)EBe) z^!@?NDrc>6&&$`*F}BilY-Xm;{(1Aq8|6Bsl_y$1*92sTc+(84hnzAS8bJLH$3GqY zh4D{lHksR)Z5!My+Xapy23Nn<06Y+p%Z-9Bo#Mt@>Q}5ql*=TJ8Jk=o$3RKALZnA4 zYg-2A#V*3LXU^WSlAy*UCMM=#5#2cN>+9=WZKuu(n|;T}qR5ws zH)0i#>v!{bVCM*3M5`_c8*lGvr}UoWtSoBd_+<2w>Arlnbvo;<^{n45)jqvZ9u8$c z1n^-t-yG%Is;cOwZs!_T?!;(f+=6IVh9$-vF)zvtrXRuoBzQ;E8I(h>TT;J|l6KB2N9|bGnt%cB%$a*HJfLiUVt5WYG&Q z(N;dggNV30npVsuteq4g6gxt$YeDKt`uFc~Rz$W#;YCGcq?B?PIt32KiQ>DnC6 zU`=jl?mnS1B-inm3D@SZDSH?z`joQ$`^phOFKHh!e>fEGe{a?A%5eigL=PEQ_q>GY zrhig_Kww2hLo43xGeqKjxJ74F#wuP;wYYzP3{%Y_^geNP$bFp#u$`|_86VS*jSPK+ z1~v$8u#~fAQh}h9YMFp|oa*{clFjvVFNJJZ22NVPo$$S8rKZOzxvkcY{LAv zm^id``kF$IDb^$y2i%(qK9KoSW3lZ$AbkrM=P$S#Y#1B4w&46}_S5E=nAf*7-s##w zhG;S8?$*F7AaK;!DBZv__kC2cSxS|!S=g8P%hJnf8vU)R@;%6BzS^Y^)dl&Ylfgq9qgT^jSVM`&c z=$;w!JCQY~mdC$Q+Suf6AfiB8lL!SZ?p0B;5b{;>rT8F#HL#5cx4f-U#vktNGY1Ld zSHkqaY#3n|fSH4q|Sb7piP#6%t&=}B7EceBvx!@NXk!Fy2|ZyqU~P#*ssEr zmxbRpBX4Q`QR@n2(|RaDno-z`*9zZ1jJ}18YP^49y#|x;J3R#1U(fTs+s!l7c4V)S zyh9EFZQqYRX{@@D@04A80BioWkP^Fb&;O*Zs`*sQPL(yqyLZ#iNPG}=PA5iu>Ki71 z4J6IyAR%<2KVBpi@*Hn)yTELAVI1ybvFhd8;|@$uU%Q;tIjzP&mASwo9z-n1>>aLe zO^rQwY+<;G}Gs0Fn523(c|JQ#Y{7&!f>21H$&*cQ2ZTl z?L?N6<@(_HKe73n!j<8-r33g^ums3WWQo`52Cva9(TxAalA6C4o8RH!`S=C1B0@0} z@j>h1O(rcalo&Fv$-?q}1FZ z-jBUq#}#)0D!vf1jbG(c8IGL9L*&SR2+v_+wR+FZmI4hnhPv!rgW3clcXwo)jx0r@4fB&pzJ<4AdlCV^3hUFyb+3pyr2=78VtsKs#s1ZKpWU#pyqQ+i)9n2^kDb1kuqlru;dL&O;ilQcReno-tX_t9{_3cOtm)&R1y8JKD|<7Y;V2Jo^xxEWt*V zIi7rlG4sfEl^(dy;g)Ia8w>%*#~T5wc{T`+6a;;)?cRjVSv_Av*ee^h18nyu$xw|o zf_O0cIZ~NN zjrG7!v9bQ*V{k?w6_rZ9$aekFg^3@l&J$%ubhuq3K~SA&b5|{)SWA-Dj74pru)JKG zzR}xVQ=BJn9zHcTMxlWZwO9%#0F1Gst$sKgzHCK6v%MIJi#X{cQ5(#)z?bycdMqv> z47nvU?4RfB3?Y10v_k2rD%3>+Ox`kY(=S(AOs1BWRuSzyVC{RxjG)+`$w;6zX8Y!- zDy{`R>EP2#Si2s?=gp+moY$M-c)q)srth*jBjNW6l$2W{x$BVgnQko0OXW%GE%imq z#oaRJum122s*rPw(Bg2hkJ19?p0f;o<-h0K#tP{X>a#)R>%!1tzGOdj$QSmd{>FXH zeYrJkK6wJEaty}uG>{XF3ft_~DxDVbyk6Mxh4~o5uL>0*5G643GSY^z#9Z0Mg?w?) zV9L=++XYuADfc%e(5v7#faVs^t+0zwg%v?#{hMp5I33UNl^4f(B?_%MM$G*|q$jwVtzVtY0h&<%`FaOG8c6CYG+W}qtUsGzbm#W=M0*h#=a3B~z_jgg>zFZql^#F!R1Ih+q(GaPsmew;*oHS%STq`XcKK%&Ri{r{ zF@*4>IWG^GSJ}MyWNRh}^>7Qtfyc=R%`+EKkZH(d6Z~cYy_?-J@JwBW@4j1XgVz!S zueDMzobW%n0FP&rQ!qccMERh>gZP`n^YW1NX?LK#YIWnf!QfxL1nu zPp3M?buj(cxm)J*FD?~A>KTL)3wQ1Bd*zvSxtJd6(f)0-xkLtoEVVgG>FXY(1USk4 zaklD4n_Ph;dp)50LAxWRDX2{WBj7F_?D52&K&|zDi7^-%_T#UunkMK@8bKfW(x)F*Q*;hGDK+g|X41KO<(o7P-%G^gIh%*Xln9+cFpmzGTX3ee2?fCLL10`A| zH@Lcf3HqU@jgjQfi)er#Vc+e?%``I_wMjFqu0Wq(Tyzm2wmN?$*w#u^*)T)86Hum9 zu&QVFQhG$fR&Sa!)fns}jF!H-jL$0AAW8qZ)OTq7fR(bDp?>SOyBSuv&_Ko_!n>{l_ zw+Zp-(i_xYdDncDDQ&Nzq)?ZsSH=;z68epe@H3+&v*I{ z<)!carQ^ZLgnY~^ze0V%{^~J&O~`5Ttt6-!0-xY4;$mtlYZp=saR-FkG;9>@0&O7n zfiTMqn=OU@Zp>4NJo8m#$+-|XD=4tMdYYz^O=FV!>XJA!(8FEbj-vu`Hokgta8)wV zn5Dk*f^^_ItR-)R0D9m-d;3i=5EuB)IVGwKk0FWgyqi2b=lK?*HXT3QMSLJ$b7tE-!moa{3TE?u~#w|555pT>%1 zPk`HKJru}L)lVh}30(G6hx4GekD3>R(+j6teNUdx9_!SMbHMQ@Cnvd#n_mcK%&P_7 z1lbqNLsdP|eb!fqu%n@&p{uJ4fHoQ!w+C*`bKo8088L3;WMtxlBW~0D{T{caC_+n0 z2KxI2D5F^;9w7Y6Se2^u7ArrswOyPZ?Ia+GQA9xATM-p&{#!Tv=NdPrrly96HCl`> zCNQmqYsXVlhQ!xE4PHHi`-f z9c0X&GW&6=T!FNheY5qXqNJqDj7Q_=nU`;|9wYti!NLB%-6S(TilVK0)V8Zzrt`+_ z=jCMnV|Iet+Izk^VZ8TR{)nE`wRo(j`@Y%&fV5c5&SFo(VFUMT$OqEr0!#F_w3O{x zq-=GhroLBQ#)fa+=R_rHiKER+kU(KBMXV4Hmwi`JVq3E+Ht=DsA^!7gn(3_xg1lGDpUAv65Qi2!3*)$>nO;IZ4T7Vd;DD%%IgtFr%vUs71S;MZ0lMd2#QV12}HbF|XtybrFNP3dYU(K$k3~uJp#q$w@>Bf8f?Hp&73eG#BD*R}dL= zJ;}Sx6I1q28LEoOQ8FKxWemLAvzTu;C>6B3FZu2N8(%`Fgn$3O^ta+=i(Pu5ezDe5 z@mq3`S7l>DJ4@!wPEKZUEtVujFe)Zn_Eh&hj4vqx4rD`O>nUklU@mL+G7pNd;|G6q zSgy|6u=IEd+miD=DPbseQ78@x!5#e$k1X`n<0A|Tv(V=pObKtH>?6&2g(F40MWg;O z$)G%$1Fa4pEv+rIKWGvsxqe$}tTaaK)|Icj&Q!~vyyE&T44V|y69<=Xb$nlPk%3=( z$aM2~LH%G`uKPgE@Y(`Y=q2`A@V2gnKAJrcr^gQrnDvUIiPS}^2xcr* zmaTb}>!2-ou_fWeHFDihAZ5E7YRa5oBS@JRB_$KZqdjtH7HZnJxG1FnxGlfj{;`_< z`u$<~q+IGP=PtptFDy<2=<-G{D?JJ{&v;d97OLOAc||23&!`yhs;UW>)lQoCDdtBS z=;;x2g$zjp-C;}Wu>k8Vv5_ieQgnvP`bZLxjMrriE+>%Za8V~DS&HLy^Y?djq?g!+ zR;;BDa9@~Afe03-QD{RM`oXqjnKzd;qVuD9w~)!532$l0)>^ly$)A_BWzSKe+bvOwReR7Xg7rV)42oeXvuVz%Nrk7WTU&EQ*Dllqh z6GX&W>Z`-)osEPVdg<{3r9TiOK>Gp9ua0rc=(qBT2s?b_A5JeP^Ca~HzAvb-9+$|P z%D`)VkZkak!@BK~60*oHVam0=#zGrx^lPWPEd!3 zKmn(74ES=25V$%;D8M1Q38lu^|8ZR9HgD&)t2HpCkTZn`n|^rq#d%4|M`?tAaEAk( zJCabz;Kx&W^mk8GO$Xx!!W2X}3Xw9QK$)&T8+l&Mh$|I`UZRW+J(T8tc)1fZZuevX zz}n8;_n7gRwYA5{~t&HlSwi6jjat$#d!MM_oKG5Bnf&So;@ax zAp!Y{#zVX-lXaH*uc=uGpHz<0=Yn{OuFho@eS~X2`BZY(7eV+$62mNV!I#j(SV4~( zYi|#<)R&i+cPT6`u_gX|nmiarO-+3lKLj*#7WcTJtXY9=aHpO28C*7=i7heky`}!I z-@oZd8Y*5+*11H5-6kSB`g_P4F>krubJer~7tuw5wyJ5YP>%m;C%C-4?9z}~Xw$kp z8MAOwPK%enAR@rcd1;7VEi>D`+S-8qx9l0SaK4?=Mi@AEc?HZ$3dJ6O}~Q_6NSq8Ql-ub4XFMnQmq z#zVjk#G^2e;5aEyw$yLktT!lUjX2B^AC6B;A#AYFsC$b%8E1{~nDfjQLULl(+_bd= zbs~BGe4wX-GWNdrh8y8yKMRd3Q_z;$+BFTCv$jd(WOeSZJ&=|Zq3h$QAa4EJsP&$p zeU>}#7!}1WR}c5q(hHZIJqLG0us6A-FLxXCMZKi7wFOj5F>6sgvg*048rs^d*AlR( zg=cup!ng_RfIVb0(c6pP_ITv8kEA~!tA>Px7;?J5diANLg&Kh?2W2E|UG9DjK1bVt zA-b`#0Z5`A99X&*;jwu{@!Hfq93u20PFpvrxt*V1tJcF1(~oOj%n84RDr37zOm~Tc zed8E!Ug+akU7r=;$Gg&M`66#$U!6XM{R+dW+0V) z_i6XR<%)~Rte;oQCq^c4v_3K2w%Zo=OO1N+I{?X`sKI;0uc|pZ;NBAO4zgmet^@UW z#-5$##jI2hQ*tJq-knd2(Wilj^NSsexLB#?<7^(XP`WnKkX_qh)on`?Ldb_1%Q2HW z68Ex$&d{pxsD}gAJlg_G8UnW?M}9>zI`F8Da7THyJjh96Wb;Hmp^yL~&DVmx~*pO4c`@?i=! z1CVtEWZzMD|EXd`I}=|cARy?W&6jEJdgSzue8F1G7tv1c9?FoOt)gr(6&em11=($Rasd%W}y=&xjEZfkKBoD`z@Az3$Z}k zXI0a0wX-vbh|Oiq8AMl?y#(5!#GJqe#|PBJ@RVQ@Dd75xcL29m42?z$InI;`*_&xl zgw`6^`$N|(H{pyXCFvb?57w|9UU4}Ybu*=sinVf`NG@YnBi-bsygV;k# zvd!GdSt7F3cM#0TUdELoZZqkxJ|7vpNSFQQ-nx^uSng|T0Tl1+056Qs#KB<$B%5C6 zry?Z2XXWl5v{>4ynVEBxQ0s3`;{uKFT-E^*$nWkF_W`H)X>y-OL zMGbhXQZEwt6qbrmbx2_v|7~obx}_+vNpD;;je)-#w-Y@7HOX@-`iDxb_OSX;U}1rq z@F;O;Q8fiF`x~QKe_NM=6YzQ;d+|hQ?YT*q=<5LUSYw@+YF(kAj@@^vL@rSv?jvas zOsxStWY=)~&kot5!kq{hE>ooVyJ=OVG(lh@(A-Y8MCoO!Me(fWA6{AYIH+CJpSgp` z>dt0V9Jfopz?&KHHe^Dvk|c9`tx)0yxSA*aOV)yS)!C2m9~>OyE$sP=??-gelR(9d zI3n!M^aoQ4rFgjRn|>=kyg;v(jT1kLP;oZCG?nHeLK3W7a0LBZ)8P15QI|R|#UA!_ zCd$v<{h(iH_NSY#jSX80df8c@etyzIEQXZk2lfnC%$dsC$?09pOQ~+BT-N!J_nRk9 zZLBmLKlzPx7C()yalQ=2vRB>Yybj#T>Zt9JOn!-NW{CL5!|6pMjB25g4p#Y!)YS@{A%TUJW7UaEGY)NL0}s+l5kY4PB)`i6$$d|O^c zYoa-{z1V=C{L!pt>N2Sud^(UDtW=S+j{j+UlwOGCK}fD^swY~kCU>65dc%=@6bn)- zjT>tt-A6^=4_P0uG@pR(O&8#KeXItC_NqBQD=fXA8ElxVQSAMM-x%FA3S?hBcfo=| z+>T*N7$fsN(T#qwr-v3d0(N_ss@wmDutoq1Ky|!%lqK&yzgG@Ubg71_gDfMgC5^7D z8o4BLP;$1Kgr?lJE!(KoDeWQ}n+x3_>>$3*5FW-g{o|Ie3F7(y1ZJ#ut;k%*mTAD5 z2HJ5%^$G4!SIW3jJ%90*1i%H4y|BeRiLB>`1|G8N!MxbO!!U+C-2|23(wsv-4Nr_i>cWihV&###T z5#sd1p6bkgE0+c;8evzX6h~3~1qim>^}^1}0`HRh(U6migM&Y#pPR5|+%A`X(QWc8 ze&^AVr+)I$l$NZz$8mO@70D%)d-d)$oXXR7?!}OFMn#|QTGPH1y8XY`gUfZ4lP&3o z`5#5(E~d-=$2;l*n@BM8AV#gL!>KpJ$f(Iff=Fc5L9B3RGSt(Xi&mjrj12%sI8Fkx zEMZoJTsQJqx5XVn0Ut|Zn=XhzsL9ls^O=P@?A{4kCp8w`klNW^LW8nJ@{UdVpHdPf z*}IiNBmkIr>;<@79o%-44*Zm-8v#)~=;}Yq%R#gzouZ-o@)E}PWJzcOqTbSJYj;un z_J9(Cb)d+kdh=nuW3TH*|ME-@&^?u+pGSx~-I2TH3o>V8Q;G;x85xT)!O0o2=VCn* zEgJ+iho=pulUO|EYEJR>N}N21Y%r{xO4<8%qn+C@C4UdB_11$+S&vm(!|%+wcoEbw!jm) z#Q~j_wgm{ZzV9w`n6clvcp_sLcjM+2FIU%mYAP+S$)wMVC#Ia3>?R5ZDlx6#vv28M z0s41B>1Jo5QbL<~B68P9wf!A7Itz+E_665h!Wf%hZAbkU%C0^|1QcRW9MvVh7PG*O@*rkzVr_Dd$i5oevhYYH{LVpH zw?!KEQo>yfuY1&EStD{;<1IZuJ)t|Dc`J_eoZnv1DGiYv#!@aK_yBAfuKZOv!UKPM z!NPwPceQcAdWkinokDpaTCsv7xC(&yk4uhP2Pqb^K-R7ppNL*+oDIV$f~|~EQ;E*~ z_>4O*Vfqs|L04HB^sZz3CNQ?ygB*mvBm#j1?C@Fx;%hrQJ55bZ_ZSlTEfd^XRYbe3 zrm~kCDozfu3qU@uuAi}|B9Qi$RDOKH!`_JGrq{_x($;KQ@joSdAD3_jHn{(dN$PSUi0c@LuL?l6D39tSTfkd`$C z9aRNvbM~Mnt?x3Ua7+wc zjU10!-|=Xvf}k+2!c5qqQQWy!x0v2WFyB&EQu1fVQ+0$woik%ZAUX_G#&2k;O5dFE zU4^MotBwS|UuwuMVTH?vn!aefjf^udZ(9*hQyUqLd_z03Vx zP5=DYJJBzadcrp#x< zoGAyrr}wNq4xo>;Ev`n^QL7Vcy_MYxk;Nsw2T#{X{LkzJ(G&@(`NiqEtkuJwOOjF7 zFGUc~D60+XBL3)Z5SycT!mDQf^v10@;q!T<^F{5Vpm2 z&H4^}^^=VTBV#th7LS06wM?b{I5=aCFg%FL__@>4yx=^tCvJqf$CDdIbU*tGGRoH)3eW3uj%qQ*#_wyf{bwk2oEg6?%GS~PELkA*!;m~|X; z3f;D9BXxIp4nQDFN~DPS*9$j~t{OHFEeE*MK!RwNmK>zuDf}352TO_l!U?j__Go60 zYuU+xiC=hRyY~*&;g&VywzYq~ANUt@;{|eqK-dhIgCM_EU>k7R?B$fhUz6oz2}?eb zvpgtazY=A#VK%n!4Hcy}6IfjJk-84r-4=(+Yc^O#I)qT?6s_b<* z>H=va=*>6?JEo_nb?ZxLh%s)yv+h*#+M`w@w*$W(_$VfaDVPz8p`*xWe1U_8(yZ3? zMsSjeM6o)t{|_*QLK}cUN)ZEWOmze99|RRCK=BUFRG2%iRzG7Uu1enY&PG{ zt_t5)6EL;1F%*D2wF76$OTi2&;K_B2#fooc$;NwdvttsEQV(boN)x+Nt_ zGNu$(HTv`qt!nO1P7dZEVv)B{(v4nwf1&eDbz<*=eR7d7ttBxF^w-Z5Z0g&NK+?_777UOQwcC1F&3Me?i zH4o0dzyVgQ^-xjWg7RML`I`gJZ5Tion+FnN91GApQFjDr%%j3Q=)Q|Z$74GAq>l_U z0Jve>u2ZUAd;3i1rGo>fzFGT_`AToMNLrEjpo@)H(> z0l8TCBX!^4-TTSfl^E3#Lj!}}e5rz4s3Q@=%rJ7RA z9-@Q}ojbnP>FOlcL#K$vAGTL%hJm)GWEvFccfcVC;3`suufl9xTwQ;@;3wGfH-Yccckbv%moQT zZEY=U1UNx^KNI&23;*KB-DLS>_qj}@uQqfwA_;seo3m2 z@)knrpszAwRntRp0vu6io6>P=A>fbvDqZ@$Lc2~)!vY$P8kLw1?q=VU{fh&P|DMHW z!LnzC+vnyli^8`ncfgUVpSHEY3Xw0qeERfNOdJS}jGX)kwB9Zm*}%*Jx-vb%n-*14 z67`}=YdB26;Jnq16}=7IIzSh@+HOR3td&qv=fmQx0Gt8jaXvkJHmwF0w))FTGDnED z_k}?NcfI52p?Hd&;PXn%kXOlZ0JOKA&ENZ7)(zuxz8sB{)lb2Ew8JuCl3WD z06}LE94=wY`AlT)p6g9{IlljnkpB+oYeL<@h+NPe1e9)#Vl4@A5Zo%(T0OARBcG)| z%z4gCO=WVRduV_11IPjUATKEBs-%;gwl@8QPs0;{)$gOC?}H>W$9I3c#(Hp`rF|?5 zuN^d908R(ax@_Op_$LK8-Yy;0`Lk9fCj2`$p`TN^4rqS3N8C{_&`^gn;RSNF9tO7IsqpgP zu?xR;Os5zk zZYkivOzPZjOVhVsAqOqyAkCmSM^EnkEJtSI*U31rRHh=o!tRdXR4#Bbp6}mJ+NifT zfgU^sJ|@-m5?h#q7hR{LCL-Oox4?nHtTgIr7r9mNz?aLO6N6(liJ-|(4F0ZN4^{?C z^_&iT1x%NS)J$Ijrh*GGFRutI|JOb1YjiZsfKkK<7=B>p7QaICRzg6Y$-kgcjS-E> T)*PUKflE>LsZ52mN#OqghXEdX diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png new file mode 100644 index 0000000000000000000000000000000000000000..a56d6427f59e33723878bfb144665e9b57f84abe GIT binary patch literal 21164 zcmcG$1yEJd*9UqPP(&Ie1Ox#EX-VnsPU(=6R#NgJ-QB4mAl)D!UBV?qq)Vi`8!m4j z|KIm!zIor9d2imlJHrs?oW1v2d+p!)tvH0LC`n^sJjQ^*U|6y;5^6Bm-C*!ViGCOS zB~pAb9ejK6PDaNG2781Lecd?^`iy-FI{T~tEZAhf49jT)bbij2MrPpF9yfPn!gn{HooSm*DUyL zR}?pzU)A#3J-&FbYiXoM&Vo+eG`sh7HX@HSmB=m)zxzJA837jOz+kj!YNTMjFJk(^-(){( z+yggYd|`*dMAWDf@4#SjcMCCLuoyxabMVL)k^hg6n6-c*jZVOz(T#oPGjgKXfuF3}jmgFz| z&_DWEb3XP`AKpCSTlmc}Q+JV{C3%u@PMj|B{`-kb z9oPKKMW4Sz18=gVhUpjQ-%JP4fEcdJh3;cBN4!=Dxi2ol|$-V%lVQaL# z;#(k$GB+^0&)V|g_ITqACp!WY+P@FY(yu?{H~ioURA(4GQuIF9u4avhRE&(ijT6R0 z1KTOW*+Dn;GABGL-OxX*49|6K=ta)$U;YJP18om*g=V}K#mi=l#lJk89V;gDQg5Tn z{(YX0lFU+yH`44$1kVxFyh#ZO>*kTmyHFM?G_R&Sd@XUO5R)yfagbYhvT?AKEyfPz zHGRpU;?7MxJkH|2yL^47DMlqaU4qOaP~w~Fj-&Ky3ABV8Uj=+|91(9G(6yxnV@8}6*A z4(wpKa@RO9HM3kMA|gt&d2>6->c@YdtxJyT@SK}O*Pz*)XRC7qBdB_jxe2>&~cy37|r`7V56hgBqibGv^Ub;`MEmD zymFW=;_7UV0ORj+EdQ}Ng>rOi7gZ`u30tzEd-YCb#xCwJru0xDbFSlX)_Eg?X43C4 zZIXKVH`I;rTcNmDeam>e&rp0#x&k>V2uIsvEVz!Rr@3;3eNM)*XPYcD_gQv_Od_g2 zghbflaKj=nlSY}oOr;!)jwnbtU!6W0cRbnbvCQ`N_=U0V>ORmxA}inwAcsw1^3&lj zweY)_Lz(Il1=@E6dCundu7}{~fBbQ<1wRDTV48NaO=4OH9PEn$Q&rj27b!0_xC(Fm zcDkA_e09~jKv6kmvU5n1c<>g0Q`=9Q*CMSqrw9p&;|=tLp@$D2j*gB7iakx~MPC~6 z*SP09(*EeD&j~qGX0W{>N!Str8i2Q4*$D?GW{S;Oi@c-h>Dt#uH=(pQ@V;!q(`lRX zkwrA{?8niX6&knJ4G1lzQWqL1m@*v}TEv{6^~HCv7`1rq&Ndi7JUkD8))4wrpRXEa zyt=vh=Jxp)XZxWg*4AtGkJ)5Y&jE_Q64gx_a%jC_A3->@Tqv7gDBnwg4=(k7jgB9v zEAOqudIqLY%31w&u~TpK{PGOkRE@OcI3)3c9&SjJUlW3Udj)>;Etg+wF$cfs0^Pv2 zM^DI{oGQ<(Xre!(fFs{Fp`GpOH8r4EDfI;9J`l6bCV8;A#9>v=Z24tP06_Y;rM&4= zm#cd|XUfXiKCQ*JK0CD&F}=9=zzROaui7HpagZrH4~bUoHGm?MSqu(ayOOAWl7h>B z-VRQEuXEiQ=>uudK^J zYHnuZIeT*DXty(6sJ>Te*Aep8#ALB{!tjV@iY)?1_!>7B^XX@;d$3p72CbFfJ~V54 zg(eGHMy!^isThAsru9D$z1 z7%0|WRe~gh&rZ|nAmj7$aIW?AOG*vW)0=n++qbuF_i({>`O@m@D=;(PaCA=U85woY zys5ubzebx7LYttj*3&<47m$#2Qi1yfiw{<@Fkmc9@_%6$5i_s8u;qqd9;eEgJJ{_! z?3npm*`;y)KK6RIY46_fX7Jua-U#rRLaX@O2Zfk1&QrF?j=*Gw#O8*Eg79vv@61n> z1Gh=SR z7Jj}Moay${%qA_{V{?$-O({|CDDRibsYQLjT|QvOzE3+n>KGFR70EaT4Q}mTde!)r z7r(XQiJ48!nVp{e_J}DhxRIwxr^44zPj74gVE`L25Ahe;d<1xxk35~t7Hro(F4Mek zp7O`AFv#YNpkA2fKvdP$*N39V&H~BX-W|!Wc{E$hj8|S+v?uqOY4@<{m%9XiTpmhW zvtVWMQC77-nCO-rJ#J`_L=XG^T_G#iBDN*ipy6N)tsjnQK z#pW~InK8C?O!AtVEnMV?s*ta|rr(-qD0yRMNoUYX)Gk_gXL%(h5D16CL4$4hNJ){7 z<{-Z6XSQ5=NAC2Z2^;7AeqQ{JS6V>!kwrI~z1^fI0_R+xr4Q&cn+$al2ckN0Xx-|n zs>Gb_RGO#qtq3tV^i3J^zNlePeD~ROY2y7d<896Rk#L1*M9<3x0>9i`LpO#;;UTtq z8R?!a8q>dP`c6&^{mqV@u}z=9it*Yu+;WZk{+5B{*GZ##`9xSZ-B?jDu^JrkvY;mq zO=s-i?W8J1y-EMW5z*n+-1E3EqHN)O*UjO#b?X{y+U6~r6aU9(H!NzkvF}BzIz1kmqjI;=~S=l+EjFpQrOITxU-SGGDR{*pf)* zNVc4aG)23Kw!gGO6bXFTe~83b$5JCTPC*SPK@{QQz4?~M=KY)bggGuX5&8&||7PPL zcpkxnr9ZKREo!{9>mRiVkOfS0VWwwAe_-9@{<5mnq3 zCgdoB!p$tKH!&wVXc^*C$Hvm-(oFVu?(<{?LjPr((T5EM)2Oca>^E3Ir=C9h+*gE8qjj+Hw;KR?loCSDFL8l z@!8uP!=h$LTun}1*ld1lv9<0+9~FNO+)R_>L_50ZcyYLbKsLw4k@iLWJ99)#X}Luy zs^oMKrIu=sjUA|POx5h$UX75AGBc{+Y0~yG^}fmMTx;_G`>GGFaQ=mfp+@CDOe^wn74X?z#!T3Q^>cCYe&J%{J? zT^tomwqIVZvPrS$(%1h@xkh^c0uNu+cV^t#LV{U+UqZj?mI}JR;VL(HLfzhH1P*$rxbc1{%?aUyuI@@rjM(6N&1S5uOf@m z%z50&4Sm>><&Ff)68)AT|BS9{WMndxFGVFiH%>+wCG!}cAKs@@@1zxTjatP$K{@;1QS9^P~2Z72p;wt%>JU7tHv@a z%2v&9{sSaV-(BQ!fZr-;(;Vvq40XegW4zLKH?a$E0;~fApR0&D!Xl}l; z-xqHVJc+CCe_d5=D^*%m&K78YG!Q&2lGEpf0&A-{2#yDEK0x1JDz=DKcl-PDk{?j7 zQkkAo<}K=l-&mvi%ju17-y<96KhRv>xeJtr*~}*YIVUcFJclLst411~_4~cBB}u$4 zXz|iAKG-+w(pIkfeRQ>*mLKT2gO#@7A2hBXI1`?mKMq>tc~YpsaG+RmM zoI`^5J5WRIjEoF@!YP1vVjU1~>_AplKhs_`szEN?y;8hvjaRcUmu>HBsgg4R| zEFp1;6s(e_RO9tPdGO`aQXm1{bt?-ADaBugxC!yk6`Kz4-rKgT9MHL&%)8tVbF{JF zr7qq9F_?uVjg0wB%ai9pdQuK+(P(s@l4PoTg|nV_Uka|`639LtQ7z|q^?+|#U4D_V zz>wifw`8OPwXPxeMLsTHD+jl!=EJ4iVh5z9O9NqG)7YQxN)2y-1#}sDhE?6>m5pvQ4Gu4yuduQTTx~ye+s$~hvmMz2Y~nJZ zxivUq?GH0OZl4M2RVCKax}!VbkQ1#f)9ev9ek6Nr?_)fpDf4@Ab3zDY zEF&0<&-lU3{-Tc<|BwS`F9(^Aebk!qXRmWiaudhH^2wh*M>XYvfoQ!Ke{py^!%EP6 zy;&^W;65b+|R|{w%3&l zpZrb-_=-_%t7BWp*;-NYZe5Lp9y;Q8;9Lnzx1nVV+WOaqZ0W5hrL7X&ZWS%ap~FKx zLJF0BU2LK>)bzJHrhE0uX$(xx5`7HvN(s@}sW2yu5X3 zIF6A}iA-x|JOCmB)0 z&zj(T{8t`FOk2hc5y~9@oR6|@q(sbPfss+xaLs?n(NhRBCs9@C7F={kJ71^dV#aE=5ukv zlV0Eg!TeD+Du@zgSh#a$cN7!SEk8zjjgW8GT0G3g&KBb%~k&&R6k3_{sGx(d=M-p3X6>Sf|!iUo-;>q`~QTmua>novyy{A>(3v^Ve;NhP( zXtS`q&!KNe!UNy;7q&=$`^1#0;aLJg))(w`S7?RGspOLZ!1mtrHUy7o!xwY?O|b3OS7! z1__#%%2|J%^ffeecTPO{H4N5gkNk5#Ruc@btf*L6)Sr#6lAs4*Xcz>uY7B!m(}Q1E zKj2W@y5&Vinjg6%oduvxCNy9Ud9#-yv3y^lR#(tj@>RR>cul&K)9vW*-=vy@hZAVW z)hbb6meMpkrGwtb4y>hC}UX8ZP|h zKQJ;fg7wA&sK$*oiSTKdy>8Dle{%Nkcz&E4m7O?A3Unkl*-lTpgb!s6-gF9IlTlwD zM3;X*#K;ty<4(D1c--I0u{1mDx#(1x%!Yl4Fdx;oeeCfBZ_q@+A7KhH>Cit5a#ItB z6dbyZLhe6}Zjf8$1@rD}e0Yz-mDi5S-%dwg6kjzx?$7omVvnP_{Zmy{rAA*;)pFC< zw>77=DLcOFk1#$;0fDp#H6aF?>wdA(4a1?Y)yDNjvfol~+Ue43!;7A1xYx>jGsoGk z0sO|r8hf=xvdU!E%*}6;yzf2y-NDsqIONoWg%bI{m&L{ODnyJn-GOzO-ce^FT3dxa zx-BkJ#SN^N#~Z@%mBj3dCDxYn-NpEu^yQV|eFj1;J*g_6imxwr7sl304a&5Nlg!cd z2sJ6PQ%Td)E{IF)uyINe6otH8P`uTTc|zB}Ct zw-@yKcCv3WSQSC1oFUc8u88176Mg%8%hCE4?aw#+=8+29Cbi-RekyJmzZZ!YgN1Kc zKrkT3H}~fLiD7_8DGIDBc(BEOUZ9H&6XaMyNX~?&rQB`ed4*ShcCs-FQ0Yi{p~)>D zXGZk?RT+&w>A*d-@rj8O|GAEII$N8x)!VEY)v@T%vXQS$vDDPL+Q+xH`N~RFD2fw0 z+2YTW<(Eh$yo=fQbA|H*J`>jq&pKW;Hy4Ujzi3L#z zJ^ct9y2vM2Wu5B>G*LOT421I}eH4{8Tv~b z8p4@`t_Wlrg$k|s!VURk7F-oLs+4In(WH|tEibBZ9~>KlgLM4Mn}UZWWof;4SA~9f zkeLXB0|c+P53{U$ZoXFjs=3+qc7D@~3NXtbPxH)W;e0d`z^7!KEHa;b(0gKVCh{5T zm1?=+97&{i3qCry4<)H)cgvM3N?np4ySyYZgMSy;)wayH{S% z{((bo@k!MKyzSoIN_{XrL97X(c#qxD4ZX=%fZxV!aROgwMQIVP{KWkV=Z+1-Y&GPJ zkWSk`6=s2xmRhH7ra{F*ED7PA?z^Kt_H_O}Kf^W`?tSPUU${|5X=!PvWspd;8#ksp zJ%>sEim7s&;pJ()B{7i&G10YPc9YBZTd#G@Eif7ET685b(Z?2!#|)p^1?l*_XUOf? z4#NHj6T(owohOg-Q+9plOuw;lh=+$uXiU%2QjfCnIUq~u>qF+LsYmrAyompm0$>%Q zE9gASn;h_D#?JL!L$$?+=J^`aOoSHa<0vCaCm2CBnuQt&a}2y+xXfS=L|QNW`&I1> z=itb7$NGl>9Y9#G&K|8i^~_R>>cG_0j)8&Mzkh$#w+@~-UeRo|ONBtR=W>uzXx_S#DH57%N1 zYl@g&iy#uwbrt&F`i8LwP_De?@g}(ILeunzxtPJ=5r(eO8rz=;9iXKYltxBULStN7 zesy>>c84(;6w;FB8e*M|ID8t&xBlHo#S|4r1kLl>cIb5>=fZ3#g(etiCyJW6zQex3 zPKE>A0Hv1h8FM~2zE3d~I%j%T8=Z&%(qPlXZBjyomUT+F&hPb$)B4*aig%flwtHLX zC=rUyQC4?de?YR_$9r`T*!uBs#e!CY|B87$1rv+4PwrNShNY( zd8VtajSfGvnaI>Jd`_*a|63^=$(PYVr*gkU&)jlNvNcb8_O{&f{9ROtHeFX+CD|uQ z2t2%h*;$akh@-=#6x%s}s=AgBd_}UqC?jgkTW-GhoEl5|Lr>$k(fW1 zFh?hkOHFmW$$fS8+vC}W-eBdRl{NdyZ*6~*@Z-N_8Z^q5O&MuW~;9DB(PJEhStFs<8fSCYNfsOZsE$HyjgF}GtJur z1!`n&+S*Tp#+p^i;i28zN4EC6I2V-Z!d6u4#pUarIKs<%(T>ZTpa@1g7u)x;!vm+= zEJ&0cUL=Fcv2-+i+r1bs9b%2I{~86g0Tgp=5mm*St(7x^hAp=@8;q~dy{w6aEr*d) zJ2S5VRJ<;?#~lIna25ky)5?DK6Hs=)$=-B>*YCs&dre+`lMd^sXS4GO%c4pocv|#l zQXZ=cP zc@FUOklmJBAvkr0m`UQGr!{eW#>t7>?tEr}Kl|&^BqOf_p<;<_&)uQ^{{EhOcq7|P zf?gl!wO(HoeK0%WN{khR(L~JM+w{i~`lC$#0hIA3Ta#Naivy%I_BibtG}2fTSLx_l z=a2gr?lGn|W_oxF)Omx_n!n_S&lMn+<+Cv|m@Aj)?7m!&kIwLKqw@U3sG2C+5_6(L zE2$hOc^Jzn28vv8r+vEIx`(THh9%oa#1D1GC51nphsQ0G%mi`T@a_nJpqiDnwL8i` z7IV<7%Z@*`)h%EkF@`!iJBC7qO}p>f<_*G1n<1OutxSdfJCxm&q>C`3mu`Z~EDwBL zSWiQvakt-XMl#`(89MGw5y(zTB);tP8oo?ZTUdSJVx9>_WfpifHUEw8B9q)nI~E|65zLv@}<#*?PpOcgW3? zE2G=XqMRBT^V{dTAIH-EZK9Fw5; zP9RZ%_XwjHhZ4E<=fygdr^Y)%tR!uH%SoH&CB$!GDsp2^elv6GLewY0$;(@1GYXYy zd_yk0>tYv_Ea_1T+|cUHg?RlqzU%gIJ3}zNUg_Pkj;h>ju{&3Np0B!wLuVQ^aR4xr z%EO1p1)*e1V&b#FMSbxB05Ps0d}yL?yGy7N_!IsV;9d4f(I0|URPL{NW!5Nd|B=~z z>+tj!W}qXKF~NNZA0j4V$;PEmpc$Fq=e^q&`i4ZRP(zIFRMt2x967!~pWyjXmlXE* zuk@kpR+rGnYZ(M@C+v9BOzB-XV;wf(1U%6HP_F}H{eSeNfN?QzK#t7YTc z7^n*QUoBMLE;;r!m_M1JX?DLb>+}d>El&QQPydgmt51{_8_z{Zz)-J5oxS56?#keU zr!ojZI>d`W0@RI;6n_f-`%7TQ00_)mxRjvQpU4b?4>SGRV9HOF_k3pP3H#Xy!36L8 zzFH^!_ZaAb9hm&twj_HQ^LbQO~_|m5WpzFnvFSY85155v{1D}!{D2A z5dRBXrDDpIu`8}F&8kYmy$GD4L+Jf$IGy)NL+NTPF$@!8U{XJ01KCgK0?eTL$JY(E zVMOo)qnLk8oJ$%kHZ%|tQ&0OES(o?^=b ze8`k=X+SoABFr$crt)P=WugY(PG0lL$s;5xEe;Ux(#*zYYqr73i;+BCV#+Ja!PeB$ z60BLG!L#B8g;mgDbSrwR!zX?S8T&#$=-bPW%`KulQhd3@SGg7W zcw7Ptf}=M2KYW#l9;JPG5+Z0+JiP6$`Kc+4jF1137OkCM6VpslqmXm>zjQ`J{hO6_S z&eiVn&Ix)9J@Dnzl?N0;{V#o#BH1Oz-<>3^0J@;|0t@{g5b(d;4_f%3_CHhuVC66g zsQp0^_7s@y|8lEH>mv-b7Axn}Rq6%QpHIL-{$DkCiYS8)ww4D`kc;@g@BGBYLNo$X zONLDC|JpeE369zMCtd0_wDEVyp(d;UWh5#os9O5}Y=t-ZTR`|u1xW69`d|b8XE$n& zNg9L?&}sT#Ci-8*&p>Y!>f`C&{*}ko^573W7?8vp0Ia&Y<++ycG6Zy?pNe!^9#FXh z0og7w0!bfB&~b~@L+GS-+(2+OFBtA{_Z5`TkeUVE4E!Q=+_aR?Z~D*UkD^CHvHoEs z9x+qw)odu%Kj4pGZ@u#Hg?5qC5hG9&s2EbrjXFc(c|KBYjES%E>A$jk|76-%D(5%i zaS$$=eP#;!SAdPpv*KF_P}+V4T5FH#@t$*-3f)>;4|CUe%!KEs}$>G#5yc;tM^M}|89-)OIAz1Opf6rb16vg@%??d32SG;+?y!RHu;wcdy(`1M{p|saCa+k@YCM$UY&Sj0Wu(gfurwF#EHC7r zDisTB`gkZ(Qho|3;~3()(GrY+F2j}wpRgcIChVklMW43Ja_>K~-r8ck4kck?P4^;b zQw0;i4${&+l{JM|^`zx1&+qY7(m;R~l3e5i5(DSsBRN*YvHh`a8>F@c*OZ`KG$Iwwvc<;`;i!mooa?`7MdH(QrFaw+}7n6-%U)zc4s=@Ba1` z6&>2WbTUYG0PVlNACHaC3h3{{(A6|utgXWwB1fHu=Rk{il%IhF%-A_1IvP=RUFzU% z^XAPPE2|gT!vv~)nRRyjiE<5PS0Hh`skd-BzJf@$n*nJ=>ibDea`Su%0QQSpeLE9=3D6`Z8cUg340vN5K8vGXHOm27%ec@88TxpDneeb3G-n3-$hyal92Ux8{b6?)kgL)hO&7JhEUBjEkOBvZC^YK1_HWI-cu zIUO4^>}IBHwQxaAhzY%8bF+vEF)Vz0O>E>?kxD6&ACzZ@}3XD4jwBogy)2usKVX} z1d1&N5*xlVa9cgL;>nz!pGogu!Dt-vNWZ}(Lj8ENQGWG~T$)gFyu(#-|4eCLAX@O9 zhlhuq=_nDJ0b)gBge_v>vUK6Hsr8EAl~iQ*UOX`HUJha5+xzhS`_Vl00?!QUth6LdNjg?Pd8b-v>bv z1+iwYJFvJvWDDm#ls6-ki8~&Gxpq6-MD_KfMSNvzlB#AV+m_5WR|6xaanrnTdt=aR zFg5FQ{C2KtyD+rxAfl)rUw)M8G{!jGZl=yIJV(2H;zsKBG4-1V%wcaB5{r`0ES?vb ztGZG+T*)3JXTyDvfyD4zBc#0IsGl&t$TF0Z?ObeFW8tKIU#!i^_((SCk?KI<23>Uh z5eEm8mfZ{ys)>U`w84VoZVj#MnDxnGXK;IuGhe1sf#5I=wWKPlCJq z6Fi!cW3P?+9K+o>lE#_r{h7Cwdyc;Ami9<~;L8Nu0x$Ejrp$pXh)H%UnsO$8$`0}w z(9*?4G{@Ae&K6%smxSfX6siaM>AiSeq{&=>hZk-pY@<`)e3V?$?Qk|+_uUp8gpti; z9odv8luicq=U-|p(^lkM-n^kc-)B>BigmdA)|9P~DJY%x0j$leT1PoStuTQ}Zq#&% z!M#a2tRdq$YDdDfG}FYdfmW;8Z|4 z$$X{`&6FpevN-S2u#3cSW@@U*!n1P_ABzrRz+?-GioSfsI>a#bkiuu^DWgJ<+Vbd_ z$g6_CU;9pox_C4!yrzdE+>f*2B{aONsya85`4~aaQ1-{1J=K|S#|RS_3X*6u%eEdN z@CAR3Zfx$d;HBoL#+l=XpyuR@*sY~nW-X>q8xQBw%jJ%|Bkz5m5oVH+nrii`j0%z2 z`cQ-wQus}ERh1(M{##Dy0d_Qbdg=7v#ISx#{;V<{P<$D72!lDwFS9 z+Pp3vo0~HLIr90kiLFFANy2WB@U~kAW?NdY*6T)R?$btj;dmZfFkZXGXVxHQIq<$c7 z_l})Toc=_$-OPyqM*23)pz|Bp#xyE)KdLyY-`3nsXF23E+KwuI* z_Bu0gy?ohcgoTUu8x)}5PANzYubJyuTri<0^jX}<8t!;}nWV+!*defxH5+h@wFf>CB40V;8; z9VHy6*@}dct}$aZ9?ehD{g@DOv|!`mm?9FuNyfaryQ5ro7$Oj@KOtW*>hsQ#SUSp^ zm@Cs_K~^bG%`q%3e1qU0Pk+Y*R47yPy)2&UM_~djB5RH7u?wF)$)20In{vVzsoeIK zVk-0of6_m&Sh4}2H4zy<5kIPPGTX{AIdWG5x>LF>Gkea*zya#BFouaqj9J&}mR3En zQXEBvWUkd^&K|B#SFTudKU@yYw~AWgd>q*}8XqS&W~-;Ec_=V&>Zgsxlqjj%g?t6W z_50(V)m!!Rh`d*}V6<#&vr$Q+LEqlicA)il-3}PJd;)p_=3)nvF+p*ZmfAI3s*^1w zWaqC=@t9J!(2fyP*x$*?l%-c-+Dy5=Duph-QNiIOPDRsU6x`B}d0_b@B}p#i!QN5B z7S+HokbF_rwQ>@5c4R^Cx36A}S#zCE2IEjx1rVW%@_nMmXt2*yQ`M|hU1+VOnTd;s z=Wv6!jKG^gc%Pm@K=4>EjVUb0ZsKtijeMN&RDa!0WF(f>iBz}pdT%tP(YJT)3la*r zsKZB-!MBn3{%dkXkRMdnvmotnzD0+oy@!WaiBf0-TF~n_I7q4QZtxfOq7#LGS{iIYf>xZe2gWVbQYX?2gIC2 z;QUynjmbUB`|>O7ue)KMu6s=ja=9-a0;gfCB9qYt?TYtP( zqsxP(UeF|zL$9Qukdc;JIA)z_-Thv_*D54uxy$LkEY!hD3#11^9SKalr|<96yT=Oh+G(U|pI9ok2M2^#8;s5(hJ+}zjF zcO>2#H-#88+WN+aGD~3$yBJKoC!csmJyW+;*IeDSr95Eqh+}E(Wv3S1l_lxv(2~(o z{Z$_zDO-(MYrIjQt=*~@b4zB}lOaU`Hj_^j7O=gy!3CN7i+qy?+4V#?-qlA1Vi=JVeC zU8wLpzRxUeO%F{yHLnB?8ovLW=m%6YPMK!Ik?yWkvcJ@o1rvwY=;-KrdqKOIww~TY zr@a@xrcm2aj(b+JQUOP*0!wnWot|n!Utb@{gl@hr+r$zoG8h?H#8@%Ts74XRWlxP* zb5F~!H1AK#myHYfY#!3t)|2>h<{_-(ok6l@PuUjWs#3X&uaqhVPNJJ(xALx{e=b5Y zt=(tw$@1!$Y#bbhY4A}$Hz?c>!LWu+*W$+d`wJV45Ux+!Dz7tdtJvl{rH0wi#3@pQ_ruC4;Z@#d+hx?ZcK zS|!TL%J7>Am>`s&#RnuopT!M%?T#NHO_u&b(!yk;ad&7a%3R0CtV&oX>fXg-KWBkU zMK8;TN;ng`*z@>GyP2c!f4HCHXtn4Y!EJTM8=TgDMq`aY8A_QP2cM(#RE5(rg}eV2E4 zjJB(6u0NUIN48KS)zPv)@~^zYo)L|(d)o1MYCc=hg-gWj9&cHM)gBx51|r5ji)v)g z1d~3RC@xMst!x}EIH~Xde5Zesa9@ACN;Ot*up0ZF39@ED6ViD2zIj_a1%E z%%tF3U$MXe4jFIHfEUImlZR_l{ zaZxojZxDWuO3if&l(XyX((O_=>Oo5(UhJBCl6hf~b(%$~mT9}~OkKWJ2}*|G7&3Yh zuXgA3-a1rM$5%f0z$c;@m}gg1AFW9?^e8keuQpk2L(x^y96J(_RMj9`$E)=p#Xb5w_34T`D&3t@F6R%Wo!(vl*lo0}ACHgw#C)iIK zXiE&_@Iy|kM_3J~Xqi+AZ^QuG-fgqg0nXT#g@oVUJrchCy6?o}W{WXI9{{I>Xw0+k z?qW>)9d~^t2v9|-Sid>lX9TZ*&A#Ql7G%Bou*z36mGb_1d}>ApXtge%e3?!|)TMhZ zU1|08gaZb{mxBH;fafbmaKyI~>7HZg^NY|zT+(^Tln>|>Ew~x1Z_Pc2zG{9I`#LzF z{R#dyi6!M-?5tiIYUbpvQ!C}WP;KFpC&bDlK<9Z&VI-jOu74~)uem461$NUliAvTG zN-mxb&LU6yWNBi5fKKOaEeY#kb+KraX6{0MSXpYW5ew4 z--Mp`WaH9^U%FX9QK+vL$W)5_s_giEB%rA|47x=@ieSdn)HG|2g0R+S)|EJEM?0z8 z(Je>iYZh;(RI6rC)@YLjFh>HN35)%|VI5P#=%~Va z@dXYWpS%+4=;`SJnW#mG=fu%QMn@CG(xZdX?>?r|^;ZCSN4D#0CxZ!>IjLOLa*(2{ zWMa%z;(3gf)IIjNwYGL|k|*&{VE8@x@DX61*hJEmWTyMccO~xp)2r}I_)b+{l%%RN zxpb-4jn3=sq$p;z>{7;(rRm!^il#C$N4~TFq#pS|l1;?O&d%;YQTjkVW~{_V!Ga<2 z!01-@<)HX$X$iq^8)Ev=Qwu<1nkN+ zY8;p+Nbg!*yZM}eE3=n~-5s+M(!j6Ew}g@}U-cJ*;L&QB5aYrNxg|gIRjU{{ z!`*VYL7_&LfO|)_eVkIk6!~5fXs1&JkPS`^<{hH2+RmdKj(pyeKOZX^NNsH`9c;Fy zsx8Iu6;;>s6ws&TTcxe?VEBPJMt-ZUwgO5)|L1hd@CipWkcp+F4xC;b)gBxeoAM_r z^mt!dwbVxTDXb1$^5MOw`B~zOEqtHslit6Og8LvWWO9B2Gd;y%(2P*>0Y}a3BgSMs z&y8n%*A$^4b5&wV<`FF73jYvemdU17iQ)$awkYoD>FLiXdzbidE|^TCM(DiYqQ@l!$dZBNO{T1}tEMg8bPQ7~%N`X1gj z5$+~c(@M{n@-tEjMQ{?A078$?7Mqyi(9Q#MgAKfhn|Mu(`lt`z0^Q7MR8 zK$Jus{lh5v8K7?NFlwGZ&CM`a)bVm30rwUINn6xXMG7WwV@VOiRv`i=i|x@%NLqGc`}vuc6WV1mOE>$uj+e!y=B^} zw&6r?&-r}`@Y=O>^b;c(PTe|YXLXgxh-)hW|>*nm*a?i@Q0^Kbxb2_rqToE1T-}0>hT^n3+r1U<< z^CSJs_6L*_Ua2u8s;OO7mjRL?ILz;|nP8O$a3gqvKw4gN45S(R%q+lL^ffIkeN{Rv z#P#)LQ3M%o_s9{Alg=m#=nwM%BxbD%>gyG2F>oOq4S2EXXsadgGALkuP~D%_z~qe>Za_jDbjyMGiUvK0R`xE~@>*TRI9jB&FTL}DA1JpNKpaK5O$m~i>E0hBVdb_B@e@*#t- zy$kg8`W%EC&VibvF56^+JtOt%tm2v%ord%NYCO49exJ z2)~bVhy>ir=r`~lLi3H69W?u`pf$>=V^LkMRs-)f|0rV|UwP46^Ow*52Xiw?-36c` ze)JfA^yq#f337S*y^&VDL@+TtSez0eDmR*L94%KS5$b0Z`idWzMwb|VR3T!o#SzqA?Or^XS3<2FMg0Uk0=4po#7auzKCj z&d+DT0o?gW#H&DI6sArliZkQ(LqeFb3aTjXGub5Cj{!ABRYcVIGPnW&&WR>70Rt2J z9MflZPEcBrIN0u|O+}60=cWrjcE5!$38qkDryKp9w!Y>6qlj~lXL|qR z_>@j{lH`z;OOgtaOC!0)&pM4(skTGqZnen>O%Yvm5Q>^xb3F|!TTM-EtfYu#%dNSL zg?<%F(@{E;Tm0VZ{PFw#vB$Q@_Wk4Y{eIt{_xt&Jz288=9J<4BTAO+m5_*u4k(;3= zm&vrEsx}mouK=syAE}`>YjL=(x#`9j z5^i8y#EGuGjZwm3&aRw>8k-kNc>p7;EipV`w&}MUa=xiA#BJE)!Q@Y_>xHRwol zZqGKZ>?9s$b?=Q|I9kJQ97)iBy2fzbM`u_MFP2%HhXJYYZeFWUnIz>A`nd4a^;0(U zPRjJ+Xmk@2`s)={x9g#m!36-wq=&bGz2x8I;#nZHXF#e{KxpBFODDTNK-G21Im^N^ z){oaoJbfq4x&7ps$(L4Z&<`LmnQ8Y@?(J^mlI`N_{;)e_>f0-rT8dB(yg^`iCy}Fv znS;ca)X-iz)r6tdgldJZoNf8qXcB52=grK(*vo&&kW-KfN@O?|&NmNO^P_M)l_0+; zXYl(ssu@1%sR}_{{2&<^=>9gnyDSKAmPY{i&3|v7DJG)ArFLo?^F?X&m%H<`qIPUS zdrS89Q;p$NVw7Al)7GZGdgn?36ziRLeDQVuRlV|S@qU_3>7 zir9j^hiAQvF6+G0$2Cu*8ZW1R%ROucwUutPgRqb^DU8FHgKcxX6ZEP6gor z0CavA=Z*<2Joel<$Ly9axgxqYykO+jE9fL56rQu9o6IEkA2X|7Tz{ev*P^7`@;?C9 zB%V3b%Q5HPxWXU_UwH5jw#cG^LM?GK$rp`z5|0;T@Y{B{D_vE4_{GJ=?zavofgOk< zPMfGUXU!6&TQG2$Po4sZ-gNZ|vb2X%=VQ)f9$|Gm zwI9-IvQo1<|0;<_yub?M>L4F)M4RP8^_K)N-PVYC*v#XDe&A4(D+n^Zl-NMuJYx)# z$!8zkme1pILFlCYSiizB?DBrX7^oq?bI6YQqU9x9G=WH9#V~8(HLPK|n6%)PqRt!y zvc|!CYYC#}u+@FG7R!^adsaxS&*hM%xt@gJuurdO+p`Ru9TIjz;adt?r1ErytuFm< zoX*Xlq#9Do80YqoLse zBROc#cxwF{;5CZ)?~cXk^nE*d=B!@%Ff9z(j3x^fHCePce%M-X!W^agcMT7e=fs(9 zZ=NV<1&S$Gps?s$r$%N6BJ%rH*oy}Z7C&2e0Qaab{Jh?^k-h+eUbbx*)w z`&033=ML;42CZzF>2SDRSs-IIdETMGKiG08;j4C&MIz(RwvfVxoBgCnZP^=W8$^|v zFp2N7crc|Z!bLT8KaO=`T`W2)=pKmAym;;L|}Qs#R&A&}S2*=g_Xee*geNDdo9t*JUog{Lvn z(uq}9AE2RADPUcRkIWoxnoch$sgKXVO44&u+)| z&5dw-iKB|_GxY_xO5Cs9yJyjiEEZt$Vnc^*siUe?6xKpSU+L|p(G@wsp@aw-<1=+X zE-&uaQ+(=X&BgwE=_t|h@%+b+9g%ABjWOL@KQTx8GutFY6rB{BSRE7XmQfFq=$ee24&S`wAp6qx zWQ~KKXzzFScpProYC)8q0Kz0&&OKr=^gW>dd>xOMimp5rf;Ye`xZ%^2k4`(xW=~3aoE;e@;t0R$(YP7LjDBG&CZ7 zw71?6`n+3)Siq%x6-EE{OtwKC{6hGjqZR{kKTUf-KSZqa)LNXN9J_&F@2))k #include -#include -#include +#include #include #include @@ -78,7 +77,7 @@ std::optional calculate_pose_ahead_of_collision( const auto interpolated_pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); - if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + if (!boost::geometry::disjoint(interpolated_footprint, point_to_avoid.out_overlaps)) { return interpolated_pose; } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 6febd15ef8052..7d845eb788ef8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -21,11 +21,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include #include #include @@ -36,6 +37,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -151,53 +153,59 @@ size_t add_stop_line_markers( } return max_id; } -} // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array( - const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, - const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +void add_out_lanelets( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const lanelet::ConstLanelets & out_lanelets) { - const auto z = ego_data.pose.position.z; - visualization_msgs::msg::MarkerArray debug_marker_array; - - auto base_marker = get_base_marker(); - base_marker.pose.position.z = z + 0.5; - base_marker.ns = "footprints"; - base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); - // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation - // disabled to prevent performance issues when publishing the debug markers - // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); - lanelet::BasicPolygons2d drivable_lane_polygons; - for (const auto & poly : ego_data.drivable_lane_polygons) { - drivable_lane_polygons.push_back(poly.outer); + for (const auto & ll : out_lanelets) { + drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon()); } - base_marker.ns = "ego_lane"; + base_marker.ns = "out_lanelets"; base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons); + add_polygons_markers(marker_array, base_marker, drivable_lane_polygons); +} - lanelet::BasicPolygons2d out_of_lane_areas; - for (const auto & p : out_of_lane_data.outside_points) { - out_of_lane_areas.push_back(p.outside_ring); +void add_out_of_lane_overlaps( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const std::vector & outside_points, + const std::vector & trajectory) +{ + lanelet::BasicPolygons2d out_of_lane_overlaps; + lanelet::BasicPolygon2d out_of_lane_overlap; + for (const auto & p : outside_points) { + for (const auto & overlap : p.out_overlaps) { + boost::geometry::convert(overlap, out_of_lane_overlap); + out_of_lane_overlaps.push_back(out_of_lane_overlap); + } } base_marker.ns = "out_of_lane_areas"; base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas); - for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) { - const auto & a = out_of_lane_data.outside_points[i]; - debug_marker_array.markers.back().points.push_back( - ego_data.trajectory_points[a.trajectory_index].pose.position); - const auto centroid = boost::geometry::return_centroid(a.outside_ring); - debug_marker_array.markers.back().points.push_back( - geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps); + for (const auto & p : outside_points) { + for (const auto & a : p.out_overlaps) { + marker_array.markers.back().points.push_back(trajectory[p.trajectory_index].pose.position); + const auto centroid = boost::geometry::return_centroid(a); + marker_array.markers.back().points.push_back( + geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + } } - +} +void add_predicted_paths( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const autoware_perception_msgs::msg::PredictedObjects & objects, + const geometry_msgs::msg::Pose & ego_pose) +{ + base_marker.ns = "objects"; + base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); lanelet::BasicPolygons2d object_polygons; + constexpr double max_draw_distance = 50.0; for (const auto & o : objects.objects) { for (const auto & path : o.kinematics.predicted_paths) { for (const auto & pose : path.path) { // limit the draw distance to improve performance - if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) { const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); object_polygons.push_back(ll_poly); @@ -205,9 +213,28 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array( } } } - base_marker.ns = "objects"; - base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, object_polygons); + add_polygons_markers(marker_array, base_marker, object_polygons); +} +} // namespace + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +{ + const auto z = ego_data.pose.position.z; + visualization_msgs::msg::MarkerArray debug_marker_array; + + auto base_marker = get_base_marker(); + base_marker.pose.position.z = z + 0.5; + base_marker.ns = "footprints"; + base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); + // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation + // disabled to prevent performance issues when publishing the debug markers + // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); + add_out_lanelets(debug_marker_array, base_marker, ego_data.out_lanelets); + add_out_of_lane_overlaps( + debug_marker_array, base_marker, out_of_lane_data.outside_points, ego_data.trajectory_points); + add_predicted_paths(debug_marker_array, base_marker, objects, ego_data.pose); add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index 564bf5de7ef2e..6e31f8c8455fd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -18,6 +18,8 @@ #include +#include + #include #include @@ -37,6 +39,7 @@ universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool {p.front_offset + front_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + boost::geometry::correct(base_footprint); return base_footprint; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index f520a564519f0..e4840d724e43d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -16,16 +16,24 @@ #include "types.hpp" +#include +#include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane { @@ -77,15 +85,13 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler) + const universe_utils::LineString2d & trajectory_ls, + const route_handler::RouteHandler & route_handler) { const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; - lanelet::BasicLineString2d trajectory_ls; - for (const auto & p : ego_data.trajectory_points) - trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); - const auto candidates = - lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); + const auto candidates = lanelet_map_ptr->laneletLayer.search( + boost::geometry::return_envelope(trajectory_ls)); for (const auto & ll : candidates) { if ( is_road_lanelet(ll) && @@ -115,51 +121,77 @@ lanelet::ConstLanelets calculate_ignored_lanelets( return ignored_lanelets; } -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler) +lanelet::ConstLanelets calculate_out_lanelets( + const lanelet::LaneletLayer & lanelet_layer, + const universe_utils::MultiPolygon2d & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets) { - const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); - const auto ignored_lanelets = - out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); - for (const auto & ll : route_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; - } - for (const auto & ll : ignored_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; + lanelet::ConstLanelets out_lanelets; + const auto candidates = lanelet_layer.search( + boost::geometry::return_envelope(trajectory_footprints)); + for (const auto & lanelet : candidates) { + const auto id = lanelet.id(); + if ( + contains_lanelet(trajectory_lanelets, id) || contains_lanelet(ignored_lanelets, id) || + !is_road_lanelet(lanelet)) { + continue; + } + if (!boost::geometry::disjoint(trajectory_footprints, lanelet.polygon2d().basicPolygon())) { + out_lanelets.push_back(lanelet); + } } + return out_lanelets; } -void calculate_overlapped_lanelets( - OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) +OutLaneletRtree calculate_out_lanelet_rtree(const lanelet::ConstLanelets & lanelets) { - out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); - const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( - lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); - for (const auto & ll : candidates) { - if ( - is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && - boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { - out_of_lane_point.overlapped_lanelets.push_back(ll); - } + std::vector nodes; + nodes.reserve(lanelets.size()); + for (auto i = 0UL; i < lanelets.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope( + lanelets[i].polygon2d().basicPolygon()), + i); } + return {nodes.begin(), nodes.end()}; } -void calculate_overlapped_lanelets( - OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params) { - for (auto it = out_of_lane_data.outside_points.begin(); - it != out_of_lane_data.outside_points.end();) { - calculate_overlapped_lanelets(*it, route_handler); - if (it->overlapped_lanelets.empty()) { - // do not keep out of lane points that do not overlap any lanelet - out_of_lane_data.outside_points.erase(it); - } else { - ++it; - } + universe_utils::LineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) { + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); } + // add a point beyond the last trajectory point to account for the ego front offset + const auto pose_beyond = universe_utils::calcOffsetPose( + ego_data.trajectory_points.back().pose, params.front_offset, 0.0, 0.0, 0.0); + trajectory_ls.emplace_back(pose_beyond.position.x, pose_beyond.position.y); + const auto trajectory_lanelets = calculate_trajectory_lanelets(trajectory_ls, route_handler); + const auto ignored_lanelets = calculate_ignored_lanelets(trajectory_lanelets, route_handler); + + const auto max_ego_footprint_offset = std::max({ + params.front_offset + params.extra_front_offset, + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset, + params.rear_offset + params.extra_rear_offset, + }); + universe_utils::MultiPolygon2d trajectory_footprints; + const boost::geometry::strategy::buffer::distance_symmetric distance_strategy( + max_ego_footprint_offset); + const boost::geometry::strategy::buffer::join_miter join_strategy; + const boost::geometry::strategy::buffer::end_flat end_strategy; + const boost::geometry::strategy::buffer::point_square circle_strategy; + const boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::buffer( + trajectory_ls, trajectory_footprints, distance_strategy, side_strategy, join_strategy, + end_strategy, circle_strategy); + + ego_data.out_lanelets = calculate_out_lanelets( + route_handler.getLaneletMapPtr()->laneletLayer, trajectory_footprints, trajectory_lanelets, + ignored_lanelets); + ego_data.out_lanelets_rtree = calculate_out_lanelet_rtree(ego_data.out_lanelets); } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index a7729deb539b6..0cb9e223c6456 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include @@ -43,7 +44,8 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @param [in] route_handler route handler /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler); + const universe_utils::LineString2d & trajectory_ls, + const std::shared_ptr route_handler); /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during /// a lane change @@ -66,14 +68,18 @@ lanelet::ConstLanelets calculate_ignored_lanelets( /// @brief calculate the polygons representing the ego lane and add it to the ego data /// @param [inout] ego_data ego data /// @param [in] route_handler route handler with map information -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler); +// void calculate_drivable_lane_polygons( +// EgoData & ego_data, const route_handler::RouteHandler & route_handler); /// @brief calculate the lanelets overlapped at each out of lane point /// @param [out] out_of_lane_data data with the out of lane points /// @param [in] route_handler route handler with the lanelet map void calculate_overlapped_lanelets( OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); + +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp index aef035200b6cc..176b0d2aed2b3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -20,15 +20,15 @@ #include #include -#include +#include -#include -#include #include #include +#include #include #include +#include #include #include @@ -47,7 +47,7 @@ void update_collision_times( auto & out_of_lane_point = out_of_lane_data.outside_points[index]; if (out_of_lane_point.collision_times.count(time) == 0UL) { const auto has_collision = - !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + !boost::geometry::disjoint(out_of_lane_point.out_overlaps, object_footprint.outer()); if (has_collision) { out_of_lane_point.collision_times.insert(time); } @@ -129,20 +129,39 @@ void calculate_collisions_to_avoid( } } +OutOfLanePoint calculate_out_of_lane_point( + const lanelet::BasicPolygon2d & footprint, const lanelet::ConstLanelets & out_lanelets, + const OutLaneletRtree & out_lanelets_rtree) +{ + OutOfLanePoint p; + std::vector candidates; + out_lanelets_rtree.query( + boost::geometry::index::intersects(footprint), std::back_inserter(candidates)); + for (const auto & [_, idx] : candidates) { + const auto & lanelet = out_lanelets[idx]; + lanelet::BasicPolygons2d intersections; + boost::geometry::intersection(footprint, lanelet.polygon2d().basicPolygon(), intersections); + for (const auto & intersection : intersections) { + universe_utils::Polygon2d poly; + boost::geometry::convert(intersection, poly); + p.out_overlaps.push_back(poly); + } + if (!intersections.empty()) { + p.overlapped_lanelets.push_back(lanelet); + } + } + return p; +} std::vector calculate_out_of_lane_points(const EgoData & ego_data) { std::vector out_of_lane_points; - OutOfLanePoint p; for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - p.trajectory_index = i + ego_data.first_trajectory_idx; const auto & footprint = ego_data.trajectory_footprints[i]; - Polygons out_of_lane_polygons; - boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); - for (const auto & area : out_of_lane_polygons) { - if (!area.outer.empty()) { - p.outside_ring = area.outer; - out_of_lane_points.push_back(p); - } + OutOfLanePoint p = + calculate_out_of_lane_point(footprint, ego_data.out_lanelets, ego_data.out_lanelets_rtree); + p.trajectory_index = ego_data.first_trajectory_idx + i; + if (!p.overlapped_lanelets.empty()) { + out_of_lane_points.push_back(p); } } return out_of_lane_points; @@ -152,11 +171,12 @@ void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data) { std::vector rtree_nodes; for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { - OutAreaNode n; - n.first = boost::geometry::return_envelope( - out_of_lane_data.outside_points[i].outside_ring); - n.second = i; - rtree_nodes.push_back(n); + for (const auto & out_overlap : out_of_lane_data.outside_points[i].out_overlaps) { + OutAreaNode n; + n.first = boost::geometry::return_envelope(out_overlap); + n.second = i; + rtree_nodes.push_back(n); + } } out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp index 33f0842c56d36..f0e0360ef1c15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index c97e10cc8706e..347a138b651fa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -221,12 +221,10 @@ void prepare_stop_lines_rtree( ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } -out_of_lane::OutOfLaneData prepare_out_of_lane_data( - const out_of_lane::EgoData & ego_data, const route_handler::RouteHandler & route_handler) +out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData & ego_data) { out_of_lane::OutOfLaneData out_of_lane_data; out_of_lane_data.outside_points = out_of_lane::calculate_out_of_lane_points(ego_data); - out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, route_handler); out_of_lane::prepare_out_of_lane_areas_rtree(out_of_lane_data); return out_of_lane_data; } @@ -255,11 +253,11 @@ VelocityPlanningResult OutOfLaneModule::plan( const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); stopwatch.tic("calculate_lanelets"); - out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); + out_of_lane::calculate_out_lanelet_rtree(ego_data, *planner_data->route_handler, params_); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); stopwatch.tic("calculate_out_of_lane_areas"); - auto out_of_lane_data = prepare_out_of_lane_data(ego_data, *planner_data->route_handler); + auto out_of_lane_data = prepare_out_of_lane_data(ego_data); const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); stopwatch.tic("filter_predicted_objects"); @@ -275,9 +273,12 @@ VelocityPlanningResult OutOfLaneModule::plan( out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); const auto calculate_times_us = stopwatch.toc("calculate_times"); - if ( - params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && - !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + const auto is_already_overlapping = + params_.skip_if_already_overlapping && + std::find_if(ego_data.out_lanelets.begin(), ego_data.out_lanelets.end(), [&](const auto & ll) { + return !boost::geometry::disjoint(ll.polygon2d().basicPolygon(), ego_data.current_footprint); + }) != ego_data.out_lanelets.end(); + if (is_already_overlapping) { RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( ego_data, out_of_lane_data, objects, debug_data_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index c3714c5609135..edf4aff8bc85c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -88,6 +88,8 @@ using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; using OutAreaNode = std::pair; using OutAreaRtree = bgi::rtree>; +using LaneletNode = std::pair; +using OutLaneletRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData @@ -100,7 +102,8 @@ struct EgoData double min_slowdown_distance{}; double min_stop_arc_length{}; - Polygons drivable_lane_polygons; + lanelet::ConstLanelets out_lanelets; + OutLaneletRtree out_lanelets_rtree; lanelet::BasicPolygon2d current_footprint; std::vector trajectory_footprints; @@ -108,10 +111,11 @@ struct EgoData StopLinesRtree stop_lines_rtree; }; +/// @brief data related to an out of lane trajectory point struct OutOfLanePoint { size_t trajectory_index; - lanelet::BasicPolygon2d outside_ring; + universe_utils::MultiPolygon2d out_overlaps; std::set collision_times; std::optional min_object_arrival_time; std::optional max_object_arrival_time; @@ -119,6 +123,8 @@ struct OutOfLanePoint lanelet::ConstLanelets overlapped_lanelets; bool to_avoid = false; }; + +/// @brief data related to the out of lane points struct OutOfLaneData { std::vector outside_points;