From ff8a90cbaa2dad12a5a71cc932f4b57ec75dfa91 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 27 Feb 2026 06:45:35 +0000 Subject: [PATCH] update --- examples/NavigationExample/libnav_c_api.so | Bin 144464 -> 144464 bytes src/APIs/c_api/include/nav_c_api.h | 10 +++-- src/APIs/c_api/src/nav_c_api.cpp | 12 ++--- .../src/diff/diff_go_straight.cpp | 1 + .../src/local_planner_adapter.cpp | 24 +++++----- .../Packages/move_base/src/move_base.cpp | 41 +++++++++++++++--- 6 files changed, 61 insertions(+), 27 deletions(-) diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index 604760de35ff12a85a5e64cac738be1de38f5507..883057967807b2b92a894e6983b9260b4d2de280 100755 GIT binary patch delta 6837 zcmZuWdq7lG^LH+wizo|%uq&@cR{zQuvubPLud zJmAc~Zxz6;>&J23IcKhvgt>dd0TSaL0u6`K-2DKeNJ)*Q% zih3SLt*xH%Xknse3}{IS{-ZTb2$UXj)lL9pC-yv!qW)e(VK|u%p;tVV9oo@rC_uuYy563PAcM5#g~3atcYX{kAdB+jU=KN#KM{T+E(KxTf1As3 zqQ?VS7U793hm}b&WKzKZkDhNKx%R#+j}E&gmOYkb=^j#Of`D=RqQJfT?s>>BLcW-Z zOq?c=^9B9EuYjb$_hbqA6>butP_%DL$5{<7;7)|1I=(!U0P!FZn+8ELS-fco3?{qr ze-ycd{}YKD4TJzPoO(rkoWpTKcDmk0%pQ%Hv0l8}g0}uZgVOd57G83+!n??LKQJC4 zd9*)YkWccd2RTRG`8@?@R7Uh$Vn+KHyTA5JA$qT2v+QsddcU9u37r|?&OI}sgb^Mz zBjJidIKc?l8KGJsJY0V&-A9xC>R|DjKf}*$ycr zxn#^x4k>rlzK>+UeTScCFx!GXdWIn_aY=dLnL5F+YRorUaZeTtBq0%AYXRN@C+ zNVmt6J7&$I*repD8pl#7$=U3avB+$+QF>igl!Yt0Uq+vWHBIW(eHk5{ zTgIknaUi~kGo&=L_-cI%7NFT^A4YwzU5QS}PSuABL2ZgMN3)G)aUz-*hciVj+c=Ib z%s>LSxWQ~Pe2Xq(7;*Ev>W1aL4qOjcE^*WAQ?DrAjB{ZG`ZFS9}JetjPL6l1(k zGH#89!{p~%2AD;BV6ZlS_iFSmD=G5*&_G+_HvrIX1^BCn3vgZi>?fV2G zvaE-&>PHni7;Sv?`WuFshBpjv8saO&3Y2Ei#3)OuogMhK9kQI16s-1Jjgytsveb_; zOrAnw8(sLX8_1iDeIfAB{oCsSwvo)18P4x)R_0?CBvmO9TP8t18QvNJpOAN3L$!NK zEbL0@6_uLSYIQm6^zkXei#qj6M<|H z6>GdI7;lTG$i%yD{Y1({>KiX9IxD3M7()ww(27?{m&t~^*I^$?y*C4b$(ehDu(8?S zPxenESjg83W%-WUC0ef&a?|xHfz{J${WV;^_zfw%9}JI4#r?^=LlMzF2<+oj)L|lI zO9!56gAXQ255|HwIrv}@3?=`45U3r2HY|>vvx(OY67X;s93!(HjszpwiT{(y)rU*F zJuI-uS4s6`)}y7}mfKKL$kj(xu#|lIS0o%E;$IQn`sXY1ET4l(@Z*v`qjMQ0xvO67 z>cql_yYD5J#-rk*ybjH~$gRiY;bSuFNjQ8;mOlBH_LGerTo;V4;&X*`jU0QD;dvab zG*+tpuoH)sRVchFDT7RYI!E>6yNoEFCS^~xkag&{r)~iC;6etr>7hS)wJp(i zevVSZDQwSRVYvv|hxM6k5Y#5dlA1Oj%ujQhH}9IG)WSUyB)7Hd`2r?A?0-7Y!92f;Q6&m zOs>;#dsqigX}LZ0f(Uxe9!Byd>qwLAlkwYXrj_j$>@J~F+cWI8ei-VrXa2n}FoP~t!5}Cy?Nxyj zcsa6#8SUDefxxO6djURY#0m5lM+kuo+U5vz;Tt-q3k>X?oyp>`0vq#a7a^ibz4}!a z*=!tSRfd}z=6rg%3nnF+)^))U2hayyz>n{oNj;sQU+>>D{x*F*uJyU;XvYckd|?Ka z?ZC@^IZmJl#$i4t(o;^53hC6)6-Ow>z-x4ES2X5I_jd)~j8|7M?a_LHB`Hptpd?9r z{R1SgDN9p}t?3N=J*|s)12J(FGQ>#sC#*z9Ze-AGOid0G!tS>h2^vQD010@)kV>8+ z1D_-ssgE^bRZ`A}mfTFLl$Kcs6|uRyluVxQph-QjI4%3T#)jV0O0SUiTj`b3TDr9x zEarb$Nf&nl4b?kCEmWJHIpfmdbEy!X5`sB;sHj9`=4}JcbM;`WkLLZ z>m;5Y>khMEEA@9nx9VuL8;tagV1D3(C9b2eqV);p%4!qS=?*s-n6Wq&WiT%eqW!y= zgLpQ~jf|U#AwRSX32!b(f;rw%7;j_^W~^dlvGn9vT9A5`C8m{IngS$uibT2L85$Ez8SP&MU8m)MxbcAm239Df`)qA1MEvU+BGuI0PwUmB}fPXF7Y?_ZqrpY?cQmnuc zkVzxG!Fxo-qUY>iW%lbA|IK~?R+zyni?%zpbhKi*+}WGwufA8lBr7C!(F_zGSr z{f9pEg>>F=K3(PqGrNsLUG0v<%{TWp=uIE{VJE&#Lv-lSDLPSyF8m)|tb=#?+J%Y> zUh|)GVLfwU=z_nw(1csU;zElRuav%}BmLpS@S)7*+!axR@ege8kHpUA`OQLBW2X4o zTTEAFY=pS%oRGZ`k27nUQq!=*Ys4t}%pZJtJRWRo(C|0r(O-Il#`*L-Sw2?Fx;EYc z%IF)tz@YsCX^QI#g{Z{>IEPA9B1}y>?gA|;ngIoB0+qC&ghN1>PMP}z-JBf zCi{DK;%BEAwUB+Jl;QZ~Uho!k1LElE-k^0Jf`Z4&7z@1!qW5~ktJ+RTvv|>IsRgf) znrOTpHu$KSRU0q1Ct19R60>NRKKPV6XCls*z39t*z$@tJT;__+gYGP|X8WKkVGSkV5>?oAHHJP;fsm06USVVQ}4w`&L!oSy_|I@cdQgy?Be1^yYz*H+F$2b ze65o1;4ZU7ua0i)3!8>tWPaNC{V>tucY-)>7Q>b7#j@<-0Yts}0_u-p8Qe+}12DMT zXl4L5vLtE>z(u}1n`B~oCs>e|C{b5c5I)uHgy>p;)e79kC*m|45TLbP%MP;S*6}=e* ziG0|bv|A92<5wrp89`v+ugtPhs*nGt(kN!gwjG_sBDW~rN&EFjrL8j+rP1+<(jAF5 zO0T`~Po*3)WUKTDZd!}d4RpZ(RC@4rMXCLpiqgac8>PEu{!?igGi0lD2W|(8()a17 z10e#Rz1If9NQk8RU_5dbW%BA{ek5}bR1mk#B z)?-K=C}jQU!mid89oOcTtkEh!>1DIN#ln(X4C-=sJqokdH3h9k>s9pQFz|u>v^)%Y zjNN1nnDyE2J32%?Epv%fFz)9@f+ILqH$X zWV1is#u~R|bBPtNkT%=k8CJYf+GL$uDScov425nyd}Nvs0T-gQbvjvg3F5eUy=3`3 z!hD1(eBChC6!|_pgbWQHljdnT?gYY8gwybG^9W%$9@eJyRhN}_Q;t^oD7snN^ zl;uK%wFs*bYBFTGt2fR=C?Kpwn90U?5c(S7-c_=!@xk%GWjO_5c$O@GhwubKZ(lBh z)2x%_!@lSs-ZcjJp}`_q-i5Gsi!AGOIH6RQixFye;zIn9ze|?aA)L2ImhlxX_w`;` zeh*>br?Om&u=X=q4)2X}U*J51ngh5Xgwqbn@;7eI4)v%&5a zXI-xSSm(eThiK=}4?7v0!?QY%aW3QS#?piN-~k4DA|FCwFa0YYhJucU6~I)OMvVm! z54BV(0DN6c6A8SH7thlKrt(i`(Y}Q+3^unRGNo-Gk`&49uj1i{S%2d>$;uQ85iFLBP=+47^9R zn-L_?dB%|tm+acbe7GH7nr`<}y%O|Cav(DhGF*v)7a#oFV;;?u)JtqF{ zqc%C1jxNRf`zljODRc%Xr#p5)1?14VJ26(gsb(is^G;v!IgsmMmu2rzV%onOrgrus zJiO$vmbV-4upMvp9Jc~ha`F~eu;J~sr59j6Uu1N8JrYF zmz;rSNH8T5Elu`fl(9E~^nz~=WtUFpiSI~pM!dk44h>H-SZD@D*j$)BZ!OdX)NJCd4 z8yz}$5jJ9W47FxZ!$1Q~tV3R&DZdV?K-=(GaiFH%kHh^H6=ihE@3^SlbmAqncb3*& zLa!2N#AT+!bn-F_L#PQQ@EeYfPHaGx33OEht}H>Xc+(3v5&OjsX@@EdH;DcQRfpvMxB8xAeOr!6XH@0bE*%;NLDD70q3`}8tc z?+T9Jml_aObrXb+f|pRvVtxIgk|p{^Lt{mjZwSChR?;aNtn6^7A@uLC1j1Pf{gbtTXeh7nHjDsbr*%F^qG86qu!l`BM$+UJMt`D|7#9=Q zJJ26eSyUjOwjeNt3>*uLhFS(e{zNeZr5IxFX)Zq3>}g4iAao&5PqD(F`Y<5`9P~k= z#_rHVTSsGzMh|7S;DKcAaPV3~>UB+KwZ;;@iqvGSY(WTL!m*I|Nir*xdvP*!F~qY` zVb{qQOP3U=t=QJ3FMze;WVru4Sj%p#i-j>Pyf6`7XY&e2!ajDOa1{K+^hL29f1e=; zvfo`*mEn=9##Sm5*qEXoe%)pfx#5ng4vxJbS3Xo##XVgRFjWEr`=rRX<7YII*Arh} zPE4Bi3acyX4$(!-3P;!i_7gm0QnBprwUlPn>cN*u>raZumNQ^}EN(+@$YJw0^nt-_ zC;g9Rr|3V8`Jfs4vi@j{t5_olQhv5cFXs;?%yw43)kwC^kU{0scbvS81dUhEdEao} zKiN9$E^aSmh3LnAL|<`Vkpoo;{hm3{?i+o_ohc^o1NbaAnnm8fzMcqeIN|1P2jK%w zxO<%l=QTnDCtT!&T8+?V0}<*t;h;w7!`5$1fH?N^#`%%4m69#NqE4OsP50h;7BwMvyL)!S#%~GD4*-B1#Xvj9DN^xg1 zy|^x`RA2=;^aj^+*^v4r@kEUE-i-n?j}hOjC>$)uef- za%H4r2pN>0;fgM85d^*>OLC&^kAqfHVi}t(t-PX7%8!`P8-#I*1qr73#DcLV{Y*(W z{~P}I-AvuwL%Jw;lN>{Ol5E!~78D{k5|;{=dPplP^^$F{ARoV?syef;Ny_gONj$D8 zovcgw&y}Y@+S4!>8nt@>WuoWVrLMtT2o;&+G#484FJ~4v%U+I#li|TLE zMJ1W^x~Q=fvOHCn`z+PHly^7LR+nhIqbshI@{a3@zZo2>derLjE>WRND+iLzN4ngT zd{FQ&(>jVBTXtnARItNWEL0-xt_FJU-O9cGP43TPt_D(h55Jn`^#{?U{IMo!rY@sZ zIop4=UtpUpoRn=EX`$sgH&3b~osXIRT99~VGy8a5;MmH|TrSsipKMK+y1Pf130c%d zTJfz1d68hd|M$t3NtVf$DVCHfT}Uo51%Z6N{DZuYW;VxKd9>{bUcZesZueygmjlFI z=h&3XT_K?2;v1Cvkj=aiW7tsQWLGOM(z3V( zE;^chdLx@l-s}o(*_fL%#g{hXAwMueS5|*BDL9MN@~VsB>7+U_^Fe%)?N`ZmP2SHe zw|u%aAQw4%`IM%!TB)WOIq_{Syhb^~mfgAtC2acbi4e>V-tG;a?Dp-9u-T0EcwMoo z-qh(6D60k8CJ%{kpj^`h3XA*!%e~VJerFYT#)@}}nde<|#3Pb+nh4qS1fSjlk6`2O z4u$q?+uhy}$$q+PHuP#StExr3>Y4w&=iw7J`rbguVny_yz>eQr*x|P#r{)^vC>wo$ zVTV~QD5>oD{TizM(gy=zCu?{R*TGn*$y1pq5iIy&NyKvnoRZPrr1Nga!$;-zlK%2n z^5k_-G#9f|4~GL|-5&LWQa0_;GX`Aygey>VwUt&Wb?lQzIsTuLmCMz-5bD#hl~o$A zM#*3akEeU={Rbz?U$gSZ23T0}{bL`1qwLHdFH$!PYBGV54QfgYp0I{*&iy7UF9>Rg zdMSUeX*nN6HBkW^Y6_zKoNWpeVU4yIE--^S3j)~+wF^vOJJh~#h@Db{L1d5AJ~@}) zru^MqO*#t()Mrq_p11K@x_^vY+6*KzWB$mb+-xN(|vM zon^w6S;kw&S(2#%j*(WZFNZOp*xdnI18m23A|${O94W#iQCNkeMCgQ7BJ_gpcvA!u zyo~;CkOsM!N;4u;f9bO|K-h4}o$q6*O!z#Ff zyWOD+L}8sf3>0mv*l9H==lfi)l~)UOj~J!t58l=&)W@@o@_gdaS}1$N^R8z3$WvTn zx#CTSocs|%d0(C*p!g~U^x;Y^pzrd4#ytrr4^}M?R*7Su@sh4m))z!OBVMCyXu+85 z!mE{n7Wlh(t`%g%a-7^6&cl2h*ai;3r`Xg6ro&L2<^jDS&%V(E+JW%_IU?EInwx>d zcQf?@da%d?@qi~pLk3>+gc5YfmQi%QWQdP%AU_47a_Xl)}(c=CicIybUjh;M+|L;B-fuD4Qsnl4! zeaNlD*xLsN21ju}=)p4b$;L`Br8;(2lROs-eV}K~luVMLytI;UFQOcb;KPCe@slXz z|KxRc0R zl<_V`m9iGc`NBLf*uV9KJ^)kkQ739IWjNUnf?)+_`$3TVG;&sp&j76Sqm3Slr~P0P zm@vzqqL7B0{hhJ>TYIY=kwxhR!5B?rPo%jrfgpx!1(Gp57Y{e;|@DK63xta^@Wx>vTKN#8!{Duck=*5irSX#O47KFPAsX;&^pZoV@5qDSs{< zXY#IQW>JR^kYn&_7zB$q(%F`C0b+eR9tej3udmZp^*{sf+H~kI$KlkN4et_7vtOgU zNrmC`Wj4Ow1>Oq!l>6Jleu~q6RjM40L&K@6_Tp>;L5sLI`Cs$nJU_+gr2NmyI8IOQ ziLTHe?*ysjWJ@pBlDH!Z zRq^sdd}yMlRF$21oi<{h2rx!{G=sa+;z4(uTWk5CvvG;I)9lsL!GY5WPo!35bsWAD z`QeL;^ApDmw&tu+>^XxSI&JjQliYzaV)2WK8$OJHxrTpFcluhR{7O~k^!qU8bcGH5 zYq*~+qW4>x)9+OIg{d6ZvX{iOM+XpH?g`SL$TPSGM@3R_3(y)#jcgpgA4!YsGmR}j z6~H%sBp7t!wB|t4=}x{g6+O)`&#;fwI`esr>dxuW30!N2xzyYqn<0sEmKX(tMDOW* zr~37E+#klKl0($nnmY$6S7ociVb^VfZVgnEn#yYBJ^VQe(!{VS_=g#WiJ7T5Gzu)@ zv8gST`lkFH`LP8LMjKR(pRyxC&baSx2`7)gr4Zzi;kR4 zvoX0Bb;F}rOhf-NDwn5^%E?IuJwL+VdqGg=6LjjNR|e+kQoeR0soXS|=?y@w@&g`?fpFcI6Zp!ilL}Hj=oQ3Vgjn#&F~7@;@~rDc#FFu8!fs+z9te}R4?TxlT~_wge(?jS35doAi=5$$Pb_TF&eRmzSQxXp!ED`l>^)k>b- z(ib|2u**Io4o<`y{u`>Q`Y1tot&6JG5!^)3gI+6)wGVh5?m>?EAF7&e5QGMTWdzgd zPwL324Rh+z6| zRh?_11?*APa|GoIRUI4gED3z3s^3MB070QE2@vc@P$p<4Xx^u)y9lNe44_xP>Gl~* zA;h=m9(sl3?&B*6FMH|NiZ6QwyyNyauh0VbpekpN3tF;d#(w zS}~ogeQ^s10eG^Q=BJ|fdIGyJaXro7h##ycZH)*UU>Mz1hi@R_Ui^3iAl+#OBk4}S zuQ`~D&uxS^pb~d)q|q*nDj}f9Z5*sZLn#3(7IGk==O)tf3eM(W7arzd2}W%uU`E?! zSO~ZA4-P7E!4?8*@FEAV;DmP_4i<8dj)yt0B5WnF1mm_s2C4pFE2OlN>8(a6b|?d5 zP-a`NP>WZ9#Y;cP%VS78f%cDJqTJuQ-=y}#!DV!1&$pM9K^uS{aN9Phf(72DaxHBj2hE!Vx(d-i|rgz;^Rj{#n4RZJOZwMulOJIYd0DX5#g!9CC({4op) z^zO$g^xM-`5JdVwFb5YL27h>Hw;hI~BBf};e<%)RczFU zLYz+fBv6Kjj?jI!+tMB~)xudI9hT#eVh~zuI<~galx(J| z55&R9|n>6m{&6dtn&2gzzJoF=_pccdG;JPS;*gKw}tb4k?U*PVa z;BDF;arF>wXl!=(re;yFQaw1n+TaRe^Hx?5YbiUH1_wCCK_jM}Bwm@l@FdiLq4A;S zz_DgO_IB4))Z&6)XipbZ)Ee>7uN00ld&z(4xkZytUIhByVqb9yjt>7H D*tGD6 diff --git a/src/APIs/c_api/include/nav_c_api.h b/src/APIs/c_api/include/nav_c_api.h index 6811104..72f8548 100644 --- a/src/APIs/c_api/include/nav_c_api.h +++ b/src/APIs/c_api/include/nav_c_api.h @@ -35,10 +35,12 @@ extern "C" void *ptr; } NavigationHandle; - typedef struct - { - void *ptr; - } TFListenerHandle; + // typedef struct + // { + // void *ptr; + // } TFListenerHandle; + + typedef void* TFListenerHandle; typedef enum { diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 0bd1393..9611e7b 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -85,9 +85,9 @@ extern "C" TFListenerHandle tf_listener_create(void) extern "C" void tf_listener_destroy(TFListenerHandle handle) { - if (handle.ptr) + if (handle) { - auto *tf_ptr = static_cast *>(handle.ptr); + auto *tf_ptr = static_cast *>(handle); delete tf_ptr; } } @@ -98,14 +98,14 @@ extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle, double x, double y, double z, double qx, double qy, double qz, double qw) { - if (!tf_handle.ptr || !parent_frame || !child_frame) + if (!tf_handle || !parent_frame || !child_frame) { return false; } try { - auto *tf_ptr = static_cast *>(tf_handle.ptr); + auto *tf_ptr = static_cast *>(tf_handle); if (!tf_ptr || !(*tf_ptr)) { return false; @@ -176,7 +176,7 @@ extern "C" void navigation_destroy(NavigationHandle handle) extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle) { printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__); - if (!handle.ptr || !tf_handle.ptr) + if (!handle.ptr || !tf_handle) { printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__); return false; @@ -188,7 +188,7 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle if (!nav_ptr || !*nav_ptr) return false; auto *nav = nav_ptr->get(); - auto *tf_ptr = static_cast *>(tf_handle.ptr); + auto *tf_ptr = static_cast *>(tf_handle); if (!tf_ptr || !(*tf_ptr)) { printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__); diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 44b6cdf..029309f 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -85,6 +85,7 @@ void mkt_algorithm::diff::GoStraight::initialize( mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { + // robot::log_info("x %f y %f theta %f", pose.pose.x, pose.pose.y, pose.pose.theta); mkt_msgs::Trajectory2D result; result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; if (!traj_) diff --git a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index 9b7b5bd..3126c0e 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -49,7 +49,11 @@ namespace robot_nav_core_adapter { - LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false) + LocalPlannerAdapter::LocalPlannerAdapter() + : initialized_(false), + tf_(nullptr), + costmap_robot_(nullptr), + has_active_goal_(false) { } @@ -139,6 +143,15 @@ namespace robot_nav_core_adapter { return false; } + if(tf_ == nullptr) + { + robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__); + throw std::runtime_error("Failed to create the tf"); + } + else + { + robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__); + } if (planner_name != last_config_.planner_name) { robot_nav_core2::LocalPlanner::Ptr old_planner = planner_; @@ -157,15 +170,6 @@ namespace robot_nav_core_adapter robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str()); throw std::runtime_error("Failed to load planner " + planner_name); } - if(tf_ == nullptr) - { - robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__); - throw std::runtime_error("Failed to create the tf"); - } - else - { - robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__); - } new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_); planner_ = new_planner; planner_loader_ = new_loader; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 3fb3859..bff0768 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -341,6 +341,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) } initialized_ = true; setup_ = true; + if(tf_) + robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); robot::log_info("========== End: initialize() - SUCCESS =========="); } else @@ -715,13 +717,38 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na // 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); + if(tf_) + { + // try + // { + // robot_geometry_msgs::PoseStamped global_pose_stamped; + // tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose); + // robot_geometry_msgs::PoseStamped robot_pose; + // tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose); + // robot_pose.header.frame_id = robot_base_frame_; + // robot_pose.header.stamp = robot::Time(); + // robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + + // // Convert robot::Time to tf3::Time + // tf3::Time tf3_current_time = data_convert::convertTime(current_time); + // tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); + + // std::string error_msg; + // if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg)) + // { + // // Transform is available at current time + // tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time); + // tf3::doTransform(robot_pose, global_pose_stamped, transform); + // robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation)); + // } + // } + // catch (const tf3::TransformException &ex) + // { + // robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what()); + // } + robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str()); + } + // robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation)); // std::stringstream pose_covariance_str; // for(int i = 0; i < 36; i++) { // pose_covariance_str << odometry.pose.covariance[i] << " ";