From f0b43d6ac887fdb9a5acc36fd894f2895d1729e7 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Fri, 27 Sep 2024 14:46:45 +0900 Subject: [PATCH] feat(pose_initializer): check error initial pose and gnss pose, output diagnostics (#8947) * check initial pose error use GNSS pose Signed-off-by: RyuYamamoto * add pose_error_check_enabled parameter Signed-off-by: RyuYamamoto * fixed key value name Signed-off-by: RyuYamamoto * rename diagnostics key value Signed-off-by: RyuYamamoto * update README Signed-off-by: RyuYamamoto * fixed schema json Signed-off-by: RyuYamamoto * fixed type and default in schema json Signed-off-by: RyuYamamoto * rename key value Signed-off-by: RyuYamamoto --------- Signed-off-by: RyuYamamoto --- .../autoware_pose_initializer/CMakeLists.txt | 1 + .../autoware_pose_initializer/README.md | 1 + .../config/pose_initializer.param.yaml | 2 + .../media/diagnostic_pose_reliability.png | Bin 14816 -> 22774 bytes .../schema/pose_initializer.schema.json | 13 ++++++ .../src/pose_error_check_module.cpp | 40 ++++++++++++++++++ .../src/pose_error_check_module.hpp | 38 +++++++++++++++++ .../src/pose_initializer_core.cpp | 24 ++++++++++- .../src/pose_initializer_core.hpp | 2 + 9 files changed, 120 insertions(+), 1 deletion(-) create mode 100644 localization/autoware_pose_initializer/src/pose_error_check_module.cpp create mode 100644 localization/autoware_pose_initializer/src/pose_error_check_module.hpp diff --git a/localization/autoware_pose_initializer/CMakeLists.txt b/localization/autoware_pose_initializer/CMakeLists.txt index d07858d0b704e..521b07f1d6fc9 100644 --- a/localization/autoware_pose_initializer/CMakeLists.txt +++ b/localization/autoware_pose_initializer/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/gnss_module.cpp src/localization_module.cpp src/stop_check_module.cpp + src/pose_error_check_module.cpp src/ekf_localization_trigger_module.cpp src/ndt_localization_trigger_module.cpp ) diff --git a/localization/autoware_pose_initializer/README.md b/localization/autoware_pose_initializer/README.md index e4b9bdd28b2c5..2d8f0343f3493 100644 --- a/localization/autoware_pose_initializer/README.md +++ b/localization/autoware_pose_initializer/README.md @@ -40,6 +40,7 @@ This node depends on the map height fitter library. | ------------------------------------ | ------------------------------------------------------------ | --------------------------- | | `/localization/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | pose initialization state | | `/initialpose3d` | geometry_msgs::msg::PoseWithCovarianceStamped | calculated initial ego pose | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | diagnostics | ## Diagnostics diff --git a/localization/autoware_pose_initializer/config/pose_initializer.param.yaml b/localization/autoware_pose_initializer/config/pose_initializer.param.yaml index 90ec78257e6cb..e403160fb55b5 100644 --- a/localization/autoware_pose_initializer/config/pose_initializer.param.yaml +++ b/localization/autoware_pose_initializer/config/pose_initializer.param.yaml @@ -6,6 +6,8 @@ gnss_pose_timeout: 3.0 # [sec] stop_check_duration: 3.0 # [sec] + pose_error_threshold: 5.0 # [m] + pose_error_check_enabled: false # check initial pose error with gnss ekf_enabled: $(var ekf_enabled) gnss_enabled: $(var gnss_enabled) yabloc_enabled: $(var yabloc_enabled) diff --git a/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png b/localization/autoware_pose_initializer/media/diagnostic_pose_reliability.png index 7e2d8dc5e632a6a2cdef186809b7df6a8b275c46..cdd38242a0f24b22b353e6b052e4a68607a99bb8 100644 GIT binary patch literal 22774 zcmdqJb8zm^`|e#^ySBB9U1Qg_cWv8!YP;R-Q`@#}+qP}*8mHfbIp@6d{^tGv&1AAx zRx-1al}vKqpX*A(739Pb;c($VKtK>BB}9}!KtS!j>-jKH-%C-0?EkDlI0;EA!+cL( zFvelu>o@>Wb-*7xQ-G_1qX~$at(}bty_1oniHWU~xgFpdyqoWPCl(83b%3y=i2=aE z&X!o&!p7vg5d?&pk(u?dwSn>XQAQXu$5C=2F%!#CY5*hKVOR(g%W-j>S&lj>2naEV zq==xhTgJtvD*#;u59-FO$T(?zFnD5%O9J!q!N)V~U|FhU#Om4!9=jslgT}Nc#M%~51j-KlEw1GBoCe?GH zuBM3X&_nsn0-GE)xFa-X1<1ukHWHyQdv?(Pz320Nswvb%!Iwtvd6WT>7(VNo zfn50+xo+|bTcOthC2l(k0y6)CV}5evZ(7eF=yMe4@xgol;$Fd{E7O|#%X%;`@y&rA z9waPidrQ>emqQxBYIa5Sh}{WzQKsRqUfYfpuIyFW8{sA#+Ow7;ZzXrtLF(u!`plgndZtpo0G%2%ok?B6h`SFp1GTS>FZzna z{nX|+#$Zor8KCV;hE?}x*2{Z}7NKwX2KmYoPB_px%gQ7d1ai3}H2@Kudq4eSG^dGN z)3+WKdTNmDb_pY6<(9ka%ZE6P>&VN~G)pf$*M@AzsycLO46!huZ{VRbycITEVvQAn z-YO_Bs2mYs$@UOMp;&)T9ZB?nN$PcxGq2gtTkr#ljyL+^?Hob$zKrC?q1Ion(myz# zfu;%H??wEI?}6p2^lgg=-);#eusa$4f{t}Yy^Xl!>Mg~Y`Y`v>I726-w~{6&r4otD z3OR`NA~5^mkmse0f9PwC!kt!}eut5A;gPmOg#zT4s>jNkNZv;? z_l@}pWE_LO5F3HEhxN$M^vF!?`37bwET^DERYYJg3)uN%--gkXqZ;F0&sRVWJYi7a zldm7PgO{;;2?cCif#AS$v4IDmU1n&_2V?i#7#64r4ZU4}J2a zAX2Wptlx3_$$HWHaU<#N*!trqbWyI;BC7q2MNad*kVN?@XR2## ziY=*!7u5pXqaSb!Dr=XX=^>hy@!VDC^WKSntM0~m^aFt_L11y}Pt7$F{I6r*l8w4$ z*TDKJE=rw*FPGL-xygP{#Y2U@L0rv7B$qCNn&+iIGzuZo>Mz}t)cdxOz$LgTpIGb~ zl7GE#spB_MDsuR6?NREPkUcec{Z`NYY|V$g?Ou41YmWE1Me=K2+Iun zq335V!YjbcPij!*71ezX@wzOhZRDo>hqIx^pU6?Uz|gq3f`UYG5OAJ~k_3TZE81Tj zrlhH3!SnzKavFdO;NCv3P>D1rN4P%QeNu$k=EwGee+(;t8grzlMj8pt+KeSL?eOir&pDmLX|p>1*{1+a15@CbRYa1N?!|Yd<1* zqs^5{Ey`*TRa2Zj+Y?wMH1P zko@Utjj6=P2r4k?|J_V|KkFYa4;Gvp8j#w_=CNckUP-#3W@5+)+xs|dW@Cc_+XWk1r>J`(G z_@A4=;J6$l+G|8YNltTcD$5e{?L^*mZsd;uz%(qoS1g05YB$`P+Ru=uC`+oH-V5i?m0Bp_4G z&OxQgX>X{x+nftgVq^GqtCfby`|q^phJ(y&`)g~1Y$Yx(j+kN3euz2vZ~JV2I%>&y$ghvwgO39B2{rrraPmU{;9w~ho?LIZ%R5;=2oNWZbca?pG ze{`F204}o<(&g~!#qlaf&SSWUC^95*@s6nw6!CVC|Azykpx9j66?Ih-ajEx=l@qQ6 z+TPSK@74k+WCj`OZZ(8?M{s&LLsPL(U3H+C3TqfY07mUlW&l47a(qFQei=HGCvAtT zf7|(!W=Sm;C7*(f+RZ`4UOf@%o+sF-#_7%_@ z`OwUGKTJqn+Sl#)?*4Fymy;xo`^n{oE0&^YyT zgcVg}Z1_e6S~GJvWV4OWPlfHUe+I;|4+X+efJ+O+CityK>+KxTDjgOD`L=_)Et`A_HjVAP`%oj7!yn6P{PJs?;PV|ZXp=4(Mk|SEaAG|98Ic4=58! z^Da<~Pn@Z5%fpAA`|8(t7r)mD%YCz=t|_|%Z>njHECY3{246VFgVM;mw~#{ z4=1gr%$ghWDPMiJ+uJ!{Qo#w=xL%sA8hLX)lboA|WSA2v0i(R6x0BD)HIDbx$`pd5 zn3F{!Mu5rpY#vz4H$#PPKYJBRwjQCX<>iJA^sP#!<7%3dRsd3wEMZ8|pP{_d$jax& zdOZ@W0z>aF(`S7h7%16y$jLwg10Hrr0&bg;h@zN(Y@|w{r<&qMTt-x|Bmz^i~bo+3^66I3P1jgNqy6hjB)ZjS9jCQ+n z*u=ec+7MD$9t;tw2&;@J0*~%=dJZs95;KW3;QasK*P8p#3=C`)0wh`~@@0wzv6&pn z#iH;7_%=QEK8CuEG9?n}FDlXfGW_<39l2P{SyPrrc7w+sUjz%P!0nRIXh;7Be}*KX zKIx^M8NLnI8(|0`#Pry?%1oeIzyxwSQA0hwukF-#O)5|{u#PFi2s?}q^J52lw~ll8 zh;~P-^>#g3N0ypB?U4OrAnd-Kkh_Rl&I7E*DfDeP{^?<_0rim8j!3BlS9~8)mf6k# zRT)(XQZL*+3FoH9OyIu~cy2w@(_aX77d)ht1Th{7RxZ<(YPH}+nYD#Ex(x5l5VKI{ zeZk6L1=WP-RhH?^e4n;R`L%ze*Q~#NVuavPPT40cq)-*We{p9b+ZjwKBRSL`?Ml6K zaRjEz8D5_{O&HM%C+7es56F0+S0#EuDX-4%%ZtV{oE?5yrU_&iCmfpD3Y@j}!J^~; zjiI;&uU{=CtW*X%6PmZ&lVl%v(`>8D@y>>hp<~tq8s6eVg{F`w5UMM?OI)gW8hoJ@ zc;?L~z9~v%u;1EhPV_*_2WSGlqTH?OB2d*%k0|0|0aH;E=+u0CK7A6gz9K-FW?~=d z!U`|N@)0jb(<80BMYbnu1TWuW(CA6_2YvIw4jBE*p|JM(>v0k{0Ec7Da0a?DClFOo z=PR^netW36lHdO}0+Tw{9os1+vcGhPFJ-9@6qVWDepe>Lr7VaT3-Ky*+n=Qr6+Z)? z_$SY7ldrhfx)+^hw=lw)62Txnv;F>o3TNX+(J@##4{#TT?C6h0uG~1$u0?m~ zb138i$3N(l4GsN;M#GMuRW-q`KdX;c(c`BD z_w&ACA0dC?4tsI!r<+E__!TFopOPoZu0zYZ!SK-6IXGL$HJp@n#3h3MwOX#<-@7uFXQi!}Z`1JeWKw`qY5NCx)G z8zM3t2+zE{nDrP72B}nUnykhDY(^z~u{5nzlr1Vx_tdfw+q?!3%^ch4t}?az{3rWQ zP4Wwe`cgsJ+8y^E(_1rXAOU>3S$&$gC})51o#rIc_BdNZLj1UzS&n+S;5_+566_0f z^NXDn(BJ5}1|z+0dC$8nID5Vlm43|BjZRBWoWo0U*9HZyiPQzfL#jxz(c^n#tNOxl zP>voL5hN9y_KB#@`zeS!Zme!}>THh~(D$DhR2C`Oh&D54wh&Jl4{cJ?ysFrZIA$#^ zm|LIdHkt-%cvbYr6)@TU z=C8UG_pJP@$xWn+(-2%5?Hr?X@OL`BrlK8V5src_3F(1aBP;VrXY8`3eYpS6T1{jG z0p4Vn2BBb_O0cE7#GGzXrZjmFZ37e~n#;R$`^ z&vRI@i#p$&pqhaA0sK#nevPSX)z~bT*zb$;Y>)3-zmZkke>YIkjB{lo3=|S_;3dpnj&KIgC{g=a+GL-jRw}{%VTxRzjTwuhV@jlN6Or|gaO>6(>$I1;#yQbBBVGhA!Qhg0IjcIl>IBNcI zk3l8_!w}kX2O)^^mpHcLs~%y6rH80YJj;IE^Jiw;Ui3w=o#=CN>eNoUgsi^_r6lUZ z$?!AtJM{E8?TR(&yVhKF_P}aT!1ZXNaul`V8ea7Vw$PXRYK!wDdDSNEGZIp; zjl*}^&++C`zdY=F-SNJe{t`qw(U*@d#H;Iq-d8`{v&xS?#lRh@Hub$Z%oOeapmilP z2bosg;S=^enr%Z;1_a&wv}_oCteUFab^ZpKbMc;!2%kVH9Xyk=r$ z-(z?>p16$a5A~tKQMR1L=Ds~okCvtENU~E%Yi$&WUW8zBI~)#qi7f;xYtf0&w^NXC?AIKEf6@Thy$_#ZL^kPG#uA}=mD7p=o)NB;r zFX#YON*J9>8*~6aocTtT=Mq8aibujI{-j6I5>XO(xBy}v*Y(pd2qKSR*Y1^|C$q-16Yu>i@(lKiisym ziE~#guz$Ld$R7~N&tp$tZvQ*LE8MiGR8+Jvke}}PTk$nZdc+$>Q9N*S>;uH!GnE7` zC2bvQI?l}1x60e%x*IC#v~h}|KN}MO%{Xh@C>DJ-TWCIGsoI&N91L|l%XqjPMe`=! zynaV zTz-r(<$0lO66A62x5=bUIXNQ2OU0BpBN+!v?LN3V>~nV?o3<6WTicq6yAt_Ay|&H? zk^G1YArW>Q#!vdwav49jAemFpP$=(4k#t|13zjqI3-V+f&ziAw!1ef@hK@C12THXm zG5E)gC0|#{7riG8=OHo)Yp52QT$U!oXN(ZDv@v+ABw2HDY_}Y=C1rRXV7VaF0<;$$puPt^+PMLUc+Lf2^7zx zQ`U2CY1<-})RG>mIG&NI8s09jLsj6Bi;wSVc)DBBSM9P!kQy&sWVds23BQVS2-U0F? zXumdeMliWBz@e{-Nf_K|Gf%I|in{vLcoIG*4#{8w6mkOAI}W)eWa2=n_4PUsJzVJ# z6L(=uk>0S4j&(pXHFM;T89CQz9Ft^|{3H8UJO@CS!Q1(f{pI3v>@6Y&IZS%cL*X4L zsXy@YG{PXs`$4@_YB`0{eFIWGZ4VK+OX0-lDftxk3E3av0^~)sylQW#l^lssCHdL0 zc+1^-)`lRrx_nA_j5+FH80j&dV6dZ!L}VyQPAzvX*$3fT{5Qi`-jIkTp=8vK@`CHZ z;tIr9)I#+^Vx3RHs#4i7n;)!Ml4v^=D+M)947>V61DAKktXNw+h+!Dydy!8^MkSyLtH-Cono zlDG-5N?>bn&yM4A&+_=8rTkSrT`Fj|y+qvx!+C)z+iMthGF#y`2K%>{Pxm#CJ*bga zEvVASpZ?(colNJQy3ZEfpjn7e2KJ%MGgsM>$X)C3f3^xBpQtQi;0UC*jK@Z&q^dpK zs*NBn?&*sG)lkR98=R$&$N@yEyEET3o~)Db{Bn}Z5kwrQ36VU;fz&bwZ6~J_Hv5lX zr2eW>k=uuT>FeQ_)aQMm1$@8_7j4ZdxfMbC@1HQiWbGA`q^W&uIy@yH)TdVVbM9ys zJcG4~3lOXoY=_LUL;3iaMU*)Op7o`Xl2hLVL2fU&Wn)(jF}wpaJL~?sg7b5RwfJMT z?E}t~IS_W)_83XhJ(_ArHD47hvFoFS%m!`SNev~Aep@4L9xAJ~gP@W^fq*?y99VEV z=UP!ZZozKB`7^|oISz-L-ArPo!!-V2du>h^jKZ7q?Zbh>)zBpMsY++Z*-6ZZHS6fP z3{yGzVze<@4kXn*V7qfyAkowFSLcUYD20V#U(qL>Sz)eSC`HGewG;3^kKuxy?-xms z4c#{=8bxk8DEK3M0dTQi@tcIMp0>baAG~c(`@L_UjOP$3mgeRy$?Z{24u(nrMAXqH zFfUCiv3el08!gz~Ax@e*IXyrds59^fEJ_Czn5Hs(<&ijnd$yEX9BKyi=H@;)Ehq$t zM6CUBo3#(F$5sCmAd;BLPrF<>?U8G%d?dYp;t!1!!CC7fixZ|jQ_(x6qcBr zf*xP{)H=UV>2&zBz5ZpPT~@-BFndVs%JR(ny#9IsQu}2X_mj_)lUoH)BtX^t%3}7TA}VRVw=W)SV;`i*AHh+z0;{&kP0KAdE)%r2EHujiRmfYS)D1* zN6snG7!=-nidEe8&y`RJ*p-b>-=hlIY;FjD23x9xj7;bn8+PJHMXX?tagFdOirNUr z^sf2^h4Js;JcsV=nv;4)>TA#`lK}l>XT)No=zT6;#oB-RbZPTwd^8Mz)JAN*CK_b< zQ@BUF%^({y=G{F#&_=7oDc~@yY*+LU5t>K7tg`Gv$vW7z7^Fb3my_Jt*lvXLI%B)u zpCHe5KUX_4(WYQ|H<*pYmt#UYuU|4YH=*hd`eYv{oV<@ZUTwclQ>h3rAOt~l%M~HR+tnCI=Sv~90U$E4>YFTJ z^uCdSO{4N*T8pr9hCw`nD7t9X(C*aV(VG5yVR)7 z#4y#d%VzpCMFRT-{>+4$1=}5e(^FM%fabt2`n9l9S%R#~N51daMQ!8!&%Nq%&gBBV z`CHvS3WVS@RtF#3e?-Wy7&TgV7DzMKitjjo+vu|OYepfs&xwgJGw^8*sqak0VLIw& z``(hmuK!o0#VXjf4=4J#$eoW#>eRG=Fb6Vgx;LyTkXoN7@<(FE?z#yiDlG9Elp%Xn zZ`V0;^+&}zo6gE#QQD_{O-v9J*zV4Vy(tyO$1veT6$Ek>gzfebvg_Dp zi?)p`xSw7pI*2t|`WP5dD75J(&S8=&c5zU`K!~8tDS80zBz)#e;QWNXoTGgAE=0Ni zQ$rl6813`zIiELpnfrm${A*v+kV{62*V@`SX8wA|@xoN8^mhH(G{%y+l=#oz)z$P&=}CnCr}`Eyqk5}XaaOe-BYqyF(QMNS-gjrk4u9^&wg zclzCpQ=`A?e^&@eHGRX`99p{x4y4t87l7xDz4b0?LN_i!+4d6D>{w1URoUW3xR$^6 z3RVXvS`k~iW(RCGPg-KTh0$0uK9ZB~UUjy&)E$dIAk;%$W%fk#dK&ElGc;dENB`}d zp@%Dm5`Bf+@o*7bS|$iOP|=z%YO1$F?gO8S4W@-_u^nvgu^Xx{;5|E$ng(feFKhTw zk~5$9VjcByxzy-Ep);X8xEX>P(mwUaxaMU=qOXHAdkJf1*LzqxC_+9RAWhjg3+M!# zItl49i;g0i>AZBTbN-a)x>Ea>WftpiXKvIb{4cez=i}zUZQ}2oP#n2LS=>qY`pt@q z!3uDf?S0p>GFa0J{`$^_(QA%L@>> z&ykrH`jj`88x-L@#df|98etdSpm2y>{#sU<7(2~@GCT-Q6O z1mw?#4}J>?_|v12nGCymSZ~$G6UYnPBT)-fe1X^t)W4m)27#l=dkIzU-r;F9%u|Pz zZUFh#6%RO;F~|^$`ddH43gDR(RTi_R8B_^%(cV5TrlE`Bj1?y+*wa?vQUxqZ0}F+> zm;tvbHDG>vou{D2&&(LhtJ@rf#*%!*m0iF0?0G|c58f9+ugeD|{n0f4u|%)drQdRb zXbD^Wo@vXR6E&3CDM@nuHw%e`~gTFB*7W z<0eCvqUsZ%3cf_rIODV*#9og?#?FpeE52kP(DHg~lEC-f4L)W?#84Ds{p`Fma+l>7 z&e8@O8MQ30S3N(w*;}ozf#{dLx+VBbm5zz|1gFn@p*h`pQV5MOmchpoqB`rYxAG5p z57_EBFz6!Y%x}pqZ5YaJ?5mBR7+rGJyuf-wf9@ZrF3%@Cw+Ah3FiU&&^#9Z$6X;$} zZB647>X}ng|6z60<&);*eZ8H#jf*fl2-~;oWIsKz$H9Pwi2frFF`%ptDwa3 za(_qGe`aE01p{%=Bzb78bkw+h3YeeYho&=2fru?AxCTMTB(6G4p$>-eDfAC^mG++A?&>lRH zKDn9w`seb6q}iD1$zyJ=_07XHmNkoDSfUco({{pR&xOc@lP85_O#o>l`43_0`1-7- zR`PuHXmA!soD4r}z>g*N3g$D$`AMG7z9tHo>c7Pc9TXC96bWNq41;9xKSpj>c=CRO z5mfA)DB^b&t{ZG&p!Il4$kMi${8`X{`Igq>yZQ%$PZSHwL34XGhZ9Q zk}5LXo)YG~Gh?-#K|*pF0P1i%c1Td+uhrcb*Y9YPUyk^`=q>8aYH( zt*6~Z2O`zbey0RT^(?{_6n76QB$$!tgX?J44a^ywP>iZpO5Qgpk{)zguAL@B(JP_UFlpMc|7dBThQB^YFp}W(r3=t<(WV zKF)BqPR|j=uUow{werZ&VEwn= z2U-o)$&{moSb}zCa7EQR`X)&?o~IoHM{86U6LS^J4$db~QVGVm(L0FeCtrKaEjuAK zFv{;c5ED(g)AB>1r91pv&(A(*=iE=tTp_3yJ*}h2RW{2PD#3NiZ{E!-1$$2fA>LnW z;&;$g#3UF$DAa?r2FJ@;9)LQTyMkA);)0y#Ko*W^k)5{={)Y zpEJx$UW^Tb&RiIC%8c~o4F3q^8_H?66aw!lxDU-F)w@e_3?WaFH84((>=TCXu*xU(I38HDURuFBu2ib-RO2(y1OELhu zh~sEyEbZ4cYevOwb+y8l3wb0?D&xp+XX9WU3NJ1@FilRWk=FGBK1-m4a5j+pR^yE30Jy|I;_7GcVrTsNXCOMoRv|JQ$o|Em>{Yfv zQPPW@gB&nx-2HuIVFgcZ@s3iW0OYNk(8^uRsf_uYN9ak%ZiLU@aLEcm?k4(=&%NsU z>-fJ$k7IB)U?)57=nEdiPW83OE7JoU#_48qg4paykV;>C9I5d$cBt+8(t^2XTGS9* zj<+R;_#$82kWfjX^7HL@qKG1(K_rq0VQ#!zhZ$fP`nJ2)MYR6WzO#SuF2L6HZlZD5 zxQ`1CSatkRY6KB#jarrutep(`zl3DQ5D~NzYY8N2ye+ zv-n~M<|S(YwEl*2mYs^{ zoh`WM#J_!kiCFfIXk-6vr?vcwi7SQ?tdI|pHhl3MR^ zeMrvGX7I@>HC!csR@A>@G@{>l?BzX!UJN!Xm*^C9{{V1<)bA46b*I&WF*A zc}5QS1oyu&b=+ILhujdSK@1~QP+%Hx9 zfEy)IadD&X?R#m{RM#0W)s(YG|1aaiTRW+RlC|2&c@eP{n18$w=2eX`X0GQV5OCvx z%4WA4%uM)2NRZwgm<4{kD{(X)C5ED_N9qnA9CtmS-<#8{usRK!q)A}SJ5GG|ak`Q` zkW8@FO>@3?eWJLA`w9NoqrZeJGS~E#8&?VUikf5^H7~^%DY@mZrGc z$napn=!3NHWcm+ky zHkC`PqQxG9LTW%jBxc$nH@=9O|NT#uYFBirb`8P&#-MyPl(K(%5h7rIupI2s zLhCR@oF{BE>ur4;7qT)yHcY}lzh3UHPnH?M3Oh3O3sr=TP7%H=FK-tA)+0Y6`jytI z-&i;Wd9Df+*9WV)IeE1aW0tzJp;8)LCsdW}UPHhGk(Qw7qvyUNVNzWPivUw{sM?ksn%aezjKLO)I(iCM-+!&|`BAXE^hz4;Lg zOuY4zpCl8j_miKv^d)-`@YmRo`?susC1r;{0KRyX8Ci3n*Vhq!cD^=)6M{Okd2%K} z$$$V{kjPNRui<|ClN)+Um#Xj^N9P&uG1rqb01V6V1 z(Yc;Ly@Vo7Fba6&5aqIgx-L1oRWrPL$+2e4J6X^080Cg1+TpM}L4a%6UgY_m(Je2{ zS65{w?}=%0y&bjo>cemZYiz+TIn{D)G1#|lwV<;TG7%Lf82nt8R~CHK6VGGr52zHJ z6()Ry{ve7Smnwd*I+^6w{uarh`A`42rPwaPF2I6RP=V3Oj`xI{WdbwWvbTUQP(;F` zEdjxhNehEQqM?*6yf~7)p1mftJit$+a4Ty{ZfT(vu)BtU#voTFhbSQVeO{xfkC+~+ zgsw>;wD7BI!jiD47#`eQn&nPY?Iw^&o7tb=5x#=ez6g~TL1o&M$=1LwQ*c8M5v)=U~zq*TZ zn6P6Yh;@Izos%ol0GAs|m1a85_OZbUQ6Km#zV*%JYKZykKzwi!09!htpl7zW>RAI= z5Pv@%*~~B+UF#vN!VN)qi3oYqlHHq@ZCL1His!f+RNndhLl?)C!ft*sHL@<|o9?Gi zhUfbqY(h?!(o(xy;oTOCq{?g$Pmh;;8<2TUh!H1t=9-cca&peOrBn1tCD9O}&?M)u zxifYx|6@LIA*VW>G2huwzTPKXcYVb8b*UX7wUH%SRCy|-21%_Ua#s%S`g8o} z26qdXGiu4r$tdC}^EXu$rx!^465oiUI}V9)3Ln4auMKY$DG6BhJ=Tdc9!rW=SAwP4 z@URy?`V;WY^*Lg3rQ)(wh6s%)C4cX#bzPPbEBE39W^Rbxr5`U^xXc1WA`x((iQUV| zx{fTquTMOV#_9$)u^)|ua`M@vY_qKKtAkvHnK&f*+P8VM`4DUh88|WGW{B$%6b_pX zT@%nKB6-;Vix!PKo^?>xfId@%dSk`3*92F>ROHD7_RuP><31&^1K7cs52`cnGo)id ze)tWeZt=VAe)Jetu`f`QTj~g{dOSnvddG;SHlCt|yn4(DRu>dB6xmfGKO5=p6^5%7 zE#)oc;d@SLPZES0y6JCMj?QJXK+ewtpKKHcC#egV?cI zl;Vzf_^O}`TOS001AE&{EF3XLkJ~RRxuN~eF7>=9aI8pc&rVjXqj0*7GATqyjDxgO z{&(=l+B@3BhST|{{FhnYqfmYHmRJrPqYigipjy3y$21@Lm|t5T_leRJZk%x-T;>|7 z4CF}jqtUiI&M)V*{IpE4q564+?PByizLYTfRvXfgd{2iPLePEK$ZAt?$vAy2SDL9* zke4Y@?T-5dxsLx@0)%X4D*?0xOg9$<%Q+LgJ?aGfkb-D9i`0yQ6VQXp+^U3P3u^%P zw6Ex)B%lCrHKtOr;2mSR3ImI__Y^|7aply*OD8i(KpC@m0?<~23d%#4=sW(S?ONn| zOs@CA_XTa=zSOXoGks@ywRFZLg+zSnKDZ9IKPB3oQOo&}1tv<8%wJ?LK9HnkJ8#2U z?J>eFezw$^QEK8Q<(Ty&*OJD97;m)TBCodbXgb>lSRM)~<_hY`d@;kVYGD~Js^HuK zty%=tlT_5dr%qU{4SLYFn)yo6ChUZ4^=ex%#^WgybSeX`v{P~Q?jY%-@zEtJ5!jFnI1`$WIcR@ldV5)hsS7htqDk!{oQ#>c}s4w+FL>nGCo|%sq^uH~=sB$Uaj<$g)}7;x?y(_j>{)cAP-vs&E`}dz*Yia#Favl*Uw(?7 ze{=imR5)2`5CZP?z~H3aRK1|}y!~zPkK^OsW5Y&p!PJ>W* zi57ZrTtwDy*WUs3B4MzZgWJz~hkI3K;}xOPMfC0WRqTFSJ<)ewMEn+1yTq%A2O11V zZpGldo{DP?l~M%=D{@8_-1{SZK}S=#y(&sxqskPC_Ed=WHE*VW(P%9VmHa}=BAOFY z)|_@=k&U!81~lhJ7MQ?QQLQDYT=r34277#eUG@yW!a z21ar}W-G<*IxybxJKy1umCKazrBdu&R;R<{68t0oq%_-8A;jow`~vemKJvdvv77nK zKWu^H2WYln@bkvCsE}Zwl*RW<*rf1&+yq8#f8oP6(VOjzgd1_aQ2R8@Q9qzrD-zeu#m?d-CO`CyP^R8{FCnDa!`#5#{YK zO7tYfEHvBYGwgPz4&JV2G{m#H_qe8aNLWoZc|${i6vBP#MJ@D+s3rF1{e$7m-$6@l zf|xv2HgOUnd_noQHH2;4E4ACGQ2*%(F`P!)%%gQ(k<32^fPm-dmr9=Moq=Y#wslNWY5la_v|`{0nHueSno{D1(-E*pf+0?Z)Q#-&+gid7I@7z+GD35=W~B- z25GkOr(1nwl3VZ6e`MUQbJ=b-C0ZRto4(btE4#bymUTEvf5dygFE+q=X7h^^9euJH zt&IHXH{4uel%)Jgb@SvxsQi0rr-!2bWjSk`v!)H)xe(gX)mAO;=PB6NT7BZU8M4{2 zgqo%Gc>PyMy4S-Pip^Fv`+b)=1r_THAFi*=36NcpO)=jVz5_4O$*!zm_K01cC)f+i za{BJ3US{wRP2AzE3Cd@YOMl{eI~N!T5#NhMA*@ZZ%@b-=n;i4)Kpy(=>C2n$o9M>T zIgLc6c#vq1M*?b5Ya;0WP1I8zDWxw9oV7btop>$b3&Es+@c}Z;fcyOyY6dK*6HAI{wcON3mQjoCijenNA+(2kK}3wFD^EPvUj$d6pUc9 zl|v5niJjlztt|FLp`U0fO@^GZNca*ra?EFG#BEBmJWpMmZlwtuJUz5mI5SE)p5Wce zWiSX4k15a509bv*Nc^b@CIga^7({zKz5o8POhI0N7Gte&zFxV)n2UGz4FeM#!EY^S zIA0S){|&Jz+_CwxW=*_AyNil%$aV#gbt4HcoBmU%w!n&9$r0GUJ+JNXwV&c$lNr2> z*J5h4(_$@ABeL`6!8-=`FuFamFh7fVBdIS7u3r8%@J7hZSffa83CIYHJmS~CJQ%7; z29cO@a~jf|FwY;TLp~?Xn+jF8*#wVJMHWBYXoqECM-ceORXO9iXSz|&gKZUkG0?2| zYkGtAXNR%rV#iOG!!h@*iFTqXxA^k5R|VK`Oj)0)QnoIUX|;OvAZQW#dxyP@jzRbNAO45#axXDnm8q2feOqsh*IGa${gZw&Jd-4C!*oPfXhg6KIFL&@}v2FNq{29`?-uwBRBPF^#po-R!Ok3U8wDkxaZDACD472|6adLil#HQ3qtH z-$zA7eKzlXtNZ~iN5Oe};iFm;`1J3~VOF}-G+8Mfp-2R;c7r4^3@QKsYm?17Wz~z< z-BkFEC92KVw?j-cM&kb}QflPfct>G5TL3ssgq;Y@?ESN~6>u_qA#B|?J-JSpVXt?A zeZ!lWL8Lt$X|6*)k)@ZgT2HvxU2n4%6_C3a#*w`hTb+#d3kcS&8TTvYr|+a*pS#7_ z?CywD#^J}}!FiJv2lT_&eQTS`4jeuVOIIy_mkoV11gRsv5Vw~Xh2jbV^rRRbUgzHx zk05um^;PS``-|#r?1=B-}cGX3QG)Q9^9Tf{wJ*m0-0^Jq;w&Lg%;P z`V|D5ozSw5q}*g1>(+-G@SA$`;l_(0_cUz&W{bMGAHQ|4XLMUV%W13$ZTF=BXt|Ac ztUAm5m*|2qvqygOoQ!k@`YLK^rp&p9Ru!}|v`SQa3!o*_xAjv;v40yD6PKVOGlHaG z3+7@B0tC(0nlQNO2cP|nRZg>Wu-!^}3hvtjNyWi7X^I_8!SuC zBFoB}G%us)Kr3kqm8BJPX+la+g%k5cP*MM&2$C{)?!JG9SuT)GTS?7rO>1gIG;eXc zW;jl{BMQ%^OeJo38Gkuf@WUaN&?%@ z-Hi`xnqV-Q;Us3RQUXOpA0j9DG;*Zl%b(LTWJ5&al}N>YNgMpP&~iG7p{vv4qx7)1 zjw0nt`sbK3=Uw|6iE4Nr*0^5z+%vUC;#@9~#VW$$=u+}|T!4yKx-RGRV*&QT zY8hMJ6Hhx}?PjK(Ur9evzkH$_GvdtsOJjF1g>36upO&0F_a6*eZKa_|NpbVZ7x1@a zEWZH_YPy&>TxL*Af*dJzQKI>e18Vd|W%p0cEOOQ;-M;1dT&Y3vI*iLLUUCGXuyLzU z82)ea!5P+bR4J;h>}Mb0sdK>sV?d)$edQDj5hv9W)PPc{6#`GREW&iJJlWV)y#$rK z(AV3q=EiLm1OXPDd~&<0Xn-yR^C5WaLzP@!#`Ei!+JX^!Ro*>xst|)^T`xJ@G6F^} zt|aj)%(U1p>5L@Bx7TCEA&pbHo-Ue^P>(v!J5eYup>M0X*8IXsuTbW|xm(?BjykA? z9P@DrRzQMAYxiuI<^o`_7aaNbyMwp_%*QLjzQHB2hRisMNX&=x=vzgY%KUbF*E!fX z!ux^A@sjhws@hl~dJ=Liy&sEZiRPFWzi@#Y0cE~X6^jct6zy<-=O4H;X+iYfPthFUk#Yu-s=yzcmS4!NFp-NriSUAak&2viCr zJCXQNZk05YNiX!4IGq!-M53)EP`HVnbMk~<2vNMCm(ZDY-dB!ix*$>qRY`Jb+D{Wl zjV5twW@n)Vp$Z)?)?ci;5e@n(!8F6Zi6TagKpiJu+j~X($)LADs?5v{UU^(B{cl5| z1fdLmT5PTmqGZ)$!b^dWY}<$lJBx+MR3tLS#R{E-Q1}iJYfEde!H_B5X`>O6WFy*5 z-Y?395T>(Vi3hwC0#$?Lh_bTEo0}`j#L^TyA)ydn{g;TdMj?bS3ccQF zcFgrhri%nyNgz3k{>v^2p+6#~Csa)qTn4NZzxz;w!6=O7MdI|aqvG(IQ6f@>kZgL1 z1sBa+L*~wHHLP>qI?-!EakURkH9K$9Nii$YPPis7{^NmXDimMzvJ*Dl=7_U(NJ~U^ z-gO)6^1M|s^hgRq&1HTV7EVy>6vi(6l_CoYdm9eyVo$k1(skg0-q^gEdl*MR6=qnWj>;1&gw zub}p)=kNyzQ-Sg1E_R)$By-z#3QeGn?ayN^9FQbOVjoSRxe81LJJ_CMytxf!yUEzA zM~E`Ez7&N{l2CZ2u=tD#jCmX}TNJz+rhInp$is+<^Q#`Gfm%XR2Qe(I1R;#%<>Z-N zRF*V4wC2vZq(!GOSk$lUQKHk;6#v!Iws&{Dm4Y*iAE04K2mbrUab)$ zL6lKkREbde6Bp};0zqkEA(sV+iy2IRusNO%&Ugm~)41C-4j4d~O1M;7hcij^zp;B? z85h4DfUk`Xolb|&#+QMsN-^Z0;Ite%k+Bf#^1LNS?tOm?o0`AJyC2Obb>|Y^9I%3= zyH>MvS|@HQq;4`@iOEVx7OOZ&Xtf$7K$wge1t>#@v3#39Gp4-HCrdVyHgh!Vzh29S z8L2$kD3M;j;SXDwH*O0{S4<>*%Q%`{_kAr|P1V1QMk64hxpMw7nJ{5s=j8wL z!ji)%8{f~Q7w)5B#X8=2dM*d9G5&TVp-?K2(4)U$=v^R$l7Ip8S@nGMH8-H7^B`Ex z29U83D|c9yQ*c&I?BFRZ`{gKm-nttt6=bjcj=eRk7pa;=6QPk1<>}0yvzLqIMdY10 zf6KFCQFNMO|DL=&XCqlTOL;U0#U))Qh$=4pU9+1lbH1CvkV$Mf_@P-JW~~ZG;i*W5H+A1#8XD=4+yoqgioaKQjQ%xQ+0i*R8`|*Ji(D#S=hWD8&5Bsj9L7ab%XX*O^At7 zaca+D2ni=V{Exq|AU8CBi&ZlJM67e(6`L8`(g!#15F(qmqsNpTR0;`gP{^NdATtLH z?9X!#1)&y&tQplD7aM1qzVzcQI8^`>d2>?;c6TQ*tQoPLhV#t{6DU1t80?5~?|XC( zbi*^a3DK<+8JBhulcXXjq=Ds5ts;$Kjl2l(^{4G?J19p-vmsBCpaZ)u&+}4yC#2F0 zUTf=vySp!Uy}65W$%*zaJi|RUV7QqL%18!}NhUyLB7fxwVmw{&@^-`C&W(tn%PFjJ z=BT0l$d|asO~Q~pkB7qDaP{`W#nv95*s*LfuNYS&mh{Uyidgw%7@lr^gf?wK`-eZ_ zpb-=f!8G);<`UEr6&r;{GLkp5FEQ;C>6S8;UDf@iDwL>jEt04peRK9`KcR8 z-?*B^AI&E1lpHgWu^g+Mx3~-*O~Y}nI7#Nd1Dq0m#PuD=qB%nds;NcEourS_S^d8e zBqzqx;hx?+oY)u@AZfH$!V@)GK&f3IEdo@WKYD;)vx;$Oat}{@u#`7C=`iY@X%yv! z=}b2J_vKRI9!>XAb6NC4bIapIN)H+~w4vleDH`u+dXHPmit({%0n`zUTCsuI!@CgS zE>Th<;Th3|;j=feVpIfbsKEvWKFM?0ymAtcbO^;mQ%+%FDQd3>+J-n#X2EN}GIS`* zeq79$B_xb`!aU34Q)Eb;H?CZaOpaov~OP| z=Hxs^CwHOqe;($=PuG)ku@1-TW$GRlWt-G0CQ)=eN1Q3U@?XOlv2tLLP$CK2q|Zgc z9bRp}Sw@1)KNmT(;(FvUsoT_YVJagzGyBnN+5z*5?%I2KZ`(x94{OGM*Q)XIc0pBg znv>_t5Xt~jhV;Pg@06k!g)DhIf$=|?57MaP#*#YnZ#=%GRHpG<+YziXt2nPBbm$V6 zKOIqbcDmL#du3$)0n~a`!HU9M4rgX!cDY6u$t7bEnt;|c^*umt{t3#I&IGsaPRi)l z8Qa74mNP+a4FeZX940fG>`(QmOwR>R8kh~bOwwxg~eC>e#joV_*4bk@D%S$G{yt7tVPJOdi zM&=)YEXE)sBO_A}x#`S1Jw5$)*&!oSCy=jsnfk&_owuwUE%R?fzUF1>3paJ%ufF=~ z?Xp8g=8lq{p3bICo8)W08o!T;Ox;9|dCSPi$kan*=Pe^6BU2BNowtmPj7&YG);HAWbu3QJAj{O6r=Lm0%bML&qL}QT(%Eq!4~K-I z^k{5mkEQ7>yOu6v!>`BiPM*ksX7VK|BV!4c&AiKY@K%B!PCgx&xb>bq#Cg^| zABU%N5J?~Gzp{JNKuP{#cK?=t?QTvh+BfDb;-#ma3kW98tbUy*pBc}) zOZHG?;s)oS2J{)c)3-Md4IR%nhi|ZV)v$-zBZl$f8#7sd%$oABGBW>8EOOqa(?5~A z^%TXYGf4gMgjtmqOR=ggV-6cu?V+IP80%N>;G!_FcS1{C{laMX+!~6i`P8f7s!>7sTC{>o9K5i`=vLT_U7|^xV37=QJM2CFZFDWzmpEFtqVS(tr)a0 zw`N70>)NVd`_vRVwG1QB%LO})8XLC|?t0{1HW&QfS}E(NKF8ZrCo=Byeb>CQSF9O? zheASdCu#JT94x(+p{j=QH|9LnnU;7 z6+Iw|_cHI9?lkqWN2_xtu;qP>U6xJx4coi^99TZCC8WI&i@%c%HqH%b{op&KpS^h> zZr#4xM1ya12Re4_NXL%xv})*xLg`6VhmKXB#kVC?_cJpdN}ySw8@3t^w(dvAh?Hc20Z-Yxv8yQES)%aJ20nZ7Kg2(=lCuJ!lL!d^%Dv7Ib={eI#f`-)R05a+&P zc)wRzvhyVPH6=RKm5Sq+QM)Sv!TIl>rc=@w7H-Q&?cWf$iX)`Yd75s?Gsvtl9wa!Q zv4}a#w{Y;(Wt2`X=qd`p-V>$aAT#i+T@Iyh- z`e*5q{0SF3P2j8bY0Mk$PsX@D3|?@`I+1Lde|`lEWJ}bNnyeEzYDY$mQgjv+SSQ6E8_m?t+PYa?pr4vxG_0e&Mp@ zO4k|tID4*;W2sNjSZUX{-`l`e`O}cbo6S<=V3EZ%0XB zyIypOZ%;z^B;qWQbQAe}_0C+*CH;@ZuRlma>$~Xr>}+1^TFwuPzqddFPe!H+>zubL zh8{^lsJYA!!@>z_ox<3Kzfx2yaZm*wt=kZ)1Ys!POyOUjiI|5FA3= zSGH0iK$Kjq21^iWs9WW$%MEDME4ratZfBVv#+wXIOib5wzC_GbGe8vREJdWg9 zu@oz5(P<1E-oBIbCNLhKM{f@Wl4L{k2|uGpP?39tlNJuS<>RVAt5X9C+!B&W^ihH# zm;DDz%(Pm2_SH%Bzp;B?85h4DfUk`Xolb|&#+QMsN-^Z0;B>8{o6oYo14zi8A|ElXjNvf{gE@@~QZv9fM(OBgnl>VK0VnAzLsu`;Exf>4! zl-1aXmE&rvq@q%Q1PX=ur@bZi)lDQ6N(B;n^f%0<6bPXtV8DDvfcqz2IZMD=;Cc zB8ZEz2b5GDs+40#&tM|Z0{g7&`fb3*(+el#kNlRUhd47gf~pBIQ7Y!{ISe5wgv&`Q zGFD>U9HwG3V_W**<{d(0^LF%@vV%$?p$!Vb|F+B=6a;qcM3`E_m@|W}0RaShyAwBY z&-DwCs&HCH+aZZ!z8=(qcAdMDG+_hP6C#v>ku-6Tz_5=u6XKb?*TQ(RC}QygKKKOs z(xA(SWEvzK+dsv?7AhpKevBQ_3>$Ht??*Jn)7^~*-Dh#wpuoGw7@lgW!m#%}ItRMp z8Qg^E)`^TuyNF3r5fst@r81JiW0DC_naE!`f*4O1yu978w{s(6=<>f58+A&0fzhq( zq3jo?bq~VT%^OcA8yo}g<-K3@fC7(*NW7F_JT{BoG0(B-&uPsTjO!mpYv&rkAjsL{T6I)lDx8+HDVMqSER9VV;boP z&i}CxsqiD^*#QLZpG@EW6Pcg7k@StLS^Uv#(rQ|rmyxN4RnA*n29KuUI9Hq`bKe0@ z2|wccj$_fBAq3r~S6Wrfi+nkI2=R?vp*Wv{N*5f}W!KO86#hImdoknhZ;79DoE;n1 zvFUI*o}sZMbd1DB0?Fk*rp$ktq^9237+vx7weSffsqqO3L~-!~mA3xG^?#f1z8XfT z62OkOum8Z>{|zL*krRgEGSt4!>Gk4bHZFLC00kKJ&NPbh!gMB^{rhsMaF3?@sJSe9 zp*boP_$1F|^U6s)(jgQNO*w^yrKr6kXdB`{*CicC%f z#YEwwtZG=>@m0QfqaU#Wj!3TV`22ZDEErdmezfY^k(dx~bmeC`RcORHv^|eZ`i}J3 zy{l%>ia-W__$kBh3Bz40;pXLu!UFs1ERNlHcj;7msn#$ir7QP#?aSEBHniy!gSQeu z!lmnU(!PC>n3MAuo!o`a|9O}fKV45w-C2p1sRu~kfB(Jc(xuBaAIi0jYoPK6h6Hxw zle10?SeehfBui4nWa={VHGe(wnAB}*xeAbxoSFUTHSK`;{AumIyq7V9?l!f1)Y`>@ zK5XuByN$Y@$~2y9JAzea(`8hI4qd|Xrz7MYrZO`BP-?w|TSZ|mhch$H7cSC8a;YAK z_3A|*#3fE0&Lp$Myz^F_E3X@yS!85n{uXN4c`Jz+`!kheYq!F#@uw~ot~b4yX=sWsT0W8yi9%Jrp{Ydj+Xhi zAz$+{^#!xeJ3c<1`1ttSZHtVIjEszh$T4pj85x;+i0r&&WMpLOA+qz9k&%(9hx{K7 Wou|1gt~EaZ0000UIo!}w3+v4u-4$I;YoJEiCzPhTby1Rei z=7)J_y1Ke+dZwq}M>_JSf+XU1yziereL|F$5>x*4>9gg>wF4~7$GJCMd-WqgyNF1u z!hRgSu%;0o_qeX&nyxAi=B^$_&Ssx1>>cdP7+g%8&CKjwEFD}gp}GY>DzRCqYPyO# zn;E%UIoOk^TG^R>6n^@|%FN1s(ALPz4bF*RW&x*{kg#xoGlMx;4X~!C>EWxeOx$xd?RSs9iq66+b{5eodA`=9%^*JeSGnY`nl_k< zqAIn&CHGqkrT6-EaHJATY+c<#Xn;hNqSO? zO7(=B$w)txSlvtn$ax#V4IIr>gpt>u&N_x}th3)HIxDMRiBe@|;zu%3`XdGSxUrrw za%1k#eU+!;cj6jG)-28_$9*vS$}A0*0G|e&oNq+1&RgZO7Kv+hBiDxut!`Fu&!Kyq zT-gzw!KI_E=-kAfWqJO{zP=ZK|yZOOnp z3T?Ki9fCjo{Dh`kPqBSI=})ySI==x+kE75_;xM7DuFkxZ*+TN`JGfUAdBy$eTz1mS zM*<^Gl9o$PlkszCSU5V`PM>Jy+k(%|esY)KTd6nOGc>CHACk7sdYvk`W$<0m?hW}$ z&LhbwMV-APo3F-`yjq?mzc6Gt zx^faX(Z|*L@oK{5_9S+NS}yI%*9*kGXy7m@}QR>SI_^VddZ$)^s1vGID#I% z8ml{)S8^Fs1lZI+6wOj+lt}32!zzNq&Nm$6yc{piU7p);-t<@>7X-IiQ^1Yy$CpLJ zsUQV>U-XWxTQ{ChK@aexgwjqS%@6+?tz#d>aWp#J(vmt-x@+fI>dFoI0vs`7HNJx@ zaqg(mM9;mjnRiO5q063OM-+X_t85mY8$zZ&NRAFGiZAZO`yBC}Tp&-U%pK3gNJ)+r z&!1wuksaAlT^7l7bwm%tuMx$6fu-yF{by7Ea%G#N#rhOH9i-9?_u4yyrR_1HTh*d8 zx9#o$pfEu!b}IAr%A;uyQ#A;?TU+*Nh7H6&w5&$|=%JOP%XM{%GuKKcegoOD<}`M` zA_uB_P@c7A)(7V@Tw?Yq7Ype)Y)h@IK3iGEWQCPxQolol36958D<&@0(hRo=GGefR zA#^v_E!k~Zs~-Airdirm?AgwQpf4t;S~Khnlkud*TmftKweN5`xOc|#GN7B@LDRm$ z7Us^hEyMSbHRwlm!+-Z3ml02pL2KFirC9S3~8 zsz|--#(gK5;l>R9hVNC$15QDU{raU6i5smtn{Y9JkbTj&d ze+}xOOPeuk#~!wJP*-%#HpHh-PTx^JPV{g!uPrjrPJmbgo}8rQE2+b=I)c7GsrY~Rw&IFI%{r$fB6AH#Mu^76|DmNsncN{0Y})*Es5Yhwb91p%3}UMYJ6tYu+?r_9twSCVKA+|fYC=(O_?DH&JkmpS8v$Fr#m6%p;m}^uEf* zYzy?Yo=IA4fJP&|lk{s2F$M2=-WpBWY^kA2PPX5c{MdDjEIou?8avV_g#)+Wk&$)61t2iY5twKuGaaq zoNaYV#z|0Z=~b^B97S2N`apPBTm3WTPE^E!u?2I_)1Yo%rWeJN%&NJkhfvkwNiumV z19a4mft(e(4v)*U%?^)ouyih9B1LozL6d2@zW$x&83XbJ&}y`ft<2!k;LKM{@*~ef z4aj%CKIUB43#V{0qPpV1N=cWTPA(b89-o+XkUjrV1fsr4w2e)#T+FsBxs$-| z8jaqbO2xEM9}Uht1)StjHL6U#Z)k4lksUPUYmB6wXPYfn8kjBU<}k@T!_mMalPp|%xbN3yYB|cPlV6sX70Am0+XjZ45h4>qe*(Br{@Ar z;%7A7TpIXZRFoEtF%E`Uvf~C67kPE2^zy;YcHWZT@lbBPil4`d<t9nB2vx6wF{dQnN^jR!D(J6CA?Co6k3OPbYI9m8{oarld7u1}K6 z(%gz!P&+5dpAdDpBz2Cej~ z7ao-oCu&`8!liXbRB2}MNyPqMYwqtm<(>1pBmfc*S3#!dG0J2~rd$`T2U??`s`q>5 z>ebkfcI2dp2sZOLS6FtLE-rPM;FW`(nQP;FOMpJS%Kg)7{%C5tlMq3QxipPU6ou?e|yo~3z6GYVF~zl?2Tiq0<&^=<8^ z#HF%qz#y!Zw|$`H>KrJ!wq|Tl#d8-UaPEfAwf3l?PtW_su=j9=!(%(X_hzBI)LAUg zKDxK`klH7~WX1OSS4#ba!6>frhFwPNoxz#y6{dPJrCCtDoQL*gD&2BJ4KOj07l8hw1QLc8rW9@SiKs%(_Re4eLMPO83AVBEr~+4ZdZ~~ zO~X_fo`WlWI4V`$I;RWsu~9SP(C_allT}a_^w^9;AvXj%oRYge(i6Zb5SO*(5J@{z zM^Hq_pCTm>F4^DxHNIaz+Pb%7)b$5;LS-$p@#~BZKA59v=?Ao=j)7V zuMrkqhHsYBURO8_vhFX0>vH{Q&}S2f45rPeQ&axfJ+E}}tP11-F6$SblMW;%pHWT zu{(^>HRn4>7}HW(JF~8D#Q6>t@_i5#t8u7Nh=LixWa$@zy$u%zY7X9gYxf5f?-W4g zLIj5Ax1J<&t_%FK0ecJI>97<;_<`a1Y~RTt=Z(Z4zI{cvJOZBdR=jX`knbqA3*0yX zc};W=394*p^PTP@%J0vDLD>@-pB?ucKV`usOG81Iu{Ds_7;$DNb+hM8vGZO3SYMLM zn50>E=0+tLW%=D-wm0tTN9pl*$dix=^V;muGuoUrnV=r3Oyx(%9jR;5F{7!LvksQj zn7_=J!=?KvlVDS~VgD|6=JO4*FWdE3GNGE0nZcW56)}|AOZr-Vtf9pD&yBsMYpZd6f>CD_g{il-?=a zlX4+1Jc$}#9AeQ{m?Bg_^35Gh6_?&tTFiR|nw~|>VMikItmnc{ymBH+cEzN5gHlsCy=+E=+mKQ>!C4-r9B6bE<9)dfapUEdF8$$8T{&F7~st zx4#U867)9-33J>KqJ zW+|@;MX7z}Jb8HsU-+g)vcdQefmeH()%iujHmpw!i_Ot}m=u;ltU$!Wzc0l10#fqq zsp(YL*F;zM(0bC~V!4iFAJ)8=%a#3;rjUAYJOvOC*P-|}(bbcC=wGs|C62U7 z;L_Ljt@pKsSbO;C#yl#GIxw$tDH|#N-;*n`U_K$;Q)0kj%S$aik!TQXRNI!}au*$l zE`|5PQ3R?op5>iSN$*wu(PN2!VelG<2^sPSC6_ zCK}%19=-1_yz;(hcxtIuFs(CAp+yg~_-kwBR!3H+AV3TD`_q_@_#Wp+vNR#Y+EY@FQAph)+1 zdp@6T^lnxYL&KEZ?>Y+jAiVLP9bwo)ul|{40#PX31G&Z7n2+}>myIhApE2>?;&nro zSr41O-~xQ{v){N965Dw(?%Ib+>!5J`s-a&y7JGv8L~^%^CXuIa+{d?8wjCs-`rL<* zUy`82y|4Ay9N70(dCd^M`Hd@$Y!6Hp+3>jP^{+4E%VSGr3!X#%9{<$tBDuZPOnqUz z%2^M<+cwWF&wX|v$~T2ARSo3}2_WvX;K)&(c1oo7`?|KM(3y}ov-gW7{w>#NTccDm z^@r}+GdF&~Va&3w-&NC^=KC;!Q^PCMnXz0wosOPhCSHiZ4bt95MRk8B{nj}Z0{nHu z4|?NU5vnKR5Ns2kra1NNR)yKVzwdlueWU|sizU%nAtwvI

B9J{CqI<85?r5j za(AUS_k+W2DPAf|bG1Gk&L#S<4&^OAsEk!_kp@hlGS|-zu5~^uD5-(H!{3k*CkADe zLWO;~90_{QgCm99Q-M_{%8e1+a95N3#v?%TmltoJkZSoaz?KrkH#h4hM zm7cO;FENX+M`3Z5Zswdq4zK8h18B5{qQN|(q)hGrG0aQAFbO1^ueOHFALI^E0MaX@ zC-Q`qcLxjVUu?(F;#62)o37D7W~iN0B#i#M$_=NQtN!OpTNGng5aQ7rOp$?MYmAUV z*=zCUv{|2qf9yZ7c;U{&yHmb{bLQNn3pW;}smbpD2!X1y3iix6BUag@XzvUdmoHCJ zco`eE>;M$bZBa^P*eXa4qk6v^f&5ouGE}SbtAc-1F&-XLEQa+^EKa!5uw~OfZB}h1 z@d>r?`Cc@<oNrK04cU-4 zu6>l!++;GVkQW1$p5?CSS!3Q}RtPgmU^|Sev<9=k8HWHinbHs}$yUwk0aR2HOC-Q$ z?c$fiGsq3$_qR}S!pi)^UeQ+ zUIjyZv{W_tZMv?mzyT-BmAuXV^~jH}j^y~Qg!-L%n6&GU;aKeyyL|teDVavTYm8!_ ziob5X5_F4Lx%g!RD0Zfz(1MR z)Ie|YRoryp-Kaw9<6_wvg(>k|GU-plN<#rJY*fmzkjBfmyk0iOJ!pcmBJNjWnVHCM zLvw|;3{Mm7!fnf4rhT^txO{&QA;|G1q9tRl`5vV6(uxEY;C~XY?h}*GEb!()=}=ip zc-pAK;xzXSnWRcDHmTbdiH9S2shUM68ghGMQ?M+LxW}mjIPBUb=H}kJEDa$Mo9|ROgi!WJE%B%E=(hr3;l-@QB7iz`B}1Cq>-OB70VzUbGZqvgtaeb&CR{aU~1K zFzTb>=&PS}vZ&c3ayt)QB;@-D&<#3+QIc0huhXm(Nf(}s-5xrKQZ=dZt{JuL%UkJyFTr0arcT$XrE%+xLmjb~Bh zFG+{0P+K5MzTLuZXUWx)Buhwc`^^ zL4#>nc)2$XOWvfa^j8T+~ zFKlbvAz&7&cjjIO(Celyiz7}&%vVKg2+R13kZyv6I(*Ecyey9HDmZ8Ao3bJXS_73D zOQ-&Jm^deS;}y$)Fw4X|$4K;CW%<*HGds`(!H8^|hQ4>|Zo!jSJR zmM9gGsBiXcgQ&DGzw79@zMII4JLg3sDcbatN3_u_yX%`>hI=nu9Ce&H&rCSSZ)p~6 z_H`_n)Ja9L`K_2$lwAIj0@9?R*kESoWQt9ARMjrTOT|@j+p!;!tpeyr+B97D7jGa$ zq>{a!JHYpm*wYP++RM$cIB4Mp)qRLM$Y1BKGUL+I~H#$itIGF-KlU*xzj^v!ztAA}X<~vvL?r#ot_T1vR_k zYIhB;&*K|b*e$p>v0jGO%5^dqSOa;x3PcomFf? ztQul_o?dZhs=2|FEDEe$}*(n}omUVmu^|E_R^C#`e2Udw-Z(gSY;9kXlz7Z$?!ia$}U|gH{w?eIo(TE}>)aTE*+EdhqNq9vHQT<)A zg0xl`GqTl`+hcJRG*yk#EY9?_X4Z=9W;`x}jF9a}rR(mY(eqw=rl}<8$n4X?OYA;U{a(gG-C!ej&0(AkSH-glVlDCpA|d1CkQP{_o|$N&q&0KBZ$ z1pciz~g2&jBngZau9GDchi!8(LKPip+HVT(Tn1 z#L*5OgL~(ywS})dI3l+UQK$LEOU&c9LB*l~qkinUR@DSgosaY=dyvgR+Ujx1bk#+4 zz{{)dwnjzw+A~YMhAm=|Pjjq^fVv^R#8)^chl#kEnjF$eU%2dQT>rAB5CJG%!DY0=h4t}5B)C*PQh5H(g}J7s+b-o5w3cU zS{Dz-b|1+^nPEL(ckOr|@EEq^@RpV@S z(#YKGgeJtohZ{NmwL9cBH84Q_1Z+HB!y!)1`S>NIKRqa>0qQcY)5<*U*_^NyTu6IU zcdi+{98S=>?9N^daqFb?SQWz>CJV@7!jPdF%Qt4QDw+?`i13MvIoLP?M+I+yxn+3z;GH<#8SI^G7 z)s?F_20`6$!b<-t#XB5FakJvedA^3MUA@2Zy>x18B2(cSDQk9%CF1L>18R*&FiV%zw0zvgH~2J6sQ?Pv3mHs$~-# zkYacVd2>-KI*5fS{?7khmck3} zxZs$;;Y!n>q&Ier2EMOx!EYD-R|uSb`u@bsE3YqE_8}48cfNf{gax}-6J_*XU{Vc@ z6x%b@EN(O{p;ksz;jY%Km<-)lxQewnCJjBgHuTtl*-AM0$8 zOLJtq*0zM|IX`z{;s9SS)}5ZKi7j=ixFLReEclbHV0w|<!3Vd_q>x{;hK)cDt+YBu0R1$YzI;nH)4v;??% zHfD<&W8H5%8Isj3F^ETW>s<5TZ^T*6{;ksSVkIIKK4sbBmR*pi;|UQRv5Gl3Y99rz z*YkBtz$c7M;hH?!EixX|E>C^f4XFO(1yC6JUaiklI}9+sYjQN+h-N=x$>SKVE_8Y5 zA#w|CkZuu#=Dl91mSlnAF63-D$U(QaH{Xl{3^a!bacx5%(r`L_^uwmy?r-;U#|Y5n z9#bn~5-td89T2bc8Y<)8Hew&n34BGld4GVHZ6Q9a@Bhl=5z%Qou&zOFKAX)y|3^rL zGp9d;3Oawlq>|{J5>Y#LZ>OL0;7Av?awhSz?7pm0y5Gx;CHXK?Ph;Dzo6d)jPH647W&XTjeoj&_%EVtNj!|uH& zT}HVpnzWbl9Kwq`L8?)zyoWq*%G2RJK%8u^aedG#K0TvB$=UakX-=?4e+D9b zC?5G=?Xftsp+6Ic2`(~V^xGif7Z^Any8Bo)oBSFufcI6v(iAb+7mVlY*rb+ z(0)6h@`%yv4)%z`AYLuW(=DsfaUR;l3Kz)4gVV7|8W*a`6L8&1puHj+g@-b^rb=mm znK&>wNAZKoQh(0h{#w_)`O&k^Y}X9xn1b;P(8UYwA^_R`S{AWldegCM{D~!vi2eg4 zsi)tT-NG%xA5k<;=O>MxQCf|2Y9#IsUjSPu?EMUAY;Th|=WC+4r^g>H^aho8U3eUQ z_>g!6|CY9D+Kg~f5s<;T6z7&Hkv%yiW*>7o+Lbom)yYgMDS2!mptt7hr`;i@NZj{XJoTH@=zD`%ti+iE5wgoTE zxO(3{sS35bqA&4HH=HGxhWd2uCG!dOim$U?r9gowmmN%ffBng%&p>~uzo~_C{k&j* zhtS2_V>(nRtJYos9h;9R!L+>EXphgQQ4P-V+&ibqd3GW6;i@v%*3S>Gd!h8=6{~g^nQXrQ86Z{ucSFkupq|tRO-N@u}XM`9}-b4qRIm ztOXnhZTk2`W=C(8y;9w@C6Ff}8FWf-<{Uy^Umr4^lv({c+_X89l2~=-q{=A~N4b2V z%QO@WV<0HnS6e^>b2L9HCU8f2dfN{B&srsuD7-UU+_fN&QeqK<+OPRtHw2`SvbZl` z#j{@%+UQAU2WC+5Bu2wu(sG>x!6Ln61<1?f6~{BD>|#j{#;hBJh(DC?Ildh{G3*;Q zP{`Dew=P);VPiCc7P|8aQ z_l@e8o|r8f;O<4aC-C#`lvBEQv3>kuo&%iz%Sb_^GSHKAo}Tg0FNU;yB%%qkd{uhQ zV3`$wLGB@MTnHoVz)_%b=YsHR^&Op(uNFV*pYAv;0>J6T5d+B;RK6Y3RioPPc}0ot zT?RPv8C42a{)I5Hl{hlk%HMdh9t(?1xKi!-LI8c>)7noSnZDY6EeX?g#o=@Qd;E?d zH~Udwf!y?7^i(roOKukJ>kNJ}HM)^$--#N5M+&JN@(yU?Zz{n;>Tka*9UAp;Nl(9a zvlk^Qk524bz#OYQ!f_(}8(+izbGKq|jZqHL_H=9+QLw;G1D?2vF79Hd#+TKQ;q&?1P6-Ji_dVN9hkOQEH0n{(z%gb=TMSj# z7L23Ah)xZ55jg$aYl=6)-mULnt+>lkL}b=xRx#kEJAO(iILT`dm&^83vOv~ez#S}{ zxPGa0o+|wD&Hu@^m$kdics=j!>y80wY^+u~@!FIf0RHHrTX@rh*HD(kzImNKQ&7JT zM&x^Us5;)Gu6yPhrM52s3$j0Jn#y^bZ0qfQ%!1--*Bw|RQ|(L)QZ2rJi>Oed%Gv@{ zcR@-tQ^BmZDoLgb(w>sR1lbRfja3Ac{yZ^-T$+RA7M~-1!DhG}&kDmieupHH%p_0z zo2FX82&u{m+f#DhN&3`q!nXNY^nf3wcUIICUp5De=fx~sQ7`#_m5B5sn;v6Ygyn{M zY&?G~J)4`-mg$}3cv2BH(Kzg4^SA>iL?i_zrz6pqF==X4Dh`)hOfXWqE7HD*Ud6th zAMCjimP(wia^Y16>y6Y2YZ3_57hmGG>bqyp8L4Af%udX#6IV*V2do#{s!;#oTM09J z%&B9V@o&EgJX=tRz}e`WyZ%i#+E-p4W)$nPQ;+`+NPXMa*=p5O4oJs#Xubd+w zu}c?q4f;d>`+t(X&{Y5zg1zYPrLN@vE?zXVk#$- zlKB67V6_dIngX2=fnS?5>?C#nJPUKt9(c~73V2R{dBn630CVa>qJqx+f*c51pu$>apfkCP#mL&HuNiL;P>?mT-Xg zbEJ{gcKxFd%)fPwO3>cp0!UJ7zcS5nZI)n-%a118g%)(6CpN=`y(Pj9##ANgC1_au+%@(rc@M5 zX!ec9H0pbtQZR4aQ3!pK4&XUm?pXZdH&@ydN~nz68f_TIkyhDs!NMo+iQLow{217> zO3hx1rbQo8rQ>aEqOQd@8X`rp7ZOoTj5C}85!me*6LaUMo2NN_yN*dpah@61sr1Y# zrOO$pBbDP~U2}Nd;a1lpfrl=lj}T8-9Ra-g3LvT!&?fU+`MYG{P0{MIERFLZ95)X~ z6JIXn*X!Q1&#D|>vNC&M_xHE{7QPf+Yz`Q$Ppo(dnG9I?LG1m|68;~~((?(^-1T2| zkWCRBcEm~5D8&bER!#NR`dX&NXwcyLVCV-zttM;oa=Tp$@A~eV_mZoRIHtl~!&Gff zUP|B}s%AC*TPr+Lv0=xI^X(t6TQqoJ(Rh%qmETSPJYJlX{EuLa`!IE?V<#l_i!dk3#aiv>sz$1IwS(%4F3KY4|Jo^Rn@ zX<;pvVBTWH+V~ z$YDtIXlS&9kvBu^{#fXcX61*R)S))n+dDmO0_U5H*&?d2H}E3dSgnc@ph2fEjOlq* z(IjS3!i#yp;?+~eZA+lX7M{9}>;lGXefBDh7nph)eYM>3LbJH^y0+GQ4TeKTrUDwG z2!dnMoo1^YL6wLtJt)C`yK_MYF@8l~FBztd@jZE`j=DA5ZP~s}>53li#>XGyNxtWd zNizJM1DBzXo|g{PS^ddN^8GF<^T?x3S z^hRc)kDT2(iQnqVSDLh$vl$=Uv8<~BTO?v`Y&{HEejNgdW!b^o8oC)<0{RT65W1#rvD z^yYs2Klt&}97yzCAD8Up#K+0)S+72go4-XCTuHrYw&y_5yFi0zxOtw(uyy0lKWi4> zTc0LK{PntG;2c0k`+;EBOkb?jqSSyfQ9<)Ea!~Bo9DbJ|av{z^*+bA+y;ddAzs3y# z_$r=g`0sG(6s&)D!EIMa+rs};1{aNkrCdgC>yLcS4 zT(IM>eZo<0r{L)yO`!uQgLE^d%E;gLO2P84N*!&U$E;L`-7>y1m-W2`#i=CKdnZv6o|H_Bl}e>(SJMHZE8|pjCg~> zob&6?U&M{=b&bTQzYH)v1@~4||30vrzLZp`eg6l?>8pts@(QgoG%@{@`%6Q|TsMl; z`XQfQr#bI{SZzQKEnK~NSM?;uqfeixtWZM&x5Krn@QWE-z1m zin`Xf9P@%!vXj@TR&y5g#EXiCqmAOa>VETMLLk)qkfmLXt}jO3C_e1ij6x?(zhd_T zfbvx&$Qv(T;J!zKVj&uiWE-vi`&W%7csDbk*h4t3!2JpJ<~(=f^`;;OuCJ5Ix%cP& zbss*yrEzw|rI1(ZY);7{c$*)|#*CKD3<$K@`4Od^xDBMd2cy5vZ8xk$0eN?qSp9`o zM=$Hl=fUU%N(9l~5!Eb{F@vUuGo=h45XQ;+%jAozWmalZ$cKwcir*=QO^YAocQ$zvMr4ZZ!g=sZ+ukyc zjXtB{SLA7t_MU1W^^>%DdhMSM?k2*?>M<>^0e-r-tO*c>ZvMhUcra_<*W*Yo_Yl%V+&f0I{}pBGU1>)&a$uR?@Y>vL@MAZ8)pT5dk)I&9^3 z{^#>3lP&%v`)%8SAk(KoD(Of7-0$NVfm;oTz=^?4$(1bPzq|)JrG@~rU^fQIGu|(F zE#S-XX*}LeBt-mXxqKlEv0IRxP@FtZ(fr9abOcyp zk@52C#HAY0K2dVWh0hjnHtC-R*?u1r<~s_JQQ`ghjpZ17&WPF4lDp0K1$w!dOwfbD zIsR$c#*^b@?8YnLumfG_X6sM($}PllwVE>E@R7VC;1f|?(;1&_NIfq9u@SYLDGQX) zIX85H9d$7gj8a}gfZ?AOzEtb6S-RGQvkIWFw? z$#{K(6vkJb#N{2(Mh=nKlMiab{I_KSL5yS0)5JA;0lZGpOXq5ocWiPznBDVa^Z;bK z3&;uc8591;43|>+a=XLL6K0<1&_MfIz=2WdDu%?~Hsnx~QmB0X#E#b(j1V3RU?|d< z^@V==HS;oD#O><85b!QfYw;*xNd~mUfst=zZ6F`ydlNp>8{a523Qi3*2GyD=X+z^vpL@MecjW2ty{C-^0PUg4Xp`y4|Y!t zW|4d!{98h|>bID?lcZLTi5rMC1I*hWkG!1yZ^Y zV4NQxEoSOGx}UgyHp(iVblCjXDqkrb#MgLqxoM=Hg)1f9{E%c1`I>c~zw2yaJM6E?y{sClUU=Z;Dis{?yzhr*-mYM`6vFB0Go0k>+s9@3w3IMibMM0} z-+vZEqTaD)+ZO)Q0o2}vh zd?c>F_s@agu6V1JqC)V?xdpo@`;Yt}_s`=L%=HsJv56=oh2!M%B!JN7> zo-d*W4Sa`Qg*Dku`kkShN=Zy-=(-S zS-B3=PZj*V>Xl5H+ELnHoN^cyd6C3NhZaI#KMYE%s$m*46(7K_Z2xBpy~<(PwLzj$ zu(|_{dOe$)KkYkaR>aHUQ`yHJ#y3P&{0|5w!+96LiXcZyB|wb$pM93n;tFEbB8I{L E0|I%-Q~&?~ diff --git a/localization/autoware_pose_initializer/schema/pose_initializer.schema.json b/localization/autoware_pose_initializer/schema/pose_initializer.schema.json index d5b92c45d3038..4602223ea69e4 100644 --- a/localization/autoware_pose_initializer/schema/pose_initializer.schema.json +++ b/localization/autoware_pose_initializer/schema/pose_initializer.schema.json @@ -29,6 +29,17 @@ "default": "3.0", "minimum": 0.0 }, + "pose_error_check_enabled": { + "type": "boolean", + "description": "If true, check error between initial pose result and GNSS pose.", + "default": "false" + }, + "pose_error_threshold": { + "type": "number", + "description": "The error threshold between GNSS and initial pose", + "default": "5.0", + "minimum": 0.0 + }, "stop_check_enabled": { "type": "string", "description": "If true, initialization is accepted only when the vehicle is stopped.", @@ -75,11 +86,13 @@ "user_defined_initial_pose", "gnss_pose_timeout", "stop_check_duration", + "pose_error_threshold", "ekf_enabled", "gnss_enabled", "yabloc_enabled", "ndt_enabled", "stop_check_enabled", + "pose_error_check_enabled", "gnss_particle_covariance", "output_pose_covariance" ], diff --git a/localization/autoware_pose_initializer/src/pose_error_check_module.cpp b/localization/autoware_pose_initializer/src/pose_error_check_module.cpp new file mode 100644 index 0000000000000..b639731372d10 --- /dev/null +++ b/localization/autoware_pose_initializer/src/pose_error_check_module.cpp @@ -0,0 +1,40 @@ +// Copyright 2022 The Autoware Contributors +// +// 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. + +#include "pose_error_check_module.hpp" + +namespace autoware::pose_initializer +{ +PoseErrorCheckModule::PoseErrorCheckModule(rclcpp::Node * node) : node_(node) +{ + pose_error_threshold_ = node_->declare_parameter("pose_error_threshold"); +} + +bool PoseErrorCheckModule::check_pose_error( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose, + double & error_2d) +{ + const double diff_pose_x = reference_pose.position.x - result_pose.position.x; + const double diff_pose_y = reference_pose.position.y - result_pose.position.y; + error_2d = std::sqrt(std::pow(diff_pose_x, 2) + std::pow(diff_pose_y, 2)); + + if (pose_error_threshold_ <= error_2d) { + RCLCPP_INFO(node_->get_logger(), "Pose Error is Large. Error is %f", error_2d); + return false; + } + + return true; +} + +} // namespace autoware::pose_initializer diff --git a/localization/autoware_pose_initializer/src/pose_error_check_module.hpp b/localization/autoware_pose_initializer/src/pose_error_check_module.hpp new file mode 100644 index 0000000000000..358156036a8ae --- /dev/null +++ b/localization/autoware_pose_initializer/src/pose_error_check_module.hpp @@ -0,0 +1,38 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 POSE_ERROR_CHECK_MODULE_HPP_ +#define POSE_ERROR_CHECK_MODULE_HPP_ + +#include + +#include + +namespace autoware::pose_initializer +{ +class PoseErrorCheckModule +{ +public: + explicit PoseErrorCheckModule(rclcpp::Node * node); + bool check_pose_error( + const geometry_msgs::msg::Pose & reference_pose, const geometry_msgs::msg::Pose & result_pose, + double & error_2d); + +private: + rclcpp::Node * node_; + double pose_error_threshold_; +}; +} // namespace autoware::pose_initializer + +#endif // POSE_ERROR_CHECK_MODULE_HPP_ diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 912c6ec9b57a2..fc9c4afab2459 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -19,6 +19,7 @@ #include "gnss_module.hpp" #include "localization_module.hpp" #include "ndt_localization_trigger_module.hpp" +#include "pose_error_check_module.hpp" #include "stop_check_module.hpp" #include @@ -60,6 +61,9 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } + if (declare_parameter("pose_error_check_enabled")) { + pose_error_check_ = std::make_unique(this); + } logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); @@ -167,8 +171,26 @@ void PoseInitializer::on_initialize( diagnostics_pose_reliable_->clear(); + // check pose error between gnss pose and initial pose result + if (pose_error_check_ && gnss_) { + const auto latest_gnss_pose = get_gnss_pose(); + + double gnss_error_2d; + const bool is_gnss_pose_error_small = pose_error_check_->check_pose_error( + latest_gnss_pose.pose.pose, pose.pose.pose, gnss_error_2d); + + diagnostics_pose_reliable_->add_key_value("gnss_pose_error_2d", gnss_error_2d); + diagnostics_pose_reliable_->add_key_value( + "is_gnss_pose_error_small", is_gnss_pose_error_small); + if (!is_gnss_pose_error_small) { + std::stringstream message; + message << " Large error between Initial Pose and GNSS Pose."; + diagnostics_pose_reliable_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } + } // check initial pose result and publish diagnostics - diagnostics_pose_reliable_->add_key_value("initial_pose_reliable", reliable); + diagnostics_pose_reliable_->add_key_value("is_initial_pose_reliable", reliable); if (!reliable) { std::stringstream message; message << "Initial Pose Estimation is Unstable."; diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index a44f7f70b4b43..b9a6a66590b78 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -28,6 +28,7 @@ namespace autoware::pose_initializer { +class PoseErrorCheckModule; class StopCheckModule; class LocalizationModule; class GnssModule; @@ -56,6 +57,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ndt_; std::unique_ptr yabloc_; std::unique_ptr stop_check_; + std::unique_ptr pose_error_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_;