From e65371adf193762c6d0309af8005b84aef9b0732 Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Wed, 6 Nov 2024 10:28:32 +0100 Subject: [PATCH 01/36] Added missing documentations --- ...record.launch.py => recordLexus.launch.py} | 0 doc/SystemSpecification.pdf | Bin 1810790 -> 1813834 bytes doc/SystemSpecification.tex | 45 +++++++++++++++++- 3 files changed, 44 insertions(+), 1 deletion(-) rename crp_VIL/mcap_rec/launch/{lexus_record.launch.py => recordLexus.launch.py} (100%) diff --git a/crp_VIL/mcap_rec/launch/lexus_record.launch.py b/crp_VIL/mcap_rec/launch/recordLexus.launch.py similarity index 100% rename from crp_VIL/mcap_rec/launch/lexus_record.launch.py rename to crp_VIL/mcap_rec/launch/recordLexus.launch.py diff --git a/doc/SystemSpecification.pdf b/doc/SystemSpecification.pdf index 67be25f198b41c35e1a89ac90e6338206be7296c..42e80fcf81430e4428d84baefcfeeabb9a9da529 100644 GIT binary patch delta 22811 zcmZs?V{~Q9_XQf;wr!go+qToOI_dBv>DcO=*tSn>8y$6Qo1G3`?!Eu_8}GyWP-E0u zvu5p8`$O$nYu7sLcN+igR0j)_Y>kQCNCYDY4Z+C{BzGmmY;-{U0}G77hQMz0!GwkZ zh9@C&CQ}LX0##P~b?)0OAh@_dn7ZIqFc6#^z&>Mecc45oDk@0A1pF%raMv4+1GMQ6 zUJHca<^siILu5lhaB}@Kj16dv2O$N)%^O~f2LUR^gFL(ZH2#P11I1<$P(bi-P=y`f zoXJ7>?7+yI%wKx8qn+5|uu0~}<~j}lmPNC-}z ze*ktM{4B!%t&o>39H|2Kv%!5fco0$r!Z<`@SV*m;aW1Nt0@iUGm-Uku^$y9l2R z5hTBh6jllJ+s0u;;X`0mv~sfXuq9*X=VSw_%_7hP^!1zxdC>xvYsplhB1rqG`GpX9 zLz8>UgRtNoU}f$9il??T=o>H|yCea&oVDjqTbAqB56Q(kg}4M?4|kE;*vH|hV{U_q zGRU}~&6+R-Tq;A^LzWQs+Mt!>Z41eEdEgb$xujRbkW0S7pU2?lv?BMzvLd~RyH`rN zz*J)c$X(AQ6?Gwnl6JO|XQOcIVTg|ihLdK~`eS59++gt(Tu{hj5946$tgn~^@bHg9 zS`hpUB|niYX4Zqg)kLQ{q2Jz}mwl^@mw}Bc+nBn3rrh9Jl;YQ5)9fy=S(t-7;91Fg zD142{9i@jX%jW}76RtH`f(Gl!1W`4hr z)ek7ZIZ*&$V_zhUcN6oD+2-q@wj;=@>cOh#f-yJipv2{WfD0g5RR;};*$7v`wr@Jko`x&1S2 zFIygM8s^#eN^kZbD3wD&{en4adBNBb-c--vs}U%Z@_C2|eDI90Lj{nUv$G_9ixB8D zc^35)X64(f`l`ls5BvG|V8TIKaML`1$0sRIiU!coSMmnl-9IjV{F4TSPIJj>8NxTW zR(pT=q2qg8YEta(EGFZ@MG*qBC<)pwt_x@4cXsc&MKczCg#((NU5Da3_nSfJqZl~l z%X5CZ(hId@Kf!JV40%VN>vw0X1}lc5m%O_=G@jk9w4gin#?&627%pNCeHan}7aNYv z$aFqG&7BzE-D{RV($P=kVq^umDEw)qo6hJCX^xpQou&cS)s8CsHNWOpWZvG^2r@)8sXXeq9a#@(5*lQi9$>E)$r^c-C{-D%xOWwFO9 zL_lcD&e`EFNI_=7mrS30i;plwakK|HNm#emf78OY>E)klTm^c&y7Uh*9{8J2b zHsl;SlM#yBV#Nuts+;*V`e`ECJtCjc#?h+!B}8tiGAiO`#K)##=a&>W{TUJwhW{M|9RBt)=T|tsRlMKf)uqRCxT&9x0orBn0;s> zUC`e?7g6+3f!?A%K7e_Zykj-Pw+Fx41l`U4CQIv79 zQRRmC5t-MgS39m#RKRqDbq_1Kxy@`R{sXN@#)2xxK-P$5;Qu<^K_B-%A` z7h({R*zJjOt=(XvtXv>AieadM<&I2YviLP^XtT)}6fffv?98mOiHqfC6 zQQan=qIisR0{iM(r(mbf&Tyj_SS!3nx>y284;zGS11@pgzOSwRP$n(cE<_XH(qa7d zDp4J;QyDd3w|Ei`&?l|cJ})TNcYRHby2{Z^h`L8@o?gJ)!kAjnE$0PsXV##t!QZ4h zw5P8W%)&j`b`2Z<@X(810dEjTDVK8ik+``SGRbdbpcB6$$*T1`^#gQp@RLcU6I{~@ z!en_>rr6nHX&$7umD0V166M;{`P*RXu<@K2xABRpf`*kGP@xPvwOKdAWMCro_(J8I zyya=|=VncZLJ*^{k8ca>azPc_s>&%YzzD^9PK9TDP$zw9RLl?Y$P$4nxN)9#QxkR2 zj{j8$^y4_G+PVj6MJ}=jlM4nv>8h(5l@1wlOK+g^&t~R82@Zk2_Z$vB!TkvrPQd}Z z09q~d$CI~%EeHN#7mop}2dsg)*quQ?FS=laxu=)zgeFgBs_Ioo*?CD_kvIp|GEx%5 z-*Ti~c?m_|1~a4Y@%rEL&IC5kJ@E`&-m#v2lYntO05&>SOfh`SqQ!QUj3xquRFVW# zU}PzMT-VxPpFxy}<_b9n%57C){RUz>%aj*bkr_b@UH&x_z|7wl45$_a2b4E8A_ZTQ>&#Hn zT+I3B?7U^zeD5%ralxF~j!)~Dq@o$Oea28UE9E#3PW-Uc|3p^#i`9H6;l97re#KF<`gjQUp*-grQb&7cD(jJreR)#MP%AIWy*0Fq9x7f69}^(xVN=Dl zzAOcgV8~y6uL;7RS%>6BrV@s;8g);o$XA<9lBg8d3eS<4dQ6hQ4$KV6f&bv%mt<2% zgkcj$2K+E2`-%+7Er;4>DVs-y8&m`dNeXj{#lbGoj|V18Y|N1^$zDbi1W%v9Wh%Mj z7@U&<4e7U8W*Vr2th~0|wGVzbVT8>}l>#-wPY+|BM6ewrM*!oH6eq3{v|~XJ)%Bd0 zlImcWuZo^Y0K<)(g)YpqmgaBPT+4?gM-#-14oL1z^|(Vt_^AYeiOp^y*~EpIk$!yW zQ}6Lhz(t%-gB|`b18X(NjDrkUQYiDT7>0qzf2F{D8WoN*cakCfM+wxR%a{@TVi+Oh{rV{B;I&HE76~73?0VY z04P`b_2r(qhvtNA080;MSY&jH%>lEn_E!qUv^A;*ep+m{DIupzl2S}=;6yS4HztmK zD17Hgui}ah_En?YsU<`$AwEod`zRPO_z8!)Ttb$Zlts6H20M6KFw`9G{+exAkQ%`+ zoG%V|kAo{tZMZyf;<)DJ1ATpLBFMB*(}2=zj+okyXUv^_TTQxVnXs3GgTGJCVKVVF z*K>Al@UXCELL7&bV+%GicE8FNAOrW#wr1XqvrNEq0b(r21kbM$&58i!IUHfG7)CpbH-APOo{}pO9B&f$*Dy-Gx&(|L$k5pMV#t z7ss2&-TSdqcRo2ZQ;vFF-)$lT0AyvUeHmMrVP9^w4vJ*xXp?#JXF2*nbl};tIiN#V1!f`h26*%5`}d?$Sa9Av=f`?wPwN8HXla!u91-Q<(z`R z34_Rv=qY-mAjT|l79&%usMAHJ^nxyzpmJ47AwN@9W(IzLY0T&)auV6S8@Qp!L(e-f zJb^QzKJcOd370d|KZkv(Xk)(lz8xx0EvW5V2qtT%5M1AavMud6nJsA34*|V)O;@{+ zn5`SW4?%NNYP!n2bs>ce*}5+9_RW}8+@9a0i$+^ZlPVM#Wr6P{4EoDtyRDI`R$%MZ~Wpwcy!TXu;Tn}O|R}3mZQlL zq(WRvtV>Y+f%u7*Xsd>MFg;+%I2&_67+|wsM;a}c-Io`abF~=bXg0~Y$c1X%N^Z=H z7Nd9y35?77?rpifl(jls4`wvM-&bJU#GqV9M({QF!rXo}`-IptU`+)?cVY}0z3^=x zs>VCGG!eTBti@Gzyn8dlK??U^slD6AxTxEQ#N_g=r!(KqHq~^ko(7n1f5$SQq1D|9 z(_!b?_I&*oOdYih|F?EpS9T!+G8Z%E1e=5uZWZ)t5xPw*HNX28S<@ugKm#xB7G}BZEC(9NLY;5Hj-(q|BET@m zV4h|Dy2ib&4!N|8>n*_XIn$GX-~y_rr}fA170=_uu(OUuE+pfZRrR}IFRn*Dmu%Gf#~lq!6qq6k!`VkzZQ$i`p~PN4LiX^q3@j1MctGx%k=LbaT*zBVS2>BW`NuLgN$YlJViN z`&M-0eFDgCM2Lyc#mhs+M&@j8hae>MzcVif2OrOWdwrLlg7Y39M)#4 zwwbp2b%6CZ=GHD-SZ7nvr%qZEeeL!7DLN>;3VXKD?y}KZHWx4h z{7Yjp1u%KVBy3NK!k%Vx=90WCy}ottds9L7U(oC7sRk|{;xyhc)q2;_r;na|iB+m! z<9#5!JU@%8$tHEYHF`IoQ>y>UQ%9ijEX-F!4X8Cfn9we-zjc7ZoZftY? zp?fQ>8z);Z5gwWBtWkBE1&vEHg6-;M0ed0PT~jR+Z%fQCqImbBcTiCn9B@yu%6x?qL;ZKEvR%m!jm0D$Yw z5i~w6l4YJ|h;UhNf5%89d@3rg2>|s6HjQg;C_D-Ajs%#{F146Dpb$hj|j{>Dng9m+(1+(&XC(sG9RQC%!csVu&8 zyKNBnn}^r&Vm2^kxOyZklJxb`XakOQQLpQBX6ew;e!-3;YM)i?gO&=rC$)iWU%C&z zn#xUIww#Bs{%q-|X?S|XuB7)URjuJ%dMJ8qJ@7vurJcejJ=^rUQ=VC?8rC?|E`>Lv zkEJ+R&jzR4-v+=GkXwiE zS_KQDkl-xIs$T&wJs$?8;3X1A_qjtV(3Xj&f@U3CSX14ZZF+eK8RW(wi^Qlsd0&|(Z7_g+Z#WI-f zu(Xh)3q%cUlVrBNcsXtu#Pox5fD0>u)HKpf_w2B+Z^IKaB9qImzN&B|28;WpWq@0S-<(^T?{=i0TeS*%md2tWLxwo8tPiY! zn$+2a!Q_?8vFx#Gd@jl8XD#1x$z4ezsAsdzE5x;6@X_3UL`Ko5Ey0hzGfBXZX9#$) z0}B zMZtDW^Z|QM8m)^Dh=|d!F26$1x!y$77|Mh=on-J}R*o%Gxml}w`f)8xaCDRcn7Fup zm14SlUJloq90_192t9lK+kN;wXt8^X2}HzBls)`>7FU9spjz}bSb;dI6AwCw)IsgZ8Olo>PUm)LNMn z-x+{~^Q-}aL+uTp;8KBFq2~~#&@6=(A;hF|i5Qc$1}|Ftj;_*oG1=APauz*=-JP-N z1Y5^h>v!q$h<}bGhw)$ zVPZ9K5Yw*t6<%!Kpwa|7-aj#O1D-6WZ3HL6-#Y>n+7v0qMScL!l|$byVPW!EN@#L! zP+iga$XbKQh(h0SxhZ^y1ZPsT0|iI_w0x?FWe148D8ExHvsj0Fz@&r9=V!hyHCq9m zXe?Fm2Mu#R{Vu3y<>|kIf}2TM394hAe-r*fc<=$ebmTjL`;{MDvWoXb>e3~FAjVE! zgj==sh$hL=PAoD=OOs$Wsh@bLLr_|eVWY&hEq34gfZ5V+z9OGt;eZ*x59?N|h#@_S zt_dJfKC=X(a{{^zx18n7>eY(uW4O=_I^Gh98De_Ry(^_@=RxsIpLFAP|a zryF)D*3&sHSQU3thRn8Re8F_w`v~9Bs-9GGy?>(CV&B!<_}NzGrFj@tbn&w-f~xXu zxz>5YFkDI&7Fs31PN=%lSEa(;9|bG6-vbFFo#R@1-thRcg&U zO)poBM;O0q)f?@ies=|;28VIr^h~SnT9NgrBzlm_ zoB=g~Y-k=ri6QxgT;8hA5H?pL6tCbeG(z%uY%qT8_3ZX-s+uZDT3^7AQ@A%GL%JAyTB5BKQrXDJlBm8%*k^%&(8)!)Zf&aKx@C8;Bn+eX_bp zH&I1*0DxadD29{+b|+m$^N+60`3$IWyAgBm!I4z-Ipme2+WyQE^Qv(iwOi1|=I`LM z>xCp%Mp0_rXehQ((Jz^kAEr#53bC$yRzj0W-}h{~p4L2mQkNY~wgEsA;aLo^P=$TL zYJ+g)n1Z0`)onACR#dk-Y=9!g6ca{Pf99Vr<8+`rUMelJ! zIbj&c6IavZXTB_+i^2bOIdr(D*cja(^>L~OaSANx8|gUV3L@&?J(P)`1C@*Fd+1`o z5Rbh(6u4ljLr7RKPZ*S0^m8TT&OYNAmXt>&-siirOuKGzqnTGD!WMfj!eg?_?eH^+ z1)}rVOh4a`8zIG7_Xhx834}leg&l!nAzZ)af>{MzN=!RWN#4*%wF%Nf4djUvR4yi^ z)6FWkESHodgCrU`Q?fU3T!P z4m%M#RUYsj$$5OvW~F@9X(G@5j>fB&r~Qe|)tv|0F-Y;gxH_SR^(x?F-}MS}2Z74S z3$f&4>-8RS<~yLz@Zc^o3f!OH*p*cWX#|Zm)0jPLK(wR9Tr4_H#>I8WgP0G}5JRpg zBmlb+U1-&Hkr#1=&x8pRa|1_VdHck?-g)&1(W-#dG2i2@HG2TjMuXBpm2v^)XXU!E z8fBgeeNTb34-`%WdN7x!KCpLJpQ2wsCwT_<%RF239Th;KLc=j5pKez zu+*BAEG82@pf+;DUZuosW>-3O=qGYC;>E)En~Lkbs)*$P7<1dDfKm0vhaW3jRqhIJ z`x>2B~{_x3YHJ322ri!Gv z{&Bz;_h8sE4R-4=2v__-g1>~TH+wO4TTBQ@&5G;)ebXg(8s%G8BZ^w#r1R3CupFza z)FYJ|KxkHzepowV$T?(eT63ldu-{(K?o?{rhcpFH)3fH+;&-mT+T3&RH7J)(-L?^4{ z%R+se=o=&n-|)>gLRgMBw`ver-8^USi~pVZy!x1$QNP=`m0j@IfjOC`)n0jerZm7C zR4FCh=Cr1eW%YyiO~`dC9fPlZ;^-ykLhW#}{Og$)xXhu+SVeo)NrXaX+)HkT@>^F` zD4ALyI$w$SthAN}kGbr}2O>%`z!;dfjYk7b#=*hE|9_`&eS_pe9-M$HlRnBC`O0s= zvcw?@MKn)3wKgUKv*L|FEW)M>3k6>+NxOjxhpy=+2U#UQ@$ zVu84pI>NH7I$3+hD0-^!ekw9$g+4Dr*ixO5tPdWsA9= z9&Ba&V2hzh(9t9>=;RLw6xV(8_0hl~M|e{XEiwd<)ku=wR$5|`HvC@64P`|f>*o_h zqU&Q9C#oEi7bBIm4XBcAW?1mg#`$AlNlultuOE%7C^*U~MXM}Gp2yQ+{Ve?Xe?U1; z^mqgXfP1xtk_mUL&A7%vpp#i3+ruCX#*pUT#d7SNcI{wV5Sb!t#)DWBlMrJ|>P5P| zh8_nnvDyOnQO?Ru%`y53;7A-UbF@%0N@GMlbKEtybj`7pQ`%T?a#i_~rux@qe3ZG= znaFA4tNHO@sjI#;!SHygJAp9H-2p|qhS z-vk3pmHKeHH5jz^>h?19M#_*A81^ZCpEbA^n!D^T*l&q@J027{@)bPu-_!seDEoU^ z&O&Ge&_i z*D6)MXuYp@%}!g%URy%rG_zj6ZHdg5>vA*Uk!lIkQ<#J?K7n*vc#d!H$UjoXC)pfu z5dVV?FW$9L#-SrKf>Y22ZY0OiHPy(=B71m7LTlMv(6gO(*mOSOrg%|AS1loV`v#kn^C_GUKpLbxH ztt(4H_wf;zK~Nm%!iF5jXa74~BGabqNndjsJC@G z1V_`mGTFn%z1$c59tK@QinCm`Cb^`d-Gf@{p10#)H}dgJ)wsC7-UFSxolCg;f0KUq zv$uw_U;{;m9|nA*6C@`#F6!`4J%44?iCTF|fU(62B{jkM5@Kz8>7CtK#JIIw`irEb zm-rj0k#wYc*)GFYM~?tW8V`%3ZZ!t0RVSGBs|(;<-n$*pK#)F2uJv4~revo=+RAR$ zS!MUs47q7v($5+j|8@uaR2O%);ty)?%N$k-YO;SC?l1+#568>d#(eeEluPq&{33XC z?_x0pdl9=mXUib#rR3m(1#6Y5_>;J}ky7+%Nj`9QKs*!2QQ0SZmPg$KLZY;?ieJBn zjPKcQAbSoVfw$#(itH;55HZgZ+49P0F8u!J%5g>eRIJCl`hl>%1vVT#ZgZvFf@KfnjeLF$sZzB1N&j00GI$EYueEzj-a-NN*j(Q3e(v|0mf)P zmc^)l_EQ49K=Ag*&LzYpFDty?RLY<K;{qd~2#`HFplMDReyMuQU5rk(+%70<1&0fTPVLyu;25H46 zqdv#qORvoqE;FEV*EevS`iQBtIo}=q71D%X{ywsfd9I_b$0_+YGn^pyQU0y?3o`2Y zQ9myrTtL8NqOk;R0xY1|D?SDU89Pulm4Fd2 zq&JaN$b+@e&yXMuu<~uS+lS8joBfF`` zw<~@D9ax88l!!c3#(AQiwHh9k-(>Q22}m9Tl_7Y9TE*1%;BL-*+)w|;V7p~CUOd@84`s3$$-3*t$O zz^~d{TVVt+Z{HLhV9FB7qTCG1g#Ww^APDOWXu5ZVft}Rd4f&2B90QZexpXn&mJ$b< z=D+Q%&5pJQb1y5EFqRpb571BQLi-Wc6l?aW+}O7?CcM|68|?~-eD2S zI*lGk@S&D?8}9HX&2kXdSi$luHmiTTOor&@%;OQc9+tfF(?>n4)}zkD^CP}@^g<&_ zBkUwuE<(qPf~<=57x5%eYTFWu;YFT+?O{Wui4H8L`l>K#ZNtT zGqmRzv_croJSZI7)bRLRLlbZ*V=OAq;|(ixv1`od{*tX~w)3*?$Z>WhK@e#1hD#7u zJ0ua{r^(|FSLUrHgGoI^n4G2&v9Z$X#rp~IOA^{&5t@uvx<;R_TJia9%6=XL<%wAQ zC=?_t#M!N}UGzO3Lh4r`psMUaiz@ptqcrW6`$m-?7#$|xQOlI?X6+|)Q1l)3XI{TJ z=g8M3p*QU9eMM-w2Im}@M*jiNbSm!*;?v5FKOVsbq2ZTpe$6fRf-2Gz4!~9A=+re9 z+G{wp2S?DZoQ@DOv1;#<(pT(~*AwUV8Zeyo*MJ41s8;TcfZ+lnz}RTnM*2Oa7zaaN zL#^#))#b_eIE85;9uS)QGXC4W$|rR# zi1YtlO!=i6&%)uM+nlQ03I~JDu$QKC)RQW_Ft_dDlViH|wB8bu{BHleMy$Dc;ap>R`2Y&noPEyJ=tBl%wuB6b==d0aU*qQ7 z6NLVZgHE-P#Om(Vkp~y~r#egF|K4uY7o^O#BWDX|dz1BQ#WT`B7k@W)()={G#`e0l z?yQrSzvsjrsVm(GaXgwtbadDW{t(k7M9TD>g40^=IsQP!atVX|XH&rh!s`%Xg3jpi zCgFgRMub?P2@bq+1z@$sXE!1M4_*w2WB1uD?#4r=0Ft_Xc89m|Xi0&pUWAxU_%kg3 z+?knU!3PmTaPcOh(xLM4|L=z>7aKpv|1Jp=ji3h{)uxR=1(jYQvOyesq{y~7(AN|7 zq1xL~bL6)|wKW{PzWAAFU0pJv`dZG9Du5|BmUOhYU)Ef%rk$FGr@d`c1W zWM~Wa=KS`+cP_5zCkz-HNc4$qLs!^Mh_w!(??{5V8{5RsNIa0R!sPs5+X7tMU^7Dz zBvl6#P?6YQsAPBhx)h-K3`Kms&=7+ZiQwoAHC3Gw#kPGC5^vqZp0H8*Ykp>sD?=tc zAHtF{`;gVe*ON%vPK-!nn|EOW7)HPo?4YWV%ENuSa+(6HR*J`87NARfPfLNz8OfOXs|dGW*MI^{TgYzJN!-x6VE0+>Hn0>5@@I+y095DhRG*euEcn3*HvN1gqIZ1SjsYTUE+D|v_}#CCXtHZz zVezH6^Z9x}e>0aP_x*40x=D7AUQSO-g!136KhfUiB%PkOZl>LOBsJm7b@RuBi&!J_ z(J5>`$7n3glOn&nmj!bbZ*1`S2*w_>Aj(|NjzmBRSXQBnkPU)SCKM7%hBaYj& zXN(ig#OhLFjv!`RZ1H4KRuZYkH}}K=yf?#fme{@-P0plK-MgzGq6*@TtH3h_qCG2C zpH2azXi-t8QN2-10LKOUEChwVNvkj!=V`PBg@Zf^*W}&Z;!ZR*gbmvasR+sB#KFiz z9d&K}v)_%Aw<(Ef`N7Ex#Ba3mnAYgU@Vf!(jsj0%gKmD~brXLgDrwB%xN=NA*DB=t zCqGvd3^A`L7PWw&p|+94Vg<&JL#BlFw8YGUT3tLjp+$CgfEUvBFMhe+wFx;e{BJfG zq|&3F?l>zlaluFd!?WXxmPcsRjwBpztnWwOqlzJl8;ywu84CQ|RB3IaqxdW90kD>u z;ueZdBpfYUb3=qb>{vZ?6;@_V5(xFK?{NoHqThx%AAeg<$Etbb2IEtA@vD)<&#Mxb|=-Hv3Q~ zHv3ITF3lU!u*J{z-oPxC6^`U1j$l(Y^=pd$&N{S=_2aj9xaY4dT9H}Ws3dbY%H(m1 z%}UJmmSIQ5AA4)R-4igx;-)K}P~2Blo14*uQSP} zR~!#Q2OOU%S{fvDt}`cm;MT`l2Jp9d*Sl7(9CD8v4fF84!`RT}$Bef8h&OYn-=&K) z;-3*!eAk5@C$X_2`MS7dys!lnAem z$)~y}56Jl-cFcYI{(FFuFLxs|?((I?edP?Zt*k~&;6UMq<~~6taH0|$D>E3Kr_r58 zgt z$6OPriyb3HY8+keT{o>vd7M31g}D`w%7EOF>11x86&pUnu_E{2@;AbYYkqWtWr~(B z8dZ35S_XQq^7&!R@yo7W3kx>RZO7{r@&F^}W|`v3IGi&#t&y@+KE@nvN1k9#W7ClY zDa5Q*ChpWBk?C>K0!{cD^~f?8skD-sXL+Qw%KOml%K2(T*-EE^tRr|oy^0i`CCxRD zG@0#4?q)B$y%@uW6E%bTbOVQ>h$KOaBy9&yVl5+jCT5{3EKBP5_#76J^uh-0BfzlT z>#r0+Lu&Q!7Nq#}Ox6jR3~T+i<3dM)l#h%e()nKbnSZ{LH=?e_l7brcKYF+OT}%03 zbm!S5h-IBY{w&Uic)7}&thJ|7NhympLIuDO6}xNNpw-9UYKr+$2#%PQW?-E%l{6OZGQ3YA zLgJ>PAs5!>^{?eX>0lgW#lXsk7P;T{DQo93xN z#8BylGUJ>a{Xx2XfsQCz)Mix4*CX9AdQrPdV%P6`cG}6q=_1iXW^S9V*0_?aF(|3|F772VLCsB;!omuk(Be>V zKP3ZPowS-+r$vVBuM=pIZefUBi#fqihL(j#W13$|gL(1y#39YB8qW_05j$alN)e6e z$6ZC{J^6)qDGe@5j`e(K zVCC?8=yF>9ZFc&-$@tOh{80VrTv`(>RIsLo32PhI>JFmP?x!{-Xgs4J3xR{Tnc{a! zpp$Wh5-YXTWLJ(wUOg5agy4~0Ii33A?$#2r<^cqRx;yPY;C;d2XCV&mh}yiPNfUE0 zme5)Mxh_9r);?c>%ayp@eKwbJouW2^Om&M}Zef>y$-PZEZ*%EbeaTGLHluQt+vSv| zxuo}rcL!OfZ_+|a3pW%!<%*peMwNEGjsh+YwSK`^W=VwO&ODWx+|L!Crvm{w;HtPr z)mNqZ%7~X0KuFh?zP%D73l69o^qcY_^=Bmww!-OJj^X{Sd?n)cLxu%KsV1iS8yxXP za$9ygiRNE&?PFRko2RoW`_#rg<=Kf@N$4+C1{M%1RkU5cJP(2d8(3dS4cLQUDs%oW zk|nEQikZh2ACCsUZ*kH6Dy#w5z1Xkzq4EuE#=VNve~jtVie3YWJ+)*22{Bt@vPZ z&{!tb&A%e1@;axYsR(;HyT4B%U8wJ5eCd7CVR=#GK$nIucxGOA;+By)3L%AGZrsHi zm{coE1DMaz*F$KQhPD+2IEtKEE@-GJq9+1*W18VDBN2rBI9 zHipiE(;mOtnP++?FS0zxGh2*O^GXF+X}#M7Fju`nXgul)C;h)CCt{^P3>Vmc7~0Bo z-2+Z^;+eJ_Nv|}mJi{vuZ}@Oz+P|1+{H6gxEs$&;vs9lHmZmPh3&$tC zHJ(X3EFLL;MVKCT@;SovO4HwrmPAna`?haSCi+#_(3mKP^z%yj(M#mn#E@@{-_thf zmWcODDeTN;&vT9Ww&l&Gc|??}iHJ73c5Hg+iY-sFer)$BBR^dKxZ^-A*J87>FTkQ+ec@mjh6W`HOqi7m$ zwAWSY5kM1#gw(T$Q!DLlLt zuW;x2u(RIDDRX#y)L3{>&d>9~@xeoT*u~FXI#R08$idiTa~MM!u9E7!jF63(-FWcl z<}{X{7x&Qo4`qo75r7MqHaeQJkq2;Acy};g1<+4NJ(Rgi{$m@!^P!hh`}mL}B6T&> z-TkrqhjFo^NBkMFSD3E|X2) z_r=EWf=$9*uI9@{lCTcU3m-tC=Z7_l4sE5z+_t>r$4SHY3yneF`zPlf9|nN;u~vt8 zXFL3FPnq4eTg2%?Gr7JcUvsG^Iqko@s~{7RVBMLV^^}<4E4Ni$fK&||XIErEOxq?|?9 z%JK{?_1VZ)x8MD-b3v|0wGTcTRYf-18u8cCRl?@G$0*zpN0e9Ako!xUe^U^jnFHWw z3IPk0tB5a24Z+R!kA(?(cEuks1?L9VUlU+~X6Nyp>HjIA7JtR(VT9o3`HvAxOE5wB zPYLuV0~_QlL6AlQ!SgR43q)*A@WlC9zw3-&%cvkf`+aM z0t6s<{+$F9q{~Qni}g=cG&aaYjIa+9ocEs!AS|H03}F-KM3T@E0)m$VSocqU#6gCT z>>pgs>p#2^q^UsY^oeu*ivvIL22hmpzb9sr{_1sKb2UY0qBAjObOQ@!1?|Q zFwT@v3Iu2Q>4D(m_=i6Ps<8Qo%J*-8ASOFPQb-8C&sT7ag9$QqB<%gPxIq-Igu8>K>AY7D0dAbQHFe$3j7f;-n5|CTm^Wqq&nwyF>(?N zOGt^VerrOY0XhQtzDl!o3Na9ziq3`IA`RJ!efK%_TOhD;+Fjo)jkp=YYnA5T|tF;U#um z3H0Q6nFa=iXkUN3P-vezq_rv<9Ap_<-eOiiv0v~! z*dPJ4tZ7kbIvF1xB&WDb!eK%Z8j)eIJ6s$9VpSQcHBga=j#n2=brKn8+)Bi7l%s1S zbV8C>SAs&YY_OVa2ZoR=y`v8&WY?*#nZ+ASDAD}-=>I9?D}$mAzqUz%r8`z&0qO2W zK#&G$mXhw2M&JiXqogc?ATBN4jnWEBEh}Bp-KDtT%m1yJXXcrCKArnI_chn~cB(P1EZSZE#%YZst@B%7>4qO~t=UO2Eqm%P zeaDrH!lO-I|HHN`-y@lo^~9j@V5yf>jYu>vlfWteSM2B^ zl!huwp3GRLZ?JXChp1S-?HjqkzpvQIs4b$W>zwGOJUJ3)#Lk&ddyn2z>rY7pOl$wv zYJ-VX^iCbbxKADagjoQi+eP0AKCN@h1@{W}Lxt&SNeKaQ-u!MVQMKs5k9AJY5W&*l zS5I3Vgk@XTP^D?IT4C-nV|xDe}kIno*CxxUsOnvv8ED3zP9#o5BQU+IXSg*uW0X5{EFT zPeLjL&#n1{VMHNXS(o7ZPb{g(-y4(jGH~X{URK`U_@AAaoxNBe$+Cf)y4V8J{`2G= zn`4t2qL=wRYqs6l?u_SG+{Gn&!`#KIJeN{F^}4?}U;kaiJF)u3=-bZ(=Lx8*{>_+s za15F>Q%zg&HF5*;%L7;Q^N*}M%eCWkD{9>9vTynIzl@igh*$Ik?765q&m1^GT07WX!V+vpbCgF1Zig&GLras;2_()~WeI3FH?-#_`%<^6Y>PRR`m znN6)Mm7Z#VnXJ|8=&enrYn~t!l)MFRZ(Fh2*dVobg_&^`?OP8-5Pj@T@h|oiq%N`TQ39A?(stIPb|IHCyDOO}FZV=jMO(m7GfR67+94d(jXyhQDHDVY~m zLI1VM9`6LPs2Xy>6urLKAF{RrLA&m3T*2M57HNTkQ3xD#+pg!>^)z>kmV_^_py&Rs zE+k0431jic2Ib;3Bd`H2tJ^}t?rlQK zM2n!Vw;U@>$svM~oYGUpBV2{kqKr8uNK1u&8bBq_Ce*CQKV0?8_}^&4GI#41hi)N4{*{@uRvKCwzvcfx2! z<@)b5yRjJ@uxWm!P$$nRc=2^6jFGCaMupFtmmjd!`4wAu z6N5Pt4<8K*E1*guH)~B<$IT;0A5F_BxR(II{z+*Xz&DJq{M6ZwCn`>B}JV6wgmj-dJ1-~ZNIl_#fN0!H>xynh`jnU`8$4s>_< zI9w^jOlX*?_5ruG?{33>2r%~*OsAKJB`#$1e~DUf1SDE3C#h}LF@0Cxj4;P63^Bu! z&s^3IwOI7C#&w6zXEV>o^{V%P5q}&t;em>-K8xcW5dbPrZsL$jKcsnelL} z_ACgP{&}Wx*1-jIx=k22&%Y9wm!(bJ9$Bl9h}g0edz#&1|I9lKMGK(8Xt+j;d+Hk>goK|J=2tEI=5u0gzI(TXFMbQMDBFlk=c(= zIb+k37$s`jduWq_R~_8IKFBUY~HA5k+R6CZ=KLU3T4Vj|mM zj>{ly+J8AZ4L+6gi;Dq?= zg(;uV5|6;2Rf)L&O$HB^ak!fPk;{IkTjTrRFO(Rv)D0*0!=J3Sn9e9M42D*JR7P%s zXWeQbQDT4Ec@7>^(6?n3W>-4x&vwm@P7BHVQIDbxN<_-pWPhG1ICyC!^mM)mC-`27 zYo{L5N>XY7O9nUOd}l5m`vVjIPSW*T zVNQt%;AYtI<;`9u%jHh7Ji=+rBqu>xQa9JvfnI#u@+FC<^xlX*cpMY|NPMq`Tv)%% z7-Dcb#6wRngKTJGFVf23{G5+gJhJ=W8 zc2*7;AQBdm5Ec@V6cXVT63&L?f@%M2=2&1*PQ~#amI6x#L{hXw=0MVJFX~MZWRsMiPQ4fTM)E~r z30~4+SGOYgUHoW1@;62f6Gi&SAt$&XO_p|p(16)Fk<)-#Ehb#^5`)9X!G9&@F&B++ zLQ9%6&8Gab0THJbR*m^a>Ufs(T@Ya@8Coz-opeAX9T)4*h(lCW@hL#2-S9$ytWZOs zjyhKb1njZoY+{G7V-DN52VZQj-`@1^1?tg%CjWi4ew)vddVYyG_633p$?5Jxm;Q~h z4FFx?vf}$}J02ix>7SnnnYm!G%_YUHwavcjn({njx7k=Sor!>GzsSPcDBrdS z0dpy`^7{#C>UUJmTx#bAbmi*rU(Q$L+fn0CATSi-`>71)>G9X2)a?mV=HqW&_eV{y z<*W=mHRUe$Yx~Dg8~6K&<2C=bZ=J8M4LFjLr28^Z!ZuYKC`y>DS({jI(vsej`_m&I zb0K3s1^!t64xwx~9oW4TOCvTRk+fVn9bTpU#8xW4uG8hYYhofSt~l+=M%!XMwtO4( ziuM%MhLHo2tcld~&yg7`eGbMXmA)WJ8f;BjcTCkM8U)tq5B~-&M>&g#+>Da6E`gYR z?iha(|C`IBi~IC*Uv^w$sz~RTzMvk!&s)TL0%M|UojP*Iioo4%#L>QRIAkc~E3ABA z{lS3&Ma?zo&9B$Iq6#HZ)!5ewVp^6zCAzg_f*J~*o_e?XsUr{an)M5dsA~#jL*n~zHw&_%gmd&cUgT{5Bo|VIVs0s@HnIx?(g)I zZQ%8kwum=sV|csmU!PjtNn8}7@%enq4nGy&n1wpy?!a%Oo2AM%31vCpc{11g)Gn98 zw^C+J`UaL}lkm@8OG_zw1ruFRE!k2^62uDsDz|P3UG^`itYg0`lDjA8&aczs@kXNO z%(yC5R-iPEI1kMzjKkd^Zj417?%&gk5yUrezaX-rAS#-;8UT7|9+-zxrQnDes z3@LcD8j=|xZ1mKT@ghJoO~aGUL+VINrN5yao2?I=Cxk4K#?cz8aIDGAD14BjA->R4 z?8aUg%KR|OlF;2zl5Z|Izrt%Jk#OW7-7#7l&z9uejp{X|JzoH)(ZI2F@JjKOMPX^n zS~Y{W`nIlvxy({r)<`MS$ko0j=~iJtef*>6(`{xOx5Ud?U4UCR9LyWC%HFg2#+=~G zRB}mtgY)4+gR`~_kNEafoKGHKwblXgT1dHMwlP)+msDA{J=6J-<{@0JG?agf#nr}- zbPLyAs^T)%^t$4z)Mr>u>L-Evwr$uy7yVMa_6Idm86H_YoW*DJe)txB&D;uR-k@wx z3g!4N4;WcJ5HBL%B>!ICp3}9VQaPGwa1zZtNzLYU}z< zPN7A}R}EzK@5`l?&}K`EPU0vsX&0k6Bl+AFX8En}Rru#o@d-siWbM13ju z3}K&{cjKIMTMJz-5a@2(YYijqM<#=*%82QxgZgA!r$-%n}%8!1=G2oCn>4jk!c zs_b=yXZkS~y8etW$^J=AuZRonjJ<+H1q@6Ya|m?pnyoD3ySIf>^mbr=u2hZ_3_8!( zkLdmC0gx{65LMj}nR$=ZfAzCnwy*1+%B4|Ry^Fs~4`myDx^?%rPIC4nsBHP@juh5RRJ;O*@Xkr0u3pc{Y$ zHN2d>ACUC_MJf;rIcW)TF<}uQDJ98gqEaH#5=vrX;zFXTLdwz-Qlg^DN(zwwkwU8Q zQ0V`eFp7&kJnj9*e25VS^)Pazi<^+-U%v4iAEh!kkEO zswgR)VosN?0LG;{V$&^*5U@q0gUpyc$So3$darwE>Za9+J(xY1ERuirvd%qjVRDUR zj2^SgRL7TnxU`n0i5=p5Fnm;EuvN1)oZI>Vnv*)DY9VlqV|~CQ?s-u{b3dEkmv>=g9T2AoqGwDgO z=}gn9o387h8~Z^s$1uloXyO2_qNK5HWm^_Imk9OkVCn zJV2%24;)$N#5~9<6(7fq^;}L<4HVTu=e(*Ye266QQ109zoHiGA5mgk-rZInTv|03# zdC^;qvHIu?A{RlC8wIfaA^ zU{=Nhp8ZqH0?acQMI?{MjzKoub2NNo$4NHtgxr204{LBrS(jo;=B?%XeL43OpL;+hPb0ek+EVOO08rHiN(>3Lhna;c3(;1(LWyy9lYrYZh3`Rt38 znr(*wp(f`*i?6SEp%ibWX6wGqWOQo8WWZ&~qNCE0>paWxBo*U!G5UDxy$ayQw|ggQ zEdi{8MxCCb7};R>i&%(%=b<*Sv)05_n>)q#!fspMawVu@W3NBYIZKOVZ6u)Vh%{e& zr&O!#lGm?zkkCqjTh+JH#~gh602C(RX9#=w+W_sF?S*LuDrkiz4&h}hvjAtAs~rvcmeWcu1kuc$XMvMLJl!u-5%-G3jjBjG{uc3z7S=+w6V` zZCWO*>!$Yjg(n})QmC-9a(5g}&e?vElh!-l5Nk?}4fsOe zf~_#c*6`ceQDhphMi}yr$nJ#TgLgyS*~1J*J6uVL2F;*y5jl3OQNEf&W~En!b|SQ> z^cU>C##yBF(5sV2Ua8EILSp79%_god`e|Bj4y@M?3FwDlBPU=g$4#XTZ|tN46Z=(H;6e(GCY0-m-vxBRvEoH^`6 zY*=7;CqU}f;k?;+HznfFiG4^hE9`r`UAvD0%z1qc7LrnCc`FB_U7g)9W#xyppOZ@P zD1yi%u2j*Z$#XmU+RWl^{Q=+0~8vI3RJ4Q86Sja`@@8+6o+<(|U(3VHp Va&Ryc(L=)#kp{7|tLi-m{VyflNo4>4 delta 19827 zcmYg%WmFwYux{88+}%C6ySuwvut0DK?lQQ$2MG`e?zV9W?(QzZ-Ge^PIrrW7e)L*h zUsZKg_w2Rj>+YK2Ik9vKSHu`^ZqBNtA-zX|E+P2#{>NJazYYkK_<+X_sc0Y;}}^jb0!&o}>Ug)+D4Rv3EUh zomx{2|3967T-lOPnmE%$CgG9lv~F5WL9A^5#sO35KzxS=v9SDO;|yZu1alcdxDmg1 z{sAohbpVt5LD-}vnPXsr^RPfsAPAPUE<*^`G(%jFB#4bO>=!Nw{0kTI`0QQyH_LxC zy#Kf&gW+JI@>|oUVvsP>E&@==(l&db#6WCZVU)d4@9iU)vKM-A;2lVt;y_^olSad6 z;ioa+f{@Y(y$N8!QH3yYkRTS0f3&P=aFg)=P971&&J%`M3iIAz-y0klu@rvz(>wUD z8Tj1{1s%llUn*g8+wkuV0W7zT7+MCwp0)*zhXLM5K`MYs^2cLK^J_uo`DZ~9K^$CR zPZ>zyrwn9Ol6T?1g#25OmAybLy#GipAP(*@X-*WdG$(4RLRz^m1REG75OppzX$8!d z)?$zTe+S|T(}zO?>%*Zln1eX}O^O}dZjbKc4dP_~Z_$;G(e*!q*!~@sEp7b*T@1v< z8Ag7I4ko|Em~eR){tf?+hU>paU}=u&;h3~}&6eh3iv2%NSuTO;~ zotQ*qG94N!E#uagrR?90)j@Y+iNMDB=ozy2M+`1&67rgz5uZlkoT#|dZW@YYmSF@Z znZU_$*!+~q2z5q8$pWqPt`dx1Pa=qY4!C0(T+`r~8Z$&V9MG_#$CwrB`i>wi8fK9m zl$;Du0UMxBO@1H6nz(l_YZ)RM%{j=kz?K>;g=H7K7%J_kXE{xb7Ztq| zEne84o6=_+Ek2=SDo0$Ab?G!rn`pL(3?4P5Si}xLW~2pnMcQIos^M!Ocw5m=UJo=8V5gFe0LCBv4D(b=?%(6(RATh9lYH$AWgd26A`s2t)fS>UL3f&B7 z*VHE+t2*z}aX$2mm7WYPb+InQPVi7JsYV-#M)r;qrBYtm0?bby+RkW+9@lnsaXnR& z2dYol-Yy!;ME^G^*q+DCIE=%!UMnnaZ`RM)Vk)wPut04qTorp%| z2ojO6si&t+ug#Of9z7QPfyV=Xe&U?CNsG(Ig}0)`m9+|KnmcQ0n%F25>9A*SKVJMb zM1U#sZY-u3lNtK-IPWAae|w>9z3!Emzmz4L!D8%dXj_NHQ`)e8gee3CR z9g|o7$Yt}1rgN2%OWF7XzEcUAhvV9r<1}Mfd(^8ywcvsNomBcF;Gy!f!^m~YLkzm_ zsFAR~s?u;&yVG(x_DQgfB(;{`5;)Uz^6;*K{4S(gF4gUNpj3`}jZ!0IP)^4bsaZr~ z;@hHw)H`I-_eF(HNV1F+>yX4G8^M%pJfueN+c#$p1E>`^4|NU$ghoU1HnT77{dz%l z?%0a~wcU7J2)029z`B{*UV3H42xR8LFS*R^x5B12%s+4;i?$e;@O!db{3R(DL_6Fe z=iEWoKI%pp4AR%SH+R=l-rJfz{a8NG+NaxXiXc#5vV0)^$gueWJU^d{13mO*||q^HJ?P{43efdQHq$I^ccsm)g>plLO})c^wJp!D&Dn}tuq{h0(Facv zy0F*1ST2!`cmAaD;4{l>FZar_{J?$9@QB7nJd9Pt_?M(qpD25Wx}7a_5B%-Lmi7rq zewKMU3|oo=72?$Lr1?b!mR(6!9K>NCxe-v05#@#)updw(5vo59UDno^DSn!te>`1U zUEM#*h?cpKKGzq-sAgx@&Qf+9vUKN#25wfuJ%K;fD59&4H;(&}g8r2}XD36C@u_fX z$y;7XNEA{CoUfow$N=1vUm<2)ZTJuK1ff@qJlWJlL7K|2GV z`I2D-F`7Jf_l!>MknutQH34TDjMT5)J|+ZPp2a7|2M~bLz-|Of(^OF&o2up0bu{qs z;pZPnArb4$MteuZ*o4j?vChfBkQB++<%r@6JKOm@5#}8D2T0qyBc3>42UmTVyVQb- z(YY8BEOmP+102V%0+)@P8{n3{%S)Qxz3>TnJu)m3V&Gi&B+yw19O$i7_UEzgiM zv6IC_k?--Q&`j;EC|HL1!HMRXgU)&GynW>RU1vU{U@-79lWzT-vtjA zKd10t@{v%(>X!);J*>9%Xis3TUX554z&~T3H>yC#$7Q`mdlra{>)LdffI&odC%RS* zI6ouADnlSgx4%LzAE@*#%-K-P}lzqBD8#N0|h-@Au{a{+zU5R8!vX%>-sxgSi zP}^dL!X%mlubqx+-VOTxMS;#(izI-f15+kbr;rvA>3TtsIvod<011VH4Qgp8ni~}+ z;2_*a6c86coYXPkl=?~)BM?2aJZL)SYlkap-YsFIL5ZYK))tHQyCuXpIqLo*shV4$ zB*Bi@`|yXb3}r8g5CwNH=~qcA5p$1lhe#wt1$$&S(`q5YU>!ZG>p-b{hRK2?l@la) z4B6aWvMx}v02E6=lj}rs0fVcfQ>~7WoJDys5pd7IX!Lr=|C?S{&}Lu|^5XGmtyMe9 z`~F*S0{0T?cT_|>b`x~d);|!UrVhuRq##xuZ>GUbTn0v*1k%wA1UFB_{yK6-V0u>S)80UBo{?BZL)x}S(cWTq}d zPKA(dB@?q2Nns!t(o3z^_1$y#o++O*H2qkxwY6GZjCT6%Gdfjn+1bTFT4ve(5Zd{f z)qFqBSbV}q`F`RtI`%K?7I}lnXOjz3h`_vI?LrW4Ij8#;E`yH8_Z7tfc_#JslH(Oh^YV%;CN zUjs{jiR$D@HIFz2X^;(I|?fYmP`Q8I(j{@dLy6lfE}+AZcNOgdnyueP#~mxfm_C^@8^Zjmz=OC+b=m5ODNUEK?ZVmjPUta zDaB^iO^eWG{clqO{H~jzH+-(s!`gw}(NzKS zEiF|^8Pn=SGg?MydGYCzk6#XTkEb48P_Kdnc*Ea_l8e@Wjh>79mEes|K=I+_)(bMq zVuqT?eW>%`;W`5b1FR#|>D8Y7DBNlA)}SM_sNbP$pv0b{J(82EoQg7WH0!wG@JEQE z|NDv2tb$71Y-igavQUG<<`TfaRXE~afRV)B&=$S^?Th#8;~Je&R_8{?TDfED>jB?d z1ox`Lsu!uSXHRw=VAmeP-FQBhOoPpQ{B#1O-sMdfRVnJ<;WNf9r%Bgdn#& z>iYsK02t+jXwsI=(TpzLTOn5Rc}r)9eDwTs>fIGBJblYwJ^N|I4;%Ugflb%`N17uc zE=C%f4g@9?8#@pCe}B#GdMmLzTv~$PvEsp{UtU!+)=XB@gGPQo? zS_=;{8?RI5W*Kvq9IUbrQyKYF#7Rosx!SGr%Z_g+akjpebuPEZFP$uTHcYS%fo`Aj zGW_JKgb|9L^BOuYYk37Yj`;u)o2F6xQd|0?-Ri}-wpEhlJuEC_#f>T(;f&qP6P$j{ z?_l0?DZyvL)nWikwrHsZ(iQPXbnfESZ5;UKGM!VgR+7<(OCu?n)>k8J{%5`zv#z-^ z#ZZ4Fw1(@3E4k`pWm03=+NAwmpVG)-xwg;Wx%n&NA+%kNmDV9+S4K?QG)>t~C|SB% z@;SSslY4Qk%Ii@yXij@s-ItWdyTQpDvtMIuw8(2Le$T*a+}k1K9LX>t&5GYoORt$s z9%k?u3bwQIbEdnafUzI~?oN6v+;;d(vL9L&QA9LhpPy*(N5_-)T~kAIS;dy5TRwU= z9V|+oqK9Ra9$wVq&eG=MVY_u=mWcun$q(=hmTPIohNx}du3gm>J`n0?y#EUE!EMMH zAL?1cd%^+CcBRPMJO?o#Nc62W^201!PwjUC6WY)wPwKElIVeP|(c>?Xszv!*NZJND zfI{?d-Q4CwFP}@nMUXnI+3pQgLO5%Q2$_hNL82lAJS7A7fkn_hEoCf3icf#VQm`6P zaCI0m54V9ZQwd2Ijg67)i^oA1E0NkjOx@keQyH*PRqnVL$Q(SG9;AaSanGU_b-OdS zZ?r@%N%7395=Fgm=?EcYLfvIi^WbtzvP>;i{X-gEv_Mc$NNHQGKRWf+>Nh5I5RD8i zt(VP(cKwlZYxxg7H@xOEpz(w?{P3x1GXRa2<`PwPEcEQhmps$OGB^Y1oIJnzImYfm zDk>m8kMPozJ%9#2*HR`HLnE{0XZw6zhupLcE*X6>p8ocTov7%8o(x8MILciITegWi zqC(#SC_VJ|Vc)x%0eT;Q&tQ7?AL$WT zxm(e0dPp(U!1vL5d)n&aF-02>D(qYIVRB9(OcRRD`VXjzW)}_x7L)dS-h2-zcAeMb3bVInkI#V&d z4>>HQ%G6=o6K~=Mue_#AkF;a-bQt&P$E-b`+PBt?96eIn$oIZA3wj*4y2dc2s15@= z10wu%;B06Hn&6rM8$FeU$JaBAhC5_eZKyPHdO`*;Dj!u*4OJ-Z&uq0|l}ZC|vc3OTZO#=Eh{N(bhY?z75uc zoJc*AoW<{T;u(>t;Lb2|-KFLl>`|J2m^G9aJlJJR4YejJItk-6KZrTxF{39cT4kAA zSF>1i=}V~BJv(Ojb+bD4L9q&G$>)LRmn6q$h;C5DP~fGkQ+3Df)Q1Y9eq9Cr^lxkh zCJq#xnbDWkKFArFCN5<(Cfx|(G^cM_b7}g{TI?D(Zu?1pMnb0KQdE+o6Hqx)@^u(5 zI}I4dN-QL3r6G*S@tNl?j1gulT_hEVa<1?Gj%2l%&Yi`25>dk%=|oO!*@r_ny_riH zG^c6G$iOk?ExloO+o!h4EpZBL6t6FvOropURlj*byI8X4U4SNSj}Qf^WQ3dgQ@eZF zl_@WGDI|TqP!vkU+K4ZhLWfz7$5Z1cQgexuC5!Vl^%>v?#8JfN%BwvUR#-^MN~S&L zzpVGS?FOht(X(l^-(o)BKHA!^GdWf)K?WU6Gz&bs={rVXxgY_=&#cn}HW71JLfOqmy8iPY zbZ{DyF-8I=357|x{B#TmFqrIN$kru4A;Vue#nmS zD8Euvt{hDK5RukR{F{a8Cg4j; zP4;WT=IxsjGQ@5icGs7q-^$PmBRHzjJ+$g3-gxG8w)wUyljF4_ z#tktWhWt61I0MP+i}O9X1(GO=?ct;mGAi)Q&A&~Z7pG7UR$U@(&Fwa{Xk1oqCWk~*UUD15|VC`%8 zPPT8@iGTz^>^WFY^oP8@h#2Jfjj0(6cTJZ_F#Lg^K0rXtYJE+2iGZFWRZ`7dk}HAZ zf(9N+s@6p6`nx?FdAwRQ*-0WBW5*?nN%hLeq;T}jLe16hu!N~TeC@R!83`1M#F?IO zJFAzErTD9>M`wHUZ`=Xz2qTVWq8ieRK~Kz!%C6B^EIb@1_L=C!%M6GLd-g8;E{{{TmJzBscs2?1sm5KPP2!Vfx)yZK9c5SDc}H z9^0`GeNC))%}E?Zw%Ost@S!!fm^Rv=fMYSnUWwvAO{$rka%bA$Mdw@ezN)_W3LM$U zF;8msCh~&HM-A)EWZXidC=V|RmP(ehZ@duti_a<%RHE|xjXmzi_U6dR=~_FbVYrwT z72}s?T2+8XV_0@n%aO$_`fsZxvC1C+DflL)fGA6v;JWK&z!;v{^xh(80O5UBxyx^7 zQc+&NY;8hOhJI$GN4;xk3oh+u7tw4FGdo)&_i;{p_0O13-mjdGi)j0K% z`XP|DVWixq?1s;5#IFuP+yMRKsR5ZcL&PIIxo|yMUZR=lkRW*4>VwcGa)~Kl63y5C zYpqR9mJ~I?V}Azw<9UK_vNVaC!vPM?QUPdRh|cfGH9XP%nx@5Vx@?zsJy!^H$j8U{ z2wmvfFNtk=%xGCKX#1qa)!=tA3y}WGw?zYYR$DY4u#^{MYxgwW$r5^XQ*s{=f7h0Y z3r!>%B-nBnb%DsgupRMj{>WWv3ds*-s+W48TluU|X~HBH_|@u(Z~1oi6J31jsM2f? zpSa!Bm)0MRtR+%4o!pMfkIrJUuZ&Mc1|?hZbm*a>uh=OyWy6LO3FO za|c{pX8MY6%?gfSwL_s=#s)|WJ(t?12YbbUjU@L<=3dLSCBzfvwcT3|9H+V*gs9u~ zz<3e@AAx(^#LN8i=Y(-6UX$+Xhg1k4KkIm$HBvRge}&t>fQ+2ZR-CEsv;{_8x3jd`SYnxPc%`&7hIu|7cCbnpV ztufG{%FTAT$+|0)nHGad4Lx*@quleT4%m(b8!^(wt6Sj8{q46^8owm#4~yY$ z5(?++o*?e+-oW zs?j0Srqo`x3!@gdm7mGP6VZKB{wtpRiYgwTB1DqlM!^cGIDGgmGAH1f z=hIFS*cLqO+~>F0!WQNkEK>Z%!b<;p5Kg@D&la}*ZVj0u`^42%`+e07EsC+*w)(_K zq-98NbcQu}X=#L^1WY2vz4l_gE+rod4Lczw{^BSGUa05?$33S06{64*G_v(%OH3{I zl1)Yl{h+NKA@&Ws*^NB_hdYtw)?F*wn%-R6mMn0?#Ejf{U*hhneabel`^c=O0^3U= z$wzZ2wN9VHBxQ8Qsms>-N$rLmo5%gYvF#x*19d>R)-y1zAjTNwL!*I7Mc#O#r8@Q` z!;gW&OI5Cpbm1_Kz+X|enOcxG;w24eC9PC$`@DX*`_$b2)Fa6NRt9&BxGx$cBP}0R#pcY=JmC!s-IIr%b;@G{{HhT$?Xy#l-911MO6;MZ=^j8M zN%TA^^pQV#;Sd0(oE%rsqqwS7_x>FHsiFw@URQt5&zRD9+yHm)x2*U}kqwMb79T(S zW~`CavWqN`zN40kb7lwG#(ug_ve8N#RUEz=KZhWN&)ecaVfIM$C*^#-3SFkVd1#@Y z(EU3yd&PutmyC4R;|!K{D3n$=uf zTP||+A^h>%PkIKg*55cf!`Z*=g|fg0V}-u5yp|C3jGO%+zj@Hv$W{lJ?T`u@&3&Oj zFor@2I@%iawJ=jbiQB(#x%UI$fA6&+*?IrHW~Kr9bmi?=q)CC2fq4a>j-m$Z19+$pWIauWyKwX0-vFz$6(?S3AL-r`uakgg=gF6 zzQa;z>D->TANl-kvnW~usqSFe)V{deT9i5X-#LE@N1#L2X3WC(6_ifc%oEr-+ecVn z@E2&C-}XNx7IWfF1ItF!s0PVApz;miDeC1H?$puX9md|Nk2~Z?@{g?{uDOZ1jc(CZ z+CTO(2J3Aq8%0Cu1B>dYmR+*v8vh>emo5TND(++AYt+iFIGBfNnvWx$+Cw8=y6)p- zCSS1EYFKn#^+LnWH+)Nl7@}65P8(&mzkm(<{VMP=O_Xt1gHm!sU8Wwo5z9#dL~U$R zxRgou^h=d&g3)ihX>ea-*!H6IETy7Nezc3LNJaxqt=3E!JeBa!>&?+ttDGlalRl)& z(Y8{(Iec!Sb@N94^DQn-XpAkCWtfWpcMZBoW+lb?l7gP3$WcJ5w_2RXp8+E!->nW@ zu8g~3Xizos@svx)=*-wEefDTBaE&Vw5EL=biPB zQ$o9A>fC;8@0`A%+U<`_!ZZp~M}2F)n2`jJfj^{eVe*Vht-OZkwUMLcLM@sYMX;B) z3xwB+lm_R-$2AzzN^L2gzymR!eSX_V;%Ar+`ebeFF{&bZX5t^1l{0GVi8Siya};e& zj*hCoqlH69en;`6UR1*PZDr%uVXQayF$g>Buw#Eiqk@fWageA_YJD0L8t3&W_+~O% z$-BJQ88vHAc;!`coIEu}SdfI6b^_a}PB4x{=2^Zgr)GpCaeyjhwvAubw-mjyh|g1ZI_$-&D8uH?mSkq2?S zSC?VnV}kucaA8E=aV~IpA1*O@8Y%@YCb(rEmzw0g6b!rrjkn1JVqpXKapHlA{<$}N zcfxA?iYM_2#KHxZiolyU2C?vfiI4CCY14!m-p`hqkMPjIz3)TQ|5I7^9u^yXA%s6f z@LneNk1u#a5C8jnRT&r9+7lmv1H{VnpG`~!J|ZEAjRl-JgzqKAa&>cm90q)c$_zekS<4!~KCcsP^GbE@3%jpx?f!?d${)NEr5b3)C@BbRq zf*VW-zJWmOEdL*LZ1yk23jSkqayS9QJH`h79Yt^s`mao_ zH|NRm6wg>fA3d0S^nEm(gOOQozf{NCNNAAvZX z?}A`DFtBOZaMW=^6c>9HyCYv4eHN7jp_fd*dScrlI-xN)zjrDD6;X0n_B8|n5tjmOlHyU3&m^Q=yvfRM zulI&;C)+0%#%!nSZZCB&Fa~c6Z&&Bna8&&y)ZQqj>c7pDxY9H0L%AXH@9g}SbM%X_Qoyq_P2bmW6#6vx0 z<0Oc=;yPjJ=j4xmB{*fFYuwHCYh9_AZ(2f2&**Z4ymrAeF?JaKrZLQ=br`DUN#E3@ zFBLtdv;L^sM2E(wf&%i-+Cf!l zwTj|LC9_^Az=Ge{W7%XlwQD^(z0mGs7Tqe9TFj~c)`Q|p=Z8Y(XjgeqLIT8o+ox*r zYP!#7CtK@TvMZpc1h~&Xw<>~4?MetZh^fj62>0Fo4kQ9?B^U~xV{nBY7}PwuxOgvj zd470JA?Ns&NVgmCQO~ZyS$hQj;G~2U%-drV&&;hNz$Xo<4QsfWM~pL|I63$A;t0*K zePqd5W7CHW>e}TobvV!=*a;;yg+M~-vwyXt?X%8o%q$3nV2>1VN~aU zXAkf_kV^RXZ9<8}fiZ7;_(D4V<4yPi(kut2$t))2b1LU$_$}pdXCPDZRJh!u+^49Y zflF#uwRs=Nk`dqHrJ^k`3EHsg%hWQ^>m01RIBsiVO|;{ay&hl=bl{R3Zd|`reCjuh zO>UGrYHD0p8_=yc=Cpp9QEH%@#?MGPS1nRMhyCVu3KqWhM26@h6jXxwUPm%J( zHY=BhBYKD;b9Ow4+%kpiZV9788OvuIwWO0+D)niK2VYIPT?OQ4$%BP!|uWzyD3m4_Z6f@|5u50~ zq`8DxRuO1EKXV@yRh+(+s4ZSPidj)Ej&1V)OxHpD1+`|9LLC>0_C7PkI2wa zC}JtqK9LlqkVlPAsaOSH73zYi!m|$Plj@e$D4mjglqC`Mc|J>`@4`0Gc{_5EeFzst zUjC>Zn4&Q}l`?2YaFIxqji#hNo`g9t;bx+&986wzfB^Git7-auR z)NI7!7pHd1vs4ie-VvF2egQzT$#=sNrEiA?&)M_yWdsG{h6P>)v_``Iy7<^P)2c@J z9qh_O3agJrOx|Rvy`q6#yUi>94Usldl<=22?+`c@1o_v=MfNDABDPrL>l&ffzGvN# zQ3vk&Z;;$xviNcucFSJhQ#}_y9AJW>%OI~uRFY_htw<%Al=;o_2OY3%_^Y=#AsQjk zK}VJ_X^>K89>)f~?C|(j7=ug$8#HmJHKNiqfzDOQ zWBYj^cU|JqBu6OhFC9^iqeQ=W#<_V6#++&zyH2hM{%7$^^t|b3+-%K8DnZb0B(6I^$_wS~ zBd4%-hP?TfVS~iZLNShplHVMW zE!SIA4Aj{j<_u2L*ClQL+Qu3#?R=w4JM=NQ*D=J3e+0S0Xx1l2|MOm7^YoRqi501A zi_q78FCan_$e4~iLVjA8%@FXn8lAtCuwPhIFJQpH4Y05-xB9D>`B$4%NDiNABJs-X zQi4wnUOi85=mXLI2E)b7o*n)THmaeTgx$(|6t8CLU*e7F@#?Ljt!(|WEO7#|6wEh< zUt?a{iHWPVmK}6(smZxYuc;yQ*^BhaB4=AXRZoY*KqJ_OcFNXXYztCe_BxD)b0To_ z&-!-6N-+<%Aa%)a6GN>*oqE~PHMnE5K0>*DbFp|ig~upfN$scQVb6-#tL(;6R2v%u zmFSXrGC8cKyiavCK~vUBQV$5_`q*Rh*d^;_t7b->?+OdBss&%@nE-g#s|E)#10>jKjFq+Fbyy{nzRcOPGxI)^}MQF@pTlSp$ik(OK91h zIw%KxA5uK``3+K=@y?TZn(iQYCLheHzoSkI18@fRXX+4MB90Fq*v+WE;-08qW?uJSb>Dsp9~j+LJvzR12%p3~$}*2Up}d5dK0ldG13axQ z2f}wbkIFy}kdbva@bY%kc}xAW|5`5$2nuE2SiImZU%zGv&-_pjQfP1|d&*K~ik*mk zjC!r@%z6&^lQsa{rcv*VD8JO___d@fJRas)ylB2jzgfI#zq!9{4D~IsZLBf*09Vgv z-F4i;0WmhlS>t)t3@s0e)V7EaV?>hd^awnj6{qYuSRZFt+eb;WyH%(dfo? zbWbio)N__WcG2Z~d2P}xv5@D>jnJUi)ee=#u0j4t@D*0ebLtr<%AdW{`04>tTOX%a zxMkq(?{$-KMwY_B3sjD>id!l0_*Wrkf;p~aYJdcy@qz&A1+wLNfz<#ytKUUn7o{wl z=;3liAA|hZBieTI`h$)+qQJO3b-d0eB4^1C!;oEw&PbfSyxCB;}q%vifw3cHmPZTNd=)s^KqH^H>GvT+CO1?KhLp%dP62 z@XPUQ+N(C(?u9kq;XsjCs)OUGo*PaRf4Peljh1iGEeI0_vF&g&aY< zj;TXg%rZvgt7p5-IqQJKJsIR(6p~Bp_kAgRM|!r2Ou??M$XsA~MB{X~>mKlW^?Bng zS&skuSNG7kwwb7OTB>wU$eL|JWTALv$nLEk*nbUzR0ER@T-9MtKe+JEhQ!PY2LBu{ z%?WY!)44Aj2x55lNw-zbMER_`ldZpSoMW`%{rG`__rXk^Ch0M)s#kY?_RmAeqx(ua z5myl@b<&7D96vB3ak`s)b}rn~Il-~}XRn-nyn=mIjl5Q4wo2+i*XM`iz6E;P24blZ zW3%7;QON?f^3>^Wxu37&#SNCtOd2z{rO?>&$hobt2%baF@~3YXj^juJAS>qt8a|D_glIda|k=4!SJL{LnziQE1lY(G6_#@YYh_*PV6| zm)13eHm~P{p-g~Cz~eGlILKBe3L$#*I!3aiR?pW^#(1cAH1Nq>-G_rjodm> zN@pM7-~%}FeyXKut9iZjc({3duzWmJGnB&GOtYv`O4ubz2u4Cam#3*POsx31yP&!p ztQvca`)7?+uU$y5kLFvPYRxfj%DdR?s4GcRMUFWl6}-wP=IS1-bta>QR$2zjDr-pk zb>>4}b!a>{_vYmAX+N8k&yc64`H0skw4TO1D^S~NTJMxo9XZ*empSHjy1?r65Adlv zQuP@=DirpQYw+_KY{l$Z8SI}bQcM`V`fq=QPGTqJZkr+}GxSzkWFv+-k5M!(Vwp4aqZ7dx)uTjgj;CL$rja8=zfb5X|U{43Vz<9(}`?SA@sr>8YDYbzxx+rA3?G&`=6 z{|Bh}xuJ*Ta~NHH7@cdS%l#AwV}3r>=+4<$PhI!DKYA4v@zKfLTUQhNjd?AbQ96w^ z0uST;>(lq|DUi8%{`b6(jhi(Mf(wrh(36j&1S56aR~6?|`@7$e-hM!Y42{uGToffP z>Iz$OwP5Kw88s>7mU;D+k)89ZnH3s!X`o3Y5mx(IT8ieNo+;Y;Wfiekkw7u0>G0q% zsj_`7`ZlvcCdUo(mBSG-m0&ggPr(kukLbHhY67VhM$2hUM}&Yq+Ewy6`Ua~%00W+T z=6q;7?p5(kp?EIB`5}nQju8Tm1J72#N;wJbAhT#_5X0bc2jEzuh#-9V*n>f|LXL)i zCykGK5J(L|Wm6`vHA5^;YK%3RF8G23GlXi%R)CO^m4%;i5>BFc9lbEvbx4kGE}0ZW zF&arFO)jbKuZ0Kvd?<(imVdzjh*Dv5*$jU8Xu)`>{$3WpM8kn_6GC1W-b+5_+>77c zV7L_XyLSB$L(&W~pdi;a+&Vwqwg6E@CKvT^13Dx8m%o3VsCob@nt&3nq{#ILUJQcV zPz+IsuZ31W#1}0yih`%gMaZFQ31Kl3Ni@i&KfdT95hSuou#H&K-luk|0;f9eV&p?s zbVQUvvA=T>Rtdc_WFE3n1h%UZ*Zfs9nfFKfeT;hjJ9F5qiP)kM5SGZ=iE2|>O+F2^p#$N_ zDdczQ!^u^}T$tVY^h(aTK#0FSQ6#94ewgRakNzetP8?k5v5&v;yDw7Ii_Sav*WPvj zqw$p&W+UbaW#;|+nHQ3r^>yZ6@bar$%iA@ifpdG2y>N%0Z!>B@(Rt@YsdCyrJB}&# zn@Ad;(d+Hjse-zv=ia&f{#$|M=mxZ%>l}3FZ2$A={x!gCWt46Q1e7(NOs9Uci)iZ7 zw_BoFp?kfbbRtJsO8pQE@Wa^@c$%{omdpx0VBIS}I4CRxf){W+K7UZK5A*XYrZ zW;@3~!~7lz9bFUf)Hd%X1Xpq)_>P^juseU;gK(BRC-#gw5fnRSZNX!9KP%&6Xi$vO zrX5PHXq+9^f{kIs(+=1E+&$D7*_cpYV@>ikuQ!14T-*4&`!8%8gRtS&b5ChYo7f4= zY?K2n$+XWz(9|7;11N`=YMUK}T`07RU%sjw@TsgfteL$5S|Uh7`ie0GYIUE-ga~0_ z4HRS8rRq2Q>b&k?2ltGQVx?cH&FRzy!7L34spz4r zc*20`cxD*m+cy!`1=AC)X0~th2Quh!oy-)qx2N4caPo7N*W0Dh$2*zXR5A>{=N;Uu zwSL;wddLM(%`j`OR78SKNWq2X13XL8QkK75ZRlqD8Jt$meS5}Dme-DlvS(W?YJ|EcaAU5uO6BG0bj<(%{V-%$y=|vPQnvPUPa_#*;UA>Nk6M|3KnlY z9X3z!e*)+W7xcP2N9k6v-envr zYS%rPkk0J&kacXOzx$HTIJI|4R?0b8Xb9KkzS?wKfA7PgqrIDfMf0o8Rd$psy#@I5 z!{_t+_OEY#dj0h6-!Bhumpdwk6y5gCzn>nzzMVgOdiZjAdVTqz)g5Kr%Q8)}{J?0I z8Ir+)P0O+aJs?@$RDY%5pFTfb2Bd<AO zEzfkhe=)Qf>#TPxz`&N}Bt?c=M9=S+=co6})63(BZ#j2}!Z;-y^0FHQqyhQi+aNG3 zus7l&h@;jNRVSy+064)Nm>@Y zaIwTVk5i?95)cO-7ebGmvzpA8ub)4j-w5W1f0Sjn3I*>b6)BV|XEreizL-vof`IvS zN)RwDO~R^xdC8}wLBLDMSs|Vd->9<=h!>B8*_mSYKnoEiUcm|gO_m7OY?6SVC6FYU z(!;32-K?|>3zJoW#01fL5CmzVpdjGXdM60dZtVpDr`9h+N&rWtRY)e4g`e}0sI@^0 ze*%su7(tNMz7+%{oAsd=e8wggHG0^a7;Oo7j)KoBHTCzw?{*=h zh=PDo=$V3;cM~TF*wo&lzdSsid8Wxbf9XLW3D-V%3Lua3!7aqrY61Z52zxf&2&1>E z9pQEh*p6_Yotw(l>vh%vFvt5+Xbkf4EVMq;p-eXe=_qEK0j_q_4)E5H-#*Tm+}(AO z42io6v@~!_z;=L7QNAF0KY!vA%^y;loa+YDYU@WRIxpfPug#L> zLc%aaVQVvtkxvbSH40YysoEO%&CD6)*N@j2IC#C2)Q%Gc`{|?`C);~_?Zzp-EATp= zw|Om$rz6P}z<_L2S%s4O!wfCAYA=iv1k}Pf*>Usyv?~W`oT*ZF5sS?k>2+4Iq)kto z*;(3@wLoavdo@aelt~l>oJv1yf255{hgCpsOiqyYv`)?&#@-q_0k0LK1VO4OR|Q?_ z7(g3TFXYU|BO#}E6WL9{mBX-FN_!!1D|jR%SYE_Tg~w*2J`$4Ih(|&)8>1BM zgj-a;5i*t?M0weN-bb`Pl5z=ZJdy%SMNVAtYw<{$*2#^)vDsxt?Hhr+e*zw?1i%+~ zauNCh&mHQrW#?sS%9WPuDCAPn!LI`^=tb=tfrnghS}w_@j* z%6o`5M^fHMv|9&W(2LqP15eBw12TZLyp0J*Qif*_z^5K%KpjchGQg3PEkn2!xU|Nz z_RYZ4EsVhe{_^ngxp|bwe<#a*84e;oHSRn3AmXPb`_r#M#1Bkt5JmjV)i@49pd4JWj9-H zj^KZK`i*q39H%hnBK^oUedbq9=_juFbNNvC=!s8plHXvZAI0WRe=MJ_d=?PD<0;H| z*R^{nuuJ~1JlDeV1O`ina3-zZI31npiVIgjmwOH@yE>MeDJ=DYbH>Hv*5RI2+?BHj z`Vv8Tgo+-1-}Z1b^QG@#d9sA%K@$2B&wNwTX8hRvPv>`bmGVRU66C`39kFfueT zFfcbUH83?aFe@-Jmuj9Hivl<_mq4Z)Ab&MDHC+lXQ)zl-AT>BO3NKe6TQMLrATeDE zFH&!BbRac2G9WM@QVK6gZf0*FH8?XMFd$M2FG)loTRcWILNrE1I7B%&I6+21Get2% zL`5<;H#aykGBZO#HAX%lJVrD^G)6=?L^(G&K}JC{MKMA|MKU)xH#jmfGebc&Mh`w+ z3NK7$ZfA68ATcyEGM7=%3MhXimf33^Wf+IQcbX7Q!s3!S&&q33q|rxPIJm!dkEgH-I}vxEIvn260CT z_ksPm3%DbM2f#txEUtfvupS)3UBWdIHh{ypWgM$OA8&LXe!4#OrP2yBO=fMo_9F7VLE`V9w6i%f#2j+3pII1(qfs43# zoXU0qEaGxFDmz#Lmv9$xD*0uwf?L4R6~Se&ic=k^UDm)lZUaY01sjx#v(%lJ=y{*V z2lQJ^v&R8$D^7ncNwdd!xO^PF8q@4?K5iRM?M$=Bg}5C!`aBkja4JHL0^MK-t_r8i z^yMWu6`aNhPal`zGB|xDzdWwMWpNro>{?uftHJ4U+3`4q(;(CMB3p5YQvvIzlId8@ zPHn_{%;o26{;An_@z12k=Kx9ZRD_@B)7`8{yk7$60kwbOFN0P7MQz_~F8!)*%8ZB^ z0P$XP+jDXU`R5CO0*Ps%Sd)RKi(Aay{89hM{ELs7t9(r139h)riZ=BWW=wPX1u$!_ zZ~GTdfb;t@!$DT8&s~(|1}8fwN{8%M>!q z<4&`eZzz8p1Jr4J&dS|Axd||A!Ok}n(q-|u<|EiUK z`J@$m_>977KwHH#R&@J&IXWO-2h?~xZ^dtZqYspxH7nVic2@Sn&(12I`2PtuEOnnB zW08t*)hd4J)CX%gePCRx7NX>%Er3FgwgSpKDgb|lR<-;fX_uuTXJPSEx|Z6AiY#^S zYlYhZMIDs_$~Gzo6l+uoDA8yqpa`Qh$bc-UvFgn)bt}!u-B$hA1BJC3-f-{Y7S_)(KC@on(ji+M4DVm5A({eTlPgI%4;HN zJeY-*3vy)3lOC^`qai~Uyha9l?=>jcI$vsG%|E(~RCu!BH6An%NQGCktEbsmUk}(= zUuWU|a$U>bd-mR|YuRznj{8QywtKeSv+aMLZTFnDeiAeTvf$Oa>RPhk)wN{7lLb!} ze4B+0Kjbm6^7YQ8mn@z3I^IXXC>R4e-gTbq0N07F zBU)$jc`yU?TBi4~IiS;7$Fa^|9k?NjU{rU%CGP ZQO=LxmmsYh9t Date: Wed, 6 Nov 2024 15:40:52 +0100 Subject: [PATCH 02/36] Added rosdep to scripts, removed deps from launcher --- launch/crp_launcher/package.xml | 9 --------- scripts/build_all.sh | 3 ++- scripts/build_core.sh | 10 +++++++++- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/launch/crp_launcher/package.xml b/launch/crp_launcher/package.xml index 02f9695..eb7cca5 100644 --- a/launch/crp_launcher/package.xml +++ b/launch/crp_launcher/package.xml @@ -9,22 +9,13 @@ ament_cmake - - novatel_gps_launcher tf2_ros - kvaser_interface - lanelet_handler prcp_sensor_abstraction prcp_situation_analysis plan_behavior_planning plan_motion_planning plan_lat_lane_follow_ldm ctrl_vehicle_control - - nissan_bringup - - lexus_bringup - pacmod3 ament_cmake diff --git a/scripts/build_all.sh b/scripts/build_all.sh index b5d7abe..39b764b 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -5,4 +5,5 @@ while [ ! -e "src/" ]; do done echo $(pwd) -colcon build --symlink-install \ No newline at end of file +rosdep install --from-paths src --ignore-src -r -y +colcon build --symlink-install diff --git a/scripts/build_core.sh b/scripts/build_core.sh index a227490..100d7db 100755 --- a/scripts/build_core.sh +++ b/scripts/build_core.sh @@ -5,7 +5,15 @@ while [ ! -e "src/" ]; do done echo $(pwd) +rosdep install --from-paths src --ignore-src -r -y colcon build --symlink-install --packages-select \ +tier4_planning_msgs \ +autoware_common_msgs \ +autoware_planning_msgs \ +autoware_perception_msgs \ +autoware_control_msgs \ +autoware_localization_msgs \ +autoware_map_msgs \ crp_msgs \ prcp_sensor_abstraction \ prcp_situation_analysis \ @@ -18,4 +26,4 @@ plan_lon_intelligent_speed_adjust \ plan_motion_planning \ crp_launcher \ ctrl_vehicle_control \ -test_node \ No newline at end of file +test_node From ef17a5f52fafaad7e322a9f444c4c6d9f17a36aa Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Wed, 6 Nov 2024 15:51:19 +0100 Subject: [PATCH 03/36] Removed symlink-install from scripts --- scripts/build_all.sh | 2 +- scripts/build_core.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/build_all.sh b/scripts/build_all.sh index 39b764b..493ae2c 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -6,4 +6,4 @@ done echo $(pwd) rosdep install --from-paths src --ignore-src -r -y -colcon build --symlink-install +colcon build diff --git a/scripts/build_core.sh b/scripts/build_core.sh index 100d7db..ac3bf53 100755 --- a/scripts/build_core.sh +++ b/scripts/build_core.sh @@ -6,7 +6,7 @@ done echo $(pwd) rosdep install --from-paths src --ignore-src -r -y -colcon build --symlink-install --packages-select \ +colcon build --packages-select \ tier4_planning_msgs \ autoware_common_msgs \ autoware_planning_msgs \ From 366bb7f6d312d0f7bd938d6cde150a5ddd5210ca Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 09:18:56 +0100 Subject: [PATCH 04/36] Updated mpc_camera_driver subrepo --- crp_VIL/mpc_camera_driver | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crp_VIL/mpc_camera_driver b/crp_VIL/mpc_camera_driver index 9c4de26..56e9aff 160000 --- a/crp_VIL/mpc_camera_driver +++ b/crp_VIL/mpc_camera_driver @@ -1 +1 @@ -Subproject commit 9c4de264a40c262b6fd3ded07e3e2794284d154e +Subproject commit 56e9affd11fb1d0361e55d1d9fde4928baaccabc From d882b9d738b0c45dc095fdc6273bcac0e9a32472 Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 11:02:29 +0100 Subject: [PATCH 05/36] Moved behavior max speed to behaviorPlanner --- .../behaviorPlanner.hpp | 4 ++++ .../src/behaviorPlanner.cpp | 20 +++++++++++++++---- .../scenarioFusion.hpp | 2 -- .../src/scenarioFusion.cpp | 8 -------- 4 files changed, 20 insertions(+), 14 deletions(-) diff --git a/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp b/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp index b3a6a9a..2ebb09a 100644 --- a/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp +++ b/crp_APL/plan_behavior_planning/include/plan_behavior_planning/behaviorPlanner.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -23,17 +24,20 @@ class BehaviorPlanner : public rclcpp::Node void routeCallback(const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg); void scenarioCallback(const crp_msgs::msg::Scenario::SharedPtr msg); void egoCallback(const crp_msgs::msg::Ego::SharedPtr msg); + void behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg); void loop(); rclcpp::Subscription::SharedPtr m_sub_route; rclcpp::Subscription::SharedPtr m_sub_scenario; rclcpp::Subscription::SharedPtr m_sub_ego; + rclcpp::Subscription::SharedPtr m_sub_behavior_; rclcpp::Publisher::SharedPtr m_pub_strategy; rclcpp::Publisher::SharedPtr m_pub_target_space; rclcpp::TimerBase::SharedPtr timer_; + float m_maximum_speed; }; } // namespace apl diff --git a/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp b/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp index 416a1f0..15d4696 100644 --- a/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp +++ b/crp_APL/plan_behavior_planning/src/behaviorPlanner.cpp @@ -10,6 +10,10 @@ crp::apl::BehaviorPlanner::BehaviorPlanner() : Node("behavior_planner") m_sub_ego = this->create_subscription( "ego", 10, std::bind(&BehaviorPlanner::egoCallback, this, std::placeholders::_1)); + m_sub_behavior_ = this->create_subscription( + "/ui/behavior", 10, + std::bind(&BehaviorPlanner::behaviorCallback, this, std::placeholders::_1)); + m_pub_strategy = this->create_publisher("plan/strategy", 10); m_pub_target_space = this->create_publisher("plan/target_space", 10); @@ -32,10 +36,11 @@ void crp::apl::BehaviorPlanner::scenarioCallback(const crp_msgs::msg::Scenario:: if (msg->paths.size() > 0) targetSpaceMsg.path = msg->paths[0]; - for (tier4_planning_msgs::msg::PathPointWithLaneId & pathPoint : targetSpaceMsg.path.path.points) - { - pathPoint.point.longitudinal_velocity_mps = std::min(msg->maximum_speed, pathPoint.point.longitudinal_velocity_mps); - } + + for (tier4_planning_msgs::msg::PathPointWithLaneId & pathPoint : targetSpaceMsg.path.path.points) + { + pathPoint.point.longitudinal_velocity_mps = std::min(m_maximum_speed, pathPoint.point.longitudinal_velocity_mps); + } targetSpaceMsg.free_space = msg->free_space; @@ -50,6 +55,13 @@ void crp::apl::BehaviorPlanner::egoCallback(const crp_msgs::msg::Ego::SharedPtr return; } + +void crp::apl::BehaviorPlanner::behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg) +{ + m_maximum_speed = msg->target_speed.data; +} + + void crp::apl::BehaviorPlanner::loop() { // possible Scenrios: "off", "laneFollowWithSpeedAdjust", "laneFollow", "speedAdjust" diff --git a/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp b/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp index 3de8aa5..b6f721e 100644 --- a/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp +++ b/crp_APL/prcp_situation_analysis/include/prcp_situation_analysis/scenarioFusion.hpp @@ -25,14 +25,12 @@ class ScenarioFusion : public rclcpp::Node void localObstaclesCallback(const autoware_perception_msgs::msg::PredictedObjects::SharedPtr msg); void localPathCallback(const tier4_planning_msgs::msg::PathWithLaneId::SharedPtr msg); void localDrivableSurfaceCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); - void behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg); void publishCallback(); rclcpp::Subscription::SharedPtr m_sub_localMovingObjects_; rclcpp::Subscription::SharedPtr m_sub_localObstacles_; rclcpp::Subscription::SharedPtr m_sub_localPath_; rclcpp::Subscription::SharedPtr m_sub_localDrivableSurface_; - rclcpp::Subscription::SharedPtr m_sub_behavior_; rclcpp::Publisher::SharedPtr m_pub_scenario_; diff --git a/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp b/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp index 8139187..7d2e9bb 100644 --- a/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp +++ b/crp_APL/prcp_situation_analysis/src/scenarioFusion.cpp @@ -3,9 +3,6 @@ crp::apl::ScenarioFusion::ScenarioFusion() : Node("scenario_fusion") { - m_sub_behavior_ = this->create_subscription( - "/cai/behavior", 10, - std::bind(&ScenarioFusion::behaviorCallback, this, std::placeholders::_1)); m_sub_localMovingObjects_ = this->create_subscription( "/cai/local_moving_objects", 10, std::bind(&ScenarioFusion::localMovingObjectsCallback, this, std::placeholders::_1)); @@ -31,11 +28,6 @@ void crp::apl::ScenarioFusion::publishCallback() } -void crp::apl::ScenarioFusion::behaviorCallback(const crp_msgs::msg::Behavior::SharedPtr msg) -{ - m_scenario.maximum_speed = msg->target_speed.data; -} - void crp::apl::ScenarioFusion::localMovingObjectsCallback(const autoware_perception_msgs::msg::PredictedObjects::SharedPtr msg) { m_scenario.header = msg->header; From a1d8573b519016beb94b78871a4cde37011feaf7 Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 11:25:52 +0100 Subject: [PATCH 06/36] Added behavior publish to UI --- crp_APL/ui/layouts/CRP Dasboard.json | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/crp_APL/ui/layouts/CRP Dasboard.json b/crp_APL/ui/layouts/CRP Dasboard.json index a81c502..1ad566a 100644 --- a/crp_APL/ui/layouts/CRP Dasboard.json +++ b/crp_APL/ui/layouts/CRP Dasboard.json @@ -103,6 +103,15 @@ "topicName": "/lexus3/control_reinit", "datatype": "std_msgs/msg/Bool" }, + "Publish!32kth13": { + "buttonText": "Publish", + "buttonTooltip": "", + "advancedView": true, + "value": "{\n \"target_speed\": { \"data\": 11.4\n },\n \"curve_driving_style\": { \"data\": 0\n }\n}", + "topicName": "/ui/behavior", + "datatype": "crp_msgs/msg/Behavior", + "foxglovePanelTitle": "Behavior" + }, "Gauge!1dii1on": { "path": "/ego.twist.twist.linear.x", "minValue": 0, @@ -453,7 +462,11 @@ }, "second": { "first": "RawMessages!2larpau", - "second": "Publish!3eqe9hn", + "second": { + "first": "Publish!3eqe9hn", + "second": "Publish!32kth13", + "direction": "row" + }, "direction": "row", "splitPercentage": 28.950826562211713 }, From ff0ba018f226d9ac45af706539c48bb0eb08d5fb Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 13:38:43 +0100 Subject: [PATCH 07/36] Removed maximum_speed from scenario --- crp_APL/crp_msgs/msg/Scenario.msg | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/crp_APL/crp_msgs/msg/Scenario.msg b/crp_APL/crp_msgs/msg/Scenario.msg index 32daade..fe1041c 100644 --- a/crp_APL/crp_msgs/msg/Scenario.msg +++ b/crp_APL/crp_msgs/msg/Scenario.msg @@ -3,5 +3,4 @@ std_msgs/Header header autoware_perception_msgs/PredictedObjects local_moving_objects autoware_perception_msgs/PredictedObjects local_obstacles crp_msgs/PathWithTrafficRules[] paths -nav_msgs/OccupancyGrid free_space -float32 maximum_speed \ No newline at end of file +nav_msgs/OccupancyGrid free_space \ No newline at end of file From 52acf8152787b3acd87cadde470467d051ab4979 Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 15:38:06 +0100 Subject: [PATCH 08/36] Created core launch; buld_all uses build_core --- launch/crp_launcher/launch/core.launch.py | 118 ++++++++++++++++++++++ scripts/build_all.sh | 35 ++++++- scripts/build_core.sh | 6 +- 3 files changed, 154 insertions(+), 5 deletions(-) create mode 100644 launch/crp_launcher/launch/core.launch.py diff --git a/launch/crp_launcher/launch/core.launch.py b/launch/crp_launcher/launch/core.launch.py new file mode 100644 index 0000000..efa4491 --- /dev/null +++ b/launch/crp_launcher/launch/core.launch.py @@ -0,0 +1,118 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import LaunchConfigurationEquals +from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from os.path import join + + +def generate_launch_description(): + + # ARGS + + # sensor abstraction + vehicle_tire_angle_topic_arg = DeclareLaunchArgument( + 'vehicle_tire_angle_topic', + default_value='/sensing/vehicle/tire_angle', + description='Length of the scenario in meters') + local_path_length_arg = DeclareLaunchArgument( + 'local_path_length', + default_value='70.0', + description='Length of the scenario in meters') + + # NODES + + sensor_abstraction = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('prcp_sensor_abstraction'), + 'launch', + 'sensor_abstraction.launch.py') + ) + ) + + environmental_fusion = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('prcp_situation_analysis'), + 'launch', + 'environmental_fusion.launch.py') + ) + ) + + behavior_planning = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('plan_behavior_planning'), + 'launch', + 'behavior_planning.launch.py' + ) + ) + ) + + motion_planning = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('plan_motion_planning'), + 'launch', + 'motion_planning.launch.py' + ) + ) + ) + + planner_lat_lane_follow_ldm = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('plan_lat_lane_follow_ldm'), + 'launch', + 'plan_lat_lane_follow_ldm.launch.py' + ) + ) + ) + + vehicle_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('ctrl_vehicle_control'), + 'launch', + 'ctrl_vehicle_control.launch.py') + ) + ) + + vehicle_control_lat = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('ctrl_vehicle_control_lat_compensatory'), + 'launch', + 'ctrl_vehicle_control_lat_compensatory.launch.py') + ) + ) + + vehicle_control_long = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('ctrl_vehicle_control_long'), + 'launch', + 'ctrl_vehicle_control_long.launch.py') + ) + ) + + return LaunchDescription([ + # args + vehicle_tire_angle_topic_arg, + vehicle_tire_angle_topic_arg, + local_path_length_arg, + + # nodes + sensor_abstraction, + environmental_fusion, + behavior_planning, + motion_planning, + vehicle_control, + vehicle_control_lat, + vehicle_control_long, + + planner_lat_lane_follow_ldm + ]) diff --git a/scripts/build_all.sh b/scripts/build_all.sh index 493ae2c..cded75e 100755 --- a/scripts/build_all.sh +++ b/scripts/build_all.sh @@ -3,7 +3,36 @@ cd $script_dir while [ ! -e "src/" ]; do cd ../ done -echo $(pwd) +echo "Build location: "$(pwd) -rosdep install --from-paths src --ignore-src -r -y -colcon build +# build core +trap 'exit' INT # trap ctrl-c +$script_dir/build_core.sh +trap - INT + +# build lexus packages +packages=( + duro_gps_driver + duro_gps_launcher + kvaser_interface + lanelet_handler + lexus_bringup + mcap_rec + mpc_camera_driver + novatel_gps_msgs + novatel_gps_driver + novatel_gps_launcher + pacmod_extender + pacmod_interface +) + +packages_string="" +for package in "${packages[@]}"; do + packages_string+="$package " +done + +packages_paths=$(colcon list --packages-up-to $packages_string -p) + +rosdep install --from-paths $packages_paths --ignore-src -r -y + +colcon build --packages-select $packages_string \ No newline at end of file diff --git a/scripts/build_core.sh b/scripts/build_core.sh index ac3bf53..03685d5 100755 --- a/scripts/build_core.sh +++ b/scripts/build_core.sh @@ -3,9 +3,9 @@ cd $script_dir while [ ! -e "src/" ]; do cd ../ done -echo $(pwd) +echo "Build location: "$(pwd) -rosdep install --from-paths src --ignore-src -r -y +rosdep install --from-paths $script_dir/../crp_APL $script_dir/../crp_CIL --ignore-src -r -y colcon build --packages-select \ tier4_planning_msgs \ autoware_common_msgs \ @@ -26,4 +26,6 @@ plan_lon_intelligent_speed_adjust \ plan_motion_planning \ crp_launcher \ ctrl_vehicle_control \ +ctrl_vehicle_control_lat_compensatory \ +ctrl_vehicle_control_long \ test_node From f31fb80b58295b5f0510c45137199f96053c1f1e Mon Sep 17 00:00:00 2001 From: AnonymDavid Date: Thu, 7 Nov 2024 15:43:48 +0100 Subject: [PATCH 09/36] Lexus laucher uses core launcher --- launch/crp_launcher/launch/lexus.launch.py | 99 ++++------------------ 1 file changed, 15 insertions(+), 84 deletions(-) diff --git a/launch/crp_launcher/launch/lexus.launch.py b/launch/crp_launcher/launch/lexus.launch.py index 5b3c6c7..ef2e876 100644 --- a/launch/crp_launcher/launch/lexus.launch.py +++ b/launch/crp_launcher/launch/lexus.launch.py @@ -77,6 +77,18 @@ def generate_launch_description(): default_value='70.0', description='Length of the scenario in meters') + + # CORE + + crp_core = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + join( + get_package_share_directory('crp_launcher'), + 'launch', + 'core.launch.py') + ) + ) + # NODES novatel_gps = IncludeLaunchDescription( @@ -198,63 +210,6 @@ def generate_launch_description(): ) ) - sensor_abstraction = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('prcp_sensor_abstraction'), - 'launch', - 'sensor_abstraction.launch.py') - ) - ) - - environmental_fusion = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('prcp_situation_analysis'), - 'launch', - 'environmental_fusion.launch.py') - ) - ) - - behavior_planning = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('plan_behavior_planning'), - 'launch', - 'behavior_planning.launch.py' - ) - ) - ) - - motion_planning = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('plan_motion_planning'), - 'launch', - 'motion_planning.launch.py' - ) - ) - ) - - planner_lat_lane_follow_ldm = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('plan_lat_lane_follow_ldm'), - 'launch', - 'plan_lat_lane_follow_ldm.launch.py' - ) - ) - ) - - vehicle_control = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('ctrl_vehicle_control'), - 'launch', - 'ctrl_vehicle_control.launch.py') - ) - ) - lexus_speed_control = IncludeLaunchDescription( PythonLaunchDescriptionSource( join( @@ -264,24 +219,6 @@ def generate_launch_description(): ) ) - vehicle_control_lat = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('ctrl_vehicle_control_lat_compensatory'), - 'launch', - 'ctrl_vehicle_control_lat_compensatory.launch.py') - ) - ) - - vehicle_control_long = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - join( - get_package_share_directory('ctrl_vehicle_control_long'), - 'launch', - 'ctrl_vehicle_control_long.launch.py') - ) - ) - return LaunchDescription([ # args select_gps_arg, @@ -300,6 +237,9 @@ def generate_launch_description(): vehicle_tire_angle_topic_arg, local_path_length_arg, + # core + crp_core, + # vehicle nodes novatel_gps, duro_gps, @@ -316,13 +256,4 @@ def generate_launch_description(): # nodes lanelet_file_loader, - sensor_abstraction, - environmental_fusion, - behavior_planning, - motion_planning, - vehicle_control, - vehicle_control_lat, - vehicle_control_long, - - planner_lat_lane_follow_ldm ]) From 6f9e6df537d9cd8f60e753bd93d96c631e66d069 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Fri, 8 Nov 2024 11:31:11 +0100 Subject: [PATCH 10/36] Add map_loader pacakge for lanelet2 visualization --- crp_VIL/lanelet_handler/CMakeLists.txt | 7 +- crp_VIL/map_loader/CMakeLists.txt | 27 ++ crp_VIL/map_loader/README.md | 174 ++++++++++ .../config/lanelet2_map_loader.param.yaml | 6 + .../map_loader/lanelet2_map_loader_node.hpp | 56 ++++ .../lanelet2_map_visualization_node.hpp | 40 +++ .../launch/lanelet2_map_loader.launch.xml | 20 ++ crp_VIL/map_loader/package.xml | 45 +++ .../schema/lanelet2_map_loader.schema.json | 48 +++ crp_VIL/map_loader/script/map_hash_generator | 111 +++++++ .../lanelet2_map_visualization_node.cpp | 297 ++++++++++++++++++ crp_VIL/mpc_camera_driver | 2 +- 12 files changed, 830 insertions(+), 3 deletions(-) create mode 100644 crp_VIL/map_loader/CMakeLists.txt create mode 100644 crp_VIL/map_loader/README.md create mode 100755 crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml create mode 100644 crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp create mode 100644 crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp create mode 100644 crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml create mode 100644 crp_VIL/map_loader/package.xml create mode 100644 crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json create mode 100755 crp_VIL/map_loader/script/map_hash_generator create mode 100644 crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp diff --git a/crp_VIL/lanelet_handler/CMakeLists.txt b/crp_VIL/lanelet_handler/CMakeLists.txt index 39337ab..19e1609 100644 --- a/crp_VIL/lanelet_handler/CMakeLists.txt +++ b/crp_VIL/lanelet_handler/CMakeLists.txt @@ -9,18 +9,21 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(autoware_map_msgs REQUIRED) -find_package(map_loader REQUIRED) find_package(lanelet2_core REQUIRED) find_package(lanelet2_projection REQUIRED) find_package(lanelet2_io REQUIRED) find_package(autoware_lanelet2_extension REQUIRED) +find_package(visualization_msgs REQUIRED) + include_directories( include ) add_executable(lanelet_file_loader src/laneletFileLoader.cpp) -ament_target_dependencies(lanelet_file_loader rclcpp autoware_map_msgs lanelet2_core lanelet2_projection lanelet2_io autoware_lanelet2_extension) +ament_target_dependencies(lanelet_file_loader rclcpp autoware_map_msgs lanelet2_core autoware_lanelet2_extension lanelet2_projection lanelet2_io) + + install(TARGETS lanelet_file_loader diff --git a/crp_VIL/map_loader/CMakeLists.txt b/crp_VIL/map_loader/CMakeLists.txt new file mode 100644 index 0000000..c6b1ead --- /dev/null +++ b/crp_VIL/map_loader/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(map_loader) + +find_package(autoware_cmake REQUIRED) +find_package(tier4_map_msgs REQUIRED) +autoware_package() + + +ament_auto_add_library(lanelet2_map_visualization_node SHARED + src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +) + +rclcpp_components_register_node(lanelet2_map_visualization_node + PLUGIN "Lanelet2MapVisualizationNode" + EXECUTABLE lanelet2_map_visualization +) + + +install(PROGRAMS + script/map_hash_generator + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/crp_VIL/map_loader/README.md b/crp_VIL/map_loader/README.md new file mode 100644 index 0000000..bc3eb80 --- /dev/null +++ b/crp_VIL/map_loader/README.md @@ -0,0 +1,174 @@ +# map_loader package + +This package provides the features of loading various maps. + +## pointcloud_map_loader + +### Feature + +`pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. +Currently, it supports the following two types: + +- Publish raw pointcloud map +- Publish downsampled pointcloud map +- Send partial pointcloud map loading via ROS 2 service +- Send differential pointcloud map loading via ROS 2 service + +NOTE: **We strongly recommend to use divided maps when using large pointcloud map to enable the latter two features (partial and differential load). Please go through the prerequisites section for more details, and follow the instruction for dividing the map and preparing the metadata.** + +### Prerequisites + +#### Prerequisites on pointcloud map file(s) + +You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: + +1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/autoware_map_projection_loader/README.md). +2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. +3. **The division size along each axis should be equal.** +4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/autoware_ndt_scan_matcher) and [autoware_compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/autoware_compare_map_segmentation). +5. **All the split maps should not overlap with each other.** +6. **Metadata file should also be provided.** The metadata structure description is provided below. + +#### Metadata structure + +The metadata should look like this: + +```yaml +x_resolution: 20.0 +y_resolution: 20.0 +A.pcd: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520 +B.pcd: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520 +C.pcd: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540 +D.pcd: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540 +``` + +where, + +- `x_resolution` and `y_resolution` +- `A.pcd`, `B.pcd`, etc, are the names of PCD files. +- List such as `[1200, 2500]` are the values indicate that for this PCD file, x coordinates are between 1200 and 1220 (`x_resolution` + `x_coordinate`) and y coordinates are between 2500 and 2520 (`y_resolution` + `y_coordinate`). + +You may use [pointcloud_divider](https://github.com/autowarefoundation/autoware_tools/tree/main/map/autoware_pointcloud_divider) for dividing pointcloud map as well as generating the compatible metadata.yaml. + +#### Directory structure of these files + +If you only have one pointcloud map, Autoware will assume the following directory structure by default. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +``` + +If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +│ ├── A.pcd +│ ├── B.pcd +│ ├── C.pcd +│ └── ... +├── map_projector_info.yaml +└── pointcloud_map_metadata.yaml +``` + +### Specific features + +#### Publish raw pointcloud map (ROS 2 topic) + +The node publishes the raw pointcloud map loaded from the `.pcd` file(s). + +#### Publish downsampled pointcloud map (ROS 2 topic) + +The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter. + +#### Publish metadata of pointcloud map (ROS 2 topic) + +The node publishes the pointcloud metadata attached with an ID. Metadata is loaded from the `.yaml` file. Please see [the description of `PointCloudMapMetaData.msg`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#pointcloudmapmetadatamsg) for details. + +#### Send partial pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. +Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. + +#### Send differential pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. +Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. + +#### Send selected pointcloud map (ROS 2 service) + +Here, we assume that the pointcloud maps are divided into grids. + +Given IDs query from a client node, the node sends a set of pointcloud maps (each of which attached with unique ID) specified by query. +Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getselectedpointcloudmapsrv) for details. + +### Parameters + +{{ json_to_markdown("map/map_loader/schema/pointcloud_map_loader.schema.json") }} + +### Interfaces + +- `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map +- `output/pointcloud_map_metadata` (autoware_map_msgs/msg/PointCloudMapMetaData) : Metadata of pointcloud map +- `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map +- `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map +- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map +- `service/get_selected_pcd_map` (autoware_map_msgs/srv/GetSelectedPointCloudMap) : Selected pointcloud map +- pointcloud map file(s) (.pcd) +- metadata of pointcloud map(s) (.yaml) + +--- + +## lanelet2_map_loader + +### Feature + +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. +The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. +Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. + +### How to run + +`ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` + +### Subscribed Topics + +- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware + +### Published Topics + +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map + +### Parameters + +{{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} + +`use_waypoints` decides how to handle a centerline. +This flag enables to use the `overwriteLaneletsCenterlineWithWaypoints` function instead of `overwriteLaneletsCenterline`. Please see [the document of the autoware_lanelet2_extension package](https://github.com/autowarefoundation/autoware_lanelet2_extension/blob/main/autoware_lanelet2_extension/docs/lanelet2_format_extension.md#centerline) in detail. + +--- + +## lanelet2_map_visualization + +### Feature + +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. + +### How to Run + +`ros2 run map_loader lanelet2_map_visualization` + +### Subscribed Topics + +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map + +### Published Topics + +- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz diff --git a/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml b/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml new file mode 100755 index 0000000..48d392a --- /dev/null +++ b/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + allow_unsupported_version: true # flag to load unsupported format_version anyway. If true, just prints warning. + center_line_resolution: 5.0 # [m] + use_waypoints: true # "centerline" in the Lanelet2 map will be used as a "waypoints" tag. + lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp new file mode 100644 index 0000000..2d3fbfb --- /dev/null +++ b/crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include + +class Lanelet2MapLoaderNode : public rclcpp::Node +{ +public: + static constexpr lanelet::autoware::Version version = lanelet::autoware::version; + +public: + explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); + + static lanelet::LaneletMapPtr load_map( + const std::string & lanelet2_filename, + const tier4_map_msgs::msg::MapProjectorInfo & projector_info); + static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +private: + using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; + + void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + + autoware::component_interface_utils::Subscription::SharedPtr + sub_map_projector_info_; + rclcpp::Publisher::SharedPtr pub_map_bin_; +}; + +#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp new file mode 100644 index 0000000..049d714 --- /dev/null +++ b/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -0,0 +1,40 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ +#define MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ + +#include + +#include +#include + +#include +#include + +class Lanelet2MapVisualizationNode : public rclcpp::Node +{ +public: + explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Publisher::SharedPtr pub_marker_; + + bool viz_lanelets_centerline_; + + void on_map_bin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); +}; + +#endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml new file mode 100644 index 0000000..5baee91 --- /dev/null +++ b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/crp_VIL/map_loader/package.xml b/crp_VIL/map_loader/package.xml new file mode 100644 index 0000000..c22c909 --- /dev/null +++ b/crp_VIL/map_loader/package.xml @@ -0,0 +1,45 @@ + + + + map_loader + 0.1.0 + The map_loader package + Yamato Ando + Ryu Yamamoto + Masahiro Sakamoto + Kento Yabuuchi + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + + Apache License 2.0 + Ryohsuke Mitsudome + Koji Minoda + + ament_cmake_auto + autoware_cmake + + autoware_component_interface_specs + autoware_component_interface_utils + autoware_geography_utils + autoware_lanelet2_extension + autoware_map_msgs + fmt + geometry_msgs + libpcl-all-dev + pcl_conversions + rclcpp + rclcpp_components + tier4_map_msgs + visualization_msgs + yaml-cpp + + ament_cmake_gmock + ament_lint_auto + autoware_lint_common + ros_testing + + + ament_cmake + + diff --git a/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json b/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json new file mode 100644 index 0000000..a55050e --- /dev/null +++ b/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json @@ -0,0 +1,48 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for lanelet2 map loader Node", + "type": "object", + "definitions": { + "lanelet2_map_loader": { + "type": "object", + "properties": { + "allow_unsupported_version": { + "type": "boolean", + "description": "Flag to load unsupported format_version anyway. If true, just prints warning.", + "default": "true" + }, + "center_line_resolution": { + "type": "number", + "description": "Resolution of the Lanelet center line [m]", + "default": "5.0" + }, + "use_waypoints": { + "type": "boolean", + "description": "If true, `centerline` in the Lanelet2 map will be used as a `waypoints` tag.", + "default": true + }, + "lanelet2_map_path": { + "type": "string", + "description": "The lanelet2 map path pointing to the .osm file", + "default": "" + } + }, + "required": ["center_line_resolution", "use_waypoints", "lanelet2_map_path"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_loader" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/crp_VIL/map_loader/script/map_hash_generator b/crp_VIL/map_loader/script/map_hash_generator new file mode 100755 index 0000000..5fb5755 --- /dev/null +++ b/crp_VIL/map_loader/script/map_hash_generator @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import hashlib +import pathlib + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from tier4_external_api_msgs.msg import MapHash +from tier4_external_api_msgs.msg import ResponseStatus +from tier4_external_api_msgs.srv import GetTextFile + + +class MapHashGenerator(Node): + def __init__(self): + super().__init__("map_hash_generator") + self.lanelet_path = self.declare_parameter("lanelet2_map_path", "").value + self.lanelet_text = self.load_lanelet_text(self.lanelet_path) + self.lanelet_hash = self.generate_lanelet_file_hash(self.lanelet_text) + + self.pcd_map_path = self.declare_parameter("pointcloud_map_path", "").value + self.pcd_map_hash = self.generate_pcd_file_hash(self.pcd_map_path) + + qos_profile = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + msg = MapHash() + msg.lanelet = self.lanelet_hash + msg.pcd = self.pcd_map_hash + self.pub = self.create_publisher(MapHash, "/api/autoware/get/map/info/hash", qos_profile) + self.pub.publish(msg) + + self.srv = self.create_service( + GetTextFile, "/api/autoware/get/map/lanelet/xml", self.on_get_lanelet_xml + ) # noqa: E501 + + def on_get_lanelet_xml(self, request, response): + response.status.code = ResponseStatus.SUCCESS + response.file.text = self.lanelet_text + return response + + @staticmethod + def load_lanelet_text(path): + path = pathlib.Path(path) + return path.read_text() if path.is_file() else "" + + @staticmethod + def generate_lanelet_file_hash(data): + return hashlib.sha256(data.encode()).hexdigest() if data else "" + + def update_hash(self, m, path): + try: + with open(path, "rb") as pcd_file: + m.update(pcd_file.read()) + except FileNotFoundError as e: + self.get_logger().error(e) + return False + return True + + def generate_pcd_file_hash(self, path): + path = pathlib.Path(path) + if path.is_file(): + if not path.suffix == ".pcd": + self.get_logger().error(f"[{path}] is not pcd file") + return "" + m = hashlib.sha256() + if not self.update_hash(m, path): + return "" + return m.hexdigest() + + if path.is_dir(): + m = hashlib.sha256() + for pcd_file_path in sorted(path.iterdir()): + if not pcd_file_path.suffix == ".pcd": + continue + if not self.update_hash(m, pcd_file_path): + return "" + if m.hexdigest() == hashlib.sha256().hexdigest(): + self.get_logger().error(f"there are no pcd files in [{path}]") + return "" + return m.hexdigest() + + self.get_logger().error(f"[{path}] is neither file nor directory") + return "" + + +def main(args=None): + rclpy.init(args=args) + node = MapHashGenerator() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass diff --git a/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp new file mode 100644 index 0000000..612a848 --- /dev/null +++ b/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -0,0 +1,297 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Authors: Simon Thompson, Ryohsuke Mitsudome + * + */ + +#include "map_loader/lanelet2_map_visualization_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace +{ +void insert_marker_array( + visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) +{ + a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); +} + +void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +{ + cl->r = static_cast(r); + cl->g = static_cast(g); + cl->b = static_cast(b); + cl->a = static_cast(a); +} +} // namespace + +Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options) +: Node("lanelet2_map_visualization", options) +{ + using std::placeholders::_1; + + viz_lanelets_centerline_ = true; + + sub_map_bin_ = this->create_subscription( + "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), + std::bind(&Lanelet2MapVisualizationNode::on_map_bin, this, _1)); + + pub_marker_ = this->create_publisher( + "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); +} + +void Lanelet2MapVisualizationNode::on_map_bin( + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) +{ + lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); + + lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map); + RCLCPP_INFO(this->get_logger(), "Map is loaded\n"); + + // get lanelets etc to visualize + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); + lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); + lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets); + lanelet::ConstLanelets crosswalk_lanelets = + lanelet::utils::query::crosswalkLanelets(all_lanelets); + lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_polygon_markings = + lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_line_markings = + lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map); + lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); + std::vector stop_lines = + lanelet::utils::query::stopLinesLanelets(road_lanelets); + std::vector aw_tl_reg_elems = + lanelet::utils::query::autowareTrafficLights(all_lanelets); + std::vector da_reg_elems = + lanelet::utils::query::detectionAreas(all_lanelets); + std::vector no_reg_elems = + lanelet::utils::query::noStoppingAreas(all_lanelets); + std::vector sb_reg_elems = + lanelet::utils::query::speedBumps(all_lanelets); + std::vector cw_reg_elems = + lanelet::utils::query::crosswalks(all_lanelets); + lanelet::ConstLineStrings3d parking_spaces = + lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); + lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); + lanelet::ConstPolygons3d obstacle_polygons = + lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); + lanelet::ConstPolygons3d no_obstacle_segmentation_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area"); + lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = + lanelet::utils::query::getAllPolygonsByType( + viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); + lanelet::ConstPolygons3d hatched_road_markings_area = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); + lanelet::ConstPolygons3d intersection_areas = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "intersection_area"); + std::vector no_parking_reg_elems = + lanelet::utils::query::noParkingAreas(all_lanelets); + lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); + + std_msgs::msg::ColorRGBA cl_road; + std_msgs::msg::ColorRGBA cl_shoulder; + std_msgs::msg::ColorRGBA cl_cross; + std_msgs::msg::ColorRGBA cl_partitions; + std_msgs::msg::ColorRGBA cl_pedestrian_markings; + std_msgs::msg::ColorRGBA cl_ll_borders; + std_msgs::msg::ColorRGBA cl_shoulder_borders; + std_msgs::msg::ColorRGBA cl_stoplines; + std_msgs::msg::ColorRGBA cl_trafficlights; + std_msgs::msg::ColorRGBA cl_detection_areas; + std_msgs::msg::ColorRGBA cl_speed_bumps; + std_msgs::msg::ColorRGBA cl_crosswalks; + std_msgs::msg::ColorRGBA cl_parking_lots; + std_msgs::msg::ColorRGBA cl_parking_spaces; + std_msgs::msg::ColorRGBA cl_lanelet_id; + std_msgs::msg::ColorRGBA cl_obstacle_polygons; + std_msgs::msg::ColorRGBA cl_no_stopping_areas; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area; + std_msgs::msg::ColorRGBA cl_no_obstacle_segmentation_area_for_run_out; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_area; + std_msgs::msg::ColorRGBA cl_hatched_road_markings_line; + std_msgs::msg::ColorRGBA cl_no_parking_areas; + std_msgs::msg::ColorRGBA cl_curbstones; + std_msgs::msg::ColorRGBA cl_intersection_area; + std_msgs::msg::ColorRGBA cl_bus_stop_area; + std_msgs::msg::ColorRGBA cl_bicycle_lane; + set_color(&cl_road, 0.27, 0.27, 0.27, 0.999); + set_color(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); + set_color(&cl_cross, 0.27, 0.3, 0.27, 0.5); + set_color(&cl_partitions, 0.25, 0.25, 0.25, 0.999); + set_color(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); + set_color(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); + set_color(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); + set_color(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); + set_color(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); + set_color(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); + set_color(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); + set_color(&cl_parking_lots, 1.0, 1.0, 1.0, 0.2); + set_color(&cl_parking_spaces, 1.0, 1.0, 1.0, 0.3); + set_color(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); + set_color(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); + set_color(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); + set_color(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); + set_color(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + set_color(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + set_color(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); + set_color(&cl_bus_stop_area, 0.863, 0.863, 0.863, 0.5); + set_color(&cl_bicycle_lane, 0.0, 0.3843, 0.6274, 0.5); + + visualization_msgs::msg::MarkerArray map_marker_array; + + insert_marker_array( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); + insert_marker_array( + &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( + pedestrian_polygon_markings, cl_pedestrian_markings)); + + insert_marker_array( + &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( + pedestrian_line_markings, cl_pedestrian_markings)); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "walkway_lanelets", walkway_lanelets, cl_cross)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletsBoundaryAsMarkerArray( + shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( + road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); + insert_marker_array( + &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( + no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); + insert_marker_array( + &map_marker_array, + lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( + no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( + hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + + insert_marker_array( + &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( + intersection_areas, cl_intersection_area)); + + pub_marker_->publish(map_marker_array); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode) diff --git a/crp_VIL/mpc_camera_driver b/crp_VIL/mpc_camera_driver index 56e9aff..9c4de26 160000 --- a/crp_VIL/mpc_camera_driver +++ b/crp_VIL/mpc_camera_driver @@ -1 +1 @@ -Subproject commit 56e9affd11fb1d0361e55d1d9fde4928baaccabc +Subproject commit 9c4de264a40c262b6fd3ded07e3e2794284d154e From 071c18e1d5cc1e5c3bf31fb78a4b82e7bf4d3b24 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Fri, 8 Nov 2024 11:34:36 +0100 Subject: [PATCH 11/36] Add gitignore --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index b1fa845..d81454e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ logs/ build/ -install/ \ No newline at end of file +install/ + +.vscode/ \ No newline at end of file From 071ee67f7264b2e4b3aad360024de49e16c0a239 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Fri, 8 Nov 2024 11:37:21 +0100 Subject: [PATCH 12/36] Remove unloaded files --- .../map_loader/lanelet2_map_loader_node.hpp | 56 ------------------- .../launch/lanelet2_map_loader.launch.xml | 9 --- crp_VIL/map_loader/package.xml | 3 - 3 files changed, 68 deletions(-) delete mode 100644 crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp diff --git a/crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp deleted file mode 100644 index 2d3fbfb..0000000 --- a/crp_VIL/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ -#define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ - -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include - -class Lanelet2MapLoaderNode : public rclcpp::Node -{ -public: - static constexpr lanelet::autoware::Version version = lanelet::autoware::version; - -public: - explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); - - static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, - const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, - const rclcpp::Time & now); - -private: - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; - - void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); - - autoware::component_interface_utils::Subscription::SharedPtr - sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; -}; - -#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml index 5baee91..c079cb1 100644 --- a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml @@ -4,15 +4,6 @@ - - - - - - - - - diff --git a/crp_VIL/map_loader/package.xml b/crp_VIL/map_loader/package.xml index c22c909..b86183c 100644 --- a/crp_VIL/map_loader/package.xml +++ b/crp_VIL/map_loader/package.xml @@ -19,9 +19,6 @@ ament_cmake_auto autoware_cmake - autoware_component_interface_specs - autoware_component_interface_utils - autoware_geography_utils autoware_lanelet2_extension autoware_map_msgs fmt From 43022e5989c23007a5c198cd43b52e57c91d7026 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Fri, 8 Nov 2024 22:06:59 +0100 Subject: [PATCH 13/36] Add map loader fixes for build sucess --- .../launch/lanelet2_map_loader.launch.xml | 3 +-- .../lanelet2_map_visualization_node.cpp | 19 ++++++++++++++++++- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml index c079cb1..e71906b 100644 --- a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml @@ -1,7 +1,6 @@ - - + diff --git a/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 612a848..60e34f8 100644 --- a/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -95,6 +95,7 @@ void Lanelet2MapVisualizationNode::on_map_bin( lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets); lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); + lanelet::ConstLanelets bicycle_lane_lanelets; lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); lanelet::ConstLineStrings3d pedestrian_polygon_markings = lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); @@ -130,6 +131,7 @@ void Lanelet2MapVisualizationNode::on_map_bin( std::vector no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); + std::vector bus_stop_reg_elems; std_msgs::msg::ColorRGBA cl_road; std_msgs::msg::ColorRGBA cl_shoulder; @@ -290,8 +292,23 @@ void Lanelet2MapVisualizationNode::on_map_bin( &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( intersection_areas, cl_intersection_area)); + + insert_marker_array( + &map_marker_array, + lanelet::visualization::laneletDirectionAsMarkerArray(bicycle_lane_lanelets, "bicycle_lane_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( + bicycle_lane_lanelets, cl_ll_borders /* use ll_border color */, + viz_lanelets_centerline_, "bicycle_lane_")); + insert_marker_array( + &map_marker_array, lanelet::visualization::generateLaneletIdMarker( + bicycle_lane_lanelets, cl_lanelet_id /* use lanelet_id color */)); + insert_marker_array( + &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "bicycle_lane_lanelets", bicycle_lane_lanelets, cl_bicycle_lane)); + pub_marker_->publish(map_marker_array); } #include -RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode) +RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode) \ No newline at end of file From d21462e0f3c6f6f8a96b12aec86a64b843a55992 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 08:50:30 +0100 Subject: [PATCH 14/36] Add CI first try --- .github/workflows/build_test.yml | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 .github/workflows/build_test.yml diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml new file mode 100644 index 0000000..50e87b5 --- /dev/null +++ b/.github/workflows/build_test.yml @@ -0,0 +1,28 @@ +name: Build test + +on: + push: + branches: + - main + - 67-crptg1-ci-setup + pull_request: + branches: + - main + +jobs: + build: + runs-on: ubuntu-22.04 + container: ros:humble-ros-base-jammy + steps: + - uses: actions/checkout@v2 + - name: create a ros2 workspace + run: mkdir -p ros2_ws/src + - name: copy the package to the workspace + run: cp -r src ros2_ws/src + - name: APt update + run: apt update + - name: install dependencies + run: cd ros2_ws && source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y + shell: bash + - name: build the workspace + run: cd ros2_ws && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select tier4_planning_msgs \ No newline at end of file From 25e9a5351b5d10765e08bb27e89205793286071c Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 08:51:48 +0100 Subject: [PATCH 15/36] Copy --- .github/workflows/build_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 50e87b5..16ee532 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -18,7 +18,7 @@ jobs: - name: create a ros2 workspace run: mkdir -p ros2_ws/src - name: copy the package to the workspace - run: cp -r src ros2_ws/src + run: cp -r * ros2_ws/src - name: APt update run: apt update - name: install dependencies From ffb414af04c62a2ed5d7d98c8ca3d069f4dd0bc4 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 09:04:20 +0100 Subject: [PATCH 16/36] Just list --- .github/workflows/build_test.yml | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 16ee532..95cf225 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -14,15 +14,19 @@ jobs: runs-on: ubuntu-22.04 container: ros:humble-ros-base-jammy steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: create a ros2 workspace - run: mkdir -p ros2_ws/src + run: ls -l - name: copy the package to the workspace - run: cp -r * ros2_ws/src + run: cp -r tier4_planning_msgs ros2_ws/src - name: APt update run: apt update - name: install dependencies - run: cd ros2_ws && source /opt/ros/noetic/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y + run: | + sudo apt-get update + sudo apt-get install -y python3-colcon-common-extensions + - name: install dependencies + run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y shell: bash - name: build the workspace run: cd ros2_ws && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select tier4_planning_msgs \ No newline at end of file From 0467c7e410bf5a1ba70ed397d7fcc78742ea0c78 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 09:06:53 +0100 Subject: [PATCH 17/36] Copy back --- .github/workflows/build_test.yml | 4 +- crp_APL/ctrl_vehicle_control/CMakeLists.txt | 1 + .../launch/ctrl_vehicle_control_lqr.launch.py | 0 .../scripts/cubic_spline_planner.py | 398 ++++++++++++++++++ .../scripts/lqr_controller.py | 282 +++++++++++++ .../launch/laneletFileLoader.launch.py | 2 +- 6 files changed, 684 insertions(+), 3 deletions(-) create mode 100644 crp_APL/ctrl_vehicle_control/launch/ctrl_vehicle_control_lqr.launch.py create mode 100644 crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py create mode 100644 crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 95cf225..f56ae73 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -16,9 +16,9 @@ jobs: steps: - uses: actions/checkout@v3 - name: create a ros2 workspace - run: ls -l + run: mkdir -p ros2_ws/src - name: copy the package to the workspace - run: cp -r tier4_planning_msgs ros2_ws/src + run: cp -r */ ros2_ws/src - name: APt update run: apt update - name: install dependencies diff --git a/crp_APL/ctrl_vehicle_control/CMakeLists.txt b/crp_APL/ctrl_vehicle_control/CMakeLists.txt index a36b8b1..c283407 100644 --- a/crp_APL/ctrl_vehicle_control/CMakeLists.txt +++ b/crp_APL/ctrl_vehicle_control/CMakeLists.txt @@ -8,6 +8,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) find_package(std_msgs REQUIRED) find_package(autoware_control_msgs REQUIRED) find_package(pacmod3_msgs REQUIRED) diff --git a/crp_APL/ctrl_vehicle_control/launch/ctrl_vehicle_control_lqr.launch.py b/crp_APL/ctrl_vehicle_control/launch/ctrl_vehicle_control_lqr.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py b/crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py new file mode 100644 index 0000000..d53a67e --- /dev/null +++ b/crp_APL/ctrl_vehicle_control/scripts/cubic_spline_planner.py @@ -0,0 +1,398 @@ +""" +Cubic spline planner + +Author: Atsushi Sakai(@Atsushi_twi) + +""" +import math +import numpy as np +import bisect + +class CubicSpline1D: + """ + 1D Cubic Spline class + + Parameters + ---------- + x : list + x coordinates for data points. This x coordinates must be + sorted + in ascending order. + y : list + y coordinates for data points + + Examples + -------- + You can interpolate 1D data points. + + >>> import numpy as np + >>> import matplotlib.pyplot as plt + >>> x = np.arange(5) + >>> y = [1.7, -6, 5, 6.5, 0.0] + >>> sp = CubicSpline1D(x, y) + >>> xi = np.linspace(0.0, 5.0) + >>> yi = [sp.calc_position(x) for x in xi] + >>> plt.plot(x, y, "xb", label="Data points") + >>> plt.plot(xi, yi , "r", label="Cubic spline interpolation") + >>> plt.grid(True) + >>> plt.legend() + >>> plt.show() + + .. image:: cubic_spline_1d.png + + """ + + def __init__(self, x, y): + + h = np.diff(x) + if np.any(h < 0): + raise ValueError("x coordinates must be sorted in ascending order") + + self.a, self.b, self.c, self.d = [], [], [], [] + self.x = x + self.y = y + self.nx = len(x) # dimension of x + + # calc coefficient a + self.a = [iy for iy in y] + + # calc coefficient c + A = self.__calc_A(h) + B = self.__calc_B(h, self.a) + self.c = np.linalg.solve(A, B) + + # calc spline coefficient b and d + for i in range(self.nx - 1): + d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i]) + b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \ + - h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1]) + self.d.append(d) + self.b.append(b) + + + + def calc_position(self, x): + """ + Calc `y` position for given `x`. + + if `x` is outside the data point's `x` range, return None. + + Returns + ------- + y : float + y position for given x. + """ + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + position = self.a[i] + self.b[i] * dx + \ + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 + + return position + + + def calc_first_derivative(self, x): + """ + Calc first derivative at given x. + + if x is outside the input x, return None + + Returns + ------- + dy : float + first derivative for given x. + """ + + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 + return dy + + + def calc_second_derivative(self, x): + """ + Calc second derivative at given x. + + if x is outside the input x, return None + + Returns + ------- + ddy : float + second derivative for given x. + """ + + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx + return ddy + + + def __search_index(self, x): + """ + search data segment index + """ + return bisect.bisect(self.x, x) - 1 + + + def __calc_A(self, h): + """ + calc matrix A for spline coefficient c + """ + A = np.zeros((self.nx, self.nx)) + A[0, 0] = 1.0 + for i in range(self.nx - 1): + if i != (self.nx - 2): + A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) + A[i + 1, i] = h[i] + A[i, i + 1] = h[i] + + A[0, 1] = 0.0 + A[self.nx - 1, self.nx - 2] = 0.0 + A[self.nx - 1, self.nx - 1] = 1.0 + return A + + + def __calc_B(self, h, a): + """ + calc matrix B for spline coefficient c + """ + B = np.zeros(self.nx) + for i in range(self.nx - 2): + B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1]\ + - 3.0 * (a[i + 1] - a[i]) / h[i] + return B + + +class CubicSpline2D: + """ + Cubic CubicSpline2D class + + Parameters + ---------- + x : list + x coordinates for data points. + y : list + y coordinates for data points. + + Examples + -------- + You can interpolate a 2D data points. + + >>> import matplotlib.pyplot as plt + >>> x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + >>> y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] + >>> ds = 0.1 # [m] distance of each interpolated points + >>> sp = CubicSpline2D(x, y) + >>> s = np.arange(0, sp.s[-1], ds) + >>> rx, ry, ryaw, rk = [], [], [], [] + >>> for i_s in s: + ... ix, iy = sp.calc_position(i_s) + ... rx.append(ix) + ... ry.append(iy) + ... ryaw.append(sp.calc_yaw(i_s)) + ... rk.append(sp.calc_curvature(i_s)) + >>> plt.subplots(1) + >>> plt.plot(x, y, "xb", label="Data points") + >>> plt.plot(rx, ry, "-r", label="Cubic spline path") + >>> plt.grid(True) + >>> plt.axis("equal") + >>> plt.xlabel("x[m]") + >>> plt.ylabel("y[m]") + >>> plt.legend() + >>> plt.show() + + .. image:: cubic_spline_2d_path.png + + >>> plt.subplots(1) + >>> plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") + >>> plt.grid(True) + >>> plt.legend() + >>> plt.xlabel("line length[m]") + >>> plt.ylabel("yaw angle[deg]") + + .. image:: cubic_spline_2d_yaw.png + + >>> plt.subplots(1) + >>> plt.plot(s, rk, "-r", label="curvature") + >>> plt.grid(True) + >>> plt.legend() + >>> plt.xlabel("line length[m]") + >>> plt.ylabel("curvature [1/m]") + + .. image:: cubic_spline_2d_curvature.png + """ + + def __init__(self, x, y): + self.s = self.__calc_s(x, y) + self.sx = CubicSpline1D(self.s, x) + self.sy = CubicSpline1D(self.s, y) + + + def __calc_s(self, x, y): + dx = np.diff(x) + dy = np.diff(y) + self.ds = np.hypot(dx, dy) + s = [0] + s.extend(np.cumsum(self.ds)) + return s + + + def calc_position(self, s): + """ + calc position + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + x : float + x position for given s. + y : float + y position for given s. + """ + x = self.sx.calc_position(s) + y = self.sy.calc_position(s) + + return x, y + + + def calc_curvature(self, s): + """ + calc curvature + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + k : float + curvature for given s. + """ + dx = self.sx.calc_first_derivative(s) + ddx = self.sx.calc_second_derivative(s) + dy = self.sy.calc_first_derivative(s) + ddy = self.sy.calc_second_derivative(s) + k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) + return k + + + def calc_yaw(self, s): + """ + calc yaw + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + yaw : float + yaw angle (tangent vector) for given s. + """ + dx = self.sx.calc_first_derivative(s) + dy = self.sy.calc_first_derivative(s) + yaw = math.atan2(dy, dx) + return yaw + + +def calc_spline_course(x, y, ds=0.1): + sp = CubicSpline2D(x, y) + s = list(np.arange(0, sp.s[-1], ds)) + + rx, ry, ryaw, rk = [], [], [], [] + for i_s in s: + ix, iy = sp.calc_position(i_s) + rx.append(ix) + ry.append(iy) + ryaw.append(sp.calc_yaw(i_s)) + rk.append(sp.calc_curvature(i_s)) + + return rx, ry, ryaw, rk, s + + +def main_1d(): + print("CubicSpline1D test") + import matplotlib.pyplot as plt + x = np.arange(5) + y = [1.7, -6, 5, 6.5, 0.0] + sp = CubicSpline1D(x, y) + xi = np.linspace(0.0, 5.0) + + plt.plot(x, y, "xb", label="Data points") + plt.plot(xi, [sp.calc_position(x) for x in xi], "r", + label="Cubic spline interpolation") + plt.grid(True) + plt.legend() + plt.show() + + +def main_2d(): # pragma: no cover + print("CubicSpline1D 2D test") + import matplotlib.pyplot as plt + x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] + ds = 0.1 # [m] distance of each interpolated points + + sp = CubicSpline2D(x, y) + s = np.arange(0, sp.s[-1], ds) + + rx, ry, ryaw, rk = [], [], [], [] + for i_s in s: + ix, iy = sp.calc_position(i_s) + rx.append(ix) + ry.append(iy) + ryaw.append(sp.calc_yaw(i_s)) + rk.append(sp.calc_curvature(i_s)) + + plt.subplots(1) + plt.plot(x, y, "xb", label="Data points") + plt.plot(rx, ry, "-r", label="Cubic spline path") + plt.grid(True) + plt.axis("equal") + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.legend() + + plt.subplots(1) + plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") + plt.grid(True) + plt.legend() + plt.xlabel("line length[m]") + plt.ylabel("yaw angle[deg]") + + plt.subplots(1) + plt.plot(s, rk, "-r", label="curvature") + plt.grid(True) + plt.legend() + plt.xlabel("line length[m]") + plt.ylabel("curvature [1/m]") + + plt.show() + + +if __name__ == '__main__': + # main_1d() + main_2d() \ No newline at end of file diff --git a/crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py b/crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py new file mode 100644 index 0000000..8a580cc --- /dev/null +++ b/crp_APL/ctrl_vehicle_control/scripts/lqr_controller.py @@ -0,0 +1,282 @@ +""" + +Path tracking simulation with LQR speed and steering control + +author Atsushi Sakai (@Atsushi_twi) + +""" +import math +import sys +import matplotlib.pyplot as plt +import numpy as np +import scipy.linalg as la +import cubic_spline_planner +import time +import rclpy +from rclpy.node import Node +from autoware_control_msgs.msg import Control +from autoware_planning_msgs.msg import Trajectory,TrajectoryPoint +from crp_msgs.msg import Ego + +# === Parameters ===== + +# LQR parameter +lqr_Q = np.eye(5) +lqr_R = np.eye(2) +dt = 0.1 # time tick[s] +L = 2.9 # Wheel base of the vehicle [m] +max_steer = np.deg2rad(20.0) # maximum steering angle[rad] + +lqr_Q[0, 0] = 0.01 +lqr_Q[1, 1] = 0.001 +lqr_Q[2, 2] = 0.01 +lqr_Q[3, 3] = 0.001 +lqr_Q[4, 4] = 0.6 + +lqr_R[0, 0] = 30.0 +lqr_R[1, 1] = 30.0 + +show_animation = True + +def angle_mod(x, zero_2_2pi=False, degree=False): + + + if isinstance(x, float): + is_float = True + else: + is_float = False + + x = np.asarray(x).flatten() + if degree: + x = np.deg2rad(x) + + if zero_2_2pi: + mod_angle = x % (2 * np.pi) + else: + mod_angle = (x + np.pi) % (2 * np.pi) - np.pi + + if degree: + mod_angle = np.rad2deg(mod_angle) + + if is_float: + return mod_angle.item() + else: + return mod_angle + +class State: + + def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0,steer=0.0): + self.x = x + self.y = y + self.yaw = yaw + self.v = v + self.steer = steer + +def pi_2_pi(angle): + return angle_mod(angle) + +# create an ROS controller class +class ROSController(Node): + + def __init__(self): + + super().__init__('ROS_LQR_Controller') + + self.cx = [] + self.cy = [] + self.cyaw = [] + self.ck = [] + self.sp = [] + + self.pe = 0.0 + self.pth_e = 0.0 + + self.state = State(x=0.0, y=0.0, yaw=0.0, v=0.0, steer=0.0) + + self.ctrl_publisher = self.create_publisher(Control, '/control/command/contol_cmd', 10) + self.traj_subscriber = self.create_subscription(Trajectory, '/plan/trajetory', self.recive_trajectory, 10) + self.ego_subscriber = self.create_subscription(Ego, '/ego', self.recive_ego, 10) + + self.control_clock = self.create_timer(0.05, self.control_loop) + + + def recive_trajectory(self, msg): + + for i in range(len(msg.points)): + self.cx.append(msg.points[i].pose.position.x) + self.cy.append(msg.points[i].pose.position.y) + self.cyaw.append(msg.points[i].pose.orientation.z) + self.sp.append(msg.points[i].longitudinal_velocity_mps) + self.ck.append(msg.points[i].heading_rate_rps) + + + def recive_ego(self, msg): + self.state.x = msg.pose.pose.position.x + self.state.y = msg.pose.pose.position.y + self.state.yaw = msg.pose.pose.orientation.z + self.state.v = msg.twist.twist.linear.x + self.state.steer = msg.tire_angle_front + + def solve_dare(self, A, B, Q, R): + """ + solve a discrete time_Algebraic Riccati equation (DARE) + """ + x = Q + x_next = Q + max_iter = 150 + eps = 0.01 + + for i in range(max_iter): + x_next = A.T @ x @ A - A.T @ x @ B @ \ + la.inv(R + B.T @ x @ B) @ B.T @ x @ A + Q + if (abs(x_next - x)).max() < eps: + break + x = x_next + + return x_next + + + def dlqr(self, A, B, Q, R): + """Solve the discrete time lqr controller. + x[k+1] = A x[k] + B u[k] + cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] + # ref Bertsekas, p.151 + """ + + # first, try to solve the ricatti equation + X = self.solve_dare(A, B, Q, R) + + # compute the LQR gain + K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A) + + eig_result = la.eig(A - B @ K) + + return K, X, eig_result[0] + + + def control_loop(self): + + Control_msg = Control() + Control_msg.stamp = self.get_clock().now().to_msg() + Control_msg.longitudinal.acceleration = 0.0 + Control_msg.lateral.steering_tire_angle = 0.0 + + if len(self.cx) <= 0: + self.ctrl_publisher.publish(Control_msg) + return + + + ind, e = self.calc_nearest_index(self.state, self.cx, self.cy, self.cyaw) + + tv = self.sp[ind] + + k = self.ck[ind] + v = self.state.v + th_e = pi_2_pi(self.state.yaw - self.cyaw[ind]) + + # A = [1.0, dt, 0.0, 0.0, 0.0 + # 0.0, 0.0, v, 0.0, 0.0] + # 0.0, 0.0, 1.0, dt, 0.0] + # 0.0, 0.0, 0.0, 0.0, 0.0] + # 0.0, 0.0, 0.0, 0.0, 1.0] + A = np.zeros((5, 5)) + A[0, 0] = 1.0 + A[0, 1] = dt + A[1, 2] = v + A[2, 2] = 1.0 + A[2, 3] = dt + A[4, 4] = 1.0 + + # B = [0.0, 0.0 + # 0.0, 0.0 + # 0.0, 0.0 + # v/L, 0.0 + # 0.0, dt] + B = np.zeros((5, 2)) + B[3, 0] = v / L + B[4, 1] = dt + + K, _, _ = self.dlqr(A, B, lqr_Q, lqr_R) + + # state vector + # x = [e, dot_e, th_e, dot_th_e, delta_v] + # e: lateral distance to the path + # dot_e: derivative of e + # th_e: angle difference to the path + # dot_th_e: derivative of th_e + # delta_v: difference between current speed and target speed + x = np.zeros((5, 1)) + x[0, 0] = e + x[1, 0] = (e - self.pe) / dt + x[2, 0] = th_e + x[3, 0] = (th_e - self.pth_e) / dt + x[4, 0] = v - tv + + # input vector + # u = [delta, accel] + # delta: steering angle + # accel: acceleration + ustar = -K @ x + + # calc steering input + ff = math.atan2(L * k, 1) # feedforward steering angle + fb = pi_2_pi(ustar[0, 0]) # feedback steering angle + delta = ff + fb + + # calc accel input + accel = ustar[1, 0] + + + Control_msg.longitudinal.acceleration = accel + Control_msg.lateral.steering_tire_angle = delta + + print("Control command:" + str(accel) + " " + str(delta)) + + self.ctrl_publisher.publish(Control_msg) + + + def calc_nearest_index(self, state, cx, cy, cyaw): + dx = [state.x - icx for icx in cx] + dy = [state.y - icy for icy in cy] + + d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] + + mind = min(d) + + ind = d.index(mind) + + mind = math.sqrt(mind) + + dxl = cx[ind] - state.x + dyl = cy[ind] - state.y + + angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) + if angle < 0: + mind *= -1 + + # Project RMS error onto vehicle + vehicle_vector = [-np.cos(state.yaw + np.pi / 2), + -np.sin(state.yaw + np.pi / 2)] + lateral_error_from_vehicle = np.dot([dx[ind], dy[ind]], vehicle_vector) + + return ind, -lateral_error_from_vehicle + + + +def main(args=None): + rclpy.init(args=args) + + node = ROSController() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + # Clean up and shutdown + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py b/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py index fd808fe..1484822 100644 --- a/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py +++ b/crp_VIL/lanelet_handler/launch/laneletFileLoader.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): lanelet2_path_arg = DeclareLaunchArgument( 'map_file_path', - default_value="", + default_value="/home/matyko/lanelet2_maps/ZalaZone/ZalaZone_Uni_track_full_early.osm", description='Path to the lanelet2 map file' ) From a26a8a69761895e9a3c071d47aa51a43c8e9fe4e Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 09:09:14 +0100 Subject: [PATCH 18/36] copy with normal names --- .github/workflows/build_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index f56ae73..8e4c37b 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -18,7 +18,7 @@ jobs: - name: create a ros2 workspace run: mkdir -p ros2_ws/src - name: copy the package to the workspace - run: cp -r */ ros2_ws/src + run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - name: APt update run: apt update - name: install dependencies From 8b806b627d4d6308a8aec298f78116087b7ad853 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:00:37 +0100 Subject: [PATCH 19/36] Add pacmod install command --- .github/workflows/build_test.yml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 8e4c37b..95b41f8 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -17,16 +17,20 @@ jobs: - uses: actions/checkout@v3 - name: create a ros2 workspace run: mkdir -p ros2_ws/src + - name: Pull selectes submoules + run: git submodule update --init --recursive -- crp_APL crp_CIL crp_VIL doc external scripts - name: copy the package to the workspace run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - name: APt update run: apt update - name: install dependencies run: | - sudo apt-get update - sudo apt-get install -y python3-colcon-common-extensions + sudo apt install apt-transport-https + sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list' + sudo apt update + sudo apt install ros-$ROS_DISTRO-pacmod3 - name: install dependencies run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y shell: bash - name: build the workspace - run: cd ros2_ws && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select tier4_planning_msgs \ No newline at end of file + run: cd ros2_ws && colcon build --packages-select tier4_planning_msgs \ No newline at end of file From b096f77934ef79e2ca25f1eff08ffa8e376b5e27 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:06:48 +0100 Subject: [PATCH 20/36] Add submodule update --- .github/workflows/build_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 95b41f8..f8dce9a 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -18,7 +18,7 @@ jobs: - name: create a ros2 workspace run: mkdir -p ros2_ws/src - name: Pull selectes submoules - run: git submodule update --init --recursive -- crp_APL crp_CIL crp_VIL doc external scripts + run: git submodule update --init --recursive external/autoware_msgs/ external/tier4_autoware_msgs/ - name: copy the package to the workspace run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - name: APt update From 5578431b889624aca1a49b0e505185b0b6cfb29a Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:10:03 +0100 Subject: [PATCH 21/36] Add mods --- .github/workflows/build_test.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index f8dce9a..a8a3560 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -17,6 +17,8 @@ jobs: - uses: actions/checkout@v3 - name: create a ros2 workspace run: mkdir -p ros2_ws/src + - name: Off secrets + run: git config --global --add safe.directory '*' - name: Pull selectes submoules run: git submodule update --init --recursive external/autoware_msgs/ external/tier4_autoware_msgs/ - name: copy the package to the workspace From 34fb749fbf8db1a5f2c1b44f606d6f043491e686 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:11:36 +0100 Subject: [PATCH 22/36] Add -y --- .github/workflows/build_test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index a8a3560..1a55134 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -25,9 +25,9 @@ jobs: run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - name: APt update run: apt update - - name: install dependencies + - name: install pacmod3 run: | - sudo apt install apt-transport-https + sudo apt install -y apt-transport-https sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list' sudo apt update sudo apt install ros-$ROS_DISTRO-pacmod3 From 78cd032fb9cf8b42748f616e8258278dfc977c9b Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:13:59 +0100 Subject: [PATCH 23/36] Add -yd --- .github/workflows/build_test.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 1a55134..d82e9e4 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -25,12 +25,6 @@ jobs: run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - name: APt update run: apt update - - name: install pacmod3 - run: | - sudo apt install -y apt-transport-https - sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list' - sudo apt update - sudo apt install ros-$ROS_DISTRO-pacmod3 - name: install dependencies run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y shell: bash From 701a15907e45d70612556a66dc4aa8436e7c5c0b Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:41:07 +0100 Subject: [PATCH 24/36] Simple build command --- .github/workflows/build_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index d82e9e4..4bdaf99 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -29,4 +29,4 @@ jobs: run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y shell: bash - name: build the workspace - run: cd ros2_ws && colcon build --packages-select tier4_planning_msgs \ No newline at end of file + run: cd ros2_ws && colcon build \ No newline at end of file From 9d23895a50881100b255e745f2d5db01a6c0232a Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:55:15 +0100 Subject: [PATCH 25/36] Mod build test --- .github/workflows/build_test.yml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 4bdaf99..8f95d9c 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -26,7 +26,6 @@ jobs: - name: APt update run: apt update - name: install dependencies - run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y - shell: bash + run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y - name: build the workspace - run: cd ros2_ws && colcon build \ No newline at end of file + run: cd ros2_ws && source /opt/ros/humble/setup.bash && colcon build \ No newline at end of file From 98bcc5f3406bf86fb4b60848458c39cb45a3db16 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:56:46 +0100 Subject: [PATCH 26/36] Shell as bash --- .github/workflows/build_test.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 8f95d9c..dc6982a 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -27,5 +27,6 @@ jobs: run: apt update - name: install dependencies run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y + shell: bash - name: build the workspace run: cd ros2_ws && source /opt/ros/humble/setup.bash && colcon build \ No newline at end of file From f0b26900366f85850a890316d6fc3d031d5be8b1 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 11:59:59 +0100 Subject: [PATCH 27/36] Add shell to build command --- .github/workflows/build_test.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index dc6982a..30dfadc 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -29,4 +29,5 @@ jobs: run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y shell: bash - name: build the workspace - run: cd ros2_ws && source /opt/ros/humble/setup.bash && colcon build \ No newline at end of file + run: cd ros2_ws && source /opt/ros/humble/setup.bash && colcon build + shell: bash \ No newline at end of file From 30b54046dbaef6b58011c43c153e0265bd3727a6 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 12:09:20 +0100 Subject: [PATCH 28/36] add pacmod install --- .github/workflows/build_test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 30dfadc..217d3e2 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -23,8 +23,8 @@ jobs: run: git submodule update --init --recursive external/autoware_msgs/ external/tier4_autoware_msgs/ - name: copy the package to the workspace run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - - name: APt update - run: apt update + - name: pacmod3 install + run: apt install apt-transport-https && sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list' && apt update && apt install -y ros-humble-pacmod3 - name: install dependencies run: cd ros2_ws && source /opt/ros/humble/setup.bash && rosdep update && rosdep install --from-paths src --ignore-src -r -y shell: bash From 04eb189a492f53a0958d2d32a8d2390fec2019b7 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 12:22:18 +0100 Subject: [PATCH 29/36] Add lat_ldm submodule --- .github/workflows/build_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 217d3e2..5240832 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -20,7 +20,7 @@ jobs: - name: Off secrets run: git config --global --add safe.directory '*' - name: Pull selectes submoules - run: git submodule update --init --recursive external/autoware_msgs/ external/tier4_autoware_msgs/ + run: git submodule update --init --recursive external/autoware_msgs/ external/tier4_autoware_msgs/ crp_APL/planners/plan_lat_lane_follow_ldm/src/functionCode/ - name: copy the package to the workspace run: cp -r crp_APL crp_CIL/ crp_VIL/ doc/ external/ scripts/ ros2_ws/src - name: pacmod3 install From 9ac2aac28a4ef91e955cd7b29922b57d0c68a044 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 12:31:55 +0100 Subject: [PATCH 30/36] Check for build fail --- .../ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp b/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp index a441c95..b985935 100644 --- a/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp +++ b/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp @@ -15,7 +15,7 @@ crp::apl::CtrlVehicleControlLong::CtrlVehicleControlLong() : Node("CtrlVehicleCo this->declare_parameter("/ctrl/axMax", 2.0f); this->declare_parameter("/ctrl/axMin", -2.0f); this->declare_parameter("/ctrl/jxMax", 1.0f); - this->declare_parameter("/ctrl/jxMin", -1.0f); + this->declare_parameter("/ctrl/jxMin", -1 this->declare_parameter("/ctrl/speedControlLookAheadTime", 0.6f); RCLCPP_INFO(this->get_logger(), "ctrl_vehicle_control_long has been started"); From d3334b3ec157eedefdd45cbb2d5ca1360864d904 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 12:40:42 +0100 Subject: [PATCH 31/36] Move for sucess --- .../ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp b/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp index b985935..a441c95 100644 --- a/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp +++ b/crp_APL/controllers/ctrl_vehicle_control_long/src/ctrl_vehicle_control_long.cpp @@ -15,7 +15,7 @@ crp::apl::CtrlVehicleControlLong::CtrlVehicleControlLong() : Node("CtrlVehicleCo this->declare_parameter("/ctrl/axMax", 2.0f); this->declare_parameter("/ctrl/axMin", -2.0f); this->declare_parameter("/ctrl/jxMax", 1.0f); - this->declare_parameter("/ctrl/jxMin", -1 + this->declare_parameter("/ctrl/jxMin", -1.0f); this->declare_parameter("/ctrl/speedControlLookAheadTime", 0.6f); RCLCPP_INFO(this->get_logger(), "ctrl_vehicle_control_long has been started"); From 1f15d28c6c49c9fed08be2ab7b5dee6e34ea6d86 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 12:50:19 +0100 Subject: [PATCH 32/36] Github Checkout v4 --- .github/workflows/build_test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 5240832..87c1a99 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -14,7 +14,7 @@ jobs: runs-on: ubuntu-22.04 container: ros:humble-ros-base-jammy steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: create a ros2 workspace run: mkdir -p ros2_ws/src - name: Off secrets From d6fd3ae88ae30e1baf4dee3fe053bbc892f65a26 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 13:00:32 +0100 Subject: [PATCH 33/36] Remove dev branch from targets --- .github/workflows/build_test.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml index 87c1a99..067f1c3 100644 --- a/.github/workflows/build_test.yml +++ b/.github/workflows/build_test.yml @@ -4,7 +4,6 @@ on: push: branches: - main - - 67-crptg1-ci-setup pull_request: branches: - main From 0990e8e215e91a9002bc6fb4166399642f5173b0 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 13:11:21 +0100 Subject: [PATCH 34/36] Moded maploader to external --- {crp_VIL => external}/map_loader/CMakeLists.txt | 0 {crp_VIL => external}/map_loader/README.md | 0 .../map_loader/config/lanelet2_map_loader.param.yaml | 0 .../include/map_loader/lanelet2_map_visualization_node.hpp | 0 .../map_loader/launch/lanelet2_map_loader.launch.xml | 0 {crp_VIL => external}/map_loader/package.xml | 0 .../map_loader/schema/lanelet2_map_loader.schema.json | 0 {crp_VIL => external}/map_loader/script/map_hash_generator | 0 .../src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp | 0 9 files changed, 0 insertions(+), 0 deletions(-) rename {crp_VIL => external}/map_loader/CMakeLists.txt (100%) rename {crp_VIL => external}/map_loader/README.md (100%) rename {crp_VIL => external}/map_loader/config/lanelet2_map_loader.param.yaml (100%) rename {crp_VIL => external}/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp (100%) rename {crp_VIL => external}/map_loader/launch/lanelet2_map_loader.launch.xml (100%) rename {crp_VIL => external}/map_loader/package.xml (100%) rename {crp_VIL => external}/map_loader/schema/lanelet2_map_loader.schema.json (100%) rename {crp_VIL => external}/map_loader/script/map_hash_generator (100%) rename {crp_VIL => external}/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp (100%) diff --git a/crp_VIL/map_loader/CMakeLists.txt b/external/map_loader/CMakeLists.txt similarity index 100% rename from crp_VIL/map_loader/CMakeLists.txt rename to external/map_loader/CMakeLists.txt diff --git a/crp_VIL/map_loader/README.md b/external/map_loader/README.md similarity index 100% rename from crp_VIL/map_loader/README.md rename to external/map_loader/README.md diff --git a/crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml b/external/map_loader/config/lanelet2_map_loader.param.yaml similarity index 100% rename from crp_VIL/map_loader/config/lanelet2_map_loader.param.yaml rename to external/map_loader/config/lanelet2_map_loader.param.yaml diff --git a/crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/external/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp similarity index 100% rename from crp_VIL/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp rename to external/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp diff --git a/crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml b/external/map_loader/launch/lanelet2_map_loader.launch.xml similarity index 100% rename from crp_VIL/map_loader/launch/lanelet2_map_loader.launch.xml rename to external/map_loader/launch/lanelet2_map_loader.launch.xml diff --git a/crp_VIL/map_loader/package.xml b/external/map_loader/package.xml similarity index 100% rename from crp_VIL/map_loader/package.xml rename to external/map_loader/package.xml diff --git a/crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json b/external/map_loader/schema/lanelet2_map_loader.schema.json similarity index 100% rename from crp_VIL/map_loader/schema/lanelet2_map_loader.schema.json rename to external/map_loader/schema/lanelet2_map_loader.schema.json diff --git a/crp_VIL/map_loader/script/map_hash_generator b/external/map_loader/script/map_hash_generator similarity index 100% rename from crp_VIL/map_loader/script/map_hash_generator rename to external/map_loader/script/map_hash_generator diff --git a/crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/external/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp similarity index 100% rename from crp_VIL/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp rename to external/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp From c3d72504a918b5c14961f0b27932c5302f67fd01 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 13:11:46 +0100 Subject: [PATCH 35/36] Remove viz msgs --- crp_VIL/lanelet_handler/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/crp_VIL/lanelet_handler/CMakeLists.txt b/crp_VIL/lanelet_handler/CMakeLists.txt index 19e1609..ce4e0b8 100644 --- a/crp_VIL/lanelet_handler/CMakeLists.txt +++ b/crp_VIL/lanelet_handler/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(lanelet2_core REQUIRED) find_package(lanelet2_projection REQUIRED) find_package(lanelet2_io REQUIRED) find_package(autoware_lanelet2_extension REQUIRED) -find_package(visualization_msgs REQUIRED) include_directories( From ef9030af330c085e0deef308bbe2fe3ad14385b6 Mon Sep 17 00:00:00 2001 From: mesmatyi Date: Mon, 11 Nov 2024 13:13:27 +0100 Subject: [PATCH 36/36] Moved to commit --- crp_VIL/mpc_camera_driver | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crp_VIL/mpc_camera_driver b/crp_VIL/mpc_camera_driver index 9c4de26..56e9aff 160000 --- a/crp_VIL/mpc_camera_driver +++ b/crp_VIL/mpc_camera_driver @@ -1 +1 @@ -Subproject commit 9c4de264a40c262b6fd3ded07e3e2794284d154e +Subproject commit 56e9affd11fb1d0361e55d1d9fde4928baaccabc