From 94609d938df62f4f442574144262a084e95a5acb Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Mon, 10 Aug 2026 19:46:09 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E4=BA=86=E4=B8=8B=E5=9C=B0=E5=9B=BE?= =?UTF-8?q?=E5=92=8C=E9=83=A8=E5=88=86=E8=A7=84=E5=88=92=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/map/nav2_costmap_binary 01.png | Bin 0 -> 8839 bytes src/map/nav2_costmap_binary 02.png | Bin 0 -> 3058 bytes src/map/nav2_costmap_binary.png | Bin 8839 -> 8503 bytes .../obstacle_nav2/config/nav2_profile_10.yaml | 10 +- src/racing_control/config/racing_control.yaml | 3 +- .../include/racing_control/racing_control.hpp | 31 ++++ .../launch/racing_control.launch.py | 7 + src/racing_control/src/racing_control.cpp | 146 ++++++++++-------- .../test/test_racing_control_helpers.cpp | 50 +++++- src/vlm_detect/config/vlm_detect.yaml | 4 +- src/vlm_detect/launch/vlm_detect.launch.py | 11 +- .../launch/vlm_detect.launch.py.wsl_bak | 113 ++++++++++++++ 12 files changed, 299 insertions(+), 76 deletions(-) create mode 100644 src/map/nav2_costmap_binary 01.png create mode 100644 src/map/nav2_costmap_binary 02.png create mode 100644 src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak diff --git a/src/map/nav2_costmap_binary 01.png b/src/map/nav2_costmap_binary 01.png new file mode 100644 index 0000000000000000000000000000000000000000..c414579d88cb7ce0e9a66e33378310c6d16a4662 GIT binary patch literal 8839 zcmc(FeLU0q`~Mq-Qz4x?ozN7=r`8Rpa?==7DK(+oB;}@ZRLYQH?q(ciN~m*ID)$wV zdvZHWk(HYfGR(|c3^Q}PvDx-}>zw+WzUT4#{{H;^v4_{*Ua#xAUf272UDx$|zIOYR zjiu5WwKV_$D4jTNVFv&*^D7@kdGJYVFXImQ=Lf&z&S(HoZd&=sJlL(Q4glML6BdUY zf>XG3)5KrSyX_d3!tQ^h3ljD{@}MXtZ~4LFmtV|ejvwFs%ev?H*EfIKHnT5?+VJ{y zL*pCE@TgRS2Fo|UnV)-pXuuOsqgUn;_fym!t8Ut|wBdnK-mU{S@Wh@DzPHY&$-fi22zCq(7H*1eM>D>uQq#QH*L>aL&uAGelkAM5M#jdB=-Tz zJ!>O89wLlSR{PL%X!MC&DE@f$ucr#@8b^i6>=68kRMOfJPdU2caL9b8?_dvasn2Y* zv;diHh@TSEW8j04NJ9N#Q++IJTAp5Kmb%j%3GE&6+0wrcD0{^+igGZl#;N?CShbXg?JOl?mr-+ueo=>9t zqO0qO(%vBf!#TU>Z)( zj1M`RnN4^~m;cQj2I=RkoEcMS>278!)raY3l^!sS-!(jHachGrHbXP-dGp-X!ssJW zGd)U6J%XvJkaA$o@3DERg%iO^^+Nn6qrB7L`qt!ypw{?Ka+R$Nj9!NY*0U$%AnI1 zaP~-=S2B(cPtLym@mhIV3X_9xoq{B$yBC}hSNxi07G57V1s$k}??^|G8O!aACX$3h zqJ%nR{0O@_2mDaLM7=R;R#|;jGJEcQ;6r4XUnt|5#+gNhH^0@^PN7rR%rZHAGwKWS z6aN8c5{%t`y<)YOC9{r{Uptq|uBBbQ`s6YX^w-Qm)Aao58f0~zw-an((@62tDM|K! zc(oB{nWHHzpK?)^*BKg#xC!kPQ+Rl)%P_O#RV3ifvfi*agWfNrpwlpYt-RBPn`>% zzK3b?&h#~|@j8&M(Vp?L#w)&yFQ%CGMu(xCJj9wtU@#tLN^IIn*yjJvsiYL8CCe{kl^$nI1~P zt~Zq*8m;!&6u|D}1y;FUq$wifmY4E@d+eW@aO?0}eyQ=c}e0o^(Uaq~gdDFt;l6Z|V&taGaO5D6}(y`WDj@iST zgT@g*3&a|l?1z;_NvMug)C9W>Ar3y9kd9b<(sLK0={~ebSJ2F%@0jgRzXInjj&Zde5@r_HDc9nVE4 z#ZbzRa0XfxUh3zUxre@5Fj$fCBzs%{RoZI&?gjel&KrwI6I2eXQP3?1+f-IBmiU_m zZUOw><;$67ZR8Er(O`aKJ(7l4mZivsYj4t<3_;)~V|zSAIX4eB1kR@x?M`AExL^hiOmEC0jIT{-YuY1MXOC3R`_EBi~Rs9M=W2D8Moh1pe zu^ZpM<^m_xh8)Xcy3?*`jscP2%w38Bz>9`S&^xc{B-gi!%zD3kcm1h6vTm@5E=tBe5$^9(uj)f0;IT*=%yiq0x<&;HAfTfrr3 zcYH12E436DR*Ml)uJ4R3z<`n*ch7>N5EF$OfWe-rgWyK{H7vK~-G z7I(@-CToAS=~vENrxoU>?p4*+(RLO`? z#|d+AYAY24V%-gQXI8_?R04tusGLPIZkfCoM_PcS~-7y2_uiNV-3e{7m*@1XXR__1b*I^>s|U>WaM;Byap?r zf4^RRy{amhY`PoV6&e1XlSdScp+{@6qBC;16;TTZ#7$wlS*Q4vU}Z^2j=6AA0K2@nPwP#w z$$DtkJD4T(?TGv41|yfpz9g}smVQuVd?CKj=0&=BQJYA>D{qR5?VIm(%6p(nyme~M zX|xfqqAOZ6KY7GG!C9fEb3l+TT}n5+kWqOKRXANx52uJbt50Z}g%=mC`ze3zl+SY; zIUEdhQvK3s@;ewUQF-lw$tV@6bZUy?3f=sM-n(lK430A0JqH-Cbuwrzw=~8gJTf zny-%9Zy;s6;gSp=xz)Vp52oXmCAs5C5$+yIhQI2HO%vJFWBuc)n9cXL`0yA~iJ)j` zJu?str{&k0%ZHZda}ZoX@=@>5aXM)slwLVTt0;F@D!f`NPNg*%2LLWcO=;+amC9`! zG5cpkGA7c64mu@bu~D-*F=GB%Y#+T(J7{;DM;*U}3h~zr8&7+iW!iSVh-Agac&W^m zBcxfTpU|x){H|66HFGwI7GzYwDX&C{@>@sw;A^#lp01$bw`axM@13484s9_e2Gk7+ z1^zxuZo-8Qu>{Q)473vG#U0|1G|_^dScEW?9@(7KWr6@-7?@1A|2LVMlNQ8f!1OsV zWdaQ`D@pomO+ndi33z(|s9!5Mq5$~(_h0#P*l@I}=!4AJm(B{tK=D6?*+RLKMi>+Z zpp1OU=HJQ%ScOb@8i3)y)YQ=u&CGUe1s?v!2TR3@?%!98$S-t_w0ceuaw>hN;?ua6 z3d1L21kx8ww{4-{)tZ;GbO{=G`0^#VC~E`wuPEpA3i*b;*LP%d?9FnVi`v_(*>ecY zdO-JoR6dQ33Jq(kxn`{C0UZMu^5+VwAu#>RH`rRHJWuGNM5wTz%N0A6DF*--j;z$n zl@gPYV6LZQcdbHY(o*5VIn1ErE>)Nvcu2vv*T7{R5J`19AJJykNvnwW3orpTzt}zq z)@y_C9{|7He)&+0-h2vLbK=d`NFJD{uWx2vDpQ(ISg#O?lr_C|L17r)m>1W3Z+n@_ ze5~Io4Is{lX3lt(Z}&>a66!U*ywI|$A)H_*Oj`=AOT+46D?q0uy&TVW1LvzHoLn_r z1A#yaU2nz}tT<4Kbo0r%b48c0-5h`8DC|0NwjLdPXzfeQRl*1JRE9MOb-28^q10r` z4Z3_!LC%JeG3n1vTi4M}`y|{XHfJJ}DVfuNp2>4=W+wFGey>1bboXd?Vz}Z#Vg}7- z+-6B#+KFMzR@o3E;ni(&>I<2?%Y*ApdWwIpXLlY8^^S%_B=|_sh~d>~^yyaxefrZx zH_V;xOjv+-vW9O5+dG=hu|)~UqaiP%G5M^n3X}!KQJ)Vw8OE;DOS2s5d(_V#*X?^I z%1nwqo~VvayNbdztG8w5OvYNVcor=oSFZL#qw{cO8r}-5*5yFfs zG?K)^qc%Ch##Bs57$}b3rz@S_^^|P)U~KGQ5)&yQRnlFVA#?)>W~2DiS*tN6)0(N9hTA9ctk4Y zEEv~^FO@!K2xn$O>!#ePQrDtqbcrhhKJU4OLQKA5vIlLn|4)q3ClHPbHdzYlMIzDS zXtM984#{N4CnMaPk^AKJNNCP6lABLv&W>`FI)_`aiOy$K(2Sqnh46e5bN8AgCW^Tg zfhZou9q%% zNS9|BZ`US!W?=T_RiIcnaYs5?Dndw&>?kGIp0xX7ia!Kwp8@*@kUDtXCGG6xkRx{| zYw{?iTKqTHm!s8e)TNPBP8R}FFPWKX9Fq6DI>Z{MFQaeMXT1@&0zu}h4-YFCni2wb z*q$T#Ly)L+e%3a3Fm3-J;vqUA+w0R6gs300`Do#wD$#WO)n&R=a2T&eC#5zmp zOqQ!<6N`5_ytgfwIQ9Fb#b9_Ht-^?TpRZPrAElZiyy3pGez>K^rK2Vh*#5^uSV~16 zLq5I8qRE&-OT*i%xMt45j=fgqURIY%bJ>)A8mkxM8H@2dp-)q1jt}jTGu0AmPPt1) z-1&{VoP|)|;Dye6A7CiNTwEMr9`$W5qA#18ZiGXc)p(5*DC+!2k%(Z2 zEYYpRo?}?4+@ftQ^OmwmV!`5Dy@(IKCaxC*3P+Vf{ZvH?)@jDaJgN%fF-E2~v^L&Z z`o4(z^AeAok@@@7rKa^Bx5d%t$i!jY{#mzcdDxbH`#YfQ<>wyJqZcZoASFwsyrKHr zzF@H(j_8VbHo!k=@9o}gChrE(SUml7UX);h(s;zUPiB?IbI$;x?q;4z(5ZLR2Ar9@ zK|u%&#M5O5*!2S%zvo1l(8!@%(-F8KI7z9>u7!0d^BJn@`aQ(ksu{Qop9LqXMtXSC zQyKP2Qo^%Z`{!pZiGt-1vU@s<{|vZFk`973Nju0WBV~>vzt0=0X}ZCGI#^>A?}p2F z9O_`T-NUua*0+Y?M_^Y%Q8Em%?kb`Gy2{r?BC-9oE9y4*rdwZ-!=`v*ha&LgdDnM@ zWFJL5y*K+hq$11oY`XPc5?^@1`z*O0E8@3gRS!P#fq6MGWUn6qu3kID*5((UdD zc#L#|!|1rn?yX>-blS81xg9cmX?A4b?eHz7#6)mpwLF_dy1Nky8JJ5R8I9RefVg`HHRAy*TpGEIM5Pom~{gAB^{ z#?(Dg0W6Sky=fW2K#!?~8F&|`JhZ!$?e&zf0XJ`n+laT>e?rV~5c4x54x-K)1y~Y0 z`X}Y;=o%HF+)FO)T}JP6#`tR|GO0y*8E6XF@oIWW+X^Hzn0_+cXae6Iqj0jrv`u^0 z7JodHJ`U|c1^1NllCxCD_QXJh{wc=g8?WhfIQx09Zy~@~ech-F6G5ycV zJts5l>vZ3dgcEtYlWtP?*T($>N&%Og3WMrI@B|UOfwwZj`(#rm0yl42daV-S$9+2e zriM3g%k!*Ol+DMQi&PLYY+642^R+9vpxS9Z_k5JHgEc4>L~v*Fu5XqbKKetzPlqz( z%6}Z&bC}Y9&*YTB%EI-$gmC#655RrfT$Fuio?&0E+veJe-BQErD<5>$`Wx1M#kUTq zD#qaHsa7WOGHCwac=@k^y@7x%54z#+hTsd526u=~GPnT!*HlqQ6xi_p7dC_PDOyzt zqSD3sf9d>g))$oRfC@G%<?BHy8a(1ow@ zbC+ivu+sUfy?b#-ZkzKi&^6>@6RTf3bI&26NN6Z(?H?;KSLnJfI8ZkAW#C2Ko~VHz z0y2;>bZ50F%B2^>NsXBH4L=0D>~)+((bW9i6_Z4sprb!>l<&&DcnrSp7$yu81Y!_Zh2R1otZl;ZqI?bOCYU3*ov#a@ra$m3S)@U(rAYRDs9_gCfN9+&VdgGr=L)d zgj~`S2BZ6n^leGF!8;LC$Vx)@95Fn^Uf^oxnt5_TE4#IlxI9N?_srAHhxB+;Y!iV< z(^iv$J5ZFPhpU=oNGhS^KD{YC0;eptTEFek6i(H0;yEuoC&IQkn z#t-$&>RbPqMYu{cjF^trY+B(4mt#((NtfLDzvfwy5|a!+ zUaU5NW;uY|0Z3C+5s9?5k8;<93FU@_>)C^?h>?MG-gv8?&ZlKQ^|FKoku3UBV7;8c z{4*VXeTqhToclS1PavPD`3Zv)U|3?2V5P}`u^Sg0Z|_W&TC}jmz-pmq8g4TuMEixk zn{j6cq(qm6CoZ@kp%1`fB9YW1+7&Y7rL%Fk+lZwNxB#8j6Z>Zc7-KjGA|z3 z-&3LLX}_2M;F4gJpcNm)yjy++qOH##T;TTT=H^m)@#Z#=BZ0>EkYQf*4%;tV6*in+t^^z~fIHf& zY>nJ}gG1ihQydpkmEdHpd*A+c`=&}Y5PSyzRQh@3ft?n4H27Y)=!%^zfS=S@ig@b} z0GoeyJ7ELzi|Ya}4k-wSpH5NtYzF*drVbm4xlfB+Ai(ez5NAGuioi6vKW~54X1^0z zBvG8{1mH&$iZW;*&$;j|VZURkSI7Z#!(BzdsV!eCL=+sJ*j^9HNG$acNvTKvM$KP1 z&K1$QDViQG|D!qpZ6En=W2zv)eEEO5AcbjJlhy!LUU?g=34ds5Ste{a(ID5bhptq=66gAWNLrHJ7oG9n zbn1fCcaSfrDT`#Y2k<*Tz;*PfA$_-ag^fPYtJ6^_9c^TI@mMz~$3+tpP3qxS))Ln&8mhzIT;@wj}U^;4kj_o7|Ow zd4gZE1MrCGc*oJ(cmnn?L|2%V-oH%&DALQ?2oh}}e;(;l{9kXvSE%(--*5=ga05_e zD+K9vApV2irFWzjVAEXrUkWV~u#c^`Md1Mc&Yd zTut5dh=#$utgWyfz7H`T)X!9cw-46r1{!3(May@CV&n%4r1d&p9OU%e5}RFdQ8|?j z54TAktI5Y*s76AQInBS6ZwDN}ZtpJ_ZnLxi0SQQRT}Ye%8s=X@)F<8eM6yC8jbJxX zq^?~6TKk=%e}-{}Y@Q*ch>@m!^v*e6bVOUGC^h{^vSK91@B1Tx1HWFV|6WaAdq^Lh zv_Z8!&}x3-l^5`6$4XeX{4;zWkIr7|wFlZnTKp)go5D z9nwbRqv1&a9K`5=*=#;`41mp9{=QzPs3o(bJvVGFAemBb>_|Ccd$Ks#={oYKt9DKa zQmdw9?7_gh?q+5_xuh<Kf51?_e~r4(wl>KYolUbk zoKdU*mY#1MSJEJv6{NOAq;0SD7l+0cS5^7}d^c7Uz~Ucn`n7#GCNbKm*L^11C(4NJ zQvYp-baFs#JR}a~_d5Vlj1NynoZ|Q4)UP=X*S?&ll$vNwIYn@CL?%7U)-+3wN=lt7 zHwRw47eoa(Q*ftlG;eQUJ_^Qkdw+BPGmRf=OIOvVj5;2#qi!P|6T^E&q5>2wta_%* z1GlQU8;Uh@Y79>Uq^VNMxK+y-37e+8n4ub@1hC*gJjvQgcOGY~YZtYu36>uVrJZSD z_rzkU3zPY18Sha=-6+dr&6%!!GuSXp6m)iW_Q`-9IU&IT!|qv;X6+PoWSNzxYtrZ2 zoN#L^veSjx?D`4=jq*jHtcE*oZE0yaNJr_H)T&kUEQdA59JP#4EEtSpxVLMTo`t$w zIPFnThEiMF1`^D6;#voSG5nj&jPRdX_7^Uru+$z56Av(ob64x}Cc%^u+pGNxX28dM3?g*QJ)RTVo_SMr~}g zdV|P4z5W!e_>msNJ(+j@d-#_!=`@;472%7tEubkc(Vm7+=Irpx(krS{AmKreh(ks%Wzj<-!W2F8r4t% z_w+<7a9*a&llexjpyW~aA?1Bql_~#bm^va7=?W0%os@Q%6tLDa;hS3UY+R zjp0giW>Z^~pX^sqv9~&;pWYJ6`tI`WF8Ijzv~}4Q(#qDGmUyeH@^S_$zWv=)VHE*O z*HBAJOLJ&tLg>~DqUcttxiKWWXNncovf8$cWpOwh4SG>M&tnZnrpGmS@Ya^EGIlFU z?yXxZXEJ#A?ZFfc#oFnClFQ{XjYe~oHvtuTNXpg)NjxvdX`CAK+-hKnlUd*|b|H-4 zzPL3JU&I~Ny)-U_7mmZPm=tj8!iM*#Lwgl$aZA#9$)_{yhgYW~!Ql2Hc0pWavK~ N_#eUh)_O;0{s!N6w5tFB literal 0 HcmV?d00001 diff --git a/src/map/nav2_costmap_binary.png b/src/map/nav2_costmap_binary.png index c414579d88cb7ce0e9a66e33378310c6d16a4662..711934e018b1a23e5dfd2b6a7d24f4bd856b9920 100644 GIT binary patch delta 6715 zcmZWu3p~^N`~RX+M_D>`R4$WJO+qP}%cdwbiBKrlB)JUbGWRd1UyX`!kW((BVn~$R zFwAKutK3$I%q)q~Y%ZI-ZU3+O{mwbR-+!;yUa!yh@_F9R^M0Ps=Xrl#U;Ih(iIhE{ zRIshZ381V)c^m+MHNhv&*sq6v(d(}!K-U|BPCEqyfVTeXUrJdHg8=|F;Eefk`|xZY zt224`+n9Y*ZsryB-k)!k8)9JY8>Tz}rC8|=n_uYW-2$Xg&k2$DSter>w*pa%YgeS7 zne^eL3@`oiV%;8hhXYnPYlow_L!Pj!*EG_=iesgkhj*jld89jM59|GYwZ%Yv#tnnW zu!S%M8Raej@cAY}J)~30eN>nxC58G;3Y`|WYaQsP7?+u`J12YaRNa`!u5c7R@?nHp zWr=22TRmqLTW5Y~jYSuo*{fdig&52{K{NZvmr<&H;+ekJ~+={Tpyg!9Bq)u0K z9OO=!~mtkeYh=D?#^Qvy{@YutVCBpG!_-URn1XwW*Q)3c5syALtR-nqCD5eU)|QK3mo1%$zh4a7+2HbIsIUI_!?VcxN$+FWXvSBv zPpQ>GoUw$3oeDx4Sye8&fFs$vISm3y7WnK9lI~l5x6o&aL!nz$Mu=D}7l}bZRZh)f zZ+oTBBgf|#jaqj!#nw*nrxzA`e0u`rhR10f3Mcn5mboAtw_6dQCBZ9mWNOZAk?C3! zVQWW2)?g9ol0T)DDlB4q2D1H&mX#tfFYOXXtC){@(#tngnc&)u4}!InZ851!@AP5& z{ACXCW`QvMJ2E3G`s?*OIcZ$2jJeOkCJSWg6zpGb!y{N6!);$_y}X#R^Ms=#Rh6;9 z9a+8pavzdkMS)Hc9HY(%@_WXS_Q7E@;_!_~l+#dX#!ZJ$WjDVac;|7FC9<*_j1awe z)sCdIeCKa;m4Y=O?!ai-Mwi}U)>)v59tjiL zfjIH-Z0JM`Z8qY%NZt({J+QVfXpt3FPp9<~ZoQQ2nC8-@j)|k`u}pkT(vX03YEAOd zFNC9E=dgJCyO1Jd+N58J7XIBK;nWlZJ&#AP>=J|1aLM#%@0@#|WFsBUgd7|W#-A;` zX0;1?xhNd(N~3mOK1wl#OZJQB0wtEwvpo*e zb3A6aVAQH1La>+5=P%t2>bt@l)C=$-E*X>+(K{y{7!f3CIF-+!CU}&8S-W#RNQxy| zI}K=1Cpinp$PF=#{!x5?J2X)=PN&^mS^1;)={4cP!c5B~YT+0UmD(vb9`>PI5YL&Q z8ruZysOu)Vm&n#mEycZOdo<`f&)5l!^(Ca0*bc;S9k)u*?J?IlU!^fv%2&^54`{1% zb-NN4kMcJe$sUHX@3A5zd7)WKzTYCh;qN%_PKLGLzl^8HF3?Aj0g1_MJ@IiW#&7Ba zqb}^aOVpxi#+}ePOF~W4olk3*Y`Tt!PzCvBvO7ZMH)+UI-%KArO^Z0U1HYILpf7fi zv}FMKhoO0-^e^{xn))i0W$mOvP}B=1QN>(pQ`q@9B|BQ(M`aS;_2F!!3U#Tcyv$`I z;QG+C{454B*cheg7Cn9P>jd}qqq8}m^m3anJ%8-;C{t^Tfq9Z!eqht$wO}V_nx9QZ zF*ee;G5w2haIL~v##B)zsrfi zGUsUiCsZCh!!rF8;~k!kUwmkbDedKoB~p&-;$0u6+eR-;1df~gL>0iWRS1zdR?y)A)Yr;&{u@G8q@($)ol`xghKl5?s)oz_$;on; zHQ&igaoN0j(^RLR&%xLRlQj(hpHaZ3i$S(CWayJdo796dc@CqHvGbTi6Y?)~ zp|HM(r~uUd>Lgq5I@Sb(qs;#mwM#~;y;LSAh8-=iGQU0nXeF7Jw^4AD13sJF4CpQM}cv%bBtrs2qwN8+bZD$!3D`avwuOLc$+dwi!U4oS+09s>wWftH_q?@^k}(j?6t*ZJdNPWAFG2Lg1)uTSZoaZ{E@ipTa{jP3 z&%-rJ&lptC2sjJ2Q7(M8F3Sj+>6wad|HF`Y-H6uW{R0%mo3uY53_iOs=YYVFR84#!5)p6n3n+tOv&7h`9KoN zhv^7Yg+GJUg9?P*WY&&$mz*i6-$;1h?hxSU7Cdfpm@N^p-Nck|kIIdpu*|Ec2D?%& zS!`VZB^=4AcgTy7FyP7SKhoZ#s;UY#ca-B2ygyR3J;+Ni%yPQ;r)zi;XhZWEhEGMh zEJ3{wjf(AK0`l!q#*JB^_RGt;JkGcnm9F3aVQ!g5tbj%PvfO6GFFh>9hzzFPon`vX zj`v5bRwlgB%VFT{t2N?rq8K4wprczvq8M`uwk?1>zh47I<|c%%u#|q%E${`U6jsGVYAH7f{X-5dIVDt^ez8D2#UVH zFQ4TAn!Z@z-dKLLE~hVp6c5yuRIHL2@MqC!3BJ5pI4*nwuW>ejP-ph{V z!(AlnEDl-90TW@oKcs3CZqp2DJ7|!LZ;j3NHjoY~Eu{#ezReJCrsM8zFaAaidvx88!qg7|aJ4KDeZyC5QLi;?jvuey@t1apn*jZC0%dIRP z>2-zQ-^A@B2eMu3j$~%V}>MsR6h1-fG#{}5XbRdV?4j?r>p$#X#j24 zY(fIG$IY`Pw51t2ymN(!YRUzNY3BNjMS1l@i zXpc$kB%<9Y+Z$dCO&Eu&;*Xn`Fo;KmiME&(7F*<^Z1{H1ENC~in1d9WBNAP?nT2@{3|_kF9rcjKrf zTaEabsZ)e^rI!8LfuutV;;BfFyhLYTg9`?V;OCT7bxr1V2}M`ms1$~fNH*l5^Q}#R z4^HJ(ZjT&mz45KvwkX5Y`_P10Ig#@1=VJonsGwq!4(*cF4g_^hY;p48OiHHP=p81elZLVnrB@qeRI z^az^hl7O>7$#i4#>`X|yQkH6r;!BF7Wg|@+S-6Qye8{$J+)wL6fTFk`1HTgefk6KL z`H?A?N^@xyKvV5|aNh^TkCr`UEO-5V<44$atvjJu{`XZ8r`h%&c2@zs6`biYPSNcD zfTpFuw}Su1EzviIe+p%Bdt>OxXZ?<<)X0*L7p7h*7v=o$t7LRb-m1Zpir=dae zOL4eX$(6Qt^t+UDx;0T?iPvvf=0M}^g^Tk7o?n+=qgrd*_9OgLI?ir}mHHrpLHNtj`bs8xsBV0$JU%TnM+ za+8dnnjtr1{&q5vLObCzu;?;BECe6yl zHr(z-{a`E--#D;TcYtMku5)r`#EOMm47q){=icwyZ2nBA8_}){Clf(!pb#eqCP8B` z=9r|tA-8E|r~)q`1}9UOo>41cF;exn;OwRtB?SWY7N64VN4kdKIM+Xz$cIP&JbpdH zS^bMgRm}YZZMGkO?sC&-?2Ah9x)RyHskC^8Z>{rlSEpQeu$6;5Nl6)Db#!|niakD$ zYlUBIpoF0%-IE8P19p9OaI|EZO#t1*GnqGVI7KVH9>B8$k^#!kdNeVJ>CyHqaiMV} zSL+Yo@CyZnK3A$Qo$OegR(m`bqE_{UHrldnwDtr%(B4kT$>BJ7Ia7ag%Ov_pcGTM$ z9Z|IBGFQ;BYq5A;bKR!jZhVGc5>KU96dQS|_-?)*HM%P0A$h$3ozDf}LKbx~YvHv{ ztU};m!+WF4afLpQ)3;!mEx0B@&VpzXghTS2`PyD4`dyvMZ^a6)dIvQk#2oMwtyEit za_zUrk9>TZZVO3$NQr*o^8<9;3pFRkVc}e}>|h;9x*ym?(xIj&2$*hJi-f3!VJed2 zSl;{ck_pXY_dqFFW|3=q7hEQT_u!&2i`DHpJUl!hTQIa#VxHa(s*ea_EF`9zkJe6v$r+^Udt;=FXo|+egSZR5@MHSY;NT zJH1kPD$Wkow32RHDt~0Eg`Sf$!^m}&49`-?sG294ECDZ)MGaupOb>}YCAH~WihUl< zte!UK`K&a#4Z5;kBFFqpTU8muD|*UDSZ3~v;7Z@!KpQ(>Y0CADc3`+b6?3WWrYD-lWLPxCW56{fjbqOXd&M{lu4gd^-^ z%_N=N>YcHsjlp@Eh?7s-8QuXiU=2Rwv3X)mRhrp5vtT(vfxI={Z-(#9+8wM8qC zD$-%@@7LjXuYa{DD{8!UDtG5D#CdgWYZqL&?5iOY?!^%{M{Fq5j!k?EvIF}~raJrA z#Xa1aTT+adZ@yPMJ1pSV+gdRLnkzw zhZlAX$kjgmz!tq$4FzhixrkyIJTuw2$iFu0uhW~DQgIPM;|@U4{r|KFQ`KDhH8;qJ zg%epM;pWSju7yn6o96u(wAptM4r%{)XHWq~eWP@Q>*M8t3uzv3MNm1l&}}U+P<0`3 zFXoIw>TZj~J)foAf5D`SC7bI0pTV04aX`_%zwkS52|9WUlEOHp`hdH<@V}s9@-l71 zI=p3ofXj%o5eQDfa&QtC4R}hz1CY zH_tbla~D zZ+H2uNt;wu+3e^-0R#ppf{W6IxZZs+K| zpQz9L-lBi71oi5F)1{pH<1um-EHoz+`27GPjjyN*I%e3a7qq|js;7nqvvI(;ClLSN z8U8-^4WeLwfw{R5gv!$i@}e1GCwpV(RT=&6br6h6NUe|8@??re_O+jkdrJN^RDaaC zfh87tWoN}|%P5hD2e?1A@hCMCUJGDt13m-)_RMcpPy7SWvEaIT z!QMObCkaK1D}rFAF1{mJ6-BZNaruaT;arg39_sm9|7M`%FOih8`!O5j$3k^t9r=%{ zv5_KM=R9qfX~tp@v%N{F7a@1kT;NLJ1tmgnU}viO$>;OO(W4AeM`ym(n&!~j|0>Kp zDs-vq_QPOMdZ9n`DW!S!tI2=p>=`wl;JYOzjMi&JhV~x;+R|3*#Otp*SzY}0;KBLT zQeFGc0Mq2uVc!+l!<(xATng^93sj{3ycv6}1E{|%5-4hx$dX`kZ%Zr~r=SRcGbhiPmz{8z`t|<+(EelG delta 7068 zcmZWu2Ut_twhrKcjpkTJz@Q*^CKj+z3*bN!1HwbpOj=k|wEn%=!@YJP7U9iM5`Wc&V4%X2Rd4|!v$w3;H~fi8`w>YKJM zZ%8vK+I7$Ykvvf2clpnw84o%Rku-xG>IpbU7-1D;A9{J{7YIZjuVcm6f?Re`O_h^# zdnESXFVUVYpIGPq^hdY*{L)eJHJzvl?U=3HQ(EGQN!fGGj`n!y2kk1CQ zye4~suF2G#vVsr+Q{msja_tOt~>ipHO+wJaO0P zg!SzW>M=RmMK4+xww2yI5)aG`C@&9)v0V7ZJIzyt>)QtQUJ|*#m{jm7$PK=b#TXAu zCa%qoV$aUyc83(dNAe`bDWT^Q%~-}dgB57b*nvP)jB;Pu&0g1lSMte+v}Pr5a3>o{ zxTSkO-UaWdMjA;a2{5PZ=c3p<{jEy&vG~@Zfk5&Cou;UYf8B4G4$xh?;DKFcGSYI* z{fwaEGeuXQQFpo`3K2wW`+{V=Qbt2b!FXE;%Eq-j`(& z-5AAz4OJ(0XCujUaVNcnBxRAhBAs*of(pH_jM)1RIPI;bjiVz6xD_@2#siW>1mR1e zblZSa_0+3ZpIzpHjLjV~ z&n}*=L%l8Xb%if&8ZXb|Nb~>2t&crBHj%;bs}j`$K4+*T;-(a~=&dx}i|kBt8o86G~FElQRN zfMh`tNmz6=RW5n_3r*EjW9C_m0#L?19NJG@dw_Wtr<1&!dgC^tbkz3U0MN)j zaOdz>8Rprs=S^Cr*`3#2Z<1+|T`_9riX#(my*33id$=LBo}$p>vf;7ZImsPHN^V~% zb{IM0VF~j-6j1Rrc=`tY)bNX-^U&R#W@VzJ+bkp0ch%IFhqJgNtT7nZ-1vRSRbcrf zCV<0l)y6hrkBk+x1^KqH&gw=Ul5s+?Q0?H$lIu>tM^EP?o)FVWIwKCUUlW4bdj)6CaMN%+~MY^31X zzyqi@dPG1|(k`I=KL0iQ3W6<|>@Or2;3K3_swHG677x~bXz5vD-S@M)KG(9y6EYmV z8}5w=%M8sj?P%m@Rkx&n_IOeGH4#o>*>&jmU`q-zg2Y{1u>&9;)0U2noN}T#Aq~H4 z+=#w?f7XU(Y!UC!Oa-|4D4!WwEQMbfP!VVmvK12Wu~@;Ju#r2`K!pcP4oI8sGHgW-Y$vn9 zbSM&-xjW$X`h*ff9w5m?q-2tWN=n2z7E@Sv!|N~KfDvReT;FAh@}NiR$(p%;b$`w< zEgkFQpM-}=85AUczS~2aVxj+?K+494US|cpAK>BIq;LD00S4o4>%wkAOAS3%6A%^v z8Y>|IYTu7QkG+MzRt*jW#VMeFM3H_wvSti_I>sa~C+4r|-%jUGr?uN^)iu86w%}FJqvt^XisHv#QCUJ=Je1QWP zl`1^T7?fBxB@aXW_^WO}N4kHF6+KqOb}-O-**stzbM+(0NYQ7MYRn2sVAtGuJAe-l z53db99j7@vJFA8UBvlgTy6ru?KUbGPP4~FN^uSp>Hud_J+pgf~Ks5%~Xjbk{dnlsT z_#4e`vCMJ39KJn6`M0}wuRc4f0Q|GNIR;rOi}z+t%PEV6YZlvH;J$%AOVL*hP{?-( zdaf_uaDadl2*|Gk-(Tok`9fskdQGx6M!5J% zO;b%>9dw=kCiH50p!duXB~zG9eT?Xg0a4jZCxM5 zPcseN;SMXc;^iu+S2To7YbJ_7t*V(Zd_JF|FCU;_?0P@QV3D9Uxv6qaULB|insS{s z5m_1#1QjYc$4d>K=YO}vJ^IGs*!#W`es$#=Y^lS`Y|F9^5uaPt5`Xv0VvlQ4nmY0JsRh@G zW~{2dXzk+65p%i^ml+>4z-QkFh8bOLEI5G z%}g)tz#E`J7Wn3i)fiQd8A~2nP`?l^9=g#yIxGob^488UsUcIsY+_-ztdGSU9F&dl zaC!bsVcsk$DN5YHjqxvV$ngYS3}iedXyj`gc`wfa=55eJZcS@?D;MLVH{aKM%W2c% z+xP=UfQ;!0;EkVn*1h8mX9Hqs;S@d=?S(hqr!O&2W>SuRoyrW`@^GskmoAg?%SP13 zLO`Dqzta{U7B3bc+5B`H-^eK%X(^IcGfAzkLMxYEt(Rm{n@oct?j|i+K}jpo_+;#S zYHWH~oI4GqC05X^-I^S``26k{+801Kba#SR1FwPt4b+aB%KAVs@3>w@vSWt%sLod* zWd!qYLG5O|zIG%fcRrLFYEr_gs=ghZ#kvva1AZKlNFh7ln@ z&~MpOxYR9?1~K_V?Zib%wEoRjnT`MsX1!809e=XH`($VMIw=4BHjO6w{)ian+dRBf}Xh^2P-&A^7P9s z_`M3F$zoS;qVed}RMWf%P_AEc;a;=kWC^>PU#dUFN(do-mDVH^0kfs!)YiyZ!P%G| zNkX{RTXxq~&h#xT7Ay64HHj83!YmqPYBF)ROgvBjurA#j$O+q9RE=Q(lJ0D>OoWt~ zICfQBd)Db6R^A)DV-5^gU>W+ZPu4TYBFF7c*XDLr>hRuQ7vI!y(3HhdSba!nqjYYr zc|Ih?sCJwqsoA*VYJDMNLWvEwusMQk{}$>K&>_z zOW|oWVkao(NMD4%d;qZAyli6@8}s$)NK99C5nVC6%(}(2i<*UXQuWAPfFFIQ!oI92 zlNB<%_G|qjNTdrA|A6IX&K(=sqhPKh)CM?c={TC#tj}7C^bcR^dDsh=>uY7y=j;Fa zCGDbMXw1gHwTvbxD9bHsUiT!x&2wbp zsQ%Y^&uc|7ZTk;&!K~#_P@}_0do$xZ;^LnQjXKjgmaj*QIp7PY- zt0dVlcn@?#O>#08x)k^O!nDmd1kQ$QO<+BNVwaI_M#n>-ZN9NR3Of$J5{Z$cOY~O> z1J~EQBNB<7?>sQ~Kx?Ya_fJo>7Pi9~#OXWDm^RM@=;Kez(Zy*2*DVbw}UQ(Kp9V{VPOMrO?(oS}>a{ zlWGs*(TBc2V$1ZVlbnbHNm*m_9o|n~msVsu-sf}a*+y1D3766BV1PdD-TA^16}>z^ zKJ;PqwsLYZ0E%|vc|7UCMi_KxZVX)>HCDJ%RMdsu&_83dVKC5A!Jz1lomt?yuySAP z7P^E+|4`7)HvumEK`|NeFmrva9eC24s6EvrhdFtJ^H5X*55Zk;5hLkAlNwP*zU3K@ z9q;G+#ZtF!oEM{rso~M=tku>$pR=z0c~zJAAIYNC7LB zO=7Fxu3aev_0ICx=i^nJ?Lk~>%0ITuajMeq@Mw?OTF3oTHSDGnuL9YmHVHW;_d~cm>5QqR51;0=j}b3} z2Y@(R<|tVH^0(k*#3w6)MEo51A-DwLjV{3c-&9d|JXml18~2CMrBj7>R% zH>V~-*8pmuzHp05Rvo2GloWKPssa(aq= zpzhGzG~&B1y&T0ihjngP6Z~q>1(?B5H3HGA@gi@K)XyxH2MRBrg2uh$3JNKppf#>C z;P8XvH<&2)rCC%oea{M&k<6wItAk%rwr#wkZF=fpe3#B7L80oreZz%UClAVY1-`QP zLJtpDVT$mL+iConV4KI65A?9;TB|}!?xaO_xjiws#eGUR0DN~yYfTfnqGLh`k7kGJ zbcdI^+@5`c3f~N2i_0X-;9o<-?Lgy=C(H~+6kU{+MYRg9IoSy}i%GO`3rQQEeMUJF zamhd!9`v=$@FWQsz8}j$)!_OUh|v*Fd=Cqc+!ISW`Rz4C@dAZ8ut>8UG2n8TW_*$M zN$oD+U>Q(g0MyQm$w2s;SXK>KZN+g$$u<7;qhPAZ5pkA=3ffY2Y2C;rQ5&3Fug}y%O6QUpOr$Jg7@*N?`cD`&J$Y?_Zfv<$b>9(HAF+^6f+dfb z+Ldc~x)l06J6m!lB`6kg_oex^)$Gagw>NIXOMA$gp9hW(y7bUL6O?bwiBy%Do|R%d zB>jt2i10ra`${gr@8+Lc0_F$`$|%NZCU8 z!bIZ8S9wGGUkSLYRO8s$o7yca9hxn*S`xbdGbB6E5Buk}TeDSR$FpS1Xx_deJ5n;< z`18fLMlgai*sFkjQ7w^3UH7DLZIn=9M5x9bZbyy}Wpk(64fMW=d6dgi22?8W?}87q zLW<9H2Ygv=Qp8FrzouI7{Cu>NJRXV@&e9mUUIqjN4m_qjVXcrBJ|Dzwy+{} zUpjf3_H;ul^ch&_!Bst#L%$^Rac<8hP>e=_JBT6SV-X}S~@_FgqS`=MfqT75sYQ~ zL`Xq&^fHWacH7m_#AGwG@m%u;DVY`KALDClKx9&v>Hu0DhQW{&NN>RO9QzH&+0>d9 z7JlEd;etw=u)@lKfKUu>xqRrVmgRFkci@iFbSo7aXAZPnFT)<1lWR{L|8PjP;C>kJ z$~?u<`q{HF=bL|CYq@d>A8HbUz*l;N9`DsQa70BU^$$Y)oHWz3?D`FROQ|0#%!kaY zBP=2%g*$q1et`D$7v~$lhZQEd|*SbA>v@OO;rZkv0+28_?|SR8g@^MpE6X*x*zg|U&0GJ=(AEl`nF zyzv?K``-|pq%~9T>v5H97CDxOm{amb$KmYE@bijFj!BGH53)X`QLz123f+avWKlV~ z`t%$0aH0CG1yx~G=L3Y1m1Cau(BlH+B>yu?aki~%Dt=*uyN7r~DvACkY81$>QidEf zLby1oZj0M;gGJspP@WJ`o8)S*pW<|{b5o6ms8=q0bLIE(G)EosMEJvK(G^E|2zEwm zIrc*!1hVBf&*Kg`3Qg-nE{-S(NAoz8JzF3FcQ{ri5_VphI}|dyb#*Z02}}eA%mYOS zYPSR&Cy+!5=F^Y>qEM7Wy@*wTEd7kq9~qj}3Xp}-zB0(Et^dvo@d!k6XCnxbT$eD}BTq%cF<%l-eP1W3ncp68^x{5Hs|)ctZ>0Jx4lX)UDIr)Yyc?vaj; zZPJG0O$tqWaDPZ-nt}fh@cTIq@GJ9q)BTs_dm0b5Nu&*GP)HZ z!_|)N?VviHzcT}5SC3L8%IC@+DCCK%Iq)C*{ju?g1_~GM)QQS)jyi;$;GiOSv|T@bg{yX%qVdKGUqhX5UYm;{&G`Z2x5I8#))TzFbRL`s!Gv58%4xJ%35^_NW1v3MJ zeElD&KsxX{!v5E6KkfI@GASV48S;eaa^J<*bQ=CRLSIP8{<>WWQf5Hd2)6DKsYm)& zryiEu`bcio#+A1h6aLXqqVWbu*-0T}r7}phgl=^CQ6-$wgX-kn^B!3zfnWVqE+7V; z=T2M?X}TS3@FQLS!`Kh%E~*_1hCt-^uDqLBAsMdY@ndS>k6oCuAp2Y$wf@lxl$_vPXX}pLgRnK6?H~@p^-MIcZS$4PQUSiJo$aaRoZ^K3Mf1& zl%C`405`Grr`v+U<+cdWSN^`|skz%X|JF1;2l@&J+t{1 remainingWaypoints( return {waypoints.begin() + static_cast(next_waypoint_index), waypoints.end()}; } +inline std::vector remainingWaypointsAfterProgress( + const std::vector & waypoints, + const geometry_msgs::msg::PoseStamped & current, + const std::size_t next_waypoint_index, + const double tolerance) +{ + return remainingWaypoints( + waypoints, + advanceReachedWaypointIndex(waypoints, current, next_waypoint_index, tolerance)); +} + +inline std::vector routeWaypointsAfterQr( + const geometry_msgs::msg::PoseStamped & entry_pose, + const std::vector & route_waypoints, + const std::size_t vlm_waypoint_index, + const bool split_at_vlm) +{ + std::vector segment; + segment.reserve(route_waypoints.size() + 1); + segment.push_back(entry_pose); + + std::size_t route_end = route_waypoints.size(); + if (split_at_vlm && vlm_waypoint_index + 1 < route_end) { + route_end = vlm_waypoint_index + 1; + } + segment.insert( + segment.end(), route_waypoints.begin(), route_waypoints.begin() + + static_cast(route_end)); + return segment; +} + inline std::vector posesFromFlatDoubles( const std::vector & values, const std::string & frame_id) { diff --git a/src/racing_control/launch/racing_control.launch.py b/src/racing_control/launch/racing_control.launch.py index 9a8677e..3c945f3 100644 --- a/src/racing_control/launch/racing_control.launch.py +++ b/src/racing_control/launch/racing_control.launch.py @@ -16,6 +16,7 @@ def generate_launch_description(): params_file = LaunchConfiguration("params_file") auto_start = LaunchConfiguration("auto_start") + split_qr_to_vlm_segment = LaunchConfiguration("split_qr_to_vlm_segment") enable_vlm_image_relay = LaunchConfiguration("enable_vlm_image_relay") vlm_image_input_topic = LaunchConfiguration("vlm_image_input_topic") vlm_image_output_topic = LaunchConfiguration("vlm_image_output_topic") @@ -29,6 +30,7 @@ def generate_launch_description(): params_file, { "auto_start": auto_start, + "split_qr_to_vlm_segment": split_qr_to_vlm_segment, "enable_vlm_image_relay": enable_vlm_image_relay, "vlm_image_input_topic": vlm_image_input_topic, "vlm_image_output_topic": vlm_image_output_topic, @@ -48,6 +50,11 @@ def generate_launch_description(): default_value="false", description="Start the race immediately instead of waiting for SPACE", ), + DeclareLaunchArgument( + "split_qr_to_vlm_segment", + default_value="true", + description="Stop at the VLM waypoint before planning the final home segment", + ), DeclareLaunchArgument( "enable_vlm_image_relay", default_value="false", diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index 7838047..e2cdf1f 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/src/racing_control/src/racing_control.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -58,6 +59,7 @@ enum class Stage enum class RouteSegment { None, + ToQr, ToVlm, AfterVlm, FullRoute @@ -212,6 +214,7 @@ private: use_trajectory_guard_ = declare_parameter( "use_trajectory_guard", defaultUseTrajectoryGuard()); use_post_qr_pose_ = declare_parameter("use_post_qr_pose", false); + split_qr_to_vlm_segment_ = declare_parameter("split_qr_to_vlm_segment", true); enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); enable_dynamic_replanning_ = declare_parameter("enable_dynamic_replanning", true); enable_recovery_ = declare_parameter("enable_recovery", true); @@ -235,7 +238,7 @@ private: declare_parameter("recovery_backup_timeout_sec", 0.2); pass_through_vlm_trigger_radius_ = declare_parameter("pass_through_vlm_trigger_radius", 0.35); - circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.30); + circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.50); dynamic_replan_max_consecutive_failures_ = declare_parameter("dynamic_replan_max_consecutive_failures", 3); max_recovery_attempts_ = declare_parameter("max_recovery_attempts", 2); @@ -303,40 +306,55 @@ private: void startKeyboardThread() { - if (!isatty(STDIN_FILENO)) { + int keyboard_fd = STDIN_FILENO; + bool close_keyboard_fd = false; + if (!isatty(keyboard_fd)) { + keyboard_fd = open("/dev/tty", O_RDONLY); + close_keyboard_fd = keyboard_fd >= 0; + } + if (keyboard_fd < 0 || !isatty(keyboard_fd)) { + if (close_keyboard_fd) { + close(keyboard_fd); + } RCLCPP_WARN( get_logger(), - "stdin is not a TTY; use auto_start:=true to start without keyboard"); + "no usable TTY for SPACE start; run from an interactive ssh tty or set auto_start:=true"); return; } keyboard_thread_ = std::thread( - [this]() { + [this, keyboard_fd, close_keyboard_fd]() { termios old_termios {}; - if (tcgetattr(STDIN_FILENO, &old_termios) != 0) { + if (tcgetattr(keyboard_fd, &old_termios) != 0) { + if (close_keyboard_fd) { + close(keyboard_fd); + } return; } termios raw = old_termios; raw.c_lflag &= static_cast(~(ICANON | ECHO)); - tcsetattr(STDIN_FILENO, TCSANOW, &raw); + tcsetattr(keyboard_fd, TCSANOW, &raw); while (!stop_keyboard_.load()) { fd_set read_set; FD_ZERO(&read_set); - FD_SET(STDIN_FILENO, &read_set); + FD_SET(keyboard_fd, &read_set); timeval timeout {}; timeout.tv_sec = 0; timeout.tv_usec = 200000; - const int ready = select(STDIN_FILENO + 1, &read_set, nullptr, nullptr, &timeout); - if (ready > 0 && FD_ISSET(STDIN_FILENO, &read_set)) { + const int ready = select(keyboard_fd + 1, &read_set, nullptr, nullptr, &timeout); + if (ready > 0 && FD_ISSET(keyboard_fd, &read_set)) { char c = 0; - if (read(STDIN_FILENO, &c, 1) == 1 && c == ' ') { + if (read(keyboard_fd, &c, 1) == 1 && c == ' ') { start_requested_.store(true); } } } - tcsetattr(STDIN_FILENO, TCSANOW, &old_termios); + tcsetattr(keyboard_fd, TCSANOW, &old_termios); + if (close_keyboard_fd) { + close(keyboard_fd); + } }); } @@ -404,12 +422,14 @@ private: if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) { finishStage("route segment final pose reached"); - if (active_segment_ == RouteSegment::ToVlm) { + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { runVlmWait(); } else if (active_segment_ == RouteSegment::AfterVlm || active_segment_ == RouteSegment::FullRoute) { - runSwitchToNormalProfile(); + finishRace(); } } } @@ -520,22 +540,13 @@ private: latest_qr_result_.clear(); selected_direction_ = RouteDirection::Unknown; qr_detection_disabled_ = false; + recovery_attempts_ = 0; + active_segment_ = RouteSegment::ToQr; + active_segment_waypoints_ = {qr_pose_}; + active_segment_next_waypoint_index_ = 0; publishSign(sign_profile_normal_); publishSign(sign_qr_enable_); - startStage(Stage::NavigateToQr, navigation_timeout_sec_); - sendNavigateGoal( - qr_pose_, [this](const bool ok) { - if (stage_ != Stage::NavigateToQr) { - RCLCPP_DEBUG(get_logger(), "stale QR navigation result ignored"); - return; - } - finishStage(ok ? "reached " + poseSummary(qr_pose_) : "navigation failed"); - if (!ok) { - failRace("failed to reach QR pose"); - return; - } - runQrWait(); - }); + runRouteSegmentPlanning("start to QR pose"); } void runQrWait() @@ -551,11 +562,9 @@ private: void runQrTransitNavigation() { - if (qrTransitTargetAfterRecognition(use_post_qr_pose_) == QrTransitTarget::PostQr) { - runPostQrNavigation(); - } else { - runEntryNavigation(); - } + disableQrDetectionOnce(); + cancelActiveFollowGoal(); + runSwitchToTask2Profile(); } void runPostQrNavigation() @@ -613,37 +622,37 @@ private: latest_vlm_result_.clear(); vlm_capture_triggered_ = false; recovery_attempts_ = 0; - if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) { - active_segment_ = RouteSegment::FullRoute; - active_segment_waypoints_ = route.waypoints; - active_segment_next_waypoint_index_ = 0; - runRouteSegmentPlanning("full route with pass-through VLM capture"); - return; + const auto split_at_vlm = split_qr_to_vlm_segment_ && + vlm_capture_mode_ == VlmCaptureMode::Stop; + + active_segment_ = split_at_vlm ? RouteSegment::ToVlm : RouteSegment::FullRoute; + active_segment_waypoints_.clear(); + if (use_post_qr_pose_) { + active_segment_waypoints_.push_back(post_qr_pose_); } - active_segment_ = RouteSegment::ToVlm; - active_segment_waypoints_.assign( - route.waypoints.begin(), - route.waypoints.begin() + vlm_index + 1); + const auto qr_segment = routeWaypointsAfterQr( + entry_pose_, route.waypoints, vlm_index, split_at_vlm); + active_segment_waypoints_.insert( + active_segment_waypoints_.end(), qr_segment.begin(), qr_segment.end()); + if (!split_at_vlm) { + active_segment_waypoints_.push_back(route.home_pose); + } active_segment_next_waypoint_index_ = 0; - runRouteSegmentPlanning("to VLM waypoint"); + runRouteSegmentPlanning(split_at_vlm ? "QR to VLM waypoint" : "QR through full route to home"); } void runRemainingRouteSegment() { const auto & route = selectedRoute(); const auto vlm_index = vlmWaypointIndex(route); - if (vlm_index + 1 >= route.waypoints.size()) { - runSwitchToNormalProfile(); - return; - } recovery_attempts_ = 0; active_segment_ = RouteSegment::AfterVlm; - active_segment_waypoints_.assign( - route.waypoints.begin() + vlm_index + 1, - route.waypoints.end()); + active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1); + active_segment_waypoints_.push_back(route.home_pose); active_segment_next_waypoint_index_ = 0; - runRouteSegmentPlanning("after VLM waypoint"); + publishSign(sign_profile_normal_); + runRouteSegmentPlanning("VLM to home"); } void runRouteSegmentPlanning(const std::string & label) @@ -719,12 +728,14 @@ private: handleRouteExecutionFailure("route segment FollowPath failed"); return; } - if (active_segment_ == RouteSegment::ToVlm) { + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { runVlmWait(); } else if (active_segment_ == RouteSegment::AfterVlm || active_segment_ == RouteSegment::FullRoute) { - runSwitchToNormalProfile(); + finishRace(); } }); } @@ -780,9 +791,7 @@ private: void maybeTriggerPassThroughVlmCapture() { - if (vlm_capture_mode_ != VlmCaptureMode::PassThrough || - active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) - { + if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) { return; } @@ -827,18 +836,19 @@ private: last_dynamic_replan_time_ = now(); dynamic_replan_in_flight_ = true; - runDynamicRouteReplanning(); + runDynamicRouteReplanning(*current); } - void runDynamicRouteReplanning() + void runDynamicRouteReplanning(const geometry_msgs::msg::PoseStamped & current) { if (!compute_path_client_->wait_for_action_server(200ms)) { onDynamicReplanFailed("ComputePathThroughPoses action server is not available"); return; } - const auto replan_goals = remainingWaypoints( - active_segment_waypoints_, active_segment_next_waypoint_index_); + const auto replan_goals = remainingWaypointsAfterProgress( + active_segment_waypoints_, current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); if (replan_goals.empty()) { dynamic_replan_in_flight_ = false; return; @@ -914,6 +924,16 @@ private: reason, [this]() { dynamic_replan_consecutive_failures_ = 0; + const auto current = currentPoseFromOdom(); + if (current) { + active_segment_waypoints_ = remainingWaypointsAfterProgress( + active_segment_waypoints_, *current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + } else { + active_segment_waypoints_ = remainingWaypoints( + active_segment_waypoints_, active_segment_next_waypoint_index_); + } + active_segment_next_waypoint_index_ = 0; runRouteSegmentPlanning("after recovery"); }); } @@ -1239,7 +1259,10 @@ private: RCLCPP_INFO( get_logger(), "QR result received: %s -> %s", latest_qr_result_.c_str(), selectedRoute().label.c_str()); - if (stage_ == Stage::NavigateToQr || stage_ == Stage::WaitForQr) { + if (stage_ == Stage::NavigateToQr || stage_ == Stage::WaitForQr || + ((stage_ == Stage::ComputeCirclePath || stage_ == Stage::ExecuteCirclePath) && + active_segment_ == RouteSegment::ToQr)) + { finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label); runQrTransitNavigation(); } @@ -1260,6 +1283,7 @@ private: bool auto_start_{false}; bool use_trajectory_guard_{false}; bool use_post_qr_pose_{false}; + bool split_qr_to_vlm_segment_{true}; bool enable_vlm_image_relay_{false}; bool enable_dynamic_replanning_{true}; bool enable_recovery_{true}; diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index fca25c2..9972c94 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/src/racing_control/test/test_racing_control_helpers.cpp @@ -140,11 +140,11 @@ TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached) EXPECT_EQ( racing_control::advanceReachedWaypointIndex( - waypoints, racing_control::poseFromXYYaw(0.65, 0.0, 0.0, "map"), 1, 0.30), + waypoints, racing_control::poseFromXYYaw(0.40, 0.0, 0.0, "map"), 1, 0.50), 1U); EXPECT_EQ( racing_control::advanceReachedWaypointIndex( - waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.30), + waypoints, racing_control::poseFromXYYaw(1.05, 0.0, 0.0, "map"), 1, 0.50), 2U); } @@ -163,6 +163,52 @@ TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints) EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size() + 1).empty()); } +TEST(RacingControlHelpers, TrimsPassedWaypointsBeforeReplanning) +{ + const auto waypoints = racing_control::posesFromFlatDoubles( + {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 2.0, 0.0, 0.0}, + "map"); + + const auto trimmed = racing_control::remainingWaypointsAfterProgress( + waypoints, racing_control::poseFromXYYaw(1.02, 0.0, 0.0, "map"), 1, 0.50); + + ASSERT_EQ(trimmed.size(), 1U); + EXPECT_DOUBLE_EQ(trimmed[0].pose.position.x, 2.0); +} + +TEST(RacingControlHelpers, BuildsQrToVlmSegmentThroughEntry) +{ + const std::string frame({'m', 'a', 'p'}); + const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame); + const auto route = racing_control::posesFromFlatDoubles( + {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0}, + frame); + + const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, true); + + ASSERT_EQ(segment.size(), 3U); + EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0); +} + +TEST(RacingControlHelpers, CanConnectQrThroughAllRemainingRoute) +{ + const std::string frame({'m', 'a', 'p'}); + const auto entry = racing_control::poseFromXYYaw(10.0, 0.0, 0.0, frame); + const auto route = racing_control::posesFromFlatDoubles( + {1.0, 0.0, 0.0, 2.0, 0.0, 0.0, 3.0, 0.0, 0.0}, + frame); + + const auto segment = racing_control::routeWaypointsAfterQr(entry, route, 1, false); + + ASSERT_EQ(segment.size(), 4U); + EXPECT_DOUBLE_EQ(segment[0].pose.position.x, 10.0); + EXPECT_DOUBLE_EQ(segment[1].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(segment[2].pose.position.x, 2.0); + EXPECT_DOUBLE_EQ(segment[3].pose.position.x, 3.0); +} + TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout) { EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2)); diff --git a/src/vlm_detect/config/vlm_detect.yaml b/src/vlm_detect/config/vlm_detect.yaml index 33ff4ed..3274702 100644 --- a/src/vlm_detect/config/vlm_detect.yaml +++ b/src/vlm_detect/config/vlm_detect.yaml @@ -9,10 +9,10 @@ vlm_detect: image_max_dim: 128 image_topic: /image max_tokens: 30 - prompt_text: 图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。 + prompt_text: 请描述这个病人的状态。不要描述边框、背景。15字以内。 result_topic: /vlm_result temperature: 0.1 trigger_sign: 9 trigger_topic: /sign4return - vlm_host: http://192.168.175.111:8000 + vlm_host: http://192.168.175.64:8000 vlm_model: /home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf diff --git a/src/vlm_detect/launch/vlm_detect.launch.py b/src/vlm_detect/launch/vlm_detect.launch.py index c7b023f..34ae249 100644 --- a/src/vlm_detect/launch/vlm_detect.launch.py +++ b/src/vlm_detect/launch/vlm_detect.launch.py @@ -35,15 +35,16 @@ def generate_launch_description(): default_value=PathJoinSubstitution([ get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'])) - declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000') - declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + # RDK X5 BPU server (default). For WSL server, use: vlm_host:=http://192.168.175.111:8000 + declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.64:8000') + declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='internvl2.5-qwen2.5-0.5b') declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image') declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return') declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9') declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result') - declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。') - declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100') - declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128') + declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='请描述这个病人的状态。不要描述边框、背景。15字以内。') + declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='30') + declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='448') declare_audio_sink = DeclareLaunchArgument('audio_sink', default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo') declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5') diff --git a/src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak b/src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak new file mode 100644 index 0000000..c7b023f --- /dev/null +++ b/src/vlm_detect/launch/vlm_detect.launch.py.wsl_bak @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + use_vlm = LaunchConfiguration('use_vlm') + use_tts = LaunchConfiguration('use_tts') + use_qr_tts = LaunchConfiguration('use_qr_tts') + config_file = LaunchConfiguration('config_file') + + vlm_host = LaunchConfiguration('vlm_host') + vlm_model = LaunchConfiguration('vlm_model') + image_topic = LaunchConfiguration('image_topic') + trigger_topic = LaunchConfiguration('trigger_topic') + trigger_sign = LaunchConfiguration('trigger_sign') + result_topic = LaunchConfiguration('result_topic') + prompt_text = LaunchConfiguration('prompt_text') + max_tokens = LaunchConfiguration('max_tokens') + image_max_dim = LaunchConfiguration('image_max_dim') + + audio_sink = LaunchConfiguration('audio_sink') + tts_speed = LaunchConfiguration('tts_speed') + + declare_use_vlm = DeclareLaunchArgument('use_vlm', default_value='true') + declare_use_tts = DeclareLaunchArgument('use_tts', default_value='true') + declare_use_qr_tts = DeclareLaunchArgument('use_qr_tts', default_value='false') + declare_config_file = DeclareLaunchArgument('config_file', + default_value=PathJoinSubstitution([ + get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'])) + + declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000') + declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image') + declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return') + declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9') + declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result') + declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。') + declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100') + declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128') + declare_audio_sink = DeclareLaunchArgument('audio_sink', + default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo') + declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5') + + vlm_node = Node( + package='vlm_detect', + executable='vlm_node', + name='vlm_detect', + output='screen', + condition=IfCondition(use_vlm), + parameters=[config_file, + { + 'vlm_host': vlm_host, + 'vlm_model': vlm_model, + 'image_topic': image_topic, + 'trigger_topic': trigger_topic, + 'trigger_sign': trigger_sign, + 'result_topic': result_topic, + 'prompt_text': prompt_text, + 'max_tokens': max_tokens, + 'image_max_dim': image_max_dim, + }], + ) + + tts_server = Node( + package='vlm_detect', + executable='tts_server', + name='tts_server', + output='screen', + condition=IfCondition(use_tts), + parameters=[config_file, + { + 'audio_sink': audio_sink, + 'tts_speed': tts_speed, + }], + ) + + qr_tts_bridge = Node( + package='vlm_detect', + executable='qr_tts_bridge', + name='qr_tts_bridge', + output='screen', + condition=IfCondition(use_qr_tts), + ) + + return LaunchDescription([ + declare_use_vlm, + declare_use_tts, + declare_use_qr_tts, + declare_config_file, + declare_vlm_host, + declare_vlm_model, + declare_image_topic, + declare_trigger_topic, + declare_trigger_sign, + declare_result_topic, + declare_prompt_text, + declare_max_tokens, + declare_image_max_dim, + declare_audio_sink, + declare_tts_speed, + LogInfo(msg=['Config: ', config_file]), + LogInfo(msg=['VLM Host: ', vlm_host]), + vlm_node, + tts_server, + qr_tts_bridge, + ])