From 95c6fe0f1e27b6059d2c00e1543100fd673ab110 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 24 Feb 2026 14:39:49 +0700 Subject: [PATCH] update 24/2 --- config/pnkx_local_planner_params.yaml | 17 +- examples/NavigationExample/Program.cs | 11 +- examples/NavigationExample/libnav_c_api.so | Bin 148832 -> 147840 bytes src/APIs/c_api/src/convertor.cpp | 19 +- src/APIs/c_api/src/nav_c_api.cpp | 6 +- .../diff/diff_predictive_trajectory.h | 22 +- .../src/diff/diff_predictive_trajectory.cpp | 214 +++++++++--------- .../mkt_plugins/src/goal_checker.cpp | 35 +-- .../Packages/move_base/src/move_base.cpp | 133 +++++------ 9 files changed, 241 insertions(+), 216 deletions(-) diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index c422931..9c205af 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -53,10 +53,10 @@ LimitedAccelGenerator: max_vel_theta: 0.7 # max_rot_vel: 1.0 # choose slightly less than the base's capability min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity - acc_lim_x: 1.0 + acc_lim_x: 1.5 acc_lim_y: 0.0 # diff drive robot acc_lim_theta: 1.5 - decel_lim_x: -1.0 + decel_lim_x: -1.5 decel_lim_y: -0.0 decel_lim_theta: -1.5 @@ -88,7 +88,7 @@ MKTAlgorithmDiffPredictiveTrajectory: max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9) lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) min_journey_squared: 0.2 # Minimum squared journey to consider for goal (default: 0.2) - max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.5 # Maximum squared journey to consider for goal (default: 0.2) max_lateral_accel: 0.9 # Max lateral accel for speed reduction on curves (m/s^2) # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true @@ -105,7 +105,9 @@ MKTAlgorithmDiffPredictiveTrajectory: final_heading_xy_tolerance: 0.1 final_heading_angle_tolerance: 0.05 final_heading_min_velocity: 0.05 - final_heading_kp_angular: 2.0 + final_heading_kp_angular: 1.2 + final_heading_ki_angular: 0.002 + final_heading_kd_angular: 0.12 MKTAlgorithmDiffGoStraight: library_path: libmkt_algorithm_diff @@ -140,7 +142,9 @@ MKTAlgorithmDiffGoStraight: final_heading_xy_tolerance: 0.1 final_heading_angle_tolerance: 0.05 final_heading_min_velocity: 0.05 - final_heading_kp_angular: 2.0 + final_heading_kp_angular: 1.5 + final_heading_ki_angular: 0.2 + final_heading_kd_angular: 0.05 MKTAlgorithmDiffRotateToGoal: library_path: libmkt_algorithm_diff @@ -176,6 +180,9 @@ MKTAlgorithmDiffRotateToGoal: final_heading_angle_tolerance: 0.05 final_heading_min_velocity: 0.05 final_heading_kp_angular: 2.0 + near_goal_heading_kp: 1.5 + near_goal_heading_ki: 0.0 + near_goal_heading_kd: 0.0 GoalChecker: library_path: libmkt_plugins_goal_checker diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 768c885..520c281 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -241,6 +241,7 @@ namespace NavigationExample [return: MarshalAs(UnmanagedType.I1)] public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback); + /// Helper: copy unmanaged char* to managed string; does not free the pointer. public static string MarshalString(IntPtr p) { @@ -572,8 +573,14 @@ namespace NavigationExample Marshal.OffsetOf("data_count")); NavigationAPI.navigation_add_static_map(navHandle, "/map", occupancyGrid); - - // Cleanup + + System.Threading.Thread.Sleep(1000); + + NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped(); + if (NavigationAPI.navigation_get_twist(navHandle, ref twist)) + { + Console.WriteLine("Twist: {0}, {1}, {2}, {3}", NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta); + } // Cleanup NavigationAPI.navigation_destroy(navHandle); NavigationAPI.tf_listener_destroy(tfHandle); } diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 3ba853654d4629765ca13826c5179e8359b46008..a758b260390e9af8f5ee570dd7915108796ae71b 100755 GIT binary patch delta 45258 zcmZ@>30zgh_rG&}2rA2S;{k4{plB}OiVF#@PesLjPb@Xf4HZpIeJHpflGtctrA20G zX1P^bS>lS7CbrsQQfb~7E@o|&`FnKNh3%$kObG72l;2 zt`iTyV2L&qwB&T4NiOlA&c&UFdmZj)aC6)naBsx@Jnl`n>Dr8&#yuam zb-k<#TNQj&!EIUvP1`Q2Ch66wd2I*oHxzBBg1Z2B3_iLsMNq6iaU%5xFP^+;jWFl z4(@umtt(QM8v;h#@I-HfdSiu)0c>KUH&x|kfX#7-x5Pgb{dC0vwo>Ib`>OhnuHYEZ zvguqtt#TAk(H-m?e%R?^t$F>bF7~C^bC$XM7Q|>;HI6{y0ae1-5BzXt7kg2R|AM(# zA6_-Y#VYbRr;DGg;%2KvToEK9+bIba$K!%regonx4QYX4>?nUZ(8WIId#bqDTU7R& zWwpSX^Ba|2e*2ohjntywu&S1(0<+yAd>vYUh@TB{@w}igzXeuHem80>PQUFgO>02X z>?RrJ_gy24vdSv4FteTBmUcf5nMkw6FWXzT?hX#~YtvL&!rn2hev>q^5#$~o6B5P_ z@}GiTJS`-aZ7_Q}zd8orYraH~vuo56PpKN_x24RwM>&ICACQfFchy+G=~lb^4)}T}D62$X zqVC|$sx@PeiJ!iNA!*gZ*lN>DZOeQy$KUe#7}DyutIQ_9KO?WAe-wNNecq{N3Q{I+JM9*_Aa{M*Y_`gsXWSM)Z^G@|7Z|!Vj+}6{WS42GJ5u*H+APY9LiDRUQ2# zzJ3RZ&zmUuRmfB_T0c3iPWe6=kX+h^3_qS2q>yE#W#+l-5@A zFP3EOycz8X(s>dO`^;iWW;@W3*|QT;K~JUVPNc-=#oym>B{e%z zRaZwS{^yXxDOS>cm54*C17RxWJmx|R+FB)Wv8`;N;SPy-OySoeaZpUGQX5li9v_Dt zFJoF3tKjM^?b7BT*_qSzNr`ww=?Pas8t}1+AEjA}5*o>dwQf=%4%3CM61c>4HEmyT zjgmm?h@QL*gEW;`tXNsmSsFTm-w1PgQa+LFaF2O)jgs1m)TC`zLE@iKk(c*C2sdgY z{g+3}J1%RRF|3Z%Q>>;!yi}u|S5qWU;yrM^yxJ+V+z*&rMP--L_axsV%AiwqWdqJO z_xtZA_{kkbXC?pNlC1rL&?bKur%6PlNyha04B3GMG76=@P6@Q!XoY_uVUyp@taCsz zPE`&3r8+QKb-?QJVKtfa8c0U3;$MJ{G@$;Mst~qjf2<0enyRT&JY4ek_fwnB0Z0QhchVl1J8iK@lB@Hh zrNAf&v?EA&#D7xG5pBC_;MZnqXl_fyHih4SC5H6uep@1F3rp98));^4alufDsj3Kv z5v3F?PSpcz4h~c+g{CGm9AjR(zcO@>tpBX=n=r8I7LlkJXMEq&i zPVh=KDUko2t{2t(ofsIg2uBoSmFT`mZkhN?sawdNHRrSz!Hz&wS2r!Ly3 zX%MOAaik(lLT)E~n!;N{lci!M@qs+?EixuG{NNtFnmNXy+K31E|ERC{JJR+7*ORK@ ziC6}y;nD}D(``gG;RDp^k(JwntyvQ)^&C+#(O$($hAt5ulq-fblntk;MQa+3KV3T% zA+Nm@SR~QfW2$F)gCxGI!Z%YxmbX_T<|ur-L>0U$CByUP^*B;8dB}RGww8O(_-ILp zRjXWoB`_OfY>wp@5;0iLSM70?51cBoz&`Veic}4#4y;!AT!nY4{g^fO{xTTxcr#b_ z%u1zm;-sQHl|ZtUqMis>bBu>cewBB$f7KWp4@~1FNB}gAQ+|~StXcCcQakO(!py@x z_}@x6`|HaF(mKip(p1B`a$Tv4>PiZ~vZ8YJn-W3Oj;?)b=M&jl>ah~hvSgY{wsv*lzLuTcV(<&twT_eq!wo3x-Dtbr{4XIyB z1&C$y`U6AYp{S03V9Pb=f9xpVRY4c4@yDB?|A9?u7J`O4?1@x6pXZdo z?zU1OOg-9eSNO-&bSnKtB1Wrxa2p+cnKGstW6PjaWI$?wHkowgN-?dYIqyR?r!>e@ zHvNvYm!8u}R~^^xqG2*r z>moZ~Jwx`ud?x+Yk??Bu{Gsht({Hz$mo=0@#VVs^T$hLyY6uotJysie%R@h@*;Mq7 z^qlcg2kkOC`J!wD+5oDT%oYBu0qM^Qv9)j3jdizSh13e zD)A?(%m_B~ictn>VPVpurfMuhm8*B}n-a6kzv7(q*%Ribi5dT}F8pfCD&p6_Su>s* z`%=Z}gC~~^5HJ-rKim>J!2Ra5`Yf2AnO1+_AJg8YGb4uob|uQiH4r60Cg#99=|zHL;vNH)0Cb)k@k0;OHXy6NwMf18nq)+OvwD2L1(ScR_!kRzf%m z>hX63Tv}PXqnN(fHPsIFeN~@g(kp1qQKyT6_FhyCnUBcWY~dA2)%n%ym?hc8vJFQ$$E^9)!|fa+)Ouxd@P4ygb*u%7R9O3eqPD^9 zojLSMnDCbC;UA>>3!#Zy-`I64h{Re~?3>wNx8^rp7;;9U5Mlw5uz_Mvmi~XS?F8jTABD*{CY- znQ|L#_>uhHY!@5C&!@Y>hbXNCV%3sCjSb;r=eWk(sVR12^sPtths(olY7Szy9Ro62;Xkkck)$B(e%dB1cQ|6y@p&^eoy z=XvfN7q6Wj$mh=uEPv-09x`H!o)EyeHyD*gP;A7{xSS-Ic*VQUk z8f=Q?*;tR*6m7KO0~!BlUg)EJY~<}W@=_aqw+$a@Yo_2cS)sj|9e=>aKOdGp8OlE4 zZ$9Z7Z!NT>JHs~D?f7#x;eTyL6x;A?ZFs|mU&zldbmQmMA%^H7tK`8%ceqfN>#;}3+Tj=s*WE=l!R&~B+L13eEHadN3U|mj|uY=J4 zMWHN+Z+^-ZVP`73pQ&y}s1DvG3qyl4Ej&t!dGk3gHjX!6;F>6GoU3d+ zzuWNJZTKH;_}w=AV>Z0?ghl0PCx_X{%WS4Z+VGcc z_!JvH&xW_Q1Z1@xpKmi!;k%k*BW#LV*!o)hKk(aaidyii3tVjtTl14`Qcv3OcWijt zlZ5d-i>vphk~M@IZ9_x!|px8FhnrrPm5VTmikPIcR~?brCfPlfVRO9F@PwxK86 zl*QYWePP2-w&B0E;nQsR@qGFc7yoBT;KK)O*u^&PlQ#T!Hhhr{@5lJROG5i_8@bp< zzG=f7HvAYn-WKI!G~U7!8lg@IOr|?FvClRBh$oa);JF@`$2uP*t}u0~N&`TjFIiWl z4Zq$(0>;?zO>F$JHhig#Ki-C)E%BVss~W!D#O|wz` zc|w*esK`dm;4I6<=Vt}-&RKy$c{bk4d^30hZM^w>9eB@W1%~4^r0m*llL_JXz;Ozi z3T)V_y!moWdti%f*y?=ja##54WqLHN*hXEzbC`X{fm!WE9=tFkN7Ms@LiD_lW1(J8xP`A=wX)e3B@S73l_ytVn8D_ndL zuqifdU49<=e=Czm-nCIz@Sv40z7sNOHtdHyVWlfP(k7E(qrS|iuXF{~w^8%>S+u-; zWng$i8#do26U9%XooOorgLd1njrcuiYHX9i7k}W3Ulkb9#D*=h$<$)}kyY4Atqcq< zwxJ)w4h!gJs{(^cZRqCw!&R>E067>^*OG`=J|G@_zSIMGIK#`s&=p+T)}(kZ-Zwu>9tff0Lc*i@TPCG0s? zhlaPak<)DC8$4mPE9fa3HG?NX<^0uw;UzX~o=qm6Z(fa%$qo$4w_zXWr&qheanf3L z?Y2=n@_Ud8Tpbu(V8eFCa+MPr{!^IImJfa%Fm<0-ZpBgjoOz7LH*7(^t^j^4Ziuz4Ghn*@n+a~ z`}66!uAoM?2+89N(%W;9^l~vGHr|c=bgnDnRhy>WHtHp8k8(pJ2HD63HgW~VTRa`g zH$i8S4LzKXecBa1!X{K~qyD>}O=G?a`<>HRZMHY>OMiaxx$2(gc<~rg4~xj}A$9+{ z>5GnjpkC*Jdd&ywK@ZgZ9;n~L3s1ZLKku)%j=p+7BKpz;_45zZziPj)!^X=MJ-u@l zMaA~+;7j*2*8T1CQPtVI!@sC5L)D#q-FW|Bg?4wi2lP~+lYxFkp(neSlkgBFoFc0| z?$^~$>6&2*9gNC=b4^B%QTu+*AjR1Ooc(g9MWyu4858C1-QoP)_GFt|s=xe~v?*3r z `@-CVPRq@TqD`l?|5Ku5Uoa|ILQ3YWu?LirPu%EKio3%F z>Z($b58-5UxSv*-3{xH1y9gLq*CXzhQUA8b=QEQ3@%g;_lQqq#cH8Ic5qHt(NSKrT zVE|M|x^ufn#Y$6>NEV@g3R1D%HYn;@mXwzozV0jN=sJN@8q?Bw+LdW`y@i)QzrYaT*odo5k@|Y=)vS z9HqDdkM}l-)FMnz6XP8=ST>d_$0Y^rnC`l0{P`!iBEjXYJ_JI)qDtNhCd@a$P+tt2bcbPmTC8v#7Q7ZX}sB~~9<9wZ|OU0ErKpx3&HsCRIbe&p=pcZ;i z3$4bM+XM9jP4?@?w_qswG>=lBJNu{+0aj`$tgNLN)si_i%ybcIZ@Uxv19iHITND*< zg#Cg3Q`7k<7!>8+S&vcEon+{KYg%^QSPHTmNg~FL{7~Q6LS8Ko`F$o=3C*{Yzr(_< z?wrLc6Ko_>&SFfb9!@zTD+rT$Jig>@rJ*@Fa8|jBXI1dV8*9vvym|=`X#cNEvZz!jj+}8~TTFIv*6e-oD zD8ikakZeej>t;d{p%`jXE|grfxIJ)K1I7LcvHyu)dfVa&lh`+s2eNJK;QoWg5426h zT}a$@=1A_pZv2<~gyNn-+|Lnr$Xv<&1GzKXbHDP9ian9or-B0$>XCU6DDT`{#Zre@ z+>+%fobA|H1}PRY$cUCK8|UB0(o(T_z9AlpVzl(m0!-!;gt&nS0Ad8rfsJl9l%x>^-jZW7 z=#CrpAVeeeB%p<>qHvEXSWB8?9EBvB&h8!+V3=W@gOc}Gv`88z!q*fB=iE8zQBLe6 zJVrx_GIGegcM(C}2{_WW=th>(JB`Q`*52cuGV*O>t|Ip%@_ZtHfF(^y-!>YVqjSM% zLFAD{etRjA7a?nysi@&sa9Y3Rx6%WWvu_(Q;2>fwW$Q$l8@k`*$9FW*!*BA^9bqXx zu+s);n^OGJb(!o7QTG;S$}M1WHadCKkQo8#O*XZjM4%k2!1a)^3#HBPBDgzzwE|263Q8Q$>LT*(7> zHuAg!L~{18?(9N$_Hn}>cDi$Frl2KvPTQaGX)X!v0Y43B@9bk#Jpv|5nBOUM(vaTS z{~Ss~Fbz#HCR+NAyL0@Cqyl$lfzzGcW3q7oM4+D|bWYow_&YtmcW3Pel}$F#S}@4R$pMDri7DpN^y?*~-JN!8V)>ZuL+?wxf#nP|amcn6}#)`JgBBQG0Y03A}# z+>Z}+=O#=43r*ab9q$sy$2cQu5eTmZ6bWZcnpfu#&9$HpGx<2+OIpiSJEH6mE z3A4`PGYz2Jg&GFV;~VR(il)p}j~cj0YCN(5gKW#K`7I92(ZDA}`O54=^BMnXU`%-f zv+NB({~kC+uBw?=0?@=VQdCGB zT|B0ti#G{0`PZFY>OOp}i~BIIyemk5rvVS&Rj27Y^dvX;p`q@~gk|)_iaYyKdLTw& z1@Oj{-w?AC;Lj6&xsCRv?n-2Rn*rs{NsbCYGU}a+D3ALb8`2Y||N3GgeH@Y%r<0m9 z?rd-w|9EZuIYXl2NhlT~{2DLW{bI;bYBM(pQ|C^FkQMLnjeG8d&^d28ZUuj7@5tDs z1C>l{P{JYnjqK;xQcp6UxXSR+gkN2T=aKvBvyHs#zQ)e!n2;(fdaq;9_@3Wt@a6kP z*ZARA3TrR*0KK97vap2zvF`~z>Z(YMU^P87y#~9pe|KYqb9x52b3(Iw1t585E(t>q zU30rm3UT(0#Xo}+dn2JDUlyY>I^|}Fvs-FXNY&)1F#J0tDhB_KiHg4o&>as+h>Oul zV=zv5B6a61aT>cykj?i^Dj)AjW?=DSZ`bc@Hfv7w``fN&wPEuN_(JNBRshFHI1LN@|-?PuvDUr~H zz07lKcrm1EFAUoC0>Bj9skqZ<%~<@SJsv2()cS>AK0KFw$;TdXv8{aWk(McAC}2DM z8M3T33jLN}4auT|c6ZL;_}-WnDel~^A?_w85zci(R%C#scWw_A+_OTSV$9Gl{~iHTv&ftiqy0hToEJz6~8pNxOv3X+DJVTQaB zfcG9l`{kxbMfT2}7G=8jBkZa)`R>Rx^|y+K@onQ?OlPY!uEP$zvPv?@@? zg38<-j?cX~8gm_!AZcvUBUp(r90yGncazVII^QBX4`7(w9g5Pw89lalc2-mZ*#i+J zVZ}5vh$ojgh@m9Y8l!{7_VmvSj!h121GQGtRawg@I=}DX<1Lwf{Tn{>giBxX17CF_ zK5Wr>%DF>hlG#n~0A%kTaRte_39aAe=T1bjZ~5I5gQ`K7?X0Dzk@PJeTR5iLz8_%v zTX~2pGff+ho#USt)~enR@!(Cgg;4%^ez%Z^7Jci~q#(k{U z|4V-R)G&SASw7&ji+#Z#KOGbPg*9ahjlD&f8oANGZNnV=`YXQubk~TV&tPAi-6jh2 zJS2;bSk*>yR1tT4_PD09ooUy_0m(wPyWVID`xe1IkIY2eme$ID&);Hqw z&;EaPea@KbW_`&QpQ#%KGyY$7H(-GA2P`r?rjII2cQF3lnOZIqKpu@HJJ8ugv$Hpg zcwo`P6*_{aUr~Sk>?yvvD8dD4_ww)4L*3cuOU{_n4}wK!_-92A>#3)B@Y#q^>hki7 z={1wHFA)p6eBDSr!yi8Ts6Op9-*PrZk2uZ$IGc2as{`OgQsPNUV_8k=F47I&*pZs+*k*(Z#TKjl5X=u_`B+9P>CFqIsF zeEb&3yFOKG67PR@kRJG{+3x$FpxwTDom13s#H(nK1kZ!jY}olS1h0SW^NFgXS=he! zzHl===M!GxtJC_XkNM|cb@zYfMR3e2D7+$ zMc+4K1^kuoe`{CoHZo$!yN;+V1Pp!CoSf7AC|*!t4NI?q9p6hR8=oAb4Ry%!%O<_; z8@Kq;AD+_tAK{7TYt$_uUf6L{QPyX)6y>yi4kWa(j6iP9!GH0RON~6O50jfN8=VR;>9fG7b^|%htHKu8 z+5koP`9utMa-Uh!h?l9l=W23+)!D!qh#Q6{TPd~b2 zT7N{j@0h#8xsa?vL{D-~!X2s1pB(%T9O%xev-l4_@IozT#_v>Xn*@fqkBuz_e9?s@ zUHr}uU5IK>3CNJFi%?-k^sn8sfAbZUB&!6N;}`x1_bYCoAOD>*G=#(UkdK! zxA34IvY}uV)>3y)*D!Y{zox(QW5uh+=fSU(75+fRhKIa2OjDkKBpmc9T#J#&qMDhm zexxBS2nqFjj#O8zVC?x&M(=wE;jaDAAt&PnUy-)~Tw{#?h`;$$=jb^fzyTeOg=Aq; zp!aP%K^(5ENWC9`gDwnR!3`e#v&&gnN=>$11v}7t?}xnY&t3KNrF_B9P3kTLGGqgF z0n0OX*o$%dyK~%5cc-9tOL@W1PwI6_d52$W>5-*;*e_%Ck=OYYxrD4H|zL;`asMmxR6T0Y5$)hE5bZ=>O3HRpoQ?6$cdz=@K- zckerV=Wo-iHQZ-vNY37Az5y~S?Bx%Y?C>w#19JxL<3E(t)m!c5|CDspd+y`$S6bE3 zNr0Ys-MMd@Z+Q&oJABENUi!AZyzojF{oQwXz||49mjb?$t>~r=}?`Lp=Rla2_pL~ZOzW$It z{}+DkdbIxDu6;F2docax4nF2ayT;GIK`qFQG;KHO5!tPFxkhtnr(u};27m8HEj?g2 z|N6%GkmaUPL!#W1LT38K=kT64+sAi#lhoi@B`N!aJhMK(Q<|PnX~TTi^{K*S07EZt zf{pif@^@|~MTA1w^4TYFs8!yzmpgxNrpFYUhELhKZ{qI@nV!3Y|MbVi;1fHn-WGcI z?dC)N9H{%h!MFVxt8aLnceztPqFNclac|W!Hx+to@8&gb&Cr|f;90k#^~~4!8@I-W zPS`H(Pqn=jOWDrr-tMOle1lKFU8~E4ZN$+X2bMVAi1l`)cupfJ9Hagq-G1D=6$(!D7sn2?okG|VNPkxi<05pD+e{gqH@ah>4#ufR#sqhcBRYmg8B=yX9)vL=J#@a3(;m;Q>5JSs72SB?jK$3MoQ^ThwnWZ)*gN; z`OwTOaRo=^P!)RDz0ObH8(nEW-Yvux824V}asP(tziipp_upzvub0oq`4aSuE&N4a zLw)pSe$+QSx$8@0;K8Ug>Z*C%9jh?ryEyYrQC)>8BDa}5fhOku7qxd@+%SnP(~rHp z;Wt)aKf753G1f?5@Upn)Vs$*LH{Tbc+2&e+4quJ?*c*q6k^Lh6Nq+%cxe1qMBk35M zWcoUYDE3l{mYqE6Eh-Cx7Z|7DzI*P_KRXe1@@@E`~`8B>cyMI!w%L^|6rqd%)y3xwmdJLgl{gC1M(Cm zvJA6CVT#E~rrV||%z-kujZ&EWGK|~A-0#RXz{%KhYr>BH7c@}p7)-ktoXA3S4h=UoRr6u-Y+=OsaabYKCYHg()Uqs~5=HdhtjV z==zR7pzAD{jAL9p4_%?Jnbd<(`6OxTS`UoKAHYKOg$!hVRpvVrW+iYg|avQEfP zFVGRn7Ob-(qQQ#%wPoc(@4L40$7SW?UK@XM?p8TwXUgbDy|c>7$GxvUFK!31 zS$cO)qZTHX1+$xaK%STv!cNtG0%rYxLv+i?cv?K-gxH%;i?dFapa-uL&Z?}H{#}mf zUX=~6aX6RUdoU`cY;x?%6}zgk);$*z1uwDP9ZKf@uRK`5LjN8at>@SS$Ke$NZU3LrgH11hJCqG?;zg_E$P7Ho zxI1-@!h-U(LY2SAKL(5$oS{2R+WpDo*;V3XDBBpFoC8$|SW`|0Yz)=Y=rJk#x8&@9 zjgc$Ga{S*SqyIsNiDb5FW6djPt+Y~n<$@3QtTuB{iz6$@(6S9k1SRy(Rsxrs&~glX z*ox@62@R>-XoYZxvB-|8hTIaxgxeU z?3_}+!SocV&LDxD6~$+S6DuPv2! zr%5+oP*uEpD0B0tO9ijP7U=m|vK#v{?(4=DijhMkXYWQGOgziYk!9uM-rsY?ZOjZYEoy$5{fCG|&jdDu-|&#% z!^Nq3ELG1~CavbC_OjsEOIE)`!gi}4!i=%3Zc$b~?q#dRsz^3bZ@EacY{0s)C8BkG z=4Npss4jEGo2x|k#bijC{)sfWDb%K2f8}gT|8eh?RpQh7*sEM$C}uW<&PAq9{f39^ z4V6kAi~g(BNe$VRUd3dQMj!W{St+VCgwkCL#I7hPeM&0bv&5$K*uwuREqdw!rA3H& zOX=PfqHh#?l+6^^B3Z6JYq6$Y;(x!q7qN+x?LYCRdGJ6{(uhUsRp*MJ#w;dyTMEK9 zw`~g8jPKJ$x5li!{_PuLQDg8NctgC{m}LdOKfvVa1XknP91$DC;%lZ$Tmo=$@|zdq z9Bw3ffFacm++pzojVpEOI(nrVm<bh^)X$O#@}7DVYx=jbGNK*)^KIM<8c zA?h|^uSU!(Z{9n%tbEj)wM?9E!sd4t^U-MT;zW1$kJx72F?yLsTz6-dWEdrLOoNg; zl+ql>dp4RNliWFT!{`hoC|#^;%Bpug04uQd!d&U0Qs`?E6A28~^1}3RV?ALYvxXqf z&}+~c`sPKpPPlGVAqdl}lj$2x z%kRt)U6G70p>f zLtuBv-8Eix@B?z%u6Yt7c{ zQB%dJHmtY)-V@^0HY`v7X0mACmbKHbPZ2ZQvi1?fo*Xh}46ljO>*I?JWQ?{y{)cVM z6Jjse+W$Nm*+PC9l;f96Y&en4@X#kC{^{7~cp_189v2JSvp)KWiQ+_iHcZcZLe!0C5v{jQ!g<72+T(Rb z=Nh4A?12KyhU0iaWPA09pd0;yINt$bv-o+zI{Wp3pU;c4@rbIh=f$;nR=ejQlFr1(PTAK-kM(|un)SvNpI-T0*#P0(nFVF~ zvU}mE%4T)FO#(%=j-H@mNN46eqmW`j$X<}IeK%1>FI%c9+M__AZ>CKEl=M9$vR@d zkywu3hfrlklBJ)#99yQJJo1T<#$I{W^vJ-bO8s65m9goll?RC$yBxo^r0M}*y#B0Z zKB@1f)K8S^U9UW9T0hoH?UegiBz9so>hC5|%T#hj<1$^QD+)vr;+Op*rgdQ9N%cup zSs0UID+qG@5+L}Wj_;;OGvA?+WqqEN9iO(r?0C?Q@yfC|XcApIv6@xgWddgNKW#8a zf;r6g6T3f2#w+ICp%$VhCA@%>va&EcE?D4$yGWMS1 zX6z+>Gp?+wnAjtsFVFh+$y^U|{iZ{&>3BIYCY9+Um*Yr@(im&-Y$q08C6$y>61zN3 z_$RXVjUF};*m%)rt5ZmlR&w=4Y#j9>$8XCsBDWih73&jOW4#g=A11QS`U}s9zZ2O7 z{qt1uLlO(qjWOa@5{vX-vW||N$3C0HYKsnC*@VEtI9g0bBi{}kD_-x)9&!bYHH|RC z-1vSBRW2C&#)*=ytYe@H#F95Dt$jE~#C2os8r3v8l!=MvAe5VE>dNt3u$D|5t{1N* z>mo(P?#SG;*OH0RqIY+8m~9c?Ct;IwV5Df&17`Mry4=huJz%D20W+T;0W+gVnP!Hb z8)-B14YP8=$P~YIWt{>qqbjY;9Vy25WL@>8Ys9NPSzOH(P=P<#a$-9aNyn^}MtkRs z5G6fX)0({=AXOgiZ8JhN?S<89<8U#r7Zdt`;UdJ%y3|-QgtVxG0`su;pJC!rH|tn4 zZ5XyMxsrkhO!|ak`Y5r-%_3bnAYp$TlJx}M<HI|RoKB!Am@dN5SxyltLJ;CmtY8Tq$E!G{+OiRvjfn;d}8-5_+`s6A3_>CHaY z#}5+I`>^RXmML>HFoBY>FYq(chKOr@SXk&VlXCx&PuC%$c3)PzP84X~ga4F0jU?lx zp}8~8P4H_bhW16YWegTe`m*Ha%g7qCdVLhNVHyrI29n`BmEq^f@ax9ogT?K>th+vb zut-S8NdMgw1KV-9n2^l6=ywMZvUZi5*phJ~d_GK^O2*;!tf3;I9}eG^3>2yTSi`za z&>(#nq0Uaj)aRl&hq`4X3=}W-V-4zhVgLWn^RIC)Mf}u{r8Rm3CSnu<8=R zxq>YJ5smWg0piL2?BUwmfXnC{rP0gz`#qMMBF^?__3BO|DbVn6Q1*9gBM66n=M5IY z16ZwUtrbtII*@CaB03FVu}yCbfVDZDqvQ_z7u3jGwJ`Sg2inI2#D)Rz#A^e@fdLF} zQwNC41K3x3Q9tof3Z8-+4H9=!kVJd+7p{SP=!ea705g(9R``6@yrVh_|88yB(Wn^=NA}>`SbE$xFqNL9DO7Yk&wD%#O6oG125+ z9N-3oWc9g+DL)TL^W(idI%32A)BGs&vhnnQXNR!xHoxPKIg$#ogU6M0qK5-t74!%& z-h?gqRMlrh!EV*L<;k!BtseG>enVjQrX^zb5SI4v>Ru1pJ-H8I((ye9o?&9*K8M?U zzDyL^ywLlNn9*P0|2`VpvXLoaz1)M#Lb6K#hVj9`dw)f#?Cb#syuL(C9Eu54)LT3~ zl=ajDdW)}zvN`&`-lFd?7FGAEo0_*N!<_w=aEOVj$s%_c8`^N8$@5f-d{UhzYh{To zWVCmrTlfuUJ!(`lNo5Cg={*e1Egl)p!W#W*_WP+=sp6chm8qy@ej|OPm)JC%b;3?9J(IabFA+O}#p-`76pxNz+4`Oy<`F=PF+Hh2_UGt65`(EC zj{xcz@q|JDVta}fBk`^ztf%NX5)E(eDId;H_8=X$hx7d=!mjjHi78VWW^lqlX%4~r zAI|6Z5WkMZ`A|Sl(P$KF=BX@&k(%Y-?9Daqbcc|6hFLQAo8AweVSWcH{Z+3z!~9fL zQ+k)5VeVHre1xnLLcH+`aP;B93xM?DTI+tYIoF{7J^9E;=5 z?(@X5v8+eKdw9Sxb8e2`k-6rVLqm{0G7D0OJrR#K&1Q+;#p0e^;a_--sZ2UF$24ie{U$Y;DXxxVPwOLbJeSHEg&nSr1b8d~Q+GfY7?GQ>n-KdF z#0ROYRkiC6OGAez5xii$lq7tqETY;Mz>_)E?2=k^;Ud&Ax>O@a3#EX|Eum-6m)Ki2Yt@-k=%d=T z)$H?xiB0Zs87XSAVsO&3f&CSI!Z1YP(&U2B!xX|_DkO;d6Io>CsmgDI6GY#MY;xeo z9Y`r{clS>hhbFQZeR+cLPQ+o3GLl9hN5D!FUj}GC2;eW|LRH^qbQH0Z zS&n|Vo%nDv%ME!3S}7Hn8zjGWV&oJy-9MJbVoz&vdJ2oFBuHzHU%(Xc?-VvzpEHH_ z(RIbFCy*yMOcvQsuz~vL?ai(JAfq)}&WXa8CW|8}u~5gx;9$!b2ZHfi8)*-|e<`U% zhEV~B#@CGkP{`uR)F0F}SVi>_?M2V2EHZQj@HD}k#!e_Q{{X_+(poH-z7fWdVewM4*I`Xlvd!eYg3+0V}mL^I)2W?anq*Em@58CW3|MJG}cMfoP}SQ z!;j(ZPn^Y)^>$-$B$hrBaIj^qIw;M1Y@FZYjBPfO|CIL3XDf(i-`9b(SM!+$_n)W^5+n5kN z8>12U{}sS#VVEC)24Gx_&v!5!|DRsKX%YAX5kUHxn70Av*TVm?_6iCGwKT0}6Q3`j zwx&G{m<* zzNZPE+FGLL*t)j>+z!|e(ESj8-xJVirfElFVQF*xswCjs!1s*v`R<^80NW95lN3^=jw1q35_dis155Ce(5!G{Zev_bTduMXzDHVs<#UV-^6W0BVgv2aIow`Co)W8W7IcF%*C)fMGE(ihWB8pc^n1a13BRU>aaC;2J>Z`%nm21b7iJ_5&DJ6MTS< z8mJ$H>rvkg7#NEBhX^*{3jk99Q$B+0K`#Pa_;|9(x!u zfOU>(N~Weu7h0}T`2yhukfXhk+0Cb^ah+z&m(jHN6h zlyW4E0bNAP11+$pcyTF*WgFY64(Kw6MuQ&mSlPyL6EY_+b{mxfvtXPr7+EqMMJvHL7?FBj~gztuN;`Ly0AI4rk9*Wk!Cj_$!REBtx{$7WWn zFhr!TWX0_VLjpRX;Nc5H?OE(+8r1n-Dm)Z7eD3hIAAv)0b%`xQQS)U-7BFVozY zH1{<%k7SjDW4Z@Nb`1{eYPvM3$|Jxe1xNO?FzzZefl}dy{%9L>RC5$X;$JmM0`Lo< z=$`pQfg^*(zLjiBgr@eT`b!8Bi`qu-`{ZP52$-J z8wY-oXmCfUImDXdL##l!VsR&8LUv9TKWDQRRnk#Di9LdWFj%{Zg_5bkqRVR5HY66U z4(aFf(NCZ_YStB9wy-)Pe>JOJF|rCavIE4C)vRamF>oag^7-gjPaW$-Xb$UI@gV4t zk>ZgY7FvzwSQ|9r9Pab=q48)fR^+fo9-7ECkX=r`dVhk50qz)ZG=y{!UuWP7fTMAs zi*PBxodV9vTZEgS^uZ+aBHVnX&%#9kwdj^NCseou(;YbRO5r3~?by^g`dl$j<2zVU(u>mm~(ECvfF~F6AzJ43_ zEJS}n^vq>-J+)!v9jxw|gqs7I9%Pihz>*;kPJZ3zTTS%g7QKg+QJx46>mBS&3Z{nw zZ8CV1cldnsh_|K1t536Ztpa@~=!=P7-K2|Oa#?r?c`^-Ki3LO`i4awuMsJFNY=eD8 z8X=EjdnGNKWNE|CduUmpKS}g$V%XE@O*7yOc))DIQzkQqWXQr~&_|%5sU-6l=+Z-@ zEc!Ii-LP;b(R-EAAF=3bK%b8`rx5+2GWrN*Q<%0B43jbD(}*F&G9XEedz#hke-6Yk z@bw}hUPjK9txvVI-vNCStT+AlX&HSY(K|yX4AC_Ptv91QuuIHX%UT7G04{l#!tE4q z0aq~*(bsve_;oFUbTY~l_KM&<7Ey5(%2|6w3zS1NY?!z1MQ|d#9ZBKhg{`cCSdhmW z1`h!x58EXA5n;zDu_KRl?UIaDv=npI{9P$`aH9SMmISqiOa`sZ(DH>g1(tpZ9f`ov zk0d%Ogj0fOAo{Iit=M>xv5rMGXc^-3^&~fJq*ng-pka3Tt$8Tha_dcZ0Dd0(2xuC6qB}HyJ-t1asD+I$070>voUy`RIqf9k*T5 zlFiS+lB%NU85R{W-BPkvuKc8AGIoq_k0vFpg6}{Nx41x6!c%Ag` z50wUvTn__>iZ$!usx}a!6yO*vg{YN9FmU@LYVR>XL~LLU8abgL%4(o}Iqxy>I>#yA z*2LSWg`dS&OY+g&2t<;c55DHqmJ9fP!A_)(UcpZB)>0PgdANKeoRm)~AATL^^C77q znz7aGDYL)?5K}BY7l1F*BR7;|!2h}>ccxs9cGqr8uJAu{jkWaCggV>C5~2g10h*UeBIVF^EpiaR(~?}q5^QN@fivNphy zdtAzO25th1d61+3NsnV3Dp-?qb!-OAq%4-KW&Yyqv-n!;QfG1RSuD{Ts`z~5GKarZ z(gtM%#m=c%i3wjpJS-4fQ^b71YI~BvZpPL=vR>{uQ!yy%VE=^J-vHj+-6aKwt)PV< z6Z@Wgrmv_R9ElB|+~)PFLQT@e7N#T*(%5{b{|h%dmHQ^2Jus)fuWW{BEu=@;Fm?7E z8a{=F*P`qgCmL;JNok#yCptcd>{J62Iu?)X^c!c6wbkXMSpFPqSid`R z;d_v#adCL0bQ_dokZT=y_}d`ruV6LA`R7=zkOR;%ZL-ftzwhbjrnEG99yu-$ZOwv~ zLekP#jC!6$RH6f~QaonnN)79thlXk3yEDV*qo1{NR8ShuKaW4^_&8oveF5L}41tz! zNXr=-EjgtFpy`Kn3^!SSMm)WVMTvzkusGIS?0f+;dM#x7kW6N|%vQ*x&G-5CQapJ` zrlF|030YvBa6`U90%Yi^%u%acCJ{^M`UO7U5t4C{%uxN$cH$Cb8=eVJ{QGEhq+LIN z|KviSkABX~v8$@e%A3*1NRhFbMKv663H2=(nvby=v&iS8pB#4dGKHeW72MLc@g`+v=>?R;Gu2HLobhs8=IJ`;d}l*UqgsG-a!7xMQRq+ z(X<1Wnr%+9)KnAQFx}#wQ_f8%jCmf_=*)8N`i&hhqopO$sazr%d)mnvN}|1z_~0cL z?fI>uvZjLMMt`)o!JW9&=WCC$!-u79sF@kD@sSyACw8e{LC%~9*Fncca&&g%w&X7V zSMDO@-m>JrG37j_U=%jxDVE@w{|eSd&RuB<9x88e4CF>wa{K=)Hw1F2mfW^-x%H4s zHRUjs954M>a18`ISb}S%AkDh;`bc7XMYWf)P&y(pXQ9MPLq9+adYLtC_K%;&2|yo1G-mswmpzgFe35t~_Ig-!`22KQYkXbtG7Rjy!i12m59 zOSZVN9Miqd7FIo^6iSC=`h25E>18~NY{4HD3>2%ju*irs=s+hmM-D8)MMgv+>fC$9 zku9ug$WC~K@Io``sb)s65ulw@Nm*%^{=LvqAkG3XW6F(et57&w;;A<18Y zsMWgSy;oShh$WWfKJ4pok>n8M<8yn(O_IC<$q~zyF|sQd;pr?0-lw`Ll1=at%h<2 ze%ToF++jXXHljV^A5=etd5^N>pGTW(^RUN>SX*;QytR#mc5H@n8io%cNjn1PaBz`) zci=Zd9$qt_M>6%%raXmeDrk!w;@@qo3HwvTY-e?&{4>}|$LAF_I_!zXxB*sa1NaR) z#lEXxqNDadvz`53A)tN5y^MA9`$$~f!RqgSYbX1^w$28+s^Z$?b4~<86ih7^i#`krk-D_Eok#eyid|2_LZP|k9%1#{-N_w1Q7XU@!=Gn0EJor8A$aU0goy9TBT zUJm{d;4fZ#uEgfvcZyO&Rt9TBEI*YE^B*SkA0qT09>j+Ro%rtx#$S}*U+)M~`%*z> zMo<6wzV*9B$8xV|D7uAMZahZSL9eqL>FU5EE{ zP3PLD*98LxHYZfyLj&{tZF-Q&_`Y2GChWq+SlN@X|1GJ4^+9_O#8bh>;9y1uKC76l z4*S}&KLk%Dd~Qtaf(+BmEMTe5=G{$yd5nxmhUsAzeSp_=EkS7zw>aNeep|vfg)`SOd=0ry`Ou}I z=N9`N$`i();7BTtF~MkNFr9q(G6mlLo8;F-_@m?xl4IC+@+|pdXYm9^LMs02EQK*U z$e$zE86u`clD|&=XY#G&?~}Lw!eCtt^1oE{!U!tY=AK_Fm`Xm3d=q&oc`f+|5&m`Z z!e6POw_i*??F@JMn-{29NW~y#u!H=W2tPu;A;Pn}=?NT&a2c@V#Jc@j9s3&nN;l*$ zioA&8VI~+F;mgTKNBC~?+6e!Md@eb!gz4Q~^F5WILMDeIL#QZwUgKZZz)U5-GQw{s zzdgc}E z{d??>MDGXjsQ$nn`zG|YX%6;uZ)Cwh@-;7dqT!9?r#~u8^+Isz){LFCGl*SU($4du zdIgjDxrTT8noEYcPrH43Cx(i_G8Xi)|GzWne?xrX**-pXwgo0S0%T@N)&-`EpNc`$ z$zU9WcuDEUd!Oep+Mn2M=XM08`SI7!&Sw_ u^xYK0UbOe)YKL*oq^nBJH`6q`KkFRlG!aB)IR$~sh6%>18%iF`g?eo#)tUy@ZW zX*MNfbv16(9x3Q((_{ZolwhH;P>$> z@TdkSr$n2Wah_(@$;|GdR^G@LV^suulpHA~UkfgUNb~GQk-tUW>el42D~2ckx$~2E z`lG3S{F%|q6*^)3S5v)W1r&1j&9l|R6U^YR;O^|@XAmJiuQrf(LEnS}!>eAiy}bBON7po^7Km`@Zu zNj?i)>|5|nB9Z&aSAok_G`xx<$PYR{COI+8$?t`cQ+&4KXY57fAz5U2a#L2UybW4+ zImRens7lklk9uxq;~hypAz9ul)=f5E48-)ePq0AZ!Q=9O`7d0WJX6L>^6hl5SWD>} zH4k~-6B%Y3c4LBhCHfI5U|il~L*(U|$jwe2{8)oEYTN;1xe)#vqD9UWJw~$l+?dVW z6ZC6%q_3JZ{g3+d#rPAr6r=MxI-ezPVQa3T{SMDvt3C7BavWSvw}=hMcUL2DfU+e_9iNIW zqoXQSm=^NEZ)TzJ>PYCI#+MA12e#!qH?Xw)fs3t8l(! z@O?_Lyt@@y2%asiB=;#CN$y=+RCBGdC*KGLG-lx4r&MussJY1dT9EF% z@zYeP{1NisfXjVQbdU04^0fGo5|n>Ni)P*_7t-QFjD6xUo9+J&^2PS(-XPX(3)DHT zp&xgku(&@!(ta(t%PIf=Xpucy%kmvsOiC`vjx~M-kF}Ct%hjrVjM_rHF_fAx zLzT1(&+&}wZ(5@bb5yBrbwf#z}+v&nkuvu4`?CGo%SIl zn;{zmo!3iMT~76T;8HsOscnoePNw0e5&MlbYJVyDz2HUC<_e$0k-MMqy$}(3%PX=> zxL8#Dfj*)g;D_Mu67Z7E(S&B^5T-1=L5`4n%O|RmH71e!!Cfzt*x@ZWg^>5rBKyhH z|Iqjf4a_lcX^{17kiMCW$1Nc_#XnZVOK3k6T`rDFf8As;t@WRo4q=3?PeS=|z+kB<4H@+=G`Qs_1oI+JW{u&XW61Vg|Tej%;=k7!91<0`AIuRO743@28*aky_{1lfUl# z+L&cHlrwL?bO5_4?X=96)YrI(l^F44W&_>>(K7)gf=b21C4UDFJ)o>Qn zjf3N`(VU7IP<&CNUrt`vu7;R)I@y<;)jJk^F8}8jpe0D!dfH5)%~i>q^J9r`Qk!On z*n`?b;8ORkyxD!-pI54U8|Nv{)BbjFxx!N-SNK(M$sK>3=PZ9p`-LMMFEtyf$Wgr+ zhXySPx>#I)e_s!-gW3<0o3dkxXAzeIW=Gm}+Hur5@5!r~VF?^9l@Wwv>3HTh7hLiy zo2Nh)B1E1Bmup>iLgU*tFe{YLHR+>DUZA7ROwjeVa<};ehi{RmUr{dm*hDTtPb4Q0 zXHBBqPUa++WW_30Lo3B<;v-NKwI6{?J-4!+v&pAmF6FAH+XFaqcVglNIg(R&UyWo_ ziO6 zsz{3>E4?Di$Xj02xNL6~*#a&_iOc3&h-@nosl;r~)oOV?j>Mw#TD4#^FoSRxOD(k& zo}E)Xxj(000!m3#$7VD^y_$Q#<#MJYx6MyzpWdN{#Vqzl^5|&x0eSOQwf`;s4;Z6` z>RY!?$-8tkT@|hCP@!CyIs6Rp3Q;ZKQ095^j(dYsJ(w_QA}RB1e*Mo)&d8OMC;k^xggVmdNf?IfWI7xN6(0-hNM~K@Cr^ zb_9;ZBKjU_zF0_cKGr0D9&{h=yZ=V{qx5(r8T5&D8#Iwq*r3WTJ%|f%9e~_kWjtG7 zPxq<%HK--`=v%43J9YVl%e61s7#UiA1TKZ!*i-9Pb{C1fWOpIcV3!N>kHSivn~7>A zJ2ged+lE6ZTHO^??^Jw|TA6#DI(WhbiC1B@6erNdr!ap4pMuLs)5gJjfI2aSlhkW^ zmx52}^=|Tbg96z@;AArE`CX0r5v~9nm)gC08jWK)k@Rl0na#A1+v4}|aGiqMJvYo~ zr=ejVf!6P29Og>mAbR&Sl6OW%xi`SID?WxJeJ1H$t3Eo}KEF%fwcyUSaRrW~N;Yz` z+lLnIj$^(bY4Il{rIhNJ8u}5U*HpdgEs+uECUBQi`71b*^J(FE;u<^2o<_c1RP&KwbI|)>vcXnPE<<=wHF$%w(VOxv7>O z;m~uEiTQn!9rMmlU36Z!Gb_1g{MD(S#=`HPlWOiC{w33Xgm2~~d(Zk(@-J6SNevhh z76-{=7xz!DFYTU63=OyUu`4bNdn8X@oR?fb;eyoRap5&#@VKp>5RS<8%kn-v(ceA{ zo-@DX1Lyi313tu#frqQY(p;{|`?Yz4 z9S57M!UseA1yxNvEY0%E^l5AFjfa)FetF)9tv&2!*yZ{qaUYJ{X2WWv_v<29rXO1! zPQo(%L)9pyUv}}~ZFZAjzk2UOY%#3?#wHb42R>YEtAVo&Te?|o@2v?dd;0aD61%1* z9M#izhGq5>iTV|oxE(Vw9F^1Bp*~yJYdFU~G%>6S4`tZR6H#uz{&VQR?a7JoZFra9 u*W51;@tVXgM39B;6s}d-G*0$mEp|McI*}5kbeUkmUoB% delta 48118 zcma&P34Be*`#*l?NF>N|5xGfh2^X;^)&!{p;UYA4wZ>Z15` z)fS}`ZIvhyE!woDbWzc2o!d}bE&ZVP_kL#P#*uzM|JVO`ojh}%eV%#dIdkTmNM=@* z(p6RR!)hcJSj3Sifh8?rU9%2Vqu0vI=(X#nwW`W~l2`1fb#JzE7Iv3cUTCo@DnMnN zr5`eZC@p#%rUpqqN3eumISrR27kNPPN1l8frV#@U1^JWgqo{&~lBXh^NQ!VwdbW8i(Ti|C&2MYBoSP2&caOH7 z^hsm^rwTJeV_>^mW*bZX)8Kdp=S-ZlaMCeb0!s$qJe>1!E>O6I3O=XcA_bj*OK~p4 znT2yXPSfGEI0&ew3hj`Izin{mE{b1P0d z-og1E&TVv|j_nHWRB)GqyDe~UkGk-J4^?9?;6Byfuiz(u2XKCh^B~ScI1l4If|Fu; z3@07MI6oIHi{%Ri37k~zF9E;C`3;4Kj?)rY&M0^m@SJLY5BLMlA90rA^d1+{_zmap zINb_a26$Dq|5T8`HPyau!co(C6F+X5kbkN6ZNNJ?@2TtifDdr~gVUpMj9Dy|aQfn` zjMEQiHJo$=09tX@#2JdyhLetPz}l*fOWFTAK-5*pdXI>f`e;YuY=G0Q@QneR;EcxE z6lZgs-lK(Tw+4(g;fdY`*KHN99bkLWZY@PSC_)@yJkHKIyC@uSB&c@R-PQd@R^sVZ z?d%M{5nyAnJgJ(Eh4JduY`%3c^{VqEttR`FuM4p8dsV|&q<-UDX7l}`ip5exkbdFg zYS`H8JT}1QdmdOJ;Ol@NlHP~M2HSXgHHYs90437*X&1BNI zeSNQA_k_n+!Z8@4A0335dgLuT$uCy5u~1=><&k>dvtW!AjNj;op@~Nah55E=p}b*J zrLQ-H_)v-=F95?z3YwUg4p`(CeE6ki%>$7iKC$E}W z!h3$)8pEC;H^TTEK{nrBUA+NJtRCb0HUc1=;5pU9*q6fALJ|3ud>sNbqFmc8ik`1; zH*fMph=A#d2bd2}sS)Dax?IzO8e!}&L1lr$&Gy3i+hN``@~P%+-71ekAL@#;atu0X z39>{ElbK9am5vbVIPfKMI#?hk4r@!e4}7L77%!+BRl{h>Z|NoVv{C&3Nw8R=ETI<5 zafydZ;@Hwk=y6(N{wooa6d^52IYiS9N4CNrN+d;AOQgCB4p#Ur4JCh}nu-e*{xfO7 zQlRuxd7xu0W|ZjIP(q;0Aim-_il2h;g}hFZz>-bV4Lwe$8js_MuY?mCN&bSaQu9=a zwv0z-->}rSSkFkjzrx1_NdC+ci5N$K${eFPP9dc-isK^*EDw-jgfF=!5$`Mf4;V7R zA0iFdeI()QBpK>+Ur2nU;sT@Dnj# zh(A+R#a;@(RP{KlLXWA`6My6h$r!8n!;l=*0c&GM{kK>;DuQo_3|;AIY1pdpuS7{j zJ3C1{trzGpYAO6hdEH43P5sUiUoutlFOb0U8XP5uuG5_nM;~S1lF&>NGE*gIzsizf zsTzVzb?fY|vVE>y8CFTUM6z3c6xog>eOBtJsql0EheJ;~!ABmYsStUm*}E`A_)>-M zsT6fl*4cx$5UK#NKe zzXKyhleR=fynP9=Dws`n&5^sA5_(GxP9eo`v( z&i+;^^-5IcRZ->Z5^NIx&Tl3EES0K>5fWdj?nId5vT7_=p)OUS_8M4&`bdfj)h#Vg z>a(C@;)tvZ`=X%Emjci$j%YP^B1cJl ze^rh+$`suLQ;$hJrjj_?(i#hfOKG`^V}t~j$_*s{*3TtktDssems&~wk~CU$<9J&F z%Zo@?QWUdSBD_oH@$F@gy_%~l1HOnHbs$}J!0XV9Du>c|RBf9xbGOv?i?Xr_14`Z4 zDkq@@b66bn!X@6S@D7C^D>BAv$yCKKO5v@l8+d0m`J* z;ZpM=Nw-u*nGtj3xJ10I4D=Q83&qNMb{(5@r8$2^s_I5{rRNrcL;Nun@Jw}kr^Gug2%b2e z!9=6%ELGWwSt5?PezF^dd*n63AdVPVA_Iv(%4zaqHkViZ%*sa9y-Y4bx}T9H^S3)J%tN{%@0HG_`9z%$CghboCu)rO0A>*KjjQc>a*>CjpUEdNzQR&jxG zLRH(yTT-#|%V~+NDc4{HY7K_u6vs_51fiBNmF?b0R1TDeD=rq_S4mf@k`93p$8&Iu zawYwQydJ9X&3$OrR0_2W=SdB0&U96$&|PuZRj%f%T*bhPN0D;ioEGoMz4Gsck1&v+My}$zXEmm~=-)VLSVO<)sG`bz zqPh=bT*NUVQu05(#O#b6GJiFBK9r1KNnpuAoe_muEiq9H#4#&LmYofJT|=9*M1>cl zBaYZElA5W8&s&t1p{|p=X-`RaUsn7(I!gX~>M?e!!oRI%N}?eVBh^&D3-LEl^hzo| z8;n8Z$w0+*V4TXQ>uSB?&8Nb6+3oACq(k4S_#K2Enn9KziC=^nMaNiFTf*lzmU#G$ z19hYwG?L-W9|T5h3{&e_Rh0586s4{TpLtL6Ziu{3O`y+^?cf6#o?XmBap(RC*~)~ zxLXau9mQXQ6*T()=z;zY?x=J_id1(QsDn5zbr6TWhUibFWKw@)=UnPO%j+$%N)k6mCiQ{KgtV&d| zTA}!#^Y&Pci8n$Q{n3BwLFzuKz`Hcmm4LOO)Zi81m>m?-n`&rQD#M*sa-}|&To%h5 zM1=U&s;PX@lm|J&7sgarEjCt$6YEIB&D5BUP$9j(duo$qe!FkB3SpJ!X3Uv5FP;0w z{LN3d?HwC8b=KUmxA(x%Y%Y zv13VU@X-H1X2X=(f9WCXZI$&7(`QW>H+@p~SlOS)6hE?${=mNIj15}hTZge@XUQ&( zojP;wq&XR+v{&*#klISO6}k7|v4dmACJ*W`Hns1dwqvKuPAjuLq*?T)JhD9ySsvA@ z19uJxsrUaKT$qgTAr*S*eYRa+e4E*KpNw@g{i^`2*Y4@c_Dbp9t^4rd zW81cB*D9WmpYmBXnnb=9AN;G#n@nv~-N)jG{apG{rYdPUEKK9{qph7xRXE8Ifo*YCq%faR`2jJv0U0sZPOo%;6uXYjz~ zXQ`>KXA62|OFea+(ewq}=X@+J)%8?CuVjhGH60AJPn2%D4w*U?x3oZuL-9M;uX_o1 zf@+q53L$&wD|$dt$q1(&YnIVUq4(V|tcoLDt$d6kIWxWZ%& z#2u=k)|Os;ro#JIz{e=VrRomr<)5OijSBev>N@PPl?#?FUW8Q{pngQ+wDP4HHCZIz zo?#m&u7RV&yZKelC(#z~?p;NEm`U#z#Y@2ZU`700_|+LUUsO?8y*RUGd(k2$98*m4 zX1w=hgpdcHm47|2W?eIf_sQxJ$GG`5`E6JD)^1i?`e&j-w`S=Y>w zXyRBzGCXQ#&G1liK^oMq$NW{0s%9yrIw7taph(Kxrw4rsLBq~$G>_CgbWHP1`8#jZ9{Q%yF^c*|EZ z+_c17%}>p;#r|TFI%m@9T@(}fo(X@7k50F-v;6#QTc1lN*+`Q`@6Dgc(I&jVX{-}X z_>&g?#GKIBb0&3ZCh}erKJ&{*9oWlf%(1a9{L~zq)`jusXNQje(!{#Jq(bcs32LSZ zKiI^dW5UyN!+YeL@GTf$vp7@(_-9V&_-ro=V1bG2I}?7V3C~UVH1)VZ-7(|$oA|53 z+T2j*D<-kyCh`gsKHZeBW_-#2z%NiWkIb9-Bh}W52_GuVl$Z?DI^N1}%&pnJkXnL{ zEHQE1RyYFwrk;u?2EJ}a&2aB#CUHcXI9BoV8LfQ}6Ph2MQPXz-8diRIuD4ZvUQOR` z1$8!Bz9M|PX4VW8EyCl-;ZgH#fg4O(-{CDWL*9{O@iWsD7&CsWN$L&$>O7nC4U<%% zi5f_@)p6W}4>#fWo2;1e*Z&7T+B7VRzqv4x_HTl~?|cl;Uz-!^E9CgnnKhkjz3u^) znmpWS!e2MxXPWT$O!#L^_%xH}|1sgGnD8En=ULBIXMgaz8MeB!Oq?4`ocn1T*FOOitrYG@!6VeF@JTDEq1ZuA#mKJsmz2wXTrDTpU$_jw!H3hHmxn=K?_17 znwkbE#}vP&s3FfiPQGW-v<5eT1)x>@wryOnTJjtk72Y zZ#-6PIC4yi%mRrffxiT&{NXU+vv|veHfL@47N$6^o2Z2Mj>87iI1t`@_?xUB@ge~e z{|A1-zwxrDI7|kFB7S3G%|00>l{-xwW_*}QasX9_-QE2SzCUT*P{ErEL+=QRPuP(N+DLi43jR!0W2s~$^{mf@9 zvV{km;(6UfeTkn!meUtx<;O{RK|c%1k*KRCe0%H-tB6;A+lnTeWXqL%U*pw2T<_nWBy<*$M|&_q3MqQ>%5 zpw>0TSPl>Qauonnf@kxvaUQkI#%0;5gn+Wb>ETqg?yWWv_tSAkue6~G+) z>nwb%2zmpP;^CZS+xYOTfbcse>=cs*J71Dz3xB~xU0|X%;@hFMOLjnDjtSeCpU<)d zs;7M+xq**^oiOl4n>20Z>wtA+2k?`i>^EsR!)Ji<0NCRuY)k%Xwk^DsNy9l4wKYEl zY7fCaZjwD`qW0jYKutDL4HLB|e+cST6ZM{n z+Kac$v4wwZqWXXPXz;$^qjPM54ihzuC%G_oIRW8)OxS3X%$xjEXw*zSi8E3A@~b(R zRjzO zQO}vEH~6VlHs>f4)i6DnIe%SW$4&1JAeYL{%Z}J*3Q9V9if#Hnzx_9nkdra?m zPlm5C;J(L0m)73#|KPG4E*-9G#=HM0v?Kl@(9MBP0h$Bt$UR`sMBE+m7eJ{_0+63# z^!hJ}9I!hDaXBf!El^PIh+k|^1L6`8q&`LH@om(aSx6mr$SsbnLaW1-ZKtPDN9Vy*4CtB%z~8<4)43$ggLwsmACHTv8CLJoI?_JBP@V+w*C23+@JNdi z3{8r_YzHRAb=jyTRH%dz6s{=uMIIior&21#J4ww#rKXfdI4|3NouKH}SfaQt7{!kX zUCa2|NCu_s3reskXr9MF{ZU~i!olp zb#m+_;~!+Qy9IXGNOhzmkJ47Sat3Kc!I_~L&k*B%#RwjnW4nk4a%m)&F6CZTa_xoO zRpV8X^T!QJ7;u^_WGK!6Wg&w&Erb8tE96zQni^jKZQNr7*Pwvnkms2c$M(vWnu$_LwwOzTJcz5h_NCUXgbLs0Hs_P-^@I zo9=I);ioq@a_*;ydAlh(wZqsOiOGwfH)&2WyNP_h}LAg+N$;<6T%=anwCyD)IVqY~^ve%Vk z`iLFeF2x;1+^dOu&OFII_B#1vQV#Bsiu)F5#u(zB_^jl90~ZdL^Ktf;ioJ-~W59ta zwtE2t9(As&VtJET0wqhy!pB%H3%9QsPGb23NdhfbvHCM&EPAmIKele(?QO=TV#H>fY8 zh#Z04G9pib_+2AH(Z3;j0@1Bm<>GgZ9~F5wk?Rq;3SP-kP@`o~r@?%`<##gtN!e9O z5YS?%bSKJ6@%?rF!`7zLc95HxaTIO3DVGuywS7G()RE~TyDk`Zn4+7(YFxY9nL6JLT9L=z7Djg_(_9%yB-`$NAHKE8tyAfVG&lGm!t-{ z*i4=x%mclh(7EmZjKbIM(5N5o!fejmTA_j)Y`2j&CWFU>kC%;l%8#VMLsA{NevV2D zO1*}S@n*B5E13NdM3`+X%|4B<(_qz+mx@L65R6=Es>3%&$1jih!-=(m3MXbh?nGDR zgbs9Y{Y-RL4B`++Uc!gioG@M&$&ipnHu7pM1PW8`6`~Z0ptk%Oii7tiTe1r?w9!W> zkc**ed11y)xb`{j$v`4ea{aMh{f?|F{xIklp`W;hEcNJ2|m# zlANO-an6yP^N8~xv`=moTp#laM{xZ__?j?PF!+uq5PvVD7P|dAd4&*E@sz5V9#Ihd z@ls(M0!{yMxUM@6|Jl`Xm`7|6)QTc`)9rPe`GRJd*L;X0D`6SE8g{sT%Ls7fCLF`p z*v9t;a(yDAdCd2`eRulyMAmm3P>$RbyFcn$?>x+o*l)1VT_NJv7ZdRF;Otn9)O_V| zfy-EI=Fc6BCjlVMX>@tVpo&Ud6>Yeu)2#8P8k>e`mS*y(|@BW z<|g0K4=Pf9{@@`WOw)WYKp%u8Q}yb zcL(-6|8e)5*0FBf9J0k01(gO1&)ZXD|=IG-hhg-Ln=Xt zH~$j>mT(Wa~GD&ipsZEl7@XmW$fcA_>nX;X^5z7NX2`5P}>}?+Xu0Du=LBreQ1zl>6JL7 z%0x%5Pb1oXrswZyAYu!RH_zj-5qFD7?)tQDml@XH#dt&l3wV0i({Nwt4QS0xwh%$Q zO-*w7*mIM8l5m|!j7ebd$xZg>KYiNisXISa9PZN>J)Yd;uv@{_ge2 zNDN8G>b)3xQ7`*$K>E^ zg^t|5R#EzM2ZaS!&$jy`JQOB+{)jD{jFv^i8h}V)M9NLVw#RB=%;8$^QvfyQ9fy=k z3k`A4Ou=2vH5d;-+b)n3-6P4xo#2KGxeK5}?USHikswE>Fl3$|bt>9eC`5dx3BL!$ zIGlNoMDX~N!nnzf;F*VM<_{-kY{t;mhjJv>w#QTp5kJcAKksu#u3G-&ZL;S>nvCX< zMH~){P9{OuCIR6!u~6X;&UR+QSPG=6_m14v_H%$K@rA)F2Z7ML>2Xn^&%$j-?jCz4 zG36!vuv5&li{BGgDHl&X!I&zDT0ZuNu}tcLaRgZe;qh^s1qK8VITJ2Lno`W7q zw;z#0=mRiLxw|GWP!&j_lCvQ2{E7Fo% zImPv?)#aDl)5lTVi}Ky#>g9tQ8*Ze-Tnu#MuZ;O71h_S>t)Md*=?WVGpBQD&0`U!oV2z6QBF>yzmkP z7_5o0R~>jF@NwimliEAJD0pQ7XP>lA84ju={&w&(Z2nsMEx#I^O)DNp?x48d80J(* zUbkRG8>@qQ!7CPnrFUMkS}V*Xy{j>EtE&1;TwLf(5CG@%l!|>g>4hbHUvfWru#_` zJ=PSaJh|b?4LUZ7=Ds|G4IAhQmUUsy0_X&7?>h?jz@@XTt zcl^)uR+hc)P5u*bzI-?YGS4@83;y2|nmFn@VGHkFv_rdHiT_;`rM2|t?NmC;aHf~UgList5fwWgwSy|>}qEhB^td%?H136eNOP~<4w(8Nsed!y}^cS=mYftgkpZAS;^Yi~(U8gUEx;MV!4?nMGhrs`- zZgv6GU4lo3Q^cs!nSU&N#22+~B!I`L81e(1O%$EId4jM0;)zNhfalZ~{k2`iyzYqz z8>Ah}f6NHQn~$=uM6H8h$rrr;i6^wgV!q}?L@0H6`K63nDX!m$1zo;oqw7m^{M|5_DJ3rOl5~4KMZ_^-!ZjJ-96^3K}`GP7~frT zN;_G^mwnyC@8U*qtUJO_eSKfse3XCj%^~f`5uW>PN59`Tfb!yD{`t2LwYNUw$4?h& zhYs_#XTr73hxxl_(*5#Y0z<+f9`)Uu+TVq|=(`?T))8LqY<;a;A#ZWEF+0Eqo&BT3 zS2wZt#691hjaxGwG*S9mJ_8&|TSf?;t9qerG&@X>t-;GL3wrxOH~HLii?rtt^6Te9 zYHzrKwIA-F$8iTe?8y4e8r;;!?r>R+*FWL)zi;3-0d!;Xr`+*;^4kNu%>Kz%S84WP(4{}EsJgCly-$5g_OI^xga1sRecB{yNK zROUwket-aC4c+oOXFt}qTCdSn`x^kUpBvvC;4OYk(z;&bvwpNU_z1}0>`PD~67%;S zu0K2_WhttTVbOVwfB0hqZQeEh{f~?55Bn2*%a?+C`5kPm1;1E`B^+*D-NGE5eSi3q zXZ(~i?mFU1>#x61coCs>_zD89L_iV&dJn^lk!VL(Vv+MR4QXL;sBdplUA>ZV@gteN z-|t7b_Cbf7Qx`lX?gj{rvF>9&`h1s0d>;Z3e>6B7wFSLzUj{P>Wo7F9lh~)m(4}AH z2hZEA!>*9Y_Hkf9@54Xlzn$-{@k=)Ga!Rsq%Ppjh^N;jIQ<;C4y&Vc;Fv2s z{pV-3qB8FOxwdw^j90!eTD$QFpLD^|VQiTxn`z|=5AOd#*?dQ2^I5cwkI@s#<}6w& z~D5B}?gT0P6E4D9)99tU8j!{IU_`Tw-}W6)4gebP>gXxFVf&mdrjH=?{19aYY_83 z!ebyG+HVf=!ImTM3z(hE{+r!g-s9)ob*nkNGyHR@&GU2;j ze8VqKHLA83oy(gMi5m%Kgz;b(1qrJN*G097am_flmxun^JYu9E(avQaEtlhqgR^I0 zJ{q@n^O3(s*9}>NxMHGK2U}UbF#p&`Y!O+20$4R`P# ze}C8S@^-kh?0vraay@O}4!-SjCvE=w+_>B(qzwsRK?dLVh>#Zl5Eb_*c7-YD&@=1RtMf3!#H)fM}K5F;f$qWB% zp_Tk79DjN1?poKLW7?;0@pacH1>Jee+uLIIlRLQ2jR9J(xB0LeF?P7PrHva6b-r7%_`DeG9 zYh(XI4%Yt(8oh#Bkq%x5ccYD3%$>KAv=*EBoWGiD@4qR!)WReYWs+#|9{=I5)>`0u zyxQ$nn(;303~=OK{><$WK~3I#qf1{>OAxMBhe8U&&Sr&Luf}W2cv7+QR`t}fCnVZ$oW+8 zv0V{^heU@9d%@Q{`H6J*bX~@ykr5yi&%#1OJeOk>@DNz(R`4wB6a`oBYej1?cH;`} zRqem+HIHXMBvDHT+ZW(zIYjK<&8vy0WwJhUG*R}y&_XQojNAo+N3Lt*ufAR9AgxwgnA%&QUvDl4=Unb;f)_G z)7rhp-+d6JRo}=@J!q`CU)4XY$?7`4c=@qhbcv#G!4qTb=k8cs7@u#%kIg!`@)EYp zM0E*+1l6enNLM%2V%f(d?nUJ(b;OakO%y!$>~dVO`?>OVk$Z`1SH?Rp_mKZ^{o!;i zeA8~@IN#-g+V zFY0-WHP)KGEF!8^eNjKnSZn9sFH%{40YARN4@9qIDx%kYy^P-56ulAU(JONMlxJ75 zJEFXO#LZ0Y>W|tKVH$R~%y#+`pvb)g=`2@)w}SO$8k?eh_JV#zV>h(dxqhJ%JFgYK zpda*M+q4=l=+k{!TW$M#{S99>q|Wbf%i*d-4|XU_*e&diRmm^dP%uvB>oqE~SncyQ zdU9npOe@USU#raQVH@*l2+F@q2+p3002$f&`VW;^o#@Ges&?DNa12(MQVO;Dz#}nV zujdC{5fycP4JN}UUmxMe>V*EjRt|OnNeW%3fzi|YvQX{gwfbg1_EV!#6k=?al@d>R zC*)lY9ig%izeeBUk8!I0f_}=M)z<#X)Bo^ijhddyg=k*Fm*ZhFDG$T-(;EEPC$akp z+nlg(sFWYmg7$o#UJ}6CXjOA` zpFlP&gsqYROf8=+*IfFnK-RYB$1YH^ieen`W%DjNa<{2(NUmo1(^6>@QL$tY1xF28 zj>0`EIrCk*5y+Y~=u@uiywtT-jF-~Y0#rF#Qu0K`7T?}slhfhIGT}r z`Xj~lkMVMro?MePZ!}%83acj8{70<4v-B5hB8E{bMGb1DWs&9b6-ERV@*wgiFX0+} zR5pH@X#Yy>q04mt5Ej{~8YY#ul-GXLrb3enOYYEeT(SEKa#8XYxeu<<$A_?`TGLGZ zw-7d{tsS~iI4FS@3O5ee3q>Y)5H3G5Op7~O9Hq&{(XFNWGqu=LS{Q~ioYn4nKT|p% zMmOb1cZ3Jd3%N(jqhfak=A_r#XUp3~?ml^XWGKthrarH431x$lCqkL>=5t~DfY>#c zb)=WDbU?nN5S699z&!3>RHmL_L*;X1N&lKVAM>v&`PY%;%>I3Y`r-BOOnJM=y){?A zZi9a>FV^RUu|a|BpD*|C9jAUKj5T^{JW!8S)|U}JBZaW|9*w?i`CM(Ad8~fhzc>?zM$NH&mt0fJnm(S zDDL6<>e?(#yZ*fN`dgtLFG^){d_Z1bAYpUxzr&32di{NQyU4w5rS7Q1Cb6;l%t)4} zy|ILJhUzt=ShJuHQC6f(ky~G(=hkK8wffKM@7H17*+TtR9p=!+Ef)N7;&#$w0eM{R z$Rx^w)P)rs*??N$Rr=g={X{(|y)aL|Ru@X2l}e))nUwlG`){RIGGy-EB1~kj($U#^ zwMZy^JwtC@A4=y-r56{PlpdY;Z>1&kD<~a6;*Km7OHA0RpD6*)dincGi)uK|nHVrT288nEb~ z;uPds$1n?QBa#)Fbom$ny3Pck4|3)kbVV7e5-0yd{Ya*UwmI-7|FI#5G4&S*B4N`r%MY z{B@e-)3J4c89v#OI}h)pt{DmG`hdo)X19C9mAeG9J6YAx-z6pz7~IT@Gs2D2Len+l zBtdL;htOvCJCpONF7Hp*-)hX_BI?a950v{MN`?&D5%*xho^Wllb+5<*AF*gH3#O*B2Pcf7qyCR0M819+4V^hQ|@}0F^Dj5voAroUXxtk zAUuzor8kadj)*lNsUdc+M$z(mp1wrSiDt{|1(T|`I@hT_LXgEV}-js!F_h#xn zo3ez6h3JV_*Gni-UR}>E)?aPPa_Zfs2%2>5dq%hiU1>(O=~7qWOnpo<);?nD46l5# zdke~vSC?y%zP%X|eadwG_hzhn^(--7RIkN{)As3lr{*lZ%4g(euJ4i8^xe%_wBM2L zNVHM2^k15@l$x!`Y#zSD^?WNcFyR?JsRb6Y>96TyTCo1w&T0CN7OaJ~Y?}U43s%QI zaT+Q6zdlIp0}AC+gBg0AmTa-sV}`!5B{Vges_$#brZ<|3mk;=mB(g4s?N|@HozaF_ zv@njD+mO6!JWY4BV$HM$oAiuUcs#iFivD&h*4_EvE+Ec$8pY6ms_4tQO|+na=k4ov zlUl1hrqxKFj?KnEQRMIy-Y9#jdohZ-H*n#oh~D-NQ+t=GO;uC!6xVi7Ex$P8-ePKB zov&xNX6v33sT2d(vFy>%?>81d3%3UflIC(w`oPVg!& zayOr^XU4L5+SA~VW%WW@<0XqD{%%Hfe94*FwOK}>;rq1Sv<-`nYKZ%FMwBD>c@&ZM zulW+}@m#boUH)e+(xcy(~P!FCGik z0ypUHHmt67K4_RJ2cr?v(eAAi_0YDgf!1S!-l;9?qved#SF~kAHOtfb&uv*m+e_oI zHL)3;`?VuFHxyUK4Jh#XFb{8qOs@@*hqZP6)%L88*61bua0eC^a~~owH`=|7e3Bm| z;!XX1@zr3iZvYg?O4+Lr(jUI0uWN^_DtuAj(T>&W`2tC2;X6XtwUMLUAyDCcTZ*q5 zeOIreYCEg2T%W5K_Q!nlUVKbv2dR@K>4)Q4_;8za$awvc>VxTs1@w;ss{FeG{LfC5 zAVcJLqWBQU_cltB!_{jFOlV}H{B0=^@4P6xy?edrc6Zk}(QTu;v?eT!c7F?Oz8b(h z-zCr|f+uyZSnqWkuO@tBl)_F@;V`RGxIsdta0IR%4fJFF=K8)!3gM)$SoTRHe{-{u z|E!l@Z|B16u&gxcb+yNgoJ!MgbYKk|(4U7YS4M$;x?GtUJnMQnc(u9SvONnMaAU00 zwve>SLG(tt15n;w2kb-S`YwSg5eu`kr7xJBUB#ud>t7Jg4w$CY-;_{AWDl+$<%syk z_Z;-!CRG(;@zi7LD=GDXQoZfhF~a-NZnE~qpu*=HN6Di8Y0xXSl_H7hEAo^K%ZnkHaqxjyQ1;ach07i)xP)e1%`&zg+Vi#xN9 zjh`0?tjFj(^`<0?Pr^mW@a^|fGM?+(XN}&kD~r(wcVSIvJ8Vf8)yP$O& zt#3+TVOsQ4`rZT<>Gwn)y&4^@7j|KF^uH6>#DJA8>5eiCMfb(0^r=s<<~GMu!V8h? zMs0C%!Du*2fA0y_Dd1^bmu-g|#>3(IjVD-##zTe^yYkVp`f(qJlX$LgR4(~AOe@GG z_agOUT~WT<!!u#=##p!*jmS+!;!lPUws~lq#f8QBi*|P>+f}A z&1$WzK&m>@oiSMdqZ{s8=LYE=x-(r{J4pYkJL?+qS}JK#n+Rebx62^CK@Zlc*3N-g z#^gx~9zN;&iStAC**#dK?F2|zItOP@(k#z-PY@x@N2%qK{8zpKD9| z>n(e-86lr3cQc_O1?vM}V`r+qqbCaseMwLr-ws-qs{hoJ)u}rXH1~lAj4OOnf1(#lX?b&i@_N0UYzW8kd&m9A@omcS^W^w7J8$du6dlb;nrJPaoHtHK_O0|K$0{Na&}((VL|=E`y8c&&ep*>Y5_jh16~< z+k2?pBt`Gohdoi}GH{t)>=t@6|9HesrReMWu=@43krZf=G`ixM$Jf|EIP}|+s(;ys z)vhsD@uaEEx(O-z{XQ(FSuF6qbGz8(;=3WPC|bD~`^OWlN{T+DFCy`KUwuJe)>7No zSAV-NJEK|p=+CF%0r{o=`u-GFS6kUfKbyi@*31$1D~a!(BaX^;7h(`m@hkpAcvYFE)4mgR}cQ z#GKy?r1$_YpLSTV4i&# zUtNRav`bANQK;XAU%E&SOoih;p3~c;vh*i5cCYC8R?(FVe9eL9o9NhYjJxOozDkZ} z^Ftr^;^#hkX(|hCJy}T5XSXV5!P(dE!~6cgyMITkd|v|n#wJyx0hmG7o_hZQtf%Hk z)?XUH=4!W+^=bo|yfh;*>pdgjM7s*IA5_{)F2kG6?fP5L2&uC z?s_3)!?H!p9+lGif_r9n{rVslqivh7HyF%Z+QlTXKhSDZH*(hekiAM`Fj?gOKwV=2 zVbHH@)%@Siag#KDqk~3DcAN-K2_2f0LMUrD4Xmgn`l%1RsBL-=Czv zH3XYSu=D#6*239G3R~cjm2!Tb5l0xY!&x@(Z10LYoDrbXFXO8n&T8s1wKoz#ZGzlQ zlq`{i6D1+e_!T(%?%+p2`fjalKi3sEa#UY86e+niQQte1^$!n2XM5uwQ-GxIM;R~2 zC@E2|JB&5$7YbqTYm(o3V_W9Y#^wF4(x%yu%L@0%j~L@y;K+{?U3CtXT0vL+;4n6_ zN*1Zkt(!7OZ!nxaqj_fQYlpM?+RwA~ox@qAb>bXUc&tOH=1wsVcGXLVV>8rej^6Dl zmfY~w>GVktz3BE`Hd}l+G#CXWt1y+=F*SToPS@XkimlVaXX^-P>uDocT-Bpp zU=`z(lAymif=#G@7T(b_KHjJo)21e#(V#%RhxoX&ZXd~_>g&`k;3K_EbP=PNp6jen z9LX9t+&Hb=CFd;R(!|GI`h1$ceI#3>g=6n-6l)x|G#DlDuXxPewVmNaUP3w{evQ`` zjbd$Tv?d%JElwbK!3cgrKQxL()TjwOg|iHJf|WY!Wus`zva{Z3H0z|zh||Z7W;Hv9 zhtM6e2<1J6eox0Zeuiwc_W_!2_aEVs#AkOTc+vDbR8Mx%-ye-yVe6>h9gR0DYdh%m z$FLW*d+qhTW7vW!W8g8u-tif|<5)am*YBvW9m`r&`I`LA^?i4mesnChWHxrtL(*8Q zx*vnvJp&baKRRb|r=o?PE(}0VgWKz4(^!MFkwQ|wi;%xf|9J<}MSJ36kLY8%#DCQR z41P!ZGHTRT#bBkng%uY4KLYC&E?wRdmJ6Y4#?^TJVj7FA`eP^A;Un>SwQ+1pKovo! zMenmyb>}!1t=*2--x-IUs`XR!U&gVi)psd{iP9;py*_F@TVMYQA|{eVB?Dew;pQdY z8Q(F!YO4oNVC^~$cpATt7wk#(ccYqm_h3EWxFJjZU*?bg-)=+g_D=-Sh+H=Av_mE= zZKp4pz?wS-$Kv8pdoa+wQ#w%4mM#~m-+2Cjq>&%1;3bLQ1QZVh`1RElqHouXpF8N+ zC$L2gKr593u|!I4qlZsoGyGC%EH1Xz^Cq$ADwj!XuJ6u?`oT$Tkk)n* zEvD<~v6E3JlPBuEC$j-sw{~J3Juo^3mUHcxgG(Z*uu#Y9_(Iv*)^NTVY>_|s{H3gl zw}BmHtWiKAkIzwmaGk2IkGIt;Kg}XT?*LB|%xYYLBJnF=#xJe)E>9!Nx3<;igQw~W zQ0SM2pKrad0M|Y*rL%$UW=)tdFMZt12@A(QJ!k5~Ruji%jO!HHXs#s%f23f}pb6t< zMoyVDZsMdlttQVIH)GP+sS^_#&DH0pv(E8IO*;O?I5%V5jPzD>CrwCbG|?+8arl4! z38{}vXSMZR>8$y_(sY)hbx4`#@nqm!k8=;s(>NdCtTW%^>49@1&TO2Ua2~*UCBx%s zF>l}ax$GBTC;hGN6YwK56l*}V^Xqv$TLB9I3kf!cAT|+?0ot*1e;%*}hObRD0v?FJ z%?+4{dG-xpq}5_+-4w=c$V9*eVfg#gfGOelp9Px%4`>6N9)bUP6wtpm{)dePXr$K0 zU(*2G0Qd#qb-*)#(RJ|O-~y%rmI3Yud_Z)-z~<;R_Do{|uK;!j990j03LNk!z{P+I z>f^th1@wu;UuABAjscbduEy&3vzE|Le`G13mB(|mfyL4ZFs&i}*FwMtfG+_~v0E%h z0WFOzmdkVvSQU9t0@xBTt}*`30AM=cIKZuda{x;LvjD@I;7@}CrUGsOJO}7N4)?$w z;vzr`{gI=MXygNa1Q>(I!E=BmfOi4YTi_mv2@;Lx*_nV{nj_SJnYbQ^q}>cy3b>~_ zLW& zZd^YB?nK=Ws|Gr5pThx5upHP5Sb)`QejpU}#gL$+r1gj6$OY>F^t3YYfL8!x04;vV zF~DfRLclJ7B?CN8PbwNQ1K|K*BH#kRe85eB^vt;vumtcZpmh*J3m60V7hoD-4C{2!m^uk^2)T6%90yFDfoTU=FcUQb*>9N*JYXc?0>IL_9#05n zL(F_c46qdNIAEr8As8_&V(_MQ6JX&YFk*USF2INUy#Y0OkXh09uxUj#&}24A~A?3h0AbQILfQ0LEk^f`HD%94N#ziOfT?V0xsj zhajd$=|+SS(E2(IV0M%gAQu1&-h?5{j*`uA6tm;{TS!L0n5~EiW<~ltxCY$$F6sl| z_4kndm<=)8Jf0lDbihKuoq+cmWBy-9Bc=%$wqr~HBi~0d0VeK*=g}~@3n9hqh}jK= zfQfscs2M{3A(9KTA?9O*9j2Vc`*DCf4?+Q;^$>bi3w(g<>ADc1$5JOBa1-#> z!{{mS1%Rc1F`s!ni-F$>7y!PQBdDZKG;^13iRlyy`BTUxz%;;YzyiR{fTiCd)POPPk$f2HOu#xA>d2ol|4Y!= ziN+Kff(t022zeo37($&^ig^y0c@aZ^kZ1lvH(40Gj8G!xWYdA7n$(;3TUVR;voJM(C(RVnC+G{;CxUlh5eKu z>k2I`h`L1l^gD{tpi%XpgZT4-Yu8urxs)}kKYF0WQoDA5pT9tYNF5`AZGb+l^;Jt* zE3Kff{_#?*$TLz1wrNaC7GvAP(a+;~0&R;hIzt$Z#N`IiruWnPFJld=?r-7o(BIUcbuuoj>5WD)ZN=TsZa8+5OAI4+cHzWoC7(~w z4$r%w?cY~_ZaJG1v?~nsimtZQoi^tQM zH2$UU&0*adoQ9S9;U3R3VDs65f`Ef&(qE8^eA43?r0LHuXEuFFKC7yCcCmgzbnnf1 z!sDU89p=+mf62wV)uT?l8;1Wc1(H4&aVx@ObD?wE4vAFRj8*?EyVzu>RdD)-z~0NsRFMtVGbXb^_!8*^D>6m*$h+aMeE?&I^i5V#QFU_u;} za;Jf_0_QDEgfoDv101?74#GW9`e3pgXGP&5eO{cM{=AbNNb9?Lx7DmcQhB3Jk%1x4%C@>tzgQ_zcc?|D39z~gfk3kgvWw0>kE z8n_(LFM*y-^iTAmd90pu4{(m{*drtyhm05rZJ}3&BDf#)RYZT*OHU38%c|5XC{mji z6xKV)niNFK75ej7mQ6c6p83T4q?cEl?$s3u`UB9H5WTIS>$mb)crZn>5KDuFM5tC< zue}DnSpa1APOOp%`74%7(zD54Z5u#84f?Z0FV-ioL2r%%7l$4S3$K^U%q1D}@Dk`- zVdxo>$pT$QXoQ#UgB!pGcsPsbPnXjvC55~l^y9EOmFRuT>BE&zVU`|X*pF4xbYf`j zbs$NfwT9K|pAO$nJ$vQA6uV*@hvuf0f4 zq_@vF*y^d*bFsSm%6!%+=n9CXSo!or8Sr^d|2Us@>v{>ow8R#F2-6Xis7=Fc?O9D! zdkgN3>z{gbi%f;H)8Rw`aP$Y8eHw=Ahc>VV`sj764SPe+U&rhXnpgLDdQv3zQ}Scn zaQef@QOx=7B(zokZXHYLShw4w;#^WG%V$NU)tXpow1B{ED^@RnKBJ|Qc4)W2`fkH$ zy&h++YLX`hpwv3X<9UIU+QOtK+h2eu1N71tm_1^uSIPVG{!B_}aTtl-+$(~vsc&&QQx#4p^AYJg~w;E6e24jaL|8( zS|QBRo4v>yG`2!Pq}RZ(N4%rJo9^WuM7)h-nU}A#sw%3Y<(nXn2W0{|4UwL*7CeBeonBQQEM!fpNNyC)Enc~6 zpewm{Aa3=_UH+F`6U%xCF7OJTeIz&?f&~zqM2>u=cY29MInN|K@-~L7lDEwvi~+Uy zybfU;)bXPr?)M7oz&nCswC+goz^o{rP8S035RYr0Tw$-KC_7Uys&+gRuO-4t{fP}MiG8GJZb1641iy%7Yv~_l zvjO&YV6Wl>_9h`T-3DuoI_tGwW(~VNsD=kN((@zo0|y1a1**>mJbz)Hi_TCdzJ?9&f!!0dPx%IbANT|5AVVsze?3fn=)ekdIEss8iJn7a+Q--^nsee-o}0MnYX{=oenaM++x6vDR8bNB!X@_WugI66h$4Y+cpq zgd{*n!jcdObV!64A*PX?0L@NVG(H#vL_+X7;xqm&%@pMWomJdPiYnW|7q|_o8Q}->R^h&xYJ)m>D z-4E_Er*U7S2Avvy<}^&BMqYkTCq1iC`dvuZEf@P0jncWnd2sV&vUqe3YoG*b1UAET z7rigy-fqDEWT!}Mf_N0s(c0V4v!f!bQSS!mRXe>_zt^LqxSdWf*2?a5?daGI#cfXU z6c*LE4BKGlUFj4*1s_C?o`7Dh)BD%&^~OVQmeYHsQLmsg2GFhjq=W~5uecnFrA{%z ziZoihd3bz`n4%hty1&85?a2IdYV2j^${JnNH5r?V{mo*rjU>B(^=Kjc2(pDMVlnzZ zgWl`SlQmfPCYTd7Iy>jz5I>K^hc}Atip@!}vpKp^ch0#fNpgG#domCyegwsmh)La~2ei5Yxz{>H zEC=wmlUcY)ckZg3<`t}24oy0bTOEnT&ZkGFS-@q3_SzHH_B*@HpIu@f3vwKThnsZm;&BCb7IxX`~Z~{2U`iBRcan5vSbL(cE z->M$W>F|=iNAB&`k|u$Cb8Is%Cif^dEw||WoS+wnCsg<^TOfN(k@zX-ih=r^8M_4! zdoL6yHR&(0*taCJ(X8L1$9Bn0jKxrIC<*IO-Sj#OL-jeFIFek@q;AzcdoBSs7P29d zodns&g5g;Q(-ZKukl6y66(mz?=5N*Qk}m6p9Zr&2xfOrIupIHa=P{_kXZ@wLgSLk* zbO`woc+{(yXP-{&0{Ap|*PZB#V?pZr-N9OK5*y(j!_ANvzQcsJ9!*Cl$LdLwh?$ z?%J*=dv!1KaV=iRr{_mfcj~V+y7kZhz};bQUZmYF{iLVJeF)fn_}6{t*L~O5eaE+C z$F6Ty9?_AXB04!CWsZE5?(s;Eq95r1R~^k_q5Jim62eFr!x|2>e{h!e# zd9>RBVKuizZ%BwFP&vjLaR&REzEDmfqHuZN2q@hmA5v(K+dymzRB5( zuy9^vz+Qbo|8so1o7Rm?r4m$Im2CWA z>vRg*CS)m}sibtsTz5d9i}N?sU)zKZDnOCZLFWsmEKBq_=0QKYyqeQl9H=RR-zYep z1`@5p56Qn;W0R^k2kJ4wn{8m=D#4EnzKHQ6b%`+WKPassHxerZF9xnv2Q}2}JcvJk zU)`58{-?0-7PVI-pHYH8DtOzC42%(+zP&{LUJ{4@B=}~*dHaangNNlKj0Y7}5K%8- z)QdtthShk%vo|r|n^a|BP`#)3}%cES5J z;x>QvpfD~IM!sZlNbnVL_({PZio??rxd4ab@Lqy92<~e1;zZ<6y;ncTIXcG8qL3Mf zKO}g59R8Z%z2fli1Rp8*Mk!btmITW8v!F2SmI2JJ!szf2OLi88v4RhX!{-Y=H4YC8 zer+87gy2T-VYJsGiVEZ8!<^3JKz%Q`TgWQ}chiXP4U_Y-F3xcayxreG#_{vsFf)no z7XEmCmp9Bq;Fr}sDuyMXm-|#h4RQD^!Ao{DmA?)+O=|&@cUWijOgRcWX};=2o(^ z+Q&FOeG~NpPOiNmu8m<>U18;IB0R7~%fvlMXM~4(TIYU_SPfb)`wcdM8nBdvO?oCt z1Sca#KKO>P#4?e%2Y4RNiK`bf?&g~3NfYwvbb1ErEApQRUf+Xpde99UPDqmii2NFn?FpR%2A*aq|N7mW?#O`}V|GGK&$fa8R!C8Cw;g2f3q@+H zXxE9h+t_|+)pF@ZOAXZOMGZ+ye@4W%Q}25bvwh_q=6og+H)Cogw*tR0aJS(10=Ip@ z|2bl$86C(j-5?Tl1%q06hP3dxf(OGH&9mBX2G5!XKk=Wn)=q-$Qs zOtl_3*{qkIPDczxZ-#ZNEd9MX`YP@C!rx?O=>q!gFU9j1)mOR{+;!>sz^&7BIbV9* zBzlgt+J6czITxbW{UAD`BZ?8%;2Vh9cuVljYuJ2Vj&oPC|F%2{`K%mncv-gIZ-dGX zIQ=B4@)y~j^WsY2{s1BmZNvPMDxN6GqAIz-@!GIA)6)A zFWu7ks^BwZ^{^W#9xf-A-I3h>@|XL(G}Xfw~^J?biA+Q~9ROs(b?gc~h6frs&9+s6p`P zS&X}RDig=0WVm(!Dp2dN>SLCDLF>ss6KSA<#jd({iYhPjsO!byp z`?$^mQfJ)+9}Jv4sLf_mbP7&1#m9IQpQ|LIvnHZtKK86Zx-v$$rFVg&D_^Rn$m~!q zjKn-%)@I0#!OgPez-{$O9TI`$^oNjMd8_CCaD)I{S(jjxMz3z>jQ zSW!&^PC3=S#=ruxxJBgaq)Nt#{13owP7_k{Me{JTWC*CSg3pj1kG+AVYXlDrXWzRg zF#aBJYq89W5lg=N4zPq9@qGo)mNH=lv2;Fg@|{<}#s%;}lXnaY;9g-j#P!mP(CKZF z`*Suoin+g;olu+bsIcq%FxxNe=3UvlwKBIlFRlho=G17GcN^IIf%~XU+V4cn4(&pg zptEM8ec_!gv;3ckel%`gY=d4%1G!FGYzix>s_;!1f^^JCHlx2WOz*ph)&sXQPw=FZ zkbTDIl4}Aw8EN`tnCxIh)ebmqv8tsnx*l?=$glpH1$&5m6>w^;esQg`1~}>?Q$=Nz z4Hbp=!XKn&`5VNXJ+J`h65$TSIzevDh|6X|*pC&r#{O;>zNbZ2V)r7qBe)(ha=AfV zP8M57fs?J0Ul^Dz_*bMKJm(yCy!sH=$Qa?S08R~ACw{xxVXu`>S5-?{-gV1kz^%tW zuz@{d=cL#v@5l0&3O=a2%{QnXj~kl{!5~9{j;ug$z=-YyP7W>F&gl>b>PKrRUHPPE zxD2MCX{bd$lY;dV`C8y)zuc~HLzEgMjJ;q`2Jv0JLowsgLoC=*mD;kPLj+BF6;ZCPQwUXqKXHQoG+C*Y39g zr}E60$`$!{kspmQOt{b6NNjKKJgTV{l)bKaxGFKr@4mQMXYu8&G zEg?^WlO)3*f!AQ{1{QHnvI?p~>{ZB-s!GP`?Fi8v;M78)e{=c?4%Cf|U#fgRGV-8! zzFHIlpD}Ll#UT2y;N=m<=~^ApufVAQKB-AJxV!d}Ht4+^F>bhR) zI$iK~y`_p0$05e;xQt>{j0P5>J1<0Qfs<`~jLoLqjH~-F-tfMK72Vb$8U@_i*~RJO z9H=VbG$jUPB%*s*+Q@c-Dm}m%xV5Dg8#3e2k{3~L%8KfEpi;c&CQxm7h zJzT#o^5sWaP_j_}CwTmd*JB{tSBF_%rY$ucI9dV2>?vl@eOaQF#IR0oIvsoQbnHFE z?7q$a8)2xMs@<$Nlmj)Ql=0}V3~Uv=(hU6w-yVEO*d==y&*dsoKLV%L{7Igr+_X}J zd6BYdptC6uy@Vtx6 z@Y)YJO&3*(($GRA@9WF*=pvS`0#1%bzvi^`C`I?9&ghQwKh~*wld1d|b@7|1q~jxy zM6OqMz;ZyQ%4L^;0bpr};L+|Z?~Xw#1h4*uNZ})>~2@zXzoE4?biUH zL6$D1b@lXwW#|43w(Tz$ab>I#i-p)f*lO&317b8QDVdd=fhbA9N0r8!bQg-~gwxcm z%!QxBgL6l776I9q(`8elKQsF-VTWF2_-r<(9=B|L$Oge5AytE7MB`Te{$r?k=_*}T z_)cs@T@2io>rDoJ6|1`h56xzPZa)wWE|VIUslqLRQ-GtvZDTG)+}eDbbta1LOD5wB zot5|l*tY7+ykn)rvaDguTe?p0_>r_xa9`Xq*xDQ=!#&5jCz$bwSr6oqX}K6)VFfEs zf=!j-lZVCD!uF2mKF}|&$M*qFO&I!#1*KQ1alom`qjCGX4W{x-tbyNwO>@+y80`4!o)i(88eB$n+*jATpX&}OORXZ&3}`DWLbx}Rx#0)OB9<(IlBGUkNd zlM-Gxv14S!k9hNtHfi$2CH-esE}lE9V(x;)VH`Q?k&&6F^qmQ<&Y!rXAFv8=XAkc= zGVImF)HTbkD3=PA@6#Q{4cQ1W50!6HmPD#NyW&CeJaYwO+2q+UK9U|`-cY) z?iwx`!`QO2wzA2}dIMaRzlJ0e# z&rC%!BXopqhW1KmYKC}*c(T3b_z?K*UKVEWGdTg+4#?bBVx|T>^l!@kjv#f$K31Mn cXpTcF^@S6hzWf5yWvHi*r?}Ki8|vx$|63^GH~;_u diff --git a/src/APIs/c_api/src/convertor.cpp b/src/APIs/c_api/src/convertor.cpp index 1960f3a..216f950 100644 --- a/src/APIs/c_api/src/convertor.cpp +++ b/src/APIs/c_api/src/convertor.cpp @@ -10,8 +10,6 @@ robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan) robot_laser_scan.header.seq = laser_scan.header.seq; robot_laser_scan.header.stamp.sec = laser_scan.header.sec; robot_laser_scan.header.stamp.nsec = laser_scan.header.nsec; - robot::log_info("LaserScan header.stamp.sec=%d header.stamp.nsec=%d", laser_scan.header.sec, laser_scan.header.nsec); - robot::log_info("LaserScan header.frame_id=%s", laser_scan.header.frame_id); robot_laser_scan.header.frame_id = laser_scan.header.frame_id; robot_laser_scan.angle_min = laser_scan.angle_min; robot_laser_scan.angle_max = laser_scan.angle_max; @@ -59,13 +57,10 @@ robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occu size_t data_size = occupancy_grid.data ? occupancy_grid.data_count : 0; - // robot_occupancy_grid.data.resize(data_size); - // robot::log_info("convertOccupancyGrid: 2"); - std::stringstream ss; + robot_occupancy_grid.data.resize(data_size); for (size_t i = 0; i < data_size; i++) { - ss << occupancy_grid.data[i] << " "; + robot_occupancy_grid.data[i] = occupancy_grid.data[i]; } - robot::log_info("occupancy_grid.data: %s", ss.str().c_str()); return robot_occupancy_grid; } @@ -75,9 +70,9 @@ robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry) robot_odometry.header.seq = odometry.header.seq; robot_odometry.header.stamp.sec = odometry.header.sec; robot_odometry.header.stamp.nsec = odometry.header.nsec; - robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : ""; + robot_odometry.header.frame_id = odometry.header.frame_id ? odometry.header.frame_id : "base_link"; - robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : ""; + robot_odometry.child_frame_id = odometry.child_frame_id ? odometry.child_frame_id : "base_link"; robot_odometry.pose.pose.position.x = odometry.pose.pose.position.x; robot_odometry.pose.pose.position.y = odometry.pose.pose.position.y; robot_odometry.pose.pose.position.z = odometry.pose.pose.position.z; @@ -136,7 +131,7 @@ PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pos c_pose.header.seq = cpp_pose.header.seq; c_pose.header.sec = cpp_pose.header.stamp.sec; c_pose.header.nsec = cpp_pose.header.stamp.nsec; - c_pose.header.frame_id = (char*)cpp_pose.header.frame_id.c_str(); + c_pose.header.frame_id = cpp_pose.header.frame_id.empty() ? nullptr : strdup(cpp_pose.header.frame_id.c_str()); c_pose.pose.position.x = cpp_pose.pose.position.x; c_pose.pose.position.y = cpp_pose.pose.position.y; c_pose.pose.position.z = cpp_pose.pose.position.z; @@ -156,13 +151,13 @@ Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose) c_pose.theta = cpp_pose.theta; return c_pose; } -Header convert2CHeader(const robot_std_msgs::Header& cpp_header) +Header convert2CHeader(const robot_std_msgs::Header& cpp_header) { Header c_header; c_header.seq = cpp_header.seq; c_header.sec = cpp_header.stamp.sec; c_header.nsec = cpp_header.stamp.nsec; - c_header.frame_id = (char*)cpp_header.frame_id.c_str(); + c_header.frame_id = cpp_header.frame_id.empty() ? nullptr : strdup(cpp_header.frame_id.c_str()); return c_header; } diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 1b1967c..e7b2fd4 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -549,8 +549,10 @@ extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped &ou try { - robot::move_base_core::BaseNavigation *nav = - static_cast(handle.ptr); + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist(); out_twist = convert2CTwist2DStamped(cpp_twist); return true; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 3232788..b7951bc 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -30,7 +30,8 @@ namespace mkt_algorithm class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm { public: - PredictiveTrajectory() : initialized_(false), nav_stop_(false) {}; + PredictiveTrajectory() : initialized_(false), nav_stop_(false), + near_goal_heading_integral_(0.0), near_goal_heading_last_error_(0.0), near_goal_heading_was_active_(false) {}; virtual ~PredictiveTrajectory(); @@ -255,6 +256,19 @@ namespace mkt_algorithm const double &sign_x, const double &dt, robot_nav_2d_msgs::Twist2D &cmd_vel); + + /** + * @brief PID controller + * @param error + * @param integral + * @param last_error + * @param dt + * @param kp + * @param ki + * @param kd + * @param output + */ + void pid(const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output); /** * @brief the robot is moving acceleration limits @@ -368,6 +382,12 @@ namespace mkt_algorithm double final_heading_angle_tolerance_; // Angle threshold to consider heading reached double final_heading_min_velocity_; // Minimum linear velocity during alignment double final_heading_kp_angular_; // Proportional gain for angular control + double final_heading_ki_angular_; + double final_heading_kd_angular_; + + double near_goal_heading_integral_; + double near_goal_heading_last_error_; + bool near_goal_heading_was_active_; // Regulated linear velocity scaling bool use_regulated_linear_velocity_scaling_; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 00bf276..70b0ed5 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -117,6 +117,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() nh_priv_.param("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05); nh_priv_.param("final_heading_min_velocity", final_heading_min_velocity_, 0.05); nh_priv_.param("final_heading_kp_angular", final_heading_kp_angular_, 1.5); + nh_priv_.param("final_heading_ki_angular", final_heading_ki_angular_, 0.0); + nh_priv_.param("final_heading_kd_angular", final_heading_kd_angular_, 0.0); nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.01); nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.01); @@ -178,6 +180,9 @@ void mkt_algorithm::diff::PredictiveTrajectory::reset() this->nav_stop_ = false; last_actuator_update_ = robot::Time::now(); prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + near_goal_heading_integral_ = 0.0; + near_goal_heading_last_error_ = 0.0; + near_goal_heading_was_active_ = false; if (kf_) { @@ -387,7 +392,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: } mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( - const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { mkt_msgs::Trajectory2D result; result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; @@ -472,16 +477,16 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( // Use Arc Motion controller for final heading alignment alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); #ifdef BUILD_WITH_ROS - ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", - xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); + ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", + heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); #endif } else { - if(fabs(carrot_pose.pose.y) > 0.2) - { - lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); - } + // if(fabs(carrot_pose.pose.y) > 0.2) + // { + // lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); + // } robot_nav_2d_msgs::Twist2D drive_target; transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target); carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); @@ -512,24 +517,24 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( return result; } - Eigen::VectorXd y(2); - y << drive_cmd.x, drive_cmd.theta; + // Eigen::VectorXd y(2); + // y << drive_cmd.x, drive_cmd.theta; - // Cập nhật lại A nếu dt thay đổi - for (int i = 0; i < n_; ++i) - for (int j = 0; j < n_; ++j) - A(i, j) = (i == j ? 1.0 : 0.0); - for (int i = 0; i < n_; i += 3) - { - A(i, i + 1) = dt; - A(i, i + 2) = 0.5 * dt * dt; - A(i + 1, i + 2) = dt; - } - kf_->update(y, dt, A); - double v_min = min_approach_linear_velocity_; - drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); - drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); + // // Cập nhật lại A nếu dt thay đổi + // for (int i = 0; i < n_; ++i) + // for (int j = 0; j < n_; ++j) + // A(i, j) = (i == j ? 1.0 : 0.0); + // for (int i = 0; i < n_; i += 3) + // { + // A(i, i + 1) = dt; + // A(i, i + 2) = 0.5 * dt * dt; + // A(i + 1, i + 2) = dt; + // } + // kf_->update(y, dt, A); + // double v_min = min_approach_linear_velocity_; + // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); + // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); } result.poses.clear(); @@ -593,7 +598,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( double w_target = v_target * kappa; if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) { - const double k_heading = 0.3; if (trajectory.poses.size() >= 2) { const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose; double heading_ref = 0.0; @@ -610,17 +614,31 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( break; } } - - // robot heading in base_link = 0, so error = heading_ref - double w_heading = k_heading * angles::normalize_angle(heading_ref); - - w_target += w_heading; + + const double error = angles::normalize_angle(heading_ref); + double w_heading = 0.0; + pid(error, + near_goal_heading_integral_, + near_goal_heading_last_error_, + dt, + final_heading_kp_angular_, + final_heading_ki_angular_, + final_heading_kd_angular_, + w_heading); + // Apply acceleration limits + double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt); + w_target = velocity.theta + dw_heading; } else { w_target = 0.0; + near_goal_heading_was_active_ = false; } } + else + { + near_goal_heading_was_active_ = false; + } w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); // 6) Apply acceleration limits (linear + angular) @@ -629,7 +647,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; - // robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -715,14 +732,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( // (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate; bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate; +#ifdef BUILD_WITH_ROS + if (result) + ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); - // if (result) - // ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // else if(fabs(velocity.x) < min_speed_xy_) // { // ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta); // ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // } +#endif return result; } @@ -879,7 +898,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( if (abs_heading_error > final_heading_angle_tolerance_) { // Method 1: Proportional control - double omega_p = final_heading_kp_angular_ * heading_error; + double omega_p = 0.0; + pid(heading_error, + near_goal_heading_integral_, + near_goal_heading_last_error_, + dt, + final_heading_kp_angular_, + final_heading_ki_angular_, + final_heading_kd_angular_, + omega_p); // Method 2: Arc-based feedforward (coordinate linear and angular motion) double omega_arc = 0.0; @@ -911,7 +938,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( } } - // --- Apply acceleration limits --- // Linear double v_current = velocity.x; @@ -944,6 +970,26 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( #endif } +void mkt_algorithm::diff::PredictiveTrajectory::pid( + const double &error, double &integral, double &last_error, const double &dt, const double &kp, const double &ki, const double &kd, double &output) +{ + if (!near_goal_heading_was_active_) + { + integral = 0.0; + last_error = error; + near_goal_heading_was_active_ = true; + } + const double dt_safe = std::max(dt, 1e-6); + double w_p = kp * error; + integral += error * dt_safe; + const double integral_cap = (std::fabs(ki) > 1e-9)? (2.0 / ki) : 1e6; + integral = std::clamp(integral, -integral_cap, integral_cap); + double w_i = ki * integral; + double w_d = kd * (error - last_error) / dt_safe; + last_error = error; + output = w_p + w_i + w_d; +} + double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) { // If using velocity-scaled look ahead distances, find and clamp the dist @@ -957,71 +1003,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob return lookahead_dist; } -// std::vector::iterator -// mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) -// { -// if (global_plan.poses.empty()) -// { -// throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty."); -// } - -// if ((unsigned)global_plan.poses.size() < 2) -// { -// auto goal_pose_it = std::prev(global_plan.poses.end()); -// return goal_pose_it; -// } - -// unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; - -// double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; -// double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; -// double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; -// double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; - -// // make sure that atan2 is defined -// double start_angle = atan2(start_direction_y, start_direction_x); -// double end_angle = atan2(end_direction_y, end_direction_x); -// double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); - -// for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) -// { -// if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) -// { -// const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; -// const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; - -// if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) -// { -// double current_angle = atan2(current_direction_y, current_direction_x); -// goal_index = i; -// if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) -// continue; - -// if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) -// { -// goal_index = i; -// break; -// } -// } -// } -// } - -// // Find the first pose which is at a distance greater than the lookahead distance -// auto goal_pose_it = std::find_if( -// global_plan.poses.begin(), global_plan.poses.begin() + goal_index, -// [&](const auto &ps) -// { -// return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; -// }); - -// // If the number of poses is not far enough, take the last pose -// if (goal_pose_it == global_plan.poses.end()) -// { -// goal_pose_it = std::prev(global_plan.poses.end()); -// } -// return goal_pose_it; -// } - std::vector::iterator mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) { @@ -1144,18 +1125,29 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto double v_target, const double &sign_x) { - if (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) - return min_speed_xy_ * sign_x; + if(trajectory.poses.size() < 2) + return min_approach_linear_velocity_ * sign_x; double preview_distance = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_); double max_kappa = calculateMaxKappa(trajectory, preview_distance); double v_limit = std::fabs(v_target); + double journey_distance = journey(trajectory.poses, 0, trajectory.poses.size() - 1); + + if (journey_distance < min_journey_squared_) + { + v_limit = std::clamp(sqrt(2.0 * fabs(decel_lim_x_) * journey_distance), min_approach_linear_velocity_, min_speed_xy_) * sign_x; + } + if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6) { const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa); v_limit = std::min(v_limit, v_curve); } + + if(trajectory.poses.size() > 2 && fabs(trajectory.poses.front().pose.theta) >= angle_threshold_) + v_limit = min_speed_xy_ * sign_x; + if (fabs(decel_lim_x_) > 1e-6) { const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1); @@ -1179,22 +1171,18 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra drive_cmd.theta = 0.0; return robot_nav_2d_msgs::Path2D(); } - robot::log_info_at(__FILE__, __LINE__, "y: %f theta: %f", path.poses.front().pose.y, path.poses.front().pose.theta); double max_kappa = calculateMaxKappa(path); const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05)); drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x); drive_cmd.theta = max_vel_theta_; - if (max_kappa <= straight_threshold) // nếu đường thẳng + if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng { - if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_) + if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_)) { - drive_cmd.theta = 0.05; - } - if(fabs(path.poses.front().pose.y) > 0.02) - return generateHermiteTrajectory(path.poses.back(), sign_x); - else return generateParallelPath(path, sign_x); + } + return generateHermiteTrajectory(path.poses.back(), sign_x); } else // nếu đường cong { @@ -1204,7 +1192,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra } } -robot_nav_2d_msgs::Path2D generateParallelPath( +robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateParallelPath( const robot_nav_2d_msgs::Path2D &path, const double &sign_x) { robot_nav_2d_msgs::Path2D parallel_path; @@ -1232,8 +1220,8 @@ robot_nav_2d_msgs::Path2D generateParallelPath( } double theta = atan2(dy, dx); - double x_off = p.x - offset_y * sin(theta); - double y_off = p.y + offset_y * cos(theta); + double x_off = p.x - offset_y * sin(theta)*sign_x; + double y_off = p.y - offset_y * cos(theta)*sign_x; parallel_path.poses[i].pose.x = x_off; parallel_path.poses[i].pose.y = y_off; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp index 09fe4c8..6fa90d2 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -29,8 +29,6 @@ void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh) { yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); } - // ROS_INFO_THROTTLE(0.5, "%s", nh_.getNamespace().c_str()); - // ROS_INFO_THROTTLE(0.5, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_); } void mkt_plugins::GoalChecker::reset() @@ -49,19 +47,27 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam if (!path.poses.empty() && path.poses.size() > 2) { double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta); + for(int i = path.poses.size() - 1; i >= 0; i--) + { + double dx = path.poses[i].pose.x - path.poses.back().pose .x; + double dy = path.poses[i].pose.y - path.poses.back().pose.y; + if(std::sqrt(dx * dx + dy * dy) > 0.05 // TODO: get resolution from costmap + && fabs(dx) > 1e-6 && fabs(dy) > 1e-6) + { + theta = angles::normalize_angle(query_pose.pose.theta - atan2(dy, dx)); + break; + } + } double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy; double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy; - if (xy_tolerance <= xy_goal_tolerance_ + 0.3) + if (xy_tolerance <= xy_goal_tolerance_ + 0.1) { double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; - // ROS_INFO_THROTTLE(0.1, "Goal checker 1 %f %f %f %f %f %f", tolerance, old_xy_goal_tolerance_, goal_pose.pose.theta, x, y, theta); - if( - (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) - // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4) - ) + if(fabs(tolerance) <= xy_goal_tolerance_) { - robot::log_info_throttle(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_); - robot::log_info_throttle(0.1, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); + robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0); + robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_); + robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); return true; } } @@ -73,16 +79,15 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam double theta = angles::normalize_angle(query_pose.pose.theta - goal_pose.pose.theta); double x = cos(goal_pose.pose.theta) * dx + sin(goal_pose.pose.theta) * dy; double y = -sin(goal_pose.pose.theta) * dx + cos(goal_pose.pose.theta) * dy; - if (xy_tolerance <= xy_goal_tolerance_ + 0.3) + if (xy_tolerance <= xy_goal_tolerance_ + 0.1) { double tolerance = fabs(x) > fabs(y) ? x : y; if( (fabs(tolerance) <= xy_goal_tolerance_ || tolerance * old_xy_goal_tolerance_ < 0) - // && (fabs(cos(theta))- fabs(sin(theta))) > cos(M_PI_4) ) { - robot::log_info_throttle(0.1, "%f %f", fabs(cos(theta)), fabs(sin(theta))); - robot::log_info_throttle(0.1, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); + robot::log_info_at(__FILE__, __LINE__, "%f %f", fabs(cos(theta)), fabs(sin(theta))); + robot::log_info_at(__FILE__, __LINE__, "Goal checker 2 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); return true; } } @@ -92,7 +97,7 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam if (xy_tolerance <= xy_goal_tolerance_) { - robot::log_info_at(__FILE__, __LINE__, "Goal checker 0 %f %f", xy_tolerance, xy_goal_tolerance_); + robot::log_info_at(__FILE__, __LINE__, "Goal checker ok %f %f", xy_tolerance, xy_goal_tolerance_); return true; } return false; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 7e520e5..7ce4f58 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -416,24 +416,24 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms { it->second = map; } - robot::log_info("map header: %s", map.header.frame_id.c_str()); - robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec); - robot::log_info("map header: %d", map.header.seq); - robot::log_info("map info resolution: %f", map.info.resolution); - robot::log_info("map info width: %d", map.info.width); - robot::log_info("map info height: %d", map.info.height); - robot::log_info("map info origin position x: %f", map.info.origin.position.x); - robot::log_info("map info origin position y: %f", map.info.origin.position.y); - robot::log_info("map info origin position z: %f", map.info.origin.position.z); - robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x); - robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y); - robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z); - robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w); - robot::log_info("map data size: %zu", map.data.size()); - for(size_t i = 0; i < map.data.size(); i++) { - robot::log_info("map data[%zu]: %d", i, map.data[i]); - } - robot::log_info("--------------------------------"); + // robot::log_info("map header: %s", map.header.frame_id.c_str()); + // robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec); + // robot::log_info("map header: %d", map.header.seq); + // robot::log_info("map info resolution: %f", map.info.resolution); + // robot::log_info("map info width: %d", map.info.width); + // robot::log_info("map info height: %d", map.info.height); + // robot::log_info("map info origin position x: %f", map.info.origin.position.x); + // robot::log_info("map info origin position y: %f", map.info.origin.position.y); + // robot::log_info("map info origin position z: %f", map.info.origin.position.z); + // robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x); + // robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y); + // robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z); + // robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w); + // robot::log_info("map data size: %zu", map.data.size()); + // for(size_t i = 0; i < map.data.size(); i++) { + // robot::log_info("map data[%zu]: %d", i, map.data[i]); + // } + // robot::log_info("--------------------------------"); updateGlobalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); updateLocalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); } @@ -459,28 +459,28 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot { it->second = laser_scan; } - robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); - robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); - robot::log_info("angle_min: %f", laser_scan.angle_min); - robot::log_info("angle_max: %f", laser_scan.angle_max); - robot::log_info("angle_increment: %f", laser_scan.angle_increment); - robot::log_info("time_increment: %f", laser_scan.time_increment); - robot::log_info("scan_time: %f", laser_scan.scan_time); - robot::log_info("range_min: %f", laser_scan.range_min); - robot::log_info("range_max: %f", laser_scan.range_max); - robot::log_info("ranges_size: %zu", laser_scan.ranges.size()); - robot::log_info("intensities_size: %zu", laser_scan.intensities.size()); - std::stringstream ranges_str; - for (size_t i = 0; i < laser_scan.ranges.size(); i++) { - ranges_str << laser_scan.ranges[i] << " "; - } - robot::log_info("ranges: %s", ranges_str.str().c_str()); - std::stringstream intensities_str; - for (size_t i = 0; i < laser_scan.intensities.size(); i++) { - intensities_str << laser_scan.intensities[i] << " "; - } - robot::log_info("intensities: %s", intensities_str.str().c_str()); - robot::log_info("--------------------------------"); + // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec); + // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str()); + // robot::log_info("angle_min: %f", laser_scan.angle_min); + // robot::log_info("angle_max: %f", laser_scan.angle_max); + // robot::log_info("angle_increment: %f", laser_scan.angle_increment); + // robot::log_info("time_increment: %f", laser_scan.time_increment); + // robot::log_info("scan_time: %f", laser_scan.scan_time); + // robot::log_info("range_min: %f", laser_scan.range_min); + // robot::log_info("range_max: %f", laser_scan.range_max); + // robot::log_info("ranges_size: %zu", laser_scan.ranges.size()); + // robot::log_info("intensities_size: %zu", laser_scan.intensities.size()); + // std::stringstream ranges_str; + // for (size_t i = 0; i < laser_scan.ranges.size(); i++) { + // ranges_str << laser_scan.ranges[i] << " "; + // } + // robot::log_info("ranges: %s", ranges_str.str().c_str()); + // std::stringstream intensities_str; + // for (size_t i = 0; i < laser_scan.intensities.size(); i++) { + // intensities_str << laser_scan.intensities[i] << " "; + // } + // robot::log_info("intensities: %s", intensities_str.str().c_str()); + // robot::log_info("--------------------------------"); updateLocalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); updateGlobalCostmap(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name); @@ -699,32 +699,33 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry) { - robot::log_info("addOdometry: %s", odometry_name.c_str()); - robot::log_info("odometry header: %s", odometry.header.frame_id.c_str()); - robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str()); - robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x); - robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y); - robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z); - robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x); - robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y); - robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z); - robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w); - std::stringstream pose_covariance_str; - for(int i = 0; i < 36; i++) { - pose_covariance_str << odometry.pose.covariance[i] << " "; - } - robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str()); - robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x); - robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y); - robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z); - robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x); - robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y); - robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z); - std::stringstream twist_covariance_str; - for(int i = 0; i < 36; i++) { - twist_covariance_str << odometry.twist.covariance[i] << " "; - } - robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str()); + // robot::log_info("addOdometry: %s", odometry_name.c_str()); + // robot::log_info("odometry header: %s", odometry.header.frame_id.c_str()); + // robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str()); + // robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x); + // robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y); + // robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z); + // robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x); + // robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y); + // robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z); + // robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w); + // std::stringstream pose_covariance_str; + // for(int i = 0; i < 36; i++) { + // pose_covariance_str << odometry.pose.covariance[i] << " "; + // } + // robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str()); + // robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x); + // robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y); + // robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z); + // robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x); + // robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y); + // robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z); + // std::stringstream twist_covariance_str; + // for(int i = 0; i < 36; i++) { + // twist_covariance_str << odometry.twist.covariance[i] << " "; + // } + // robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str()); + // robot::log_info("--------------------------------"); odometry_ = odometry; }