From b7e4c73c14f4b40cad35c38a3d3d7d147ad61890 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 25 Feb 2026 09:14:09 +0700 Subject: [PATCH] fixbug --- examples/NavigationExample/Program.cs | 7 ++- examples/NavigationExample/libnav_c_api.so | Bin 147840 -> 147840 bytes src/APIs/c_api/src/nav_c_api.cpp | 66 +++++++++++++-------- 3 files changed, 46 insertions(+), 27 deletions(-) diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 520c281..48015e8 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -579,8 +579,11 @@ namespace NavigationExample 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 + 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 a758b260390e9af8f5ee570dd7915108796ae71b..c9c303771948257ffd5734cae2f64049754722e3 100755 GIT binary patch delta 14854 zcmaJ|34Bf0*1zizl9VI_ndK(Ah$$06NJt{X4WS|CF@`E06(nj-RJ6S!Tn?706>1(@ zrLWpPC8cSquR}U(QPHZNi)U&Lt?K#y`44XNNzT`E6b-tHR}=hIyWnP)V=-@#;R z3Xp1}o6&4q-O&Kgm8E7GbW_f1mPr-z>SobYEx(6yx$MzgloOg~N_pY(bIqe9Q&;&g zhSs5o<5-7asP zn1+NHW_QvV-3)ur2N`yClBfD-z>3%X$4WlE<)#5ZkB~ilY%0{sGN!t_=!wV!{o?zeI|HZWSsA103vjFMtn z%WDFoJbdo!GOx*}1KUVBL*(0myQoIq7!+MpeMfI{&d#M69mc1X4j-x$Q}1Y+-~bTM_mKozKTIg6O4alwrq)$Da zkzk+r}_0XI1jb_PEQyeFIhD8M& zG6JZr(L62OsbN5vUqHE8@0_F)(?Ue9m;t-IB+O4l>ZY)75DYWJ$7balF7_~hhWhKG zk4NtB|08l&cG2Z4$PbV&X#0t;f0+}#oto7`SE;+y)qh6-a^ay)hZf(AN& zoDqeHQA>1~{V@-w7%`Xgn=bv1?6CBbV%o{=T4jilw2GF@;qvxYQGq^>^*n*;6Ok43 zmwW}Xb~(1SU!c8>u6()Spu@(fljG#^t+9Gr%4x$X8ydR-of|5kay_Smekxh9hmbXQfC3<$(lI6ei#TDs7ZXHB?J z;HKoR+MtbZe<<1>QnFY+&LFjZDRl)WWoLPhAJKNhdZ8-!D`-R*$e`sj$%Tcd% zl*SI1N53+bPRj4TGEbt3aO-qn!~%02LZ zt2|{_G+mcB?h2Qlm@B`#D@=-8bpcP z1xb5O-nYjt<(J5bdw-&xN6h=~X2=1z+O(WyEscgdtjqpWkeOXJ(c)t*d&5$W$H zu-YmeZik%ZDu>S@Hn*TK`QokEqD$^4y_X#m3QD6>Y+v~G_#|a?%9xa~r|i~c z6@`7F+)<>es&}ZV$ck}B#(t4)QT9b}1d zq@cT!Cp6nGTs`M>@|stjPOfx3>nc|{R-^14S`2ecLNsO2VC#}=V#03AYkm!AIiq&c zQ~x&UFUarx+F?+jp}x+f`#n18-l54RG40U=vn23olcXg z93%cejE8sHw)!6!4eMZB-7ojP+a>u3jMmkAm+VIQ0|X=(4JUcusKC0(E}Mq+gN2e` zm1fBiBebG`j3<`Tbu1eB;N2cYz3b%2Ew2bSKOV>9ACF@UP;;0z*`FbVt+ro)s4UV< zoYBbQ-J~C)Y{npqk9U*7J}8Z{n7x~fHlsAr664+EsTh=|S(3e*Ob<$SEEIu!5~`iE zb`*x5)?Y#|8A??zExGAyS-f&nd@r`QH7-e!F+5>eY9( z;Ek;~rMFc%NM7;#D0gM92{W!tmv8^-0g!Jz%^3n%G z`yPH-c=(tlANfp#jhj+#a72dFGt5eZQ!QpplUb;79?dzEM^7e z#|%t|bL7dcpgUzPJ7Bp9iyU2Cyeda8SGm%WYLwlqwo{HkdHuub6mX>eU!Ekryjk{9 zdrQB+Bo9`{OLsQOZ>zm2M}DYUsQtQd8dlU)*aEMJ{hvN!lfe<&DV=kDqb^e;yrN~| za|TCbU0voQgH!I}yrpp-_1q5dQjP%_Yo)|8j7;l(rx{Yg7M@BJCN0^(Wkju|wp%oB zQg{B6D5j__Iw{T86neOGgCD3$Qb>yy zM#pl|QH$*Q;Ba1FY9U=&!#f(%IU2=F+$o%<@D6usLrwT&cM2v8JKU*t+s3Oj*Dsy~ z$R&2nMWZ$NcWDy~|nneDb>WO)oyNbI!K_3TyzxuyVz+L5L8MkaiGbx1a zji{%z*v{3BD31DYy~Z@mH?~a7!!b*aE9gV{na0#POCR{DNXe~WpiPR+i zQ{av*kn*u;MK>*j7}N2V!K=+7yH>E$nA(NTcVV5^vG!<_VPK8oST71dMD_KeccrgZ zvZpuJ#Kq;@(Hm>Rwvs<_r-1r@DO;BFEN|)_ri_KtuiD`BIi;|oEa_Xp_+BsuzleW1 z{E0UO4s5m7>HSl}vsxaBsO33A>y)Eit9;7Q(^al=AdnM$Xetdqy2FRMkTmOg zKG%eXwjB(gjNO}&xqCID*BP)iwPleVa-i37*2SxGyzVMjI+ogbd^1{t zMRTbc4T>8CXzdDrL)oZVam-TTT5(;4UhH47Ge{HnMq>XO!Bd;l2+99N{xpDsdbTgo zP4^e3n;n7T`mA-U-DqOrU@barO>~tj9o`(;f=Z;lOL%7s%1v9l)Md+QP4h8}9&qB? z_beL4blF|jJ*9CTwQc|>`XO6cpVzG$`P?Jcc?#>E7TxMv$5={E>o&Q{m5%9r!w=Sd zSHy+>l-v0ICAHQ)$6xzXt6^)vdL$WcgmKD3!ChL}&6;q&@Jq1*#nxHeB55mdjMir% zZU?la_M}m*>*5|fFo5Qh4}TIs!96329~r}T5ggH?SKTp`VMRH|V0V=(9TT`sAT5;k zF5;bmlpDIX$Tfzxiv{d4OS#LR&la&?5Veo(10g+wXsdpjWvaU6j$0x!2eS>Y(F_ z>VOt3_&aE^P*m6K(O8*I&|dseOMr&X(?Nq4=%8VX>wtEi|98-2p{OosG8Uf`G@L!n z0KM?64tjQ;4tjB69njzA{vFgL6x9Wdz$$Wrx^bT{N|3IX@Uk#mPEM3?rJ(i_b_=JT zG>+54saIqimNWJd`j%aEO&Me@Yd$;2WP1BB0%fpviyeO|hxdn*Sz1!SpM+CnV?VT8 z%i=3Ql)fxQP&dlr9uX8qkvuwrmNjnoq$aQ*L>a?3A}FERrT!Z0AXu>#Cvc52DNQ)h zLP6cXp5wIol-2gNBM)m>A4HYaR_&OCYMq%C7PkM@Bp ztV=6E(oM?0XRGQ7AED%8Ax3ERe&&?;_ldPMWY1py!9qI&FJny^dK^Ey%BLK^E#a41 z(cHv_!sBI&a2D_bwn_JuqX5Ds)w=X*u`&|yIJU@6xG7frG@OHAxlgi|7W!kWtSp+% z16q@BYGeVVN*7_NryI$%h0Za9g9~<5L4ZcPXe3gN6Q~z&X-l@2_k{E60Nr~= zvkX9aO5YjWzAXhv)iZfaJBkn3nD6Xf<(P{6;e_hX2ij3LX~PV@+>UztEESN*d^p9q zfveFVPH0avJx&S>OWjJh^O5!x*}yjw0XB*+wx@ng#|WKe3oT~#OFi(Wb8-i4Pq898 z(34Wlbbhr1b&w8E=W}33teq~9j%gc7YgvhPU+yjW>1#%us%_ zBTcsiY%`f~qi)7C5OGUyA?+kf(~AW;I|KG)aAp*>mww#J1yK|$y|$HiM^W#heaUD$ zI2Bp5+ca$uDdl3f0-NJ!r0byKw7C{k$;aKEc?$*-y{-``E_3;#*6-SX>2Tn$|uxn!}b$cklkWq?@{BexYumG2yBD1)RdprJ^ z1Ucv7km-~hR^>;V*%3z}K5wE49y=ZhZ;p1Hd74{v!Va)x5_j)JeWVX3^7EZ&2$}eN zCkpD+48m7;U}SO+jBGuwYDyS}=d>Vxla_-Y>UpgpLhs+3_@#IXmj1Jm-|IsD(d~6@ zqaBBZMOweXxmMc)BuI?Z?PKgv^v;xu~^lMuiK`mWd6kTfavkFK^`4ohbeRqnj#}*$>rOF@Ep=#M}dOhB9`Mm@RNLfA3L0L9pNWFC+DxPz&eL7hz1EQ9ll6eVhbH?YQ<3hBN>?bJ zU8@`LP}U5{?qR^4(y0bD+OZ5Z?R4>xFvOZ!C0wts8CewTt{DS)fsUN?_E=+R$pCY$ zaRKmO*0=zx9rI;bAG3$7k2zC7x$?$o)a-TlkXX#;S3a`*zFFhM6Hdo@ke0*NtkK5o zSv|(EY$$)wgSxeTRjXsyRSDtmL|3ux(sp)}m2Ey2pscY+H=&5ZAg?Wdx8K^Z+Ln?K1tQ6)A zm2y@I%gp>iFQnZcSMt9SsTF%9(|c0W(Yz&z{INgyIT=hf&{O(ZUu?0TZ4w=O% zhsQ|APeXZsDz$5V^sy$-k&Ych`ARDFA$N}NO&g>WL-@1a)XVpJt^hF(3$&w>X&jbD zJ(|}H#&)GlZ@|HVcv5wKIL}NYv)=fu_oU$vZ#@4yjdC)_ z2@7oRS<43Dz(I7sr~EY#h^236=Mq^7KNR3`+dU;qVBw(!9(dvL#0}+lI7U1iL#G?c zieWrHolZ-i58#d&G`-nv!`Nc1!))vZ+?1MJel>&qTYR9kJbJ2gIG4|5P;kgvv^kFb z>AKy>#}u}By+=mw8OOS8^RVV^^&G&%wW&k*oD;-4XIperF`k)Abx1Y;o9{aHfK>p z%Z<>NWvg#7&Taj(@aPW*w>WCLs~pVXaaj}|GW>ta{H09i@+_L!DrkUkTGGN+&{XuK zR?ZjY*?M`RC_l|z`cSW6Ka8a~(PF|433kDG`s``@(}z#;>OK@2a!zzYn_0AJcQbCv zhk}PeoA{GH6x{R`LnO~QZNdNx$k^jvMjTo7qX?;>m_O=AlYOUV z>9&g5*CWiz>HR4p(1N~>J=iB3_pFNCM@YIIE9U3=Q(p?@Py5rU=m#2CI2MPp?%pMR z9%7we2Gfy@l3kplV7FNyCixfTul`)|Bn5Puu-IfuslsL*zmyNs>zlG{>F!DhOtD>$ zRh6|+Ezb7gfMwaR09s?YQ4U$C5y$4x%$|Q_JZ|(EZ5Ra>cX0=S=gjb#sxC#5=CpX! zh9CNIO%An)TCeqhGoAkOF1h&twl4cC#P#ak?ZH& zIYJzZ!fi`F?;B35rPBqRG(x2C*_=0mx_SQA3raEf3zK>K2$~T3ZK|jXpXpB!a1*Az z&Y?X%xYCm?BPlGDAtZp!P9af=F`hB(>&X*GQtODR&$vvAo~@ZQ@llgDKEnq`(n{&{ zEKV9lt^Hzsk@z|%Vbz}K2_wpq%z_cY^G8vqCSwH)c2*?|dO`8#=&_jvuqWjJfmH#=0=6Nhm1WKu!`V4m+i(BU>dF5E@ z=<&HQwA5|ZGyMKo>Mwniz|Ha~D)=1;JEs4os&9ECupBukVT6^aK*^XqipU$Ob=03M zWNW88zb6PN@nk`(UJ+H>dBKUvPWT0R>5L)ZBeEVgQR2bK`vxyx&;S3_I^9tIlgJnI z$n1F*{uN_AmB>$wqsdPsXzgOl*oGI6Lp0t=(40 zKSjCHPm{$?xh2O=q88-F)=4x_O6{uckp~Xz1hu6W3v!oPBoZ;IkKF<^fL(ZnE zAA|bf>2YzGsmNq!RSga|nJP9T&_Rn|QdRK^ZO&Fzy$M>pO;uM|Os4WzRP{b+%?=of z?VWiS>Y#bMRW+|Q+V`mHhoCi}rZ#BbtE$~V^FZ@K%R%>m)`0rsM5}tgs!on9f@KF( zwLac+&3RQ-D?n>r10Z&=HxB^_uN<2WtLj9&9$NDzrl2F}Td=$-+CkTXn%)L5s2TJa zXbfmIXfo&}&>YbFpmtD^o@zj?pf^EB6`>IG4lD%C2VDVL4!Q?4?_U4_tpKfP4!L76 zv>EE4$*3p4tE!D!pbi=Xe$H`K%>k_ey@vL@6RNsU)Iqm^=2gIO@T);T6!Pz>YEd2- z6)5}$M$Ac7ZHD?G&~i}oDOK$N1vwvJ4nd1gtLg#JYS6DheLg}z(GJ=SiP8QgTnbwA zovONHj$^)uhd`@Atzj_Wtg6g7?XTM5a-Z{X4O|}c14b(71xx{4UwjF{ z2x?bU)dy3Ma}`q}rr;W41vKwEi`cn$0DCW0sMJnHb~8hB0~!qT+D!0U-u>@rv2ayfYyPhEj0 z<#t}Zf_$5=00!>XwVyx!*Kcexy~S^@prDX*Xo43^roLW2OQj)R?wO}d-B3-0$W6Yr z0+*$N-5ga$EhWny?q5djq!;({eAMYaZ!E*Z`Ey)Zh6{h%bJ9w% zn;qmas7iBp^YWGGr0`y_=?tG1t?mc;is0U7k5z!W#__Ab?Xa7NtOEBM7p|gSQ1DMd zKIS{C@V?4#9AX!^N4TdQMs+^IW9&$B=3_?>qh2rKq!`}=L`H^}&k8BU%m4X$nO@~2 z**v}c)4Y6Az1(~AuQE=^cW}~LiWa;2Ey$Ir?6aBtc=K9HYV+0@ReeXmJo$H+A{R_L zt7|E&P0UzBxYPeHoVkv|y!JpiULgBHcreddM}55PPTmEC{b2rd9kuh?;N(?lyawy3 zUDI;#3Ps=7z_WVANPRhVJ!Lfg(8+(+#ox4^T0h~Fr>f#>llo(ubI~el(IV69CQ0vl zdPSyrnc?^~dpXra986aolAC zEtaN@<2N_ZCaUHE8!?*|JaZ%cgVyr-jkpy5$;~&>0=Hu1m@*!)jGFP@O_;G4XYfax zXp=OpkSA@XO;4ESsp?^2Ur|#&v4O(5;}*OmW|_|~Z-LaX1>F55Xt>5-zXa%VPI;MF zvKI08FVmza%*CoIJ^-jc!-u!-!ploDwqhG~pZ9JhTwb_IIm|f2S%TE?5F+<|An3TN%0vGgJD-9gX7(6F6C!7(1OlLo>i2Y1qD$$Fgo?1CRx z*m(UeC^Da5X*cZ>7VoAYDY$|^*iCU#WW~`xcGFqOJa&LdrEpDncGsnwJb_xP7V z7m#~Id(C70{s3Qm>XDzzKLS^ZiQ}f9({*U2FUU_acjLe>C<9*!@%S&$?9-jsd_lYL zy%cx<60=&(Q@*4Vf%$kJQT+NF8F?6%qCkaM>Q^);?(xr0UcyyHYd1a*OEpIEZzn&F zuY843);z7_b~4&~A=8Rcm?v}G*MzS#c;45DrkKBY-`6xI?(t7|T4E!li9a^zWblQ0 zQ&;Z(4Wh7|H-1B_K-zvw(E*QtX%m8Nk3g~W%x`H`oT<5PlUQfs*9E&FP03@QC=CW* zC`pdvxHFUyV0_MGTA+6`%{J@(DtPr7`W8c(@*Ry8{{QGZINa2Qn|?3mqZ6kJn$w$S z3F^~|w+m{{NcHvhO}I)5|5Wgj3tAj+ynr}1_28y8$bRMoUS5NhpO?s2YA}xC zI1aq1P075AFr}LJf&}Id)iH1BkfsP+PBovm>zi8*Sml2t!cph;XGUaT$Ok=|yUzKAK_+lK@#xxbO$1k)M zAFA=bUog2jo_re=U$L}~GlpoL16&mB=`qYW1TtP}TcjA3u z2UHs$Y-Q?0HT~IFS6RbhS81$IaTnbObBtz=4VNO09=S@>2VlDqmLx@crWs$bA%BfX Il3LgQKk+K~82|tP delta 14694 zcma)jdt6k-`~N;iP(X!Mkei?&D}onb@dhXg%4Jd16;bhml6lV(3(Z@CqS=b*>QNsx zOT6K=@=}?li5ijBr^vGGVq#hCrsbtHvuwZbIp^$x_W6BYzx`vMnP+C+^PXp(dCr_U z=aiIsmXvy0H+r4=LZ|#reQ{`e56LfV;WwL)4ZCx?_p>3Zn$Hja;2F!Zp+~Ms27Pc{ z4k;DatMzrY0Xkhfowv?vlso#yNLBsh(Y`Ukb+^^xNm{LtwrZ`qZt`~DnD~-J6=a@P z{7z?`t|dUscT`2BTC7{sMFY=~CBFmP z;25c*y}UL!#13m?(vDmZdx$gyB+tcrO{>+5$IK)67m zpBx;TA^d1U=;y(;n)#Y&NX-o@(v9wNPMdL3U5cbHgZm zg~tSibybU_wc;7=oIMN;4+zqSsin!P&@F`LiW$(!i^Btir*01aoXqm9cH^_^G!uJj zKqCXyst^69m8{|N_V$Q9S`DY| zPa+{atKz==GwSMOV`M3VW{LA66@06prN6{8J2mcZBg6+nvKXPkPnz|poUn7Vv}l_A+0I(YHbbu5l_0H|Dc9}lOx@+? zm0c)Q?uCCJd0J&GjgvQ3wv!e-EuW|imp0Co>ng)XyoNYzd-o=Kxe4OnoHrke*My=@ zRV){ZokH=ks(4)}WTALlRs0|nON8PjRpBR(+-;(!a>edq>Bog~_@0h5QO@4eMyf8B zXY2`;-YSvHAYNT8@7rUQ>PqF_d%vevN5b~q$!G*K$o@Aw_z%b~pJ>pVEcZ?2uNo>a z@vxk&i9F_}(X@WA!(l2*Gt8Z6>gv`$3T3msO^e@o4v92NU4H7=Gq#rd9FE_7+T1f) zs_iWfI@2}w5Tqx^__SS$x;pvN&Hiz}c%1fG_8M~8O9hNvt{wW;*3kI(XLKtPIomMupr)#r6T!*jWY6qDsE zQ7_?^k3QR=&$j$ylK0;VEv_~tUV=kR+#|4sf0U;g3QdVM!bud}Qo|d7sUP1|jPE^Y z(k!+y7+`9C>bTVLXS8wXOv|cf4@6h?xekZJr!BY^8KR8mvMmnV(ETdYIFqGD*epuW z8HIuToNf8hHgUHgAhczE zP0bZkx8b*T{^|d1>bA(A|JFG>R_pM8Ox*!gadzFTL3aU4cPalreO;3`{N6dUSWMFY zc9XwR#o5=gmz?PuyA^4lwrXE;$nEw`XDJG$Z+R^8*$whrZntmTLCMP>-wvg&^6lHv zAJ2X9y4d%>-HaE<6Tn;@6rqgIP?02?>M}BrG%J#6Nl!_iYe)qIL>XJnU4XT~1Y} zI+el~vv~DW;Y6070B>uqm}q7*gHMa5zxm9}HRyd>4AUbu))3~?VoVrP6Af`bEgp?S zYK9@jr^RC_SC`sG1fWY{+APaE_JMFh=c<#v@3x~Ma^l@C6U5{s)cbe>OqPNLEV!JR z?9(<)rxRShRJoYI<(UeBsd5pG5gz*MufAd1wWHp;Z)3fpv9iE*a@E~5x*)s%*J z7ewD5*?iBJq_!3ED}Nr@cqk{o_Sa%oe%A(ODF5J?AT4`-eHQhny7lE` zptsf^ro7_qn^mhMxU6E;G-AJ^+F+5UsS{RFBIFEBBch>NXSAlNaA{16=0VGKgD&+r zfU$O#8uCcT7NVJw+Y5Y;C|qi>i9;odl#Xpt%t`O_qY}jxAKL_@?c9LBt@wlG^iRNY zz8h7`?G~0B?Xq08ebhFm6>ID>UFkFS5e?Fl>(j!zV!rKUi-Hl*uurSVxh4c4)xt>fuUC?qOUDLt-S@>Dzf zX_`7=!(Iywf$(G9I(`{g0bj`vVSR#1ws+XxghKr*mBQnO3eiw;?RC)bje+DZ&6l~G z2Yo}=xP>RRqd&QuCv~80Jl2y!=vAKQNs-YntX53#Jq3_0)?hBSti|7QReN_bw0~Kl zn6sMS^Q7?nmoU@nLOo+&?Mm0!UvH2;>q=MK8`RG(->FW>$FA}-_F1lUwS5<7c+rzm zn#>CuQ6Qi5!n}A``Q4`UVaRl_`fn3($5yh6Uv5UT=p_zoPJJZrHC)h~;^`}vn^T_e zsd8bv;|8tEr2np3xo`J$k-I?j}Mb@pXb*#U#NAvG=grO3l%xo#V)HB&-}HXSBS zS06-rl=EC~>e$xHg>_NI+M`T{hINfkc~cNP>T7R0Ax&J(n|-h*W|i>?AFPR>)tv81 zL2m!pI+wAer=;+?1u*)86)<|^mCD>q5sK+T@xycYtK)n<1?Rj5kI`mASXK!nN9uL< zR~n>Gy3#fF8?N*j`$w*HjUA4BR!`H(ezaQ)>OqpUoM*J8kY?NiDg3=bPPz?U|)&n^R!WvM}F=Gq@EH539y*&+R78?~Iz`%)+R zo5_#bNuJBOy&px=A$;KM`1|? z@M)*R7S^XlbTFe|`(rL$jXl+suD1KG;ah&R7>lN;73IcXStg>=ufjK?MUOB@l`_fV zf>VP+arrlkjV10r#K!Un-)lvqr9IE_xL|7C=dGopN}3^1Z2iLQ!H8>Wv-+)Hjr}ZE zos;W_u5`71E5G7TOQpmm+&zGXr5S*waT%ppA%@s<_`l9 ztu8#Pvb(zE0d`)3-5;V^Lv|fj)wAp9N>|(Ob8#TpO)FvdAR5*@7g$bqr93H!+UK=X z;2Q+PO(17$CbXs138uu0vp*FpP~1_tR^YFTRn~=SP`jjM8=Ck7{w|2-Qw8S-Q%Ij9 zB@c{YyKs(^qE*8&gknWGEga@bSKEK%qrtRLN?gRSX+?7$r&sQC9s7!(12nlcMM$huB}t{|TTV4hf+Gsbq;__`b4&?sAL6g7j9=xFP9I zSo%)VF|KsAomTLL5Sk>#F66$U)KBWPh)YAsBpDYAaQ~j)iA`MEGlGh%he={;&7#+a zJ$PB1JyhGTac~=YRJuN21--gZ1-)6^0Mu>azd`k)qlTcrV4bM6tL-22=(YfTWu6MU ze}M{mXi)>ucjo^aw06NmptW##C+K0`90t&tb5+pC=BuEyo@oHObl$%~t)iobOpjpw zIziX-SK*W>En3E{+Tn6CWEl?^GHe;IhoqbQSv%?*bsDS8we1y6Q*%w_tr`v0>5d(S z7Y{dCZkx)x4;#w8A}CC1F`M%vD608xGaS6UyBXDNU-Q}sN}whnY&R1=_F_5ZOx1LawPfodjlk}cfwLn*Me!KstJPz7wnp% zAlo{jL7SCAF8jL3&t$oHb?;=W(9JF49vvtl_*;}|7P03b5~%4j_8yD*xeip`y8{f^ zkZH|q1=Cb|rogt~Ns)et=^Q81&>|ihMZQB#VmxIhtV-Y#(YL3fXh^-is&P>dwD~GV zxNd7IB&hoe>Khg2clos_$_g5c5-0J?hz3sL)f^B_&xURlJR4ZpM+F4$6x(n7W;6xU z9zGvUeS#7(3MbH2EP5x<$K1IiS=uZY#%lnyd?xZ@s$hVxXYyOo6eJZq$-j1_?m^MB zoXuPgr6Z6lVzi0Q(*$-(_Fi1(*Bd#kpW%w>H%&6YY$ z;CP(YK@)FQdf%V z`Q21R3#&4gGWT*(4H2cJI1tAgO^UGRDGn2taUc5%aBv-ZUaD=Nwb1`T;ckV%om)$ke_dKk`{P{ zD&E?S3f{=ti7M&52k?TCtEVe`Vz`rSnfrNx2a4{>W>4)S>w!ML->6QMyV^-$f31@b zRH>0JpU8`QQbfBH(bfqjtbCN6gq4XiZ{+aq)LMFq`9yaL9D3+cm2#p$Q)kd==Q}8J zo(@x5pjc#RM-Be4DmAi2$kjWDHgjd}-38P`@Ow8n z=?nP&8vG;`K48ZLg?+wV^iywR#&fh*;i6?jOLTBSDVC_(pjzVl^@=4@!v&VhhXu0J z3Ci4)(ez&Cfyj~^XrV;3;wy?b~ zq&5{(RhM55mGyzSIPY{z7lFsts|3o|DFm`VC{PI4hN-Y`3#fd1E98^|#s~Zms|MF> z=c)npgR5qpiriF1PR>%1^X;`rs5n^UtI_JeEOOatBxTHV)~RFOB&$aMV!UgtSlOe1 zXLTN)D#5ed{c+$brdQX-WEY5gL9CsujM=L|z=f30m-MBck*$<+jb4f}1NHP2q%!w;s|CH0Qr&97ER08_Anrc7ng=FRd)|~nr=;!q z+_?`0Vuvt06)Xp@YQXYXDp)ced)hYVfo1#A3d?{CqZ+W>uVgOSO2=}qzLeDLC*)O{ z<)ipeKio(yUdgxnk+IdYz%Z39&|^asCeArL$Jn3B<6ix#W2?-EioC|yyXEox{xpC# za&>>&C=D6Oh5@ncRpn$NEAiVR z525^WIO^dM1|C8w#}^hcd_1v2qwBViQG6?%&PtEu@QDn1+;53SwgihO8~XuwTVXEu z$)rI45lYE}=Q{mzc}6CMgc?w0KmNDth9etK^Dw($p6uS4cV)uSN`~=AnUvjSiC`mG zuQP}~6yl&UR1iO)5x*#iU$;#e#+fEclO7qyfSpYtl%5)kJ-9EVI%?pib9JkJ*LTT1;F3-a0+M=O+FpDDEM5DVbi-=N8&v652R{Q21*Ajs)A_ z0(J6~4QjcW!v|8^&>5l;%ECmMay#R0%M&_`*~<9?DWqjrtxAD*;1*%#vVjyAeRBw? zmi0EMo9pk96NYMyX@Ahe&hnLk@RUk!l1(wv(?hstHl3$hZaxUNwvjn(8iZXz=3t&O zh`RU&Dt$M?Q%#?xD={aw<=MP@5ScQP#i&6v1;+@7(3d!W6_s9sQ`*9^wL&opv)F>K zu&o?~l~h`PT|%^p#+lV%ijdqE^XS1e)%RYO%2iCh>R=c6JSqgg3OxHBY?QT|R$IhC zQPVwpF}EB-1L+kWH-yf_mML0cSe(jw_$(c8AIp3Wl=c*)tm1SH+sz^|$v@dv4PmpH zg1Y{;2+xRXuv^D3_1$zgU6v)?!}b!WSbjA+YzENQ#ie%NYWht2RS@3m*UO%V+70zrfuadi#$^ zxsDUi{a$``C`DnyE)Jzk$s>y+bLc5L!kcrbeVboQqVEPsSfyVI9bBPBHebx4;Suwd zIt$F|;n@r|=Za%KW9*|$JbW0X`?XYxTt{<7={B8-Umiw*?SE7Ty&z77xS-}-5Us^y zL;GpII*gK}<(b?e7mIvcCYy69Ffi?*%59a(F_~PFOL6o&AIhZ_(xG(aZa-#hh8Rx6 zTjl|(2J1t;+YhxR2n~i8m%)pMnz9QZ+8n zK$|Qy04)>p!Ta#J>D*%kPJKKwc+LpwTg*q~aL7`BEcq6S1g(ZVc5AwN9&t7q zI>Do?KS$(Iv{cZa2j|fkkFMZ_lc1e^D34}HL*{VXQPft7nZqfgC``X|4&pgB8zRO| zwLQ_F3rFFkGi?r^8%606_i;C)MAc- zC6S5KDkb&h-^Wm7#63K2)F_>NN})9IK}yjxIe9Ftl14qn{~1e>0jGQs_})pzsvXi7 zB+8Qy3dIrj%BQX^uJ=)Sj!YHul5Hyw&ZpKbK80G?askvrp5giVl!$Y`SM#Ztl-!$d z=96#Ip8?`pSdC~d-iy)O-a9YwyI+A?T*HqcD;lTv67sU~obgVUrAm}s;y&Xj6UQ9S zj>7_+nZ&P*qxE!w)5p_1Pj65~G+8~9YsTZ&`4YD&pe~+|2<~O>(oD`Rpuy7UB;Hv- zF(Cs`+5R}f-y1$~EW23*!t&(ddW`TA|5`v11y{ug;dtWGo~<13SdxTBJor$u^F`Kj zQE1|_6MsTnnyOXMi>QY^lz1|dtLX~W_@Aycx^8l&p$#_o1RS+ zpkhONl)Wdw8;shLV-~ed?P1g_A;8Ijd2uTxYecOewl;Rkg14N08S$SrA$F{mB zZ+(=yC2e~Q?=tx~%pP_uDd&a9an9ZoIgI^uGmO2uhe&sC(^AaDPqxYg%zkPDH=jtI zOor~l6$C$wSnIz4jj?C+P#5JA(HhPh43eVx5=dpEZ>hHVYXg5GiAyF@8J*&0lW3LC zT7Zhhuk3-`d+^>#^myYqF%5?}WHLp2vcN2J_n5|mC(|(LscB-P+=fq0CVyJb7ba7V zbgrkeMb5ExMYm-J1LA9#2qa=uV{u$%8;=6p&)rlqcxU9Qr(h)#I0wFNt3rvOJ5>w@ zc^x>3d|pr9FonWeJqx{9QF_|}0Hq@3X4}pmOhHIa=*f+zVj440fY%mmeYzfvn@Y7! z)|OdcFQjjp6@2O;AiZIBg^ zDLB6q&l%=IrYv$e{)W5(S=2_St15Olu0fhh9F7@y3A$jZ!x0sRdKQP{L&y>`yjyO8FcAc`HHbr z0d{}-Jr2ivkm40GT?dpyCP3Cg7DDRxIvjf-&5(gnAhO@#m>Pxt3l2COZqexQC5NL5 z()2q3)JTX@Lfn247!&9B4ic%{<(2AISD3Lw`(7DDcUv_c+-tbnYAJP3IO zvI_Dpr0y6jiU?(fG(i?Xjx9!_0*OM%I>?of`ZoaxSqpguGVU!9YlV7{Zhpughv|_& z2-(aZ`4bLD9Q0L?W=QkfFg?m^As34LNrz(#WG!R~^eI)al&B9`TmVJfDfj>s6_9?& zhn;pfDj=% zH!yI&uKWV=@75f=_}EHn9hwanh81+Wf!_L3X@s{&<{4cAvb|BIfN!qEmFY?)$COhW zY29ufTuvROyL)&(@-&q0Lq6VlVH&G;e)B zZ;w>|O~xttI!<0kvEQGyyW?-ylyn0CB9N@DBR%TIH_) zyK?4w3iqCi%J3YWTL3B#;yLSSfVbJHy9B2n#2>Avj@~Axu13){-as8&ra(7Iw0#9S zlXsldpZjf~jFx#${nIY}<_#3t)C#@$qQh-`D=uD5{>?MJZ;*7z%R4H~J1mu7UP7VV zlqu1@cY(vPo3of&dynw)4uio`6)R=(lZ?amq5^)KX^!+^0r%KQi==M~`1OsnnNoP@ zCd^|T&)P(fksDv!gzNAOZnc>fxZgn3Dd(Zf$dC7K#$4Tfg5Tdvo274xc=GeKxoP2C zhvTr|R@{S(8E|h{{o=(ocbcM^u{9o>P4E|)LQIt zh!^bLX6kusB^h|;R_vjs^4_g91*BS3fQE^)gfR1BA@cZ`5O8J)2@i!jZo^A0as0?O zJXES(!iTrfbgAetx7kiHl*@y*;{t5rFSk>ybhnhfcF;ILOxrI-9H&qJ8 zyKxx3@+7~zo8qO7Cy(CSOIq(bv6#D7P`( zc0U>FbI%;02sbx<<8Zd_r_e_KP8b`H;v1q!B=iR;)NLh=+|0%UWN4Jy@A;#72k4+) zdUeWCuM@PvLnO9WJt5ob)MXHHD{rN+*Ns zMyjZTwOE{oWhhWlt>0%fC;s8jJ6^!`L@Ccv+x64feBjg@`RC6tO0)KHN>{DCFJi11 zg>?$YpC^3%!1K<-n=0<}zVkFLzT}}#W7=Q~q*Q;%SSiqp?hAWx(&z9(J#YG)RzpO8 zL9sy(f5{Sx4Ua(4@vJXsb$p?p%2}*4@#}$okkZLRp8ySqUUYKM$nh5_BS^1(AhSSi zrh6((Z5PLDF3=Yk%Cs+Oys-cKU&7#p-MQsgVm{)zpO97k_(>s447^=PYYw0L3iYgi za`4w8-;M`-O&Ol94^4Q}*EG}9_2mw?_y*a!+iK^N*zyfM-QsnP%0gV71ywz_xQM{5 zd`shk!-lH^7w;E|-@I70an*1>{Vi1n#pSEzlABt- z8P`W)pEzE834UD5n=ZkR3zN8I9ipE#fmhUF<=6J&pX)G=8%7SktW3$g%b=3Ndm)1B zMyQxKRY+X~E~JXhL;2QapujrQzr#v$eI>@m?{U6;up_Vd9^fVY_~-8dUKh{7S7=;N zZ5tI%ggf!O*jt6u^O`FZ5dQFYbK%KqvFm$2K6{0h3)Gw+um%cv@eiOH$A?sL`3KxA z>NxsGcxGWY9{nTg=-KikjSqi#RgQ<_i*XdX=Ed_T+KLasc;8Q$+$vAL1qrZ38=ZtP z{wcGu5H${vN0ehQqJY qc>Rs;YVjPcn2qhEh@(fY(&Iz1(FjkLV!d9~79rx-=wvC(handle.ptr); + auto *nav_ptr = static_cast *>(handle.ptr); + if (!nav_ptr || !*nav_ptr) + return false; + auto *nav = nav_ptr->get(); robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); } @@ -324,8 +326,10 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, 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_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance); } @@ -360,8 +364,10 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS 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_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); return nav->moveStraightTo(cpp_goal, xy_goal_tolerance); } @@ -380,8 +386,10 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped 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_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal); return nav->rotateTo(cpp_goal, yaw_goal_tolerance); } @@ -397,9 +405,9 @@ extern "C" void navigation_pause(NavigationHandle handle) { try { - robot::move_base_core::BaseNavigation *nav = - static_cast(handle.ptr); - nav->pause(); + auto *nav_ptr = static_cast *>(handle.ptr); + if (nav_ptr && *nav_ptr) + nav_ptr->get()->pause(); } catch (...) { @@ -414,9 +422,9 @@ extern "C" void navigation_resume(NavigationHandle handle) { try { - robot::move_base_core::BaseNavigation *nav = - static_cast(handle.ptr); - nav->resume(); + auto *nav_ptr = static_cast *>(handle.ptr); + if (nav_ptr && *nav_ptr) + nav_ptr->get()->resume(); } catch (...) { @@ -431,9 +439,9 @@ extern "C" void navigation_cancel(NavigationHandle handle) { try { - robot::move_base_core::BaseNavigation *nav = - static_cast(handle.ptr); - nav->cancel(); + auto *nav_ptr = static_cast *>(handle.ptr); + if (nav_ptr && *nav_ptr) + nav_ptr->get()->cancel(); } catch (...) { @@ -452,8 +460,10 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle, 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_geometry_msgs::Vector3 linear; linear.x = linear_x; linear.y = linear_y; @@ -476,8 +486,10 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle, 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_geometry_msgs::Vector3 angular; angular.x = angular_x; angular.y = angular_y; @@ -499,8 +511,10 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS 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_geometry_msgs::PoseStamped cpp_pose; if (nav->getRobotPose(cpp_pose)) { @@ -524,8 +538,10 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &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_geometry_msgs::Pose2D cpp_pose; if (nav->getRobotPose(cpp_pose)) {