From c30a810b12d08c493bb3b08f28323f167bc8dfd3 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Tue, 11 Aug 2026 19:38:10 +0800 Subject: [PATCH] test for gc --- src/gc/项目总结_yiliao_ws.md | 2 +- ..._binary.png => nav2_costmap_binary 03.png} | Bin src/map/nav2_costmap_binary 04.png | Bin 0 -> 6253 bytes src/map/nav2_costmap_binary 05.png | Bin 0 -> 5880 bytes ...nary 01.png => nav2_costmap_binary_01.png} | Bin src/map/nav2_costmap_binary_06.png | Bin 0 -> 5615 bytes src/map/nav2_costmap_map.yaml | 2 +- .../config/nav2_profile_10 copy.yaml.20260811 | 340 ++++ .../obstacle_nav2/config/nav2_profile_10.yaml | 36 +- src/racing_control/CMakeLists.txt | 21 + src/racing_control/config/racing_control.yaml | 22 + src/racing_control/config/waypoints/main.json | 106 + .../config/waypoints/main_1.json | 155 ++ .../config/waypoints/main_2.json | 155 ++ .../candidate_waypoint_selector.hpp | 89 + ...date_waypoint_selector.hpp.unc-backup.md5~ | 1 + ...andidate_waypoint_selector.hpp.unc-backup~ | 190 ++ .../include/racing_control/racing_control.hpp | 58 + .../src/candidate_waypoint_selector.cpp | 314 +++ ...date_waypoint_selector.cpp.unc-backup.md5~ | 1 + ...andidate_waypoint_selector.cpp.unc-backup~ | 312 +++ src/racing_control/src/racing_control.cpp | 378 +++- .../src/racing_control.cpp.unc-backup.md5~ | 1 + .../src/racing_control.cpp.unc-backup~ | 1706 +++++++++++++++++ .../test/test_racing_control_helpers.cpp | 158 ++ ...racing_control_helpers.cpp.unc-backup.md5~ | 1 + ...est_racing_control_helpers.cpp.unc-backup~ | 386 ++++ src/vlm_detect/launch/vlm_detect.launch.py | 24 + .../tcp_vlm_client.cpython-310.pyc | Bin 0 -> 5919 bytes .../vlm_detect/serial_vlm_client.py | 157 ++ src/vlm_detect/vlm_detect/tcp_vlm_client.py | 146 ++ src/vlm_detect/vlm_detect/vlm_node.py | 99 +- .../vlm_detect/vlm_node_http.py.bak | 165 ++ src/vlm_detect/vlm_detect/vlm_node_serial.py | 202 ++ .../vlm_detect/vlm_node_serial.py.bak | 202 ++ src/vlm_detect/vlm_detect/vlm_node_tcp.py | 231 +++ 36 files changed, 5589 insertions(+), 71 deletions(-) rename src/map/{nav2_costmap_binary.png => nav2_costmap_binary 03.png} (100%) create mode 100644 src/map/nav2_costmap_binary 04.png create mode 100644 src/map/nav2_costmap_binary 05.png rename src/map/{nav2_costmap_binary 01.png => nav2_costmap_binary_01.png} (100%) create mode 100644 src/map/nav2_costmap_binary_06.png create mode 100644 src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 create mode 100644 src/racing_control/config/waypoints/main.json create mode 100644 src/racing_control/config/waypoints/main_1.json create mode 100644 src/racing_control/config/waypoints/main_2.json create mode 100644 src/racing_control/include/racing_control/candidate_waypoint_selector.hpp create mode 100644 src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ create mode 100644 src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ create mode 100644 src/racing_control/src/candidate_waypoint_selector.cpp create mode 100644 src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ create mode 100644 src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ create mode 100644 src/racing_control/src/racing_control.cpp.unc-backup.md5~ create mode 100644 src/racing_control/src/racing_control.cpp.unc-backup~ create mode 100644 src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ create mode 100644 src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ create mode 100644 src/vlm_detect/vlm_detect/__pycache__/tcp_vlm_client.cpython-310.pyc create mode 100644 src/vlm_detect/vlm_detect/serial_vlm_client.py create mode 100644 src/vlm_detect/vlm_detect/tcp_vlm_client.py create mode 100644 src/vlm_detect/vlm_detect/vlm_node_http.py.bak create mode 100644 src/vlm_detect/vlm_detect/vlm_node_serial.py create mode 100644 src/vlm_detect/vlm_detect/vlm_node_serial.py.bak create mode 100644 src/vlm_detect/vlm_detect/vlm_node_tcp.py diff --git a/src/gc/项目总结_yiliao_ws.md b/src/gc/项目总结_yiliao_ws.md index f5e90a2..5e30e7d 100644 --- a/src/gc/项目总结_yiliao_ws.md +++ b/src/gc/项目总结_yiliao_ws.md @@ -12,7 +12,7 @@ ## 一、项目定位 本项目是 RDK X5 机器人上的 ROS 2 Humble 工作区，服务于第21届全国大学生智能汽车竞赛的**医疗赛道**（智慧医疗）。整体采用**调度型架构**：底盘、雷达、相机、二维码、Nav2 导航、VLM 图生文、TTS 语音等功能由独立模块完成，`racing_control` 包作为总调度协调各模块完成比赛流程。 - + --- ## 二、项目目录结构总览 diff --git a/src/map/nav2_costmap_binary.png b/src/map/nav2_costmap_binary 03.png similarity index 100% rename from src/map/nav2_costmap_binary.png rename to src/map/nav2_costmap_binary 03.png diff --git a/src/map/nav2_costmap_binary 04.png b/src/map/nav2_costmap_binary 04.png new file mode 100644 index 0000000000000000000000000000000000000000..f1ae4ea4027a469794b07dc772ead95c4c71c7f0 GIT binary patch literal 6253 zcmcIpc{r49-@YwT5>XE-OKH(!O?G3ZXu%VuMaK5XQkJp}vW(&Bm87VMWLHCCNQ`AL zqZCPu(2yBMiJ7s?&a@9CJA4c3r>izJBL*{!R&}Y>sb`*eU@4 zzy`|`7Ipw2#98^Q6$78Nlc=}AUqZol$BzJIor(%d>02pQyDSH^J}g0!}yt z1AwOf%120Xy_FRJCcra&vEgznW9S_h4>CXFlgGW%O<#qDMc?Xq z=mqQ5JyZ``^Jy1f?1jW#N$u;FS}AN=usTUSYHv@KAN-!Dr7J|b3+ZRAYMpvPYa-rb zAG(SJ2Y~dF92@{NeYUc=oFmc{*K__ z>A>9>*#F;>Kak_X0Zw}F9w$qGkcwEkj~>-BLM*k0()lJ>G^U=zv@&ASdImiGU9Va0 zPUAWlSD$q&2@vYME@uo#U)cIU$>qsr(>rp!49D>gy|A&=B*^w|Daq6`(*0S{T#Emr zUV-`E_%%3XJPjQeJ(h-NN#D__DL>cN%{+*YrrawsxsX_mITZ$pcIQe#%m}7-F?Lpd z1A)oYDw^D8B9&Ix!!^KHA5E0qZM9FwA~GPH9Wv!eV)!0o_u2HFwyM(@=_6n!#^<=Q zr@g2}=b2i;xQ4A^RC7#7eM5Zp==@+-nJ>kQtDM_FaBhc7Q#w-m+8CGfbNlV$Op!xn+2Qc~G`oxQ zTkn$J&BP*6m*Vh`P0c5VKDQDcbTds|yndS=zbCOnOW&I!y79X_ZErdSmhoJHt#r z_s~MQVyOlu48K^5+J0eCH7`q6wtgLQ{YB}Hg^>>+37C!7snSY~}C z0lMqtdgnZm=jFeDy0@pxU#;<^;2|wocsRpTf_>8>R{35zUlhpMe=?6H8N46`Q|lJx z?X?$|08CFlgAw(Wg#p_KYF*?`6`{QK;b21Y~8hz5QK6dG@P~2zvz6Lf5950E1SW=s;JcMXXClka(VB0=R&; z`)~d0E!>FhC{6^L)_7ykxM<5qU?1nKTkYNGwf02O++XIymodt#t#vIYfNRPlMs$ak?iFvbsTv62!hN)- zS@(a1pY0v$T!!BpSs8Q>o9O5}(+U;nL<`utxwySe8Eps_i#BYE=p68>Okfd~1-);8 z;kb1i537AM%)$C={JAT=<0Emur@`;DTl*$LO3#s)Tesb zt&YClxD;~`ieqK;uc8X6DySV*b`zCrDSkR-GS(~R&#Oon= zy*xdbUB*X>@Vd&D-HGBCo^j?jq@qWm+&L3Y45MG%kGLeDmAghPb`gDm>&V9vd-9aFWb+)M!>bZfpmO7- zsIg)Gf}n;aL~1BOV&QKhkUcgf*p`XD@{uCwl6M1=udn)Qi?*)x?C&lrVdT#l4CRfLaQ;Bk$5`$HSh%!f4t+#Diaf=NXL98`&@1lyA4j z7~oj_jUAARGyx#%NVV?cs+IOW6zgIYs1XL_lw#BN8+3!P#(A$`3^*UWAq8Y*gXjmh z4y#%NLgQb36A@-RfXSWbKnLY3U^x8hAfVXxe7aVDH28(yUkS>dH$nlsXoFsg<<9p< z&9>7Tu|hFwGOLWmZcRj5;6|X|{uzi;E)Snc z`~WTqj^CgfFnzWdtV#qJBeqkhKi+`}G<~v^r?*Mf0*3cV@MlAV2#_w>J}4%G_XA8A z#M5CTPvhtW79K=dfOMG)`JBy3T zP>cm>3@iVj%l_CLO5@`#@XjNw*a^xV*gpYvKr!91SD`L4j zMHjYdO{?$W+b7LL3#4aSG_}qCiYFWdl zxMGWby7Q-*NgoXP%t-X&h)OG$ib9R6Dt-OpNCy!ppkkvOmh`1PU8_h_?6|&-*M9Wt z74T-EmLpU525P+j(kMvVsxdJ4?igP|bFsEjvw>!fLJ{3e``e#4f2*~# z0`HQ`m^c2>?99t1p~)|nYMxM`z0%2PnO$o9IsrLSOpi(+ooEP>b`Y(@h3tz(>{nRb6ay2+NNi2P;~S z-mJk%T8oI-)y4P|#%e|!%t}1@0W=tabK~nuUMFTU*CKFvLEw>%8@1ocp25Oq?v^*o^H9${l*ou-ytQ(}pi*Dns42tXR1+8H$`8e;|S1gd1Qy00Z$foTK)wHQU(wdiaIxXzLarqm5=;;n6dF;CU4*z`4z3y^uioOB7QXczo(s)M>D~B z@~a^DAKK#_pv1+`H~iJ=-$$e;`Vf^*A%pB~&=L5#E1cGvoZc1*mxn7!t&CDBkE~;a z8#mIQkl^GSxB%95V#X>Q6+Z+lL7=;vbaf>yOH8UPMh{b+lxlYo3(v+WOn(tIF23WY z*&90vF1imc+S$dpA%Ryh^Zj1m(*Ah2uF;`&>3p<3Sge+WKmdHHIPJ!Zim+qC*_sbW zr99NSEXz}6o3*oW3Jn``4H{+XPl`gMR*E`gxUW;e_-+RHV&Du|=mc6#9qX-0xaDMA zs3RA~xZ&1i#d&f0@D0sw#4C`PV@U%reySR2^r25QcXhs%QCAi73H5 zV!@z2g2yYG$PqaC-pO5>ywuWVqHPl$#U49pL5Y@bA-a@gzqi6*?;L+okDo5tD}BCA zL$&M@zqR3&-<8Vg`9Kk5MO?p}vR0UgZ*c=x#eaP=Oa3VzUe=@x9p3B7h;d^qK?JCAk=;E}wWb^YuEikt9N%D4w@f}f&9c@e0<-j5bC0@89+3xx% zCDqOfBq8--tS=yA;R<^dzjBGbP8)g?&KPwK8s~C3vo&%R%QJl)-)I_(3j{2w-%r27 z1=?IC(@E-A)k43{6>y5?KbA-T29m$TiHV7)SY}x{+@gNLH5EsdKb5C4wh(`TJ8{@K zXc9)PZ^d<>8Xbub?5rFy=L17Ru#gV%?biuQv;H%#%d@#Ngk?4%5Q$U*?~3FOuT6b@ zeFhNo=8Ye&K6`ToZHTW;-CMaMtImyRbR?MQOx4_AsR3Dbk^cHc@mZ@#@rHQ)wOn1- zAv@`SV(D^1s_cZ4_%(L;rQ|wgWvYNZ73m)tHPxuNZ04s=rd_$Q!^{aeAQ*xtzJOx} zG7bE{4AzI)YVK;MT!=$p| zA?>r>jIv42-rWW28Uc(LkK)!#ZYLt65ct%&kU`ArQ2YtsDI*(45kp&ZN|n?3l5>)o zJA9(Dr<7gUQFDu}IGDB3vQBRu`irJivF6yU8)PG~5#oz%coRu5jbHQaB10Y;gI+^% zCRQ)HMm`vmC&F#1c_gE|R{LZ|3nwbe#{1;XC&+2s&{q%LXX^}qczjajDG_3M#d|>u zazmilUXSvbnxSla^>ERwbKREa25jgi`iO-gjUk+o3TI|tBk$R?j~4jR3su}U(L7pt zRtmYy)|nhp-aM$qw3B`%ND;Uk+^I6L!}~))lm;=ho0a%LayV|PN0_GI<0Lc49JSzl+nB##=w$wpM9ow3jh;xp;-CxFTIUX3H1E%X8f}$|kr-8aEPjZ$s zQh_GTFfV-Wb48#@sL=jCIKu|SwrEqucwY;cMe6dv^Z1z~24wQl+*(thNf(r!0`kGx z1$J;kzfL~jz?Dd@{LXP80AI~LYyc9;@E<}|NOW^3Fnq9)BZsx(s*?*!_3itM}%MMRj1f6ysQ0fR&Ze^Qqb^ ztPE%#NvvLSNH_V3{D7g=o_d1?#RU0PNiC1m7Ea65e;q(8)dz-UFN2BwP}rLEyBvMP zIxmR;;B-+oDg_k($c+q+{1ir_IG`C1R{9r}uJ#l&UJ7XYu{7i%Whv^MPw(E(x0pb{ zP~iv-qX*8}Z+{l^z=HG3t4IZ@s^BFm@>6UME3s|Vp!}u;Xf|In!~NpSPXuY2ya6yo zp*Mqbe~jM`onio#F`64^*rb6u;|icDd<))1&uh{CZ`ID5;psGM*Qyu;|4Xw=txF41 zKQ@Xx^(g_JeGhw9l1ccH%wO(eVAm-}r{k}uXQ5#2sz}U5$B~IUAB1As(d~bEv)@Y; zyY;hvF%ZBY?Is2GoGSl7tE0!`FJ&tN4GnfysdfdVOvU@h{&7HBq4VE5i0)yxMVs1F z%>H?;T%SzEEi+b}Kv8j%P(XxUK{iejXo@|g^$St?&kKJlGurD308(eIOGgUx%PORR zbgNrG9frS)E~Vhe`L5%QXA3Ggm>LFr`|z`95(~FVAi*tO&$$A_FG_8lfngo%6vL1g zLI%BJVAOKPS=hN*f^&pn$L4F#LK06}lrkaONk!bit2{iyFDn^Yejva*chG$l>n~hW z)%Ce_-IZZ)O-!9IF!|pl;S1>(j_xaYI3y2IWiQb9%FUC;#BiQLt3{C~Uu4M#Gw5Vc z_;iEN_BB6F*Rj{3ixf~C8b0X#^Qn&^7ik8)7ikLLkM=9ywi5&Vlm1LHr??Ork0r=D zu&xX4I&)2^@$RaCO*3eA`<7Kte?&qNz;MtnUYdAgrjs)SIhm54H2NE(LNJOt@Erax zP<-!IvTt2HNFFz@p6nGRT>f1+I7{CCiBk?AF#?6k`#(lZQVo<;Tc^UiQ(+rzN&p)~ zs}pPwkAE<7%D435``MG*F5Z;WVCIm;fUNYuNeVa{?^q)5nRLNjLeNvP1SyIh8-T*gq1 z%S?t=8KszV84}|XrZAIShM8vO`KZ3N>lKCj;&zj?jpHS;~+^F8N$&*xm;=Oq5@ zYPWio`YH&5Ry)|+!XXGYxBQip0hG2L)>ZHW3xnHPLzV3s@4$srn6<+(IqwK06xVMGn$|O269U_9 zBM~ZVulU{ZkUG`V-fLB}!eIWLKgo8z%C{1im{mr|yt`nQ9=^jO`RG`%c}m)OoyaLa zGgL5r&PkpdR=2c|E*1^DCzaV%(UWoxQm9(J!~4paYraa7}p* zG5O{Yg!{4NIr6fZ9;FR=O#@fr8Lh2n%IIisAJ>ygC;qLO|GeRD# z!!t+KgS%)t$GbyIMvCeiajEWs4dj<~H;qWBhSw+*zqsyD@UHt*+o4HPV}A8E9S-9k z&+3&OC$N_`mB617@uFK=!ZR^Z#xd>f?P})9G@!WkR^hPA28weFOPNn=oS)#41+wtK zyoUt?liaWvXuL3vrgHS4#ixgplt30&Bnq;-Fj~Mx4Pvej^e5S9;OLn|k;_`PpeBLK zUka+Fa=Dz@Uc_7Iz~w`3S0y2ij*Ei%38p+mPbbFjb5TGNB&$fyeZOlnEZw5}mww6T z$>U@*bf0fsBOPr^>hThvn)v-*#0zHmJv+wDop;^K_2Hwx8`{Qe}YOL|&`?J$oa z#VI9~lhvaG?Q^_1mwI@@nwu_nZK!-&0afn6miY5eyAZzj`_36iGGy@i&br$NX&7pQ zqSXee=)EOriywxYR}2dv&MZuy7?_5V_UP(oBLcm>z2(AjEoTaK46hfSD3b^>jK6Ij zAiQjGX^{}8#904Xn!diiMxNxp!vq~)MNNXsSqUT#a~k|xW*lsc4Q{i;waCq#x?U*x zz~L)TLt6KnL!Q@pT$Y{gp`Dt9&5>7~TGyLe#k_jJSehCc`IWN;`p_yBZQarVd%cyB zT1sv?eSZ)3v(D?gWzVEqxs3}2yd#0#)1PV2j}g&zn;66V=x z?%<)z*&K-Y9k_uNzRvD})QiX6#$VhtuE9v3xdG&`#3FQs#{yaQOcJ;PH(ELhf3e}_ zq9_Z%ZqaOk*p?a5^0IIb6{i2Z|LwWJWet&}B)>~j!{Mt7J`Q+9>241oS~na7`ckwk zfu(&l3;{^@f0#>QkAuWc2{yCX-CO%dZ>_**d-y;hNnCm}T13ZUG8Xu(#RKWR8=I8< zHt2UCJvJ5q6Gle1P%5cv?|n>vC6Nc^=Bh)PLK+NnhAFO|kaTvB7RWs75?anpqS>&{ zYUSRI{H2uG*!O%J7OC1sdAE`h&NYI%Tu*4SFr8b|IR2{b( zEjEo?>vQwP=@?rUuR%V1_pO=y5>+`RY8s48f)7=UZUZ8yBA#j4aJgD0>EE zEK5l(__Phn?<_d{_(fkOHT?;O-nWt3hMDRmUbi`s71oH2A7mBkwbHSo_?iTs9=Eqz zE7qqr9ldx?nYo&{K;vr7ReNp3=RZdVGU(|~F*`DCb`na^g6R`9UzVsaHkKubnWCG` z@wE6$2j-?zggkMHpfN~sK=l2RSHINXwWb@|jKG2CNGT}G<=gU6Dlw4V44rZRwm_9i zVBSp(%HVt*)`D8MW2F{+ZF`;_NC?WZsYySw+#{`WtPDzeK?RE`C*OXS^tl8YUICQ^ zL^X6R6{se@ORj;2Xs6hfjEc$MVeR3LFlAUDd@|H?JM0H6i>KEbBR|Qtm$m) ze(nwzf@G7$l_bA=Upx15eg9nhkAc@DI;ye}6VO!iE;Mv9^p$k2d{={|Iz-GV5&fvSxM#kawyzU zw*iq;3a?~ue%T>e{>PS}aNH@=NjHYv8sE=S8>Ak* z#Kd5A@1kS#&%)mnNeZW?rUq+YL>5bjoEv?8@69Ve^zl4XcUCf~(X9#lZbBg9VA9}l zzZSqM?zV2^X1Qr%ltWG(MLwoSG0ZBDic9XXtyumUftn@2o?(UzE<+Ea8?d}xMiv%ucZv?J@ zEx-zdmg1$3ADnDtns1-}t}XbKz9|E?uqQu!VY-&per(OXtk+%V9F{$mIs)D9LXgOe z!tL5mU}qKYG*3|u)w-J;abjIQ{c@7AwG!3z1t9f}&<#9q18}s+Y!t1H1qnE!9J>h0 z_HaVv&Nl_ZDgPNDyd@>})94RQk4w!gEmOdW>0_U*$PzEoEk~$a+AOV8Q3&5R`tjoh&Xy1ME&iRqsmX=sRQH_&=V4dF{h?u#D&95P zW?h9^G}b0l06J4Rxl?1AbPZsS#{Txa)kGEczQVMkxIq=X>oqA;7Zr9;Z=Pc9oKf=! zhLq{McUOYkSiHgDoUM$rzdh4zbtQBv7AlcbSRX3n2A)JI!smK<^#WMAiH|Bih5@J&X*_>weF5;JU_ z1_s3NL+r9pjdSJCo8DF>%-27o2wzHz|AEU(rz`DhNHN&vm^Tq+`(KZe^1H6ALS;$zeX%q(N6!lHAr}rWC!e=L z*3~`|IG2CH`}kwdxMFt+O?$gqvt@^QI3^;>JxH;h>t}k=I6r7JumO{iPGrUr9 zaerP92#vzug5!BPzzgsVJ3vK`s5kyIvJtp9b#V6{rA>x-SJg=Lb;(wHLZ{UIH~1;w z#B_KOS5SZL(bgGk=MLa}#DtXsAg@jx0p&AWK0Jqzu5_w4pYrI~&GEVN)>2<}-_Sdi z+!*5A6-Dm-v8xYG?`6!!YDh(TP}8HC5r01XZqOf5(w3LjlFmQwgE2blaPeHTBAOV6 zFzmA~$Y^Mb=1id3qXp4RJY%KV%E=K~%SC*~iLF;U??fW~m(*Rnl+)jE@|jdh;*q%- z`q=4`C>}Q@rAUkNia5^^n0=bL-A=uPj@#RPHA|m0PiKkeb19
Ay?l`Mmwc}7V50H*24vo2V3*DO`Q zi>I@Q7fDpnr#m7VKn0Oh@IDik;;2t!2C>We{5ZfU0&39}09;tXoV-hJ>Ad4st26cz zaYuS`0GaMl?heRo3TJj(k%(i(w(A^kSvy{5trdIsw#6wMX`wiR4f}JE7d+W+{&q$g zgY`A`=@3%h-cf=U%s2fYD+)?!q!yoh@9b5psdD|qct?R{%8S#gmY;@|M~xeoTs|#z z#3tWyD+tIlI!Q(xA{y~&DZ#WQZB#?_S_~`La@|qcp`!~yH=O5HEEaCO6?KT~IxCbg zmdQb7kM8X&NrMCLF-bmj087qGJwFy_E|kg~d=mA3tY9y1mMR`)s#l9=nBBsZ6N{r_ zzzcLw&PzIP_@(-a8h~A*h1$j^!Lfq=i$Z<`z{Q7|5w*N2YUH@&jYnv)5cz2)MZ8uluoQ2|%*>ntD6se3 z{?MqXsBXh9-gon5h&LWrn+-D8`0cxF*u(wF?#xu-WPj=A($anw8=sqN6M^2;En>(F zV_SH}-ICrE152##_MTdep`?ktfw6myf_be*nlUQHts%3yLjZsm)rCdxzsBq51`>~e zGs1wd0q~SR7$;>S9Wh&PO(J8J*a_tvLMW7k6(fQ!It|oKDfJ3t9-e=oqLY6T>4~~N z8&Y-=t?8Dw=E!YM$a~zWGP?h-_Azylmkj1ReVJ?em$s05;yr6*s+=+2WN)Bl?xqE0 zabLf=#n=*i0Ud$Qxc6yKS9R%#t5T`!`1~*CD$2s>$bkX9!JA`0?Yg1kWVs~cJn(3q z-j^XhN_us1wql;6+GQ#W=D;HF2ux%-=cq!tZIU`>T8Em3&O>C`O=;a#Hg^bmFVc!p z6mI5l$6vi3t>1IGdRDkw=ak$mV$z+UtU6rLDZN)0SzrC3hT~kUL3h^_7CWJ|TuJbs z=9W_ERVgt)hu^O|YAz>mj)Wyx(Vr<)d zVk{~MGC4wZG9oizCRa;3%vyIpekf2U9Y-crcGbNO4fNB{#K~@UN+tm~?EOOaT}I;{ zYoNLpok@*W&_9`|mfdKO&18dz~*ku-+7U`?bV}kE+&4LU$hZkf0maSH@h%bfvFhh7xq= z)_MN4pWe!d{MT|Nq=>=^yRPu>)G~`a1#@fAPgecK_wAkruV3WMS_OGNh)8npLTo9z zrXTP#j_a}NOf%^1mX$sdKG05?b(y}^SW~tdGLimf2~@Qj>|Dt6 z=b)y0-2q^LxBm)1du5_^#}GaTpb*msE0X>itD6!pSb!Heywd*I{Ji@GB*%5gw5nTX z(5`;YVieIG4A6W3we95+hCl5j_xxA_gtUIe-E2yp`^3%Hc3MWcYr&y3S8YUoGkMqG zEh>B^p$t;m2pH^dBz<*qcdxB&tna6XJ(K|*HOMnV)LFTwS^Ukrfd$u))M^2J4#Qo3 zHg4tFD*fuKa1Z0kDiSc3d7~5T2%yBT8-xGwKcl-Q4> zZU1#cp#SGtIBckk=u|OURycU*X5dR@OuOk%(8Wt7X+M_OGLd69_DTHR0uD?fxK3jf zowHsZN7unXf48ISG1A}q>)N1v1>t5BZHbduq-`Em5o*HkGx%>1g1zP08$SqA>Pq*d zyW1drfxpwg|J!AtClhfit^9@G6>(VDAR7Aka(PFxvOC0ArmZ3OBqU6Wp{BE5V163X zKbmS1aSwL9O9r4FHPc-P;atQ`wTx^D9=WnQF`+wY=arec{XMt2PV|-wQ&dt@X#*Oa z&h%GZbXL$*b2%f)3k!I$78+9fk26@_RpKKO-P7(}9hfb$JB5m+arZnAn#@JVj%S@g zp<)S0a!)zN_v&jE2>vzdC<(pE1!h{NP_FhjX&A!qa!8JXJa5CFMY7hPmHxXqvx+l1 zE1E{8{rLO*0k+SD{-t)_aF!e{$^?krwsj>j&UJto%GLiyi>D8a4W(_biq^KsQJ|4v7_bX0aUdZY^GyT`>`pj2hBa1t=!i1lnxQ6u3Qx-mB=vY<+ zgYQl$g${`!LD$KmT6qHs*j7Uhhg@wd Itxx{;Ps{+D{r~^~ literal 0 HcmV?d00001 diff --git a/src/map/nav2_costmap_binary 01.png b/src/map/nav2_costmap_binary_01.png similarity index 100% rename from src/map/nav2_costmap_binary 01.png rename to src/map/nav2_costmap_binary_01.png diff --git a/src/map/nav2_costmap_binary_06.png b/src/map/nav2_costmap_binary_06.png new file mode 100644 index 0000000000000000000000000000000000000000..0a93bb88ffeb42c52dae82d8a2fe636052dd3399 GIT binary patch literal 5615 zcmcIodo+}5`+f(73eiDMjgn+nRE{|fnhH7WbkdYlNFj%u#xOHdJ4&dC5HqDAF%CO& zYKGm(ZevV2OwJ?>!%UN5m@zZotG)H5wb!@S?~k9gW?Ap^Jn#KJ_x)V=b>Gi*<5$PS zGOLtV0RSLlYXf%%00?LCC$$nBY3dxf3I2nGIv=(MN+_!1;Dbb{we4{!@GnNnI{|!_ z4z@WR3IIBWi$93!YCAgs*l^7jZgm{_V5;v(pen5^c8GP{NK-}0d!uoJ0bA^DNbID# z<=fML;LN=tnLc-S`=lL|DDxT-6U*JEplLU0B8q)_@)=V5d3MVLk1`EBNBWj$7gtdV z&uA0c5&qz|I$B5o&@f~N-;^uf@KWiaxTRQ}EFdjq0&G*K1Gi)G|K9^;xzUjPcEp&Q z1d!nTS8w7Mb&W8<1`mI2D|;oW8De~}z{J_eS~6BbLJd$@3r<;5A?gsT9|k=3TESqj z6ewoSlljIapVxqm#|Yc8jc&pkk@&`I}3Is|{ZvXi8{KJ=+# z`u=(U8rVHgaZlvdx21ACa{csF$_cwJ%v9sTqe-%;ap4@q5`Mr6w`%^&YzZUq1(`n| z*`j=FfXx5O<>Hc-;TSzEJXoN^Hy z-@WdiFkLxr)mot|0nFa{g$_`ESqhrXI*#*A{~B>>u|7DIuDJ>|wj&bAy0-&dq_ebW zDb!c=txCi7xLbiT^~%oUG+kTJ_=U_RSQL-B0l@tO&x;b3|Ig6Te zwM^}mQv;0mFG2A3Whg2sL76tTG2)ItR2p#dGL|%NB8M$PM14d`4aoOa%9OkHGa?ot zaz+1e4Qn&eW$Pnjk!q}@@i)5{57E!|1Q@b2>K8PHbB(aK@p8~dEPjomG;%8;ZR*L_ zy)5@q+EM?cmh?9C(joyr**8F8}IO&T2U z>u9>;i7U_dU(Yx1u78M{S}v!Cceog*#3@5uv_7gF(BUQiRu><<#8w2t1y;CZKV2oPk89~|Qy7*y{fQ5Y@o+F@B@At(fMrHS2+MSu0m`Kvu zHQc&6-G6T9fZDwb=SQ8tOvr%2wZJTF%5FJtYR%#cDX_^cbFSloFD!%~+1LLd@4SI7 z5D~8hnsC_ewS_p*$CTDWb77S**DkG1dZ_aFW+r0$(tp! zR-oZj+zq^Vuzgn?vv>Qy(Cc@;1@Y3^(whhX&z5t*<5jh$@eu0M;~;gMl>Vo1SU$z$ zCd~aHgCCt&r~wGgr6ga<>_yrKJY>5lxArZP`yv)UA((&}f@pBU=mW_0i1Sbrz~T45 z(>Bh{-=V5<1(0%YJIGP{mN?301IK=Bomhj#f4{N12?*XMc+F4J#@#)HOFSvw1Rr+07*`d%OyjE>H)`N|!s5h0pk1*)1!4Kj3mbpwX28dN}D- zU1q;=$(7r`(7W=TK!X0Z3vh>r#(>FJN0~swJ6lDLx5`;S)s|?`F&S8k7jN+L$%;?} z*~`P6D`wU?KOY5$FHHKv>bPF}E?qRwF6_z|hko#K@{OVlB@{z95?*tT87A8E4FJdY zOjJa|fNPKnc45vd(!HLAP7&nRZRQIKEQDm1(Lzr+-{B-~;`+;ThvBr3G7 zwIJC|()jm6?N7roZIb=If(*MXc@w}a{+QZB-O-B5>FMdYp?K)1+Zw>w7W`P5B*@2u z7bMhd@>qO2AH(O>gZZkIaedA8#H`&{v?JA%hR&<--F8UKdj5rWBBUW_CP3nW?6M)w zyp>YR{ZMqO)X{KMUdzlHx~Wh+#pfDjZ_edrjg7&cna*}Gf*BfnqN8Yz$0!|hQ{70P zVbIt((SnwcQviDd<`GEW@fzK(Lzw9nBla{-WQJiUx;IzDcl9x7rNewyeG0AAy_K{h z)zN}3Pm`l7Y_@kirmab#Q0(6rdT_f3L|`TORL3Guzi3-C?W{jMG2zENIGA~hmA_*5 z+OVwhu7FWDiR6F*Q_2uX3skvy;Feg71^Ie0Imok2D{78LAYRw`#Y1 zn6pZLw?C0xHy{XPzJVAkgKLJiYPh3kzyQCzBhy;Bx{12gN~_YdduDR7c@r~P_@++hoGZ%z8+ zvGv>WE5Pj>Xm)KP$-D}kL@+CCmKR>A>{yu^-a+q#tYORdSWF3^%{dx;z4O$%=gLMGQyo+i!C5ka?C z=#E-O{GF+VH4;0htH4+w7={y{`d7B<5f#2QN>`4n0=7jUd>boFcPoTB?k>5=)X`*9E-}T=Zp*t7>~Nr-Z){tuG6SGw5G~w zs+KbDukT}C{5hgDid2;SG0{R8T0{B}GdJCM=Dic?194`kG;)rcUCa9VYG+#E(bt@> zeG5W9EcY0zUg>jBQ}py$4|g6~kL5DlvBXBKsEgH@0Bnn$~2>Cx(mPjMJoENEPpDmK83yNS!c^@eu@g`o&32RB(_GWxNw9d z5Sp-sg1Fk!@eLnesi+d{FYU1x#K>WVUj&h-UKsT)%okwlYfI|bA)rsX*!iY8@T1DU zzP{xMhfA^L-iuAy3`(uC6UqMcW;WmK5Tn9_6n%ow;kSV?V^Ukfk6C+E6ug%A-ZOM~ z!zl7M3t?UsPebJ-71sGsMQXUU-rt0Kn}wyf8b$Egy+L*uKXH!%nY$rhSUh1}L7GFK zHlsu)j{WCCqAocBY!qRFai?LU`gw?_&ZVIxw;j{F)=D~ZS%ec+$DdZ?#i;hQ$Bwk8IonOu>h!;eS_isPC8k&TGFJ$3 zLf;G>Z-V)NC&?bD?@SO>)tA%@k=J0tDn{+c0K;-O*dLx64~jkEZgw~71ye!no&llZ zqLEXlPQ4HB>thrtA1xIr5_(vQzf$FQ`B0fsXJ?yeJVXn05~d58n_pKwfNzL3=d418C^c(o=~&_db1jM}R9$y;Ndw z%?@3dETmN(t9zVpA?#f9`Saf6a*~C!@v>4}Jt69tUIU>{#LsX(hi+WiKUe)M{_Kn# zOgP-g?Hp!boV|>UuI~zf1-5ZRLgq1i6w%->zsje&?&Mown`nmG0!haQT9i`#i8yh! z5OXzsSt@-xs91*OVw5{8F)7q@t2&gh4(4HA?shBMxDqLM$tg|YXg+zcedW9MTdl8k zriMtvrOq%k3vn*`1z2vh9nXjxJD|^|oIP1|f|G4gI~P_F9WyacABJX$?H=K9A0|8O zW`5S`c;^N+6lb@m_|nYHK9NXf@hhS(k89y#3w491`2iWCw&D+SpH4ISpj5i54IwSz zviVFGvDQhWSi!)LQ;B=v++SI7|L`QVy}-ie(gXv>wcr%Yp{ksS=DBC;_C&Zrlckl; zx%rc1dBN%rro*n-+nox`L7s_}eXKh6G`atSraROTN4QLHh4sgydy^jZB$^srEfZN` zTf6>@KX2pLz5|!;8%kBZw zR@Ysd>r&XLjt;tA5@Tl|aq_U_pzVcsZ1A3v$QZV&ZG)vM0M$)N8Uwy(Wr0DU?$llI zn&*PwGTMV@ooz=`M#O;GJ!k3XqwDCI8Ik~UQ6yOH19oWJo|Tw##YzZ)EaABn6W~F~ z6q-79Kr+^BiR|$z17&J}%gPMQidgUOgFWUcEoATu$+hEEx+*_i9m{@o~wTh@~U|m`D)6CzBMVPz~FAs=KeO^+26LAC{_Woef)-OPrl)=QT&oSn5C1cm| zklm5Mp9h!fOl{!Ui)n|cy!w!nDnOY8*wXx0_z1WPo>GW~3J~1w7Y@E<^Zl*oR=R}V z`-zf3`YZ4zx=fq+?J^9kEQh|^$BZ$@ias1v)I9rM4RGvX3a1f8U`OTsqr|dWzZXmB zR3@wk%iNmpfnr2ZOa|I5cu$5(z(#A$t9klQyR&%ExDt3kcD~66qiDDk8fwQ!I2`LQ ziXw(b=ePia6&zfNvfhIKwdLAQgP0601#C#bi@5sgrSj^M;c`&B=nN~^H4-p(Jy6gG z_TUQg-zw!#%FDNVcFEfla1Kr=4y5BdQNsf|7JZw*^Vaz}=HKKbOT{6>LZB>+p}?r# z{3P=IXbnAH*t~r|fW#2eb`an+*GQWrV7a{W{r<5S6PH#J&}3wh(r2guv^t^x=Tchr z&4d#bQM#N0k8G*f`EdDgrVjwfdLyO(Mc)G9^KG{Mo=fA5WuJ!{kZ$+pU)=rPbGXnD zd1;KqrZu9`Q8Eko?Db=4El=6qK={L7&$dTWH2M4(o(3MzY;vexS-vPLgWlDZ5An!c)P3&$2(sV0-I~n7BX|C> z+xzYE<{wQjlF*3RkD1nqbD#Xg!_aBh_sT)UT(uxv1n5*xe(1cn)kLP mv`o literal 0 HcmV?d00001 diff --git a/src/map/nav2_costmap_map.yaml b/src/map/nav2_costmap_map.yaml index 1f1edcf..da2e8f2 100644 --- a/src/map/nav2_costmap_map.yaml +++ b/src/map/nav2_costmap_map.yaml @@ -1,4 +1,4 @@ -image: nav2_costmap_binary.png +image: nav2_costmap_binary_01.png mode: trinary resolution: 0.01 origin: [0.0, 0.0, 0.0] diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 new file mode 100644 index 0000000..dd59fc3 --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260811 @@ -0,0 +1,340 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# 改了控制频率，微调了一些参数让它不那么喜欢倒车 +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 15.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 40 + model_dt: 0.06666666666666666 + batch_size: 900 + vx_std: 0.24 + vy_std: 0.0 + wz_std: 0.48 + vx_max: 1.00 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.6 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 11.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 100000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 11.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 10 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.20 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: false + width: 8 + height: 8 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.35 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.15 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 25.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.45 + reverse_penalty: 4.0 + change_penalty: 1.2 + non_straight_penalty: 0.7 + cost_penalty: 3.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 700 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin","wait","backup"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.00, 0.0, 2.0] + min_velocity: [-0.75, 0.0, -2.0] + max_accel: [3.73, 0.0, 3.2] + max_decel: [-1.1, 0.0, -4.5] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml index 3015acc..f693e05 100644 --- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml @@ -71,13 +71,13 @@ bt_navigator_rclcpp_node: controller_server: ros__parameters: use_sim_time: False - controller_frequency: 15.0 + controller_frequency: 10.0 FollowPath: plugin: "nav2_mppi_controller::MPPIController" - time_steps: 40 - model_dt: 0.06666666666666666 - batch_size: 900 - vx_std: 0.25 + time_steps: 36 + model_dt: 0.10 + batch_size: 700 + vx_std: 0.22 vy_std: 0.0 wz_std: 0.45 vx_max: 1.00 @@ -112,12 +112,12 @@ controller_server: PreferForwardCritic: enabled: false cost_power: 1 - cost_weight: 9.0 + cost_weight: 4.0 threshold_to_consider: 0.5 CostCritic: enabled: true cost_power: 1 - cost_weight: 5.0 + cost_weight: 3.81 critical_cost: 300.0 consider_footprint: true collision_cost: 100000.0 @@ -135,7 +135,7 @@ controller_server: PathFollowCritic: enabled: true cost_power: 1 - cost_weight: 5.0 + cost_weight: 4.0 offset_from_furthest: 10 threshold_to_consider: 1.4 PathAngleCritic: @@ -181,7 +181,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.20 + inflation_radius: 0.35 always_send_full_costmap: True local_costmap_client: ros__parameters: @@ -224,8 +224,8 @@ global_costmap: extra_inflation: 0.02 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.35 + cost_scaling_factor: 2.0 + inflation_radius: 0.3 always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -246,16 +246,16 @@ planner_server: allow_unknown: false max_iterations: 1000000 max_on_approach_iterations: 1000 - max_planning_time: 25.0 + max_planning_time: 5.0 motion_model_for_search: "REEDS_SHEPP" angle_quantization_bins: 72 analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 - minimum_turning_radius: 0.45 - reverse_penalty: 4.0 - change_penalty: 1.0 - non_straight_penalty: 1.0 - cost_penalty: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 3.0 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 retrospective_penalty: 0.015 # 5 m covers the rolling planning horizon without the startup and memory # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. @@ -264,7 +264,7 @@ planner_server: viz_expansions: false smooth_path: True smoother: - max_iterations: 700 + max_iterations: 1000 w_smooth: 0.3 w_data: 0.2 tolerance: 1.0e-10 diff --git a/src/racing_control/CMakeLists.txt b/src/racing_control/CMakeLists.txt index c277d47..d865264 100644 --- a/src/racing_control/CMakeLists.txt +++ b/src/racing_control/CMakeLists.txt @@ -1,6 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(racing_control) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -15,8 +18,21 @@ find_package(rclcpp_action REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +add_library(racing_control_core + src/candidate_waypoint_selector.cpp +) +target_include_directories(racing_control_core PUBLIC + $ + $ +) +ament_target_dependencies(racing_control_core + geometry_msgs + nav_msgs +) + add_executable(racing_control src/racing_control.cpp + src/candidate_waypoint_selector.cpp ) target_include_directories(racing_control PUBLIC $ @@ -33,6 +49,7 @@ ament_target_dependencies(racing_control ) install(TARGETS + racing_control_core racing_control DESTINATION lib/${PROJECT_NAME} ) @@ -63,8 +80,12 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_racing_control_helpers geometry_msgs + nav_msgs rclcpp ) + target_sources(test_racing_control_helpers PRIVATE + src/candidate_waypoint_selector.cpp + ) ament_lint_auto_find_test_dependencies() endif() diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml index f8e1422..e84081c 100644 --- a/src/racing_control/config/racing_control.yaml +++ b/src/racing_control/config/racing_control.yaml @@ -8,6 +8,7 @@ racing_control: enable_vlm_image_relay: false enable_dynamic_replanning: false enable_recovery: true + enable_candidate_waypoint_selection: true vlm_image_input_topic: /image vlm_image_output_topic: /vlm_image @@ -17,6 +18,7 @@ racing_control: vlm_result_topic: /vlm_result odom_topic: /odom_combined recovery_cmd_vel_topic: /cmd_vel + global_costmap_topic: /global_costmap/costmap trajectory_guard_input_topic: /trajectory_guard/input_path # Nav2 actions and plugin IDs @@ -42,6 +44,15 @@ racing_control: recovery_backup_speed: -0.2 recovery_backup_distance: 0.04 recovery_backup_timeout_sec: 0.2 + # Used only when the initial ComputePathThroughPoses request fails. + planning_failure_backup_speed: -0.1 + planning_failure_backup_distance: 0.03 + planning_failure_backup_timeout_sec: 0.3 + recovery_clear_wait_sec: 1.0 + recovery_search_radius_m: 1.0 + recovery_rear_clear_distance_m: 0.5 + global_costmap_clear_service: global_costmap/clear_entirely_global_costmap + local_costmap_clear_service: local_costmap/clear_entirely_local_costmap max_recovery_attempts: 2 circle_goal_tolerance: 0.50 @@ -57,6 +68,17 @@ racing_control: sign_vlm_trigger: 9 sign_profile_normal: 10 sign_profile_task2: 11 + vlm_trigger_repeat_count: 5 + vlm_trigger_interval_sec: 0.5 + + # Candidate waypoint JSONs reuse the saved-point format. + # home is intentionally never replaced. + candidate_waypoint_json_dir: /home/sunrise/yiliao_ws/src/racing_control/config/waypoints + qr_candidate_group_name: qr + entry_candidate_group_name: entry + vlm_candidate_group_name: goal_005 + clockwise_candidate_source_file: main_1.json + counterclockwise_candidate_source_file: main_2.json # Pose parameters are flat x/y/yaw-radians triples in frame_id. qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392] diff --git a/src/racing_control/config/waypoints/main.json b/src/racing_control/config/waypoints/main.json new file mode 100644 index 0000000..c8be5aa --- /dev/null +++ b/src/racing_control/config/waypoints/main.json @@ -0,0 +1,106 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-08-07T14:42:49.402Z", + "count": 2, + "points": [ + { + "name": "qr", + "captured_at": "2026-08-07T14:42:33.118Z", + "received_at": null, + "yaw_degrees": 65.22491304455252, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.333107888975101, + "y": 1.028867429995691, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.5389539275964809, + "w": 0.8423352443821446 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 65.22491304455252 + } + }, + { + "name": "entry", + "captured_at": "2026-08-07T14:42:45.961Z", + "received_at": null, + "yaw_degrees": 87.09617569507752, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.504811372926262, + "y": 2.2798075935366624, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.6889631335573172, + "w": 0.7247963856138373 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 87.09617569507752 + } + } + ] +} diff --git a/src/racing_control/config/waypoints/main_1.json b/src/racing_control/config/waypoints/main_1.json new file mode 100644 index 0000000..401f226 --- /dev/null +++ b/src/racing_control/config/waypoints/main_1.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-08-07T14:44:10.775Z", + "count": 3, + "points": [ + { + "name": "goal_004", + "captured_at": "2026-08-07T14:43:55.328Z", + "received_at": null, + "yaw_degrees": 89.33764149250207, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 0.7534956931109804, + "y": 3.766501234923627, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.7030077953706314, + "w": 0.7111821423855668 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 89.33764149250207 + } + }, + { + "name": "goal_005", + "captured_at": "2026-08-07T14:44:02.099Z", + "received_at": null, + "yaw_degrees": 2.2906028734862383, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 3.8567885105264077, + "y": 4.329424195458402, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.019987949834902125, + "w": 0.9998002209748693 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 2.2906028734862383 + } + }, + { + "name": "goal_006", + "captured_at": "2026-08-07T14:44:09.250Z", + "received_at": null, + "yaw_degrees": -92.25458353863968, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.5144339572664096, + "y": 2.4337692660037766, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": -0.720881318872454, + "w": 0.6930585286256215 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": -92.25458353863968 + } + } + ] +} diff --git a/src/racing_control/config/waypoints/main_2.json b/src/racing_control/config/waypoints/main_2.json new file mode 100644 index 0000000..1df63f8 --- /dev/null +++ b/src/racing_control/config/waypoints/main_2.json @@ -0,0 +1,155 @@ +{ + "topic": "/odom_combined", + "message_type": "nav_msgs/msg/Odometry", + "saved_at": "2026-08-07T14:43:29.485Z", + "count": 3, + "points": [ + { + "name": "goal_001", + "captured_at": "2026-08-07T14:43:14.866Z", + "received_at": null, + "yaw_degrees": 88.52360610794565, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 4.2128251001861265, + "y": 3.7376334819031842, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.6979380047775942, + "w": 0.7161581818893582 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 88.52360610794565 + } + }, + { + "name": "goal_002", + "captured_at": "2026-08-07T14:43:20.968Z", + "received_at": null, + "yaw_degrees": 178.26427158937722, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 1.1720794040064113, + "y": 4.319801449605877, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": 0.99988528505826, + "w": 0.015146508639358486 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": 178.26427158937722 + } + }, + { + "name": "goal_003", + "captured_at": "2026-08-07T14:43:28.294Z", + "received_at": null, + "yaw_degrees": -88.21009502116203, + "odom": { + "header": { + "frame_id": "odom", + "stamp": { + "sec": 0, + "nanosec": 0 + }, + "stamp_iso": null + }, + "child_frame_id": "base_link", + "pose": { + "pose": { + "position": { + "x": 2.4951887885861144, + "y": 2.318297930897253, + "z": 0 + }, + "orientation": { + "x": 0, + "y": 0, + "z": -0.6959760577153663, + "w": 0.7180649880665239 + } + }, + "covariance": [] + }, + "twist": { + "twist": { + "linear": { + "x": 0, + "y": 0, + "z": 0 + }, + "angular": { + "x": 0, + "y": 0, + "z": 0 + } + }, + "covariance": [] + }, + "yaw_degrees": -88.21009502116203 + } + } + ] +} diff --git a/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp new file mode 100644 index 0000000..39df737 --- /dev/null +++ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp @@ -0,0 +1,89 @@ +#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ +#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ + +#include +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +namespace racing_control +{ + +struct CandidateWaypoint +{ + std::string group_name; + std::string source_file; + std::string point_name; + geometry_msgs::msg::PoseStamped pose; +}; + +using CandidateWaypointGroups = std::unordered_map>; + +struct RecoveryCommand +{ + double linear_x{0.0}; + double angular_z{0.0}; + double duration_sec{0.0}; + std::string label; +}; + +CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & json_dir, + const std::string & frame_id); + +std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + int occupied_threshold, + bool treat_unknown_as_occupied, + double search_radius_m = 1.0); + +std::optional rearClearRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + int occupied_threshold, + bool treat_unknown_as_occupied, + double rear_clear_distance_m); + +class CandidateWaypointSelector +{ +public: + void setGroups(CandidateWaypointGroups groups); + void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap); + + std::optional select( + const std::string & group_name, + int occupied_threshold, + bool treat_unknown_as_occupied, + const std::string & preferred_source_file = "") const; + + std::optional nearestFreeRecoveryCommand( + const geometry_msgs::msg::PoseStamped & current, + int occupied_threshold, + bool treat_unknown_as_occupied, + double search_radius_m = 1.0) const; + + bool isPoseOccupied( + const geometry_msgs::msg::PoseStamped & pose, + int occupied_threshold, + bool treat_unknown_as_occupied) const; + +private: + bool isCandidateOccupied( + const geometry_msgs::msg::PoseStamped & pose, + int occupied_threshold, + bool treat_unknown_as_occupied) const; + + std::optional commandForRecoveryAngle(double angle_rad) const; + + CandidateWaypointGroups groups_; + nav_msgs::msg::OccupancyGrid costmap_; + bool has_costmap_{false}; +}; + +} // namespace racing_control + +#endif // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ diff --git a/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ new file mode 100644 index 0000000..5a1d0c5 --- /dev/null +++ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup.md5~ @@ -0,0 +1 @@ +4773a55f2a69d3a5f983b4f5cbe49312 candidate_waypoint_selector.hpp diff --git a/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ new file mode 100644 index 0000000..a9949f6 --- /dev/null +++ b/src/racing_control/include/racing_control/candidate_waypoint_selector.hpp.unc-backup~ @@ -0,0 +1,190 @@ +#ifndef RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ +#define RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "racing_control/racing_control.hpp" + +namespace racing_control +{ + +struct CandidateWaypoint +{ + std::string group; + std::string source_file; + std::string point_name; + geometry_msgs::msg::PoseStamped pose; +}; + +struct RecoveryCommand +{ + double linear_x{0.0}; + double angular_z{0.0}; +}; + +using CandidateWaypointGroups = std::map>; + +inline geometry_msgs::msg::PoseStamped candidatePoseFromJson( + const nlohmann::json & point, const std::string & frame_id) +{ + const auto & pose = point.at("odom").at("pose").at("pose"); + const auto & position = pose.at("position"); + const auto & orientation = pose.at("orientation"); + geometry_msgs::msg::PoseStamped result; + result.header.frame_id = frame_id; + result.pose.position.x = position.value("x", 0.0); + result.pose.position.y = position.value("y", 0.0); + result.pose.position.z = position.value("z", 0.0); + result.pose.orientation.x = orientation.value("x", 0.0); + result.pose.orientation.y = orientation.value("y", 0.0); + result.pose.orientation.z = orientation.value("z", 0.0); + result.pose.orientation.w = orientation.value("w", 1.0); + if (point.contains("yaw_degrees")) { + const auto yaw = point.at("yaw_degrees").get() * M_PI / 180.0; + result.pose.orientation.z = std::sin(yaw * 0.5); + result.pose.orientation.w = std::cos(yaw * 0.5); + } + return result; +} + +inline CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & directory, const std::string & frame_id) +{ + CandidateWaypointGroups groups; + for (const auto & entry : std::filesystem::directory_iterator(directory)) { + if (!entry.is_regular_file() || entry.path().extension() != ".json") { + continue; + } + std::ifstream input(entry.path()); + const auto document = nlohmann::json::parse(input); + for (const auto & point : document.value("points", nlohmann::json::array())) { + const auto point_name = point.value("name", std::string{}); + if (point_name.empty()) { + continue; + } + groups[point_name].push_back( + CandidateWaypoint{ + point_name, entry.path().filename().string(), point_name, + candidatePoseFromJson(point, frame_id)}); + } + } + return groups; +} + +class CandidateWaypointSelector +{ +public: + void setGroups(CandidateWaypointGroups groups) + { + groups_ = std::move(groups); + } + + void setCostmap(const nav_msgs::msg::OccupancyGrid & costmap) + { + costmap_ = costmap; + } + + std::optional select( + const std::string & group, const int occupied_threshold, const bool allow_unknown) const + { + const auto it = groups_.find(group); + if (it == groups_.end()) { + return std::nullopt; + } + for (const auto & candidate : it->second) { + if (isFree(candidate.pose, occupied_threshold, allow_unknown)) { + return candidate; + } + } + return std::nullopt; + } + +private: + bool isFree( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool allow_unknown) const + { + if (costmap_.info.resolution <= 0.0 || costmap_.info.width == 0 || costmap_.info.height == 0) { + return true; + } + const auto mx = static_cast(std::floor( + (pose.pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution)); + const auto my = static_cast(std::floor( + (pose.pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution)); + if (mx < 0 || my < 0 || mx >= static_cast(costmap_.info.width) || + my >= static_cast(costmap_.info.height)) + { + return false; + } + const auto value = costmap_.data[ + static_cast(my) * costmap_.info.width + static_cast(mx)]; + return value < 0 ? allow_unknown : value < occupied_threshold; + } + + CandidateWaypointGroups groups_; + nav_msgs::msg::OccupancyGrid costmap_; +}; + +inline std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool allow_unknown, + const double max_distance) +{ + if (costmap.info.resolution <= 0.0 || costmap.info.width == 0 || costmap.info.height == 0) { + return std::nullopt; + } + + const auto yaw = std::atan2( + 2.0 * (current.pose.orientation.w * current.pose.orientation.z), + 1.0 - 2.0 * current.pose.orientation.z * current.pose.orientation.z); + const auto cos_yaw = std::cos(yaw); + const auto sin_yaw = std::sin(yaw); + double best_distance = max_distance; + double best_forward = 0.0; + bool found = false; + for (std::size_t my = 0; my < costmap.info.height; ++my) { + for (std::size_t mx = 0; mx < costmap.info.width; ++mx) { + const auto value = costmap.data[my * costmap.info.width + mx]; + if (value < 0 ? !allow_unknown : value >= occupied_threshold) { + continue; + } + const auto world_x = costmap.info.origin.position.x + + (static_cast(mx) + 0.5) * costmap.info.resolution; + const auto world_y = costmap.info.origin.position.y + + (static_cast(my) + 0.5) * costmap.info.resolution; + const auto dx = world_x - current.pose.position.x; + const auto dy = world_y - current.pose.position.y; + const auto forward = cos_yaw * dx + sin_yaw * dy; + const auto lateral = -sin_yaw * dx + cos_yaw * dy; + const auto distance = std::hypot(forward, lateral); + if (distance <= 0.0 || distance > max_distance || distance >= best_distance) { + continue; + } + best_distance = distance; + best_forward = forward; + found = true; + } + } + if (!found) { + return std::nullopt; + } + return RecoveryCommand{best_forward >= 0.0 ? 0.2 : -0.2, 0.0}; +} + +} // namespace racing_control + +#endif // RACING_CONTROL__CANDIDATE_WAYPOINT_SELECTOR_HPP_ diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp index f523724..defc1b2 100644 --- a/src/racing_control/include/racing_control/racing_control.hpp +++ b/src/racing_control/include/racing_control/racing_control.hpp @@ -33,6 +33,64 @@ enum class QrTransitTarget PostQr }; +enum class RouteRecoveryPhase +{ + Initial, + AfterPlanningBackup, + AfterLocalClearReplan, + AfterFinalBackupReplan +}; + +enum class RouteFailureKind +{ + Planning, + Following +}; + +enum class RouteRecoveryAction +{ + Fail, + PlanningBackup, + ClearLocalAndReplan, + FinalBackupReplan +}; + +inline RouteRecoveryAction routeRecoveryAction( + const RouteRecoveryPhase phase, + const RouteFailureKind failure, + const int final_recovery_attempts, + const int max_final_recovery_attempts) +{ + const auto finalBackupOrFail = [&]() { + return final_recovery_attempts < max_final_recovery_attempts ? + RouteRecoveryAction::FinalBackupReplan : RouteRecoveryAction::Fail; + }; + + if (failure == RouteFailureKind::Planning) { + if (phase == RouteRecoveryPhase::Initial) { + return RouteRecoveryAction::PlanningBackup; + } + if (phase == RouteRecoveryPhase::AfterLocalClearReplan || + phase == RouteRecoveryPhase::AfterFinalBackupReplan) + { + return finalBackupOrFail(); + } + return RouteRecoveryAction::Fail; + } + + if (phase == RouteRecoveryPhase::Initial || + phase == RouteRecoveryPhase::AfterPlanningBackup) + { + return RouteRecoveryAction::ClearLocalAndReplan; + } + if (phase == RouteRecoveryPhase::AfterLocalClearReplan || + phase == RouteRecoveryPhase::AfterFinalBackupReplan) + { + return finalBackupOrFail(); + } + return RouteRecoveryAction::Fail; +} + inline VlmCaptureMode vlmCaptureModeFromString(const std::string & value) { std::string normalized; diff --git a/src/racing_control/src/candidate_waypoint_selector.cpp b/src/racing_control/src/candidate_waypoint_selector.cpp new file mode 100644 index 0000000..be2f58d --- /dev/null +++ b/src/racing_control/src/candidate_waypoint_selector.cpp @@ -0,0 +1,314 @@ +#include "racing_control/candidate_waypoint_selector.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace racing_control +{ +namespace +{ + +double normalizeAngle(double angle) +{ + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +} + +double yawFromQuaternionInternal( + const double x, const double y, const double z, const double w) +{ + return std::atan2( + 2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z)); +} + +std::vector sortedJsonFiles(const std::filesystem::path & json_dir) +{ + std::vector files; + for (const auto & entry : std::filesystem::directory_iterator(json_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".json") { + files.push_back(entry.path()); + } + } + std::sort( + files.begin(), files.end(), [](const auto & a, const auto & b) { + return a.filename().string() < b.filename().string(); + }); + return files; +} + +geometry_msgs::msg::PoseStamped poseFromJsonPoint( + const nlohmann::json & point, + const std::string & frame_id) +{ + const auto & odom = point.at("odom"); + const auto & pose = odom.at("pose").at("pose"); + const auto & position = pose.at("position"); + const auto & orientation = pose.at("orientation"); + + const double x = position.at("x").get(); + const double y = position.at("y").get(); + const double yaw = point.contains("yaw_degrees") ? + point.at("yaw_degrees").get() * M_PI / 180.0 : + yawFromQuaternionInternal( + orientation.at("x").get(), orientation.at("y").get(), + orientation.at("z").get(), orientation.at("w").get()); + + geometry_msgs::msg::PoseStamped stamped; + stamped.header.frame_id = frame_id; + stamped.pose.position.x = x; + stamped.pose.position.y = y; + stamped.pose.position.z = 0.0; + stamped.pose.orientation.z = std::sin(yaw * 0.5); + stamped.pose.orientation.w = std::cos(yaw * 0.5); + return stamped; +} + +bool fileContainsGroup( + const std::string & source_file, + const std::string & preferred_source_file) +{ + if (preferred_source_file.empty()) { + return false; + } + return source_file == preferred_source_file; +} + +} // namespace + +CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & json_dir, + const std::string & frame_id) +{ + CandidateWaypointGroups groups; + const auto root = std::filesystem::path(json_dir); + if (json_dir.empty() || !std::filesystem::exists(root)) { + return groups; + } + + for (const auto & path : sortedJsonFiles(root)) { + std::ifstream input(path); + if (!input) { + continue; + } + nlohmann::json data; + try { + input >> data; + } catch (...) { + continue; + } + + const auto points = data.is_array() ? data : data.value("points", nlohmann::json::array()); + if (!points.is_array()) { + continue; + } + + for (const auto & point : points) { + if (!point.is_object() || !point.contains("odom") || !point["odom"].is_object()) { + continue; + } + CandidateWaypoint candidate; + candidate.source_file = path.filename().string(); + candidate.point_name = point.value("name", path.stem().string()); + candidate.group_name = candidate.point_name; + candidate.pose = poseFromJsonPoint(point, frame_id); + groups[candidate.group_name].push_back(std::move(candidate)); + } + } + + return groups; +} + +void CandidateWaypointSelector::setGroups(CandidateWaypointGroups groups) +{ + groups_ = std::move(groups); +} + +void CandidateWaypointSelector::setCostmap(const nav_msgs::msg::OccupancyGrid & costmap) +{ + costmap_ = costmap; + has_costmap_ = true; +} + +std::optional CandidateWaypointSelector::select( + const std::string & group_name, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const std::string & preferred_source_file) const +{ + const auto group_it = groups_.find(group_name); + if (group_it == groups_.end() || group_it->second.empty() || !has_costmap_) { + return std::nullopt; + } + + std::vector ordered; + ordered.reserve(group_it->second.size()); + for (const auto & candidate : group_it->second) { + ordered.push_back(&candidate); + } + std::stable_sort( + ordered.begin(), ordered.end(), [&](const auto * lhs, const auto * rhs) { + const bool lhs_preferred = fileContainsGroup(lhs->source_file, preferred_source_file); + const bool rhs_preferred = fileContainsGroup(rhs->source_file, preferred_source_file); + if (lhs_preferred != rhs_preferred) { + return lhs_preferred; + } + return lhs->source_file < rhs->source_file; + }); + + for (const auto * candidate : ordered) { + if (!isCandidateOccupied(candidate->pose, occupied_threshold, treat_unknown_as_occupied)) { + return *candidate; + } + } + return std::nullopt; +} + +std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + return selector.nearestFreeRecoveryCommand( + current, occupied_threshold, treat_unknown_as_occupied, search_radius_m); +} + +std::optional rearClearRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double rear_clear_distance_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + geometry_msgs::msg::PoseStamped rear = current; + rear.pose.position.x -= std::cos(yaw) * rear_clear_distance_m; + rear.pose.position.y -= std::sin(yaw) * rear_clear_distance_m; + if (selector.isPoseOccupied(rear, occupied_threshold, treat_unknown_as_occupied)) { + return std::nullopt; + } + return RecoveryCommand{-0.25, 0.0, rear_clear_distance_m / 0.25, "rear-clear-backward"}; +} + +std::optional CandidateWaypointSelector::nearestFreeRecoveryCommand( + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) const +{ + if (!has_costmap_) { + return std::nullopt; + } + + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + const std::vector distances{0.25, 0.5, 0.75, std::max(0.25, search_radius_m)}; + const std::vector angles{0.0, M_PI / 4.0, -M_PI / 4.0, M_PI / 2.0, -M_PI / 2.0, M_PI}; + + double best_score = std::numeric_limits::infinity(); + std::optional best_command; + for (const double distance : distances) { + for (const double angle : angles) { + const double sample_angle = normalizeAngle(yaw + angle); + geometry_msgs::msg::PoseStamped sample = current; + sample.pose.position.x += std::cos(sample_angle) * distance; + sample.pose.position.y += std::sin(sample_angle) * distance; + if (isCandidateOccupied(sample, occupied_threshold, treat_unknown_as_occupied)) { + continue; + } + const double score = distance + std::abs(angle) * 0.1; + if (score < best_score) { + best_score = score; + best_command = commandForRecoveryAngle(angle); + } + } + } + return best_command; +} + +bool CandidateWaypointSelector::isPoseOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + return isCandidateOccupied(pose, occupied_threshold, treat_unknown_as_occupied); +} + +bool CandidateWaypointSelector::isCandidateOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + const auto & costmap = costmap_; + const auto & origin = costmap.info.origin; + const double resolution = costmap.info.resolution; + const auto width = static_cast(costmap.info.width); + const auto height = static_cast(costmap.info.height); + if (resolution <= 0.0 || width <= 0 || height <= 0) { + return true; + } + + const double origin_yaw = yawFromQuaternionInternal( + origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w); + const double dx = pose.pose.position.x - origin.position.x; + const double dy = pose.pose.position.y - origin.position.y; + const double cos_yaw = std::cos(-origin_yaw); + const double sin_yaw = std::sin(-origin_yaw); + const double local_x = cos_yaw * dx - sin_yaw * dy; + const double local_y = sin_yaw * dx + cos_yaw * dy; + const auto mx = static_cast(std::floor(local_x / resolution)); + const auto my = static_cast(std::floor(local_y / resolution)); + if (mx < 0 || my < 0 || mx >= width || my >= height) { + return true; + } + + const auto index = my * width + mx; + if (index < 0 || index >= static_cast(costmap.data.size())) { + return true; + } + + const auto cost = static_cast(costmap.data[static_cast(index)]); + if (cost < 0) { + return treat_unknown_as_occupied; + } + return cost >= occupied_threshold; +} + +std::optional CandidateWaypointSelector::commandForRecoveryAngle( + const double angle_rad) const +{ + const double angle = normalizeAngle(angle_rad); + if (std::abs(angle) < M_PI / 8.0) { + return RecoveryCommand{0.25, 0.0, 0.6, "forward"}; + } + if (std::abs(std::abs(angle) - M_PI) < M_PI / 8.0) { + return RecoveryCommand{-0.25, 0.0, 0.6, "backward"}; + } + if (angle > 0.0 && angle < M_PI / 2.0) { + return RecoveryCommand{0.25, 0.7, 0.6, "forward-left"}; + } + if (angle < 0.0 && angle > -M_PI / 2.0) { + return RecoveryCommand{0.25, -0.7, 0.6, "forward-right"}; + } + if (angle >= M_PI / 2.0) { + return RecoveryCommand{0.0, 0.7, 0.4, "left"}; + } + return RecoveryCommand{0.0, -0.7, 0.4, "right"}; +} + +} // namespace racing_control diff --git a/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ new file mode 100644 index 0000000..4747cf1 --- /dev/null +++ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup.md5~ @@ -0,0 +1 @@ +38f483033e12bc75920274763b0c7e0b candidate_waypoint_selector.cpp diff --git a/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ new file mode 100644 index 0000000..4987551 --- /dev/null +++ b/src/racing_control/src/candidate_waypoint_selector.cpp.unc-backup~ @@ -0,0 +1,312 @@ +#include "racing_control/candidate_waypoint_selector.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace racing_control +{ +namespace +{ + +double normalizeAngle(double angle) +{ + while (angle > M_PI) { + angle -= 2.0 * M_PI; + } + while (angle < -M_PI) { + angle += 2.0 * M_PI; + } + return angle; +} + +double yawFromQuaternionInternal( + const double x, const double y, const double z, const double w) +{ + return std::atan2( + 2.0 * (w * z + x * y), + 1.0 - 2.0 * (y * y + z * z)); +} + +std::vector sortedJsonFiles(const std::filesystem::path & json_dir) +{ + std::vector files; + for (const auto & entry : std::filesystem::directory_iterator(json_dir)) { + if (entry.is_regular_file() && entry.path().extension() == ".json") { + files.push_back(entry.path()); + } + } + std::sort(files.begin(), files.end(), [](const auto & a, const auto & b) { + return a.filename().string() < b.filename().string(); + }); + return files; +} + +geometry_msgs::msg::PoseStamped poseFromJsonPoint( + const nlohmann::json & point, + const std::string & frame_id) +{ + const auto & odom = point.at("odom"); + const auto & pose = odom.at("pose").at("pose"); + const auto & position = pose.at("position"); + const auto & orientation = pose.at("orientation"); + + const double x = position.at("x").get(); + const double y = position.at("y").get(); + const double yaw = point.contains("yaw_degrees") ? + point.at("yaw_degrees").get() * M_PI / 180.0 : + yawFromQuaternionInternal( + orientation.at("x").get(), orientation.at("y").get(), + orientation.at("z").get(), orientation.at("w").get()); + + geometry_msgs::msg::PoseStamped stamped; + stamped.header.frame_id = frame_id; + stamped.pose.position.x = x; + stamped.pose.position.y = y; + stamped.pose.position.z = 0.0; + stamped.pose.orientation.z = std::sin(yaw * 0.5); + stamped.pose.orientation.w = std::cos(yaw * 0.5); + return stamped; +} + +bool fileContainsGroup( + const std::string & source_file, + const std::string & preferred_source_file) +{ + if (preferred_source_file.empty()) { + return false; + } + return source_file == preferred_source_file; +} + +} // namespace + +CandidateWaypointGroups loadCandidateWaypointGroups( + const std::string & json_dir, + const std::string & frame_id) +{ + CandidateWaypointGroups groups; + const auto root = std::filesystem::path(json_dir); + if (json_dir.empty() || !std::filesystem::exists(root)) { + return groups; + } + + for (const auto & path : sortedJsonFiles(root)) { + std::ifstream input(path); + if (!input) { + continue; + } + nlohmann::json data; + try { + input >> data; + } catch (...) { + continue; + } + + const auto points = data.is_array() ? data : data.value("points", nlohmann::json::array()); + if (!points.is_array()) { + continue; + } + + for (const auto & point : points) { + if (!point.is_object() || !point.contains("odom") || !point["odom"].is_object()) { + continue; + } + CandidateWaypoint candidate; + candidate.source_file = path.filename().string(); + candidate.point_name = point.value("name", path.stem().string()); + candidate.group_name = candidate.point_name; + candidate.pose = poseFromJsonPoint(point, frame_id); + groups[candidate.group_name].push_back(std::move(candidate)); + } + } + + return groups; +} + +void CandidateWaypointSelector::setGroups(CandidateWaypointGroups groups) +{ + groups_ = std::move(groups); +} + +void CandidateWaypointSelector::setCostmap(const nav_msgs::msg::OccupancyGrid & costmap) +{ + costmap_ = costmap; + has_costmap_ = true; +} + +std::optional CandidateWaypointSelector::select( + const std::string & group_name, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const std::string & preferred_source_file) const +{ + const auto group_it = groups_.find(group_name); + if (group_it == groups_.end() || group_it->second.empty() || !has_costmap_) { + return std::nullopt; + } + + std::vector ordered; + ordered.reserve(group_it->second.size()); + for (const auto & candidate : group_it->second) { + ordered.push_back(&candidate); + } + std::stable_sort(ordered.begin(), ordered.end(), [&](const auto * lhs, const auto * rhs) { + const bool lhs_preferred = fileContainsGroup(lhs->source_file, preferred_source_file); + const bool rhs_preferred = fileContainsGroup(rhs->source_file, preferred_source_file); + if (lhs_preferred != rhs_preferred) { + return lhs_preferred; + } + return lhs->source_file < rhs->source_file; + }); + + for (const auto * candidate : ordered) { + if (!isCandidateOccupied(candidate->pose, occupied_threshold, treat_unknown_as_occupied)) { + return *candidate; + } + } + return std::nullopt; +} + +std::optional nearestFreeRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + return selector.nearestFreeRecoveryCommand( + current, occupied_threshold, treat_unknown_as_occupied, search_radius_m); +} + +std::optional rearClearRecoveryCommand( + const nav_msgs::msg::OccupancyGrid & costmap, + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double rear_clear_distance_m) +{ + CandidateWaypointSelector selector; + selector.setCostmap(costmap); + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + geometry_msgs::msg::PoseStamped rear = current; + rear.pose.position.x -= std::cos(yaw) * rear_clear_distance_m; + rear.pose.position.y -= std::sin(yaw) * rear_clear_distance_m; + if (selector.isPoseOccupied(rear, occupied_threshold, treat_unknown_as_occupied)) { + return std::nullopt; + } + return RecoveryCommand{-0.25, 0.0, rear_clear_distance_m / 0.25, "rear-clear-backward"}; +} + +std::optional CandidateWaypointSelector::nearestFreeRecoveryCommand( + const geometry_msgs::msg::PoseStamped & current, + const int occupied_threshold, + const bool treat_unknown_as_occupied, + const double search_radius_m) const +{ + if (!has_costmap_) { + return std::nullopt; + } + + const double yaw = 2.0 * std::atan2(current.pose.orientation.z, current.pose.orientation.w); + const std::vector distances{0.25, 0.5, 0.75, std::max(0.25, search_radius_m)}; + const std::vector angles{0.0, M_PI / 4.0, -M_PI / 4.0, M_PI / 2.0, -M_PI / 2.0, M_PI}; + + double best_score = std::numeric_limits::infinity(); + std::optional best_command; + for (const double distance : distances) { + for (const double angle : angles) { + const double sample_angle = normalizeAngle(yaw + angle); + geometry_msgs::msg::PoseStamped sample = current; + sample.pose.position.x += std::cos(sample_angle) * distance; + sample.pose.position.y += std::sin(sample_angle) * distance; + if (isCandidateOccupied(sample, occupied_threshold, treat_unknown_as_occupied)) { + continue; + } + const double score = distance + std::abs(angle) * 0.1; + if (score < best_score) { + best_score = score; + best_command = commandForRecoveryAngle(angle); + } + } + } + return best_command; +} + +bool CandidateWaypointSelector::isPoseOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + return isCandidateOccupied(pose, occupied_threshold, treat_unknown_as_occupied); +} + +bool CandidateWaypointSelector::isCandidateOccupied( + const geometry_msgs::msg::PoseStamped & pose, + const int occupied_threshold, + const bool treat_unknown_as_occupied) const +{ + const auto & costmap = costmap_; + const auto & origin = costmap.info.origin; + const double resolution = costmap.info.resolution; + const auto width = static_cast(costmap.info.width); + const auto height = static_cast(costmap.info.height); + if (resolution <= 0.0 || width <= 0 || height <= 0) { + return true; + } + + const double origin_yaw = yawFromQuaternionInternal( + origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w); + const double dx = pose.pose.position.x - origin.position.x; + const double dy = pose.pose.position.y - origin.position.y; + const double cos_yaw = std::cos(-origin_yaw); + const double sin_yaw = std::sin(-origin_yaw); + const double local_x = cos_yaw * dx - sin_yaw * dy; + const double local_y = sin_yaw * dx + cos_yaw * dy; + const auto mx = static_cast(std::floor(local_x / resolution)); + const auto my = static_cast(std::floor(local_y / resolution)); + if (mx < 0 || my < 0 || mx >= width || my >= height) { + return true; + } + + const auto index = my * width + mx; + if (index < 0 || index >= static_cast(costmap.data.size())) { + return true; + } + + const auto cost = static_cast(costmap.data[static_cast(index)]); + if (cost < 0) { + return treat_unknown_as_occupied; + } + return cost >= occupied_threshold; +} + +std::optional CandidateWaypointSelector::commandForRecoveryAngle( + const double angle_rad) const +{ + const double angle = normalizeAngle(angle_rad); + if (std::abs(angle) < M_PI / 8.0) { + return RecoveryCommand{0.25, 0.0, 0.6, "forward"}; + } + if (std::abs(std::abs(angle) - M_PI) < M_PI / 8.0) { + return RecoveryCommand{-0.25, 0.0, 0.6, "backward"}; + } + if (angle > 0.0 && angle < M_PI / 2.0) { + return RecoveryCommand{0.25, 0.7, 0.6, "forward-left"}; + } + if (angle < 0.0 && angle > -M_PI / 2.0) { + return RecoveryCommand{0.25, -0.7, 0.6, "forward-right"}; + } + if (angle >= M_PI / 2.0) { + return RecoveryCommand{0.0, 0.7, 0.4, "left"}; + } + return RecoveryCommand{0.0, -0.7, 0.4, "right"}; +} + +} // namespace racing_control diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index e2cdf1f..a131342 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/src/racing_control/src/racing_control.cpp @@ -23,10 +23,13 @@ #include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/srv/clear_entire_costmap.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" #include "sensor_msgs/msg/compressed_image.hpp" #include "std_msgs/msg/int32.hpp" #include "std_msgs/msg/string.hpp" @@ -129,6 +132,7 @@ public: using NavigateToPose = nav2_msgs::action::NavigateToPose; using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses; using FollowPath = nav2_msgs::action::FollowPath; + using ClearEntireCostmap = nav2_msgs::srv::ClearEntireCostmap; using NavigateGoalHandle = rclcpp_action::ClientGoalHandle; using ComputeGoalHandle = rclcpp_action::ClientGoalHandle; using FollowGoalHandle = rclcpp_action::ClientGoalHandle; @@ -143,6 +147,11 @@ public: recovery_cmd_vel_pub_ = create_publisher( recovery_cmd_vel_topic_, 10); guard_path_pub_ = create_publisher(guard_input_topic_, 1); + global_costmap_sub_ = create_subscription( + global_costmap_topic_, 10, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + onGlobalCostmap(std::move(msg)); + }); if (enable_vlm_image_relay_) { vlm_image_pub_ = create_publisher(vlm_image_output_topic_, 1); @@ -167,6 +176,10 @@ public: compute_path_client_ = rclcpp_action::create_client(this, compute_path_action_); follow_path_client_ = rclcpp_action::create_client(this, follow_path_action_); + clear_global_costmap_client_ = + create_client(global_costmap_clear_service_); + clear_local_costmap_client_ = + create_client(local_costmap_clear_service_); tick_timer_ = create_wall_timer(200ms, [this]() {tick();}); startKeyboardThread(); @@ -198,6 +211,12 @@ private: odom_topic_ = declare_parameter("odom_topic", "/odom_combined"); recovery_cmd_vel_topic_ = declare_parameter("recovery_cmd_vel_topic", "/cmd_vel"); + global_costmap_topic_ = declare_parameter( + "global_costmap_topic", "/global_costmap/costmap"); + global_costmap_clear_service_ = declare_parameter( + "global_costmap_clear_service", "global_costmap/clear_entirely_global_costmap"); + local_costmap_clear_service_ = declare_parameter( + "local_costmap_clear_service", "local_costmap/clear_entirely_local_costmap"); navigate_action_ = declare_parameter("navigate_action", "/navigate_to_pose"); compute_path_action_ = declare_parameter("compute_path_action", "/compute_path_through_poses"); @@ -218,6 +237,8 @@ private: 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); + enable_candidate_waypoint_selection_ = + declare_parameter("enable_candidate_waypoint_selection", true); vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); vlm_image_output_topic_ = declare_parameter("vlm_image_output_topic", "/vlm_image"); @@ -236,12 +257,22 @@ private: recovery_backup_distance_ = declare_parameter("recovery_backup_distance", 0.04); recovery_backup_timeout_sec_ = declare_parameter("recovery_backup_timeout_sec", 0.2); + planning_failure_backup_speed_ = + declare_parameter("planning_failure_backup_speed", -0.1); + planning_failure_backup_distance_ = + declare_parameter("planning_failure_backup_distance", 0.03); + planning_failure_backup_timeout_sec_ = + declare_parameter("planning_failure_backup_timeout_sec", 0.3); + recovery_clear_wait_sec_ = declare_parameter("recovery_clear_wait_sec", 1.0); pass_through_vlm_trigger_radius_ = declare_parameter("pass_through_vlm_trigger_radius", 0.35); 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); + recovery_search_radius_m_ = declare_parameter("recovery_search_radius_m", 1.0); + recovery_rear_clear_distance_m_ = + declare_parameter("recovery_rear_clear_distance_m", 0.5); const auto vlm_capture_mode = declare_parameter("vlm_capture_mode", "stop"); @@ -291,6 +322,20 @@ private: singlePoseFromParameter( "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835, -1.652041913138445}); + + candidate_waypoint_json_dir_ = declare_parameter( + "candidate_waypoint_json_dir", "/home/sunrise/yiliao_ws/src/racing_control/config/waypoints"); + qr_candidate_group_name_ = declare_parameter("qr_candidate_group_name", "qr"); + entry_candidate_group_name_ = + declare_parameter("entry_candidate_group_name", "entry"); + vlm_candidate_group_name_ = + declare_parameter("vlm_candidate_group_name", "goal_005"); + clockwise_candidate_source_file_ = declare_parameter( + "clockwise_candidate_source_file", "main_1.json"); + counterclockwise_candidate_source_file_ = declare_parameter( + "counterclockwise_candidate_source_file", "main_2.json"); + candidate_selector_.setGroups( + loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_)); } geometry_msgs::msg::PoseStamped singlePoseFromParameter( @@ -470,6 +515,11 @@ private: if (recovery_timer_) { recovery_timer_->cancel(); } + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; cancelActiveFollowGoal(); publishRecoveryVelocity(0.0); publishSign(sign_qr_disable_); @@ -540,9 +590,10 @@ private: latest_qr_result_.clear(); selected_direction_ = RouteDirection::Unknown; qr_detection_disabled_ = false; - recovery_attempts_ = 0; + resetRouteRecoveryState(); active_segment_ = RouteSegment::ToQr; - active_segment_waypoints_ = {qr_pose_}; + active_segment_waypoints_ = { + selectCandidatePose(qr_candidate_group_name_, "", qr_pose_).value_or(qr_pose_)}; active_segment_next_waypoint_index_ = 0; publishSign(sign_profile_normal_); publishSign(sign_qr_enable_); @@ -621,7 +672,7 @@ private: const auto vlm_index = vlmWaypointIndex(route); latest_vlm_result_.clear(); vlm_capture_triggered_ = false; - recovery_attempts_ = 0; + resetRouteRecoveryState(); const auto split_at_vlm = split_qr_to_vlm_segment_ && vlm_capture_mode_ == VlmCaptureMode::Stop; @@ -631,8 +682,17 @@ private: active_segment_waypoints_.push_back(post_qr_pose_); } + auto route_waypoints = route.waypoints; + if (vlm_index < route_waypoints.size()) { + route_waypoints[vlm_index] = selectCandidatePose( + vlm_candidate_group_name_, preferredCandidateSourceFile(), + route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]); + } + const auto selected_entry_pose = selectCandidatePose( + entry_candidate_group_name_, preferredCandidateSourceFile(), + entry_pose_).value_or(entry_pose_); const auto qr_segment = routeWaypointsAfterQr( - entry_pose_, route.waypoints, vlm_index, split_at_vlm); + selected_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) { @@ -646,7 +706,7 @@ private: { const auto & route = selectedRoute(); const auto vlm_index = vlmWaypointIndex(route); - recovery_attempts_ = 0; + resetRouteRecoveryState(); active_segment_ = RouteSegment::AfterVlm; active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1); active_segment_waypoints_.push_back(route.home_pose); @@ -664,7 +724,7 @@ private: startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_); if (!compute_path_client_->wait_for_action_server(2s)) { - handleRouteExecutionFailure("ComputePathThroughPoses action server is not available"); + onRoutePlanningFailed("ComputePathThroughPoses action server is not available"); return; } @@ -676,8 +736,12 @@ private: auto options = rclcpp_action::Client::SendGoalOptions(); options.goal_response_callback = [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (stage_ != Stage::ComputeCirclePath) { + RCLCPP_DEBUG(get_logger(), "stale route planning goal response ignored"); + return; + } if (!goal_handle) { - handleRouteExecutionFailure("circle path planning goal was rejected"); + onRoutePlanningFailed("circle path planning goal was rejected"); } }; options.result_callback = @@ -686,10 +750,10 @@ private: return; } if (result.code != rclcpp_action::ResultCode::SUCCEEDED || - result.result->path.poses.empty()) + !result.result || result.result->path.poses.empty()) { finishStage("planning failed"); - handleRouteExecutionFailure("circle path planning failed"); + onRoutePlanningFailed("circle path planning failed"); return; } active_path_ = result.result->path; @@ -725,7 +789,7 @@ private: active_path_, [this](const bool ok) { finishStage(ok ? "FollowPath succeeded" : "FollowPath failed"); if (!ok) { - handleRouteExecutionFailure("route segment FollowPath failed"); + onRouteFollowFailed("route segment FollowPath failed"); return; } if (active_segment_ == RouteSegment::ToQr) { @@ -789,6 +853,44 @@ private: vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str()); } + void onGlobalCostmap(nav_msgs::msg::OccupancyGrid::SharedPtr msg) + { + std::lock_guard lock(costmap_mutex_); + latest_global_costmap_ = std::move(msg); + candidate_selector_.setCostmap(*latest_global_costmap_); + } + + std::optional selectCandidatePose( + const std::string & group_name, + const std::string & preferred_source_file, + const geometry_msgs::msg::PoseStamped & fallback_pose) + { + if (!enable_candidate_waypoint_selection_) { + return fallback_pose; + } + { + std::lock_guard lock(costmap_mutex_); + if (!latest_global_costmap_) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no global costmap yet", + group_name.c_str()); + return fallback_pose; + } + } + const auto selected = candidate_selector_.select( + group_name, 50, false, preferred_source_file); + if (!selected) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no usable candidate", + group_name.c_str()); + return fallback_pose; + } + RCLCPP_INFO( + get_logger(), "candidate selected for %s from %s -> %s", + group_name.c_str(), selected->source_file.c_str(), selected->point_name.c_str()); + return selected->pose; + } + void maybeTriggerPassThroughVlmCapture() { if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) { @@ -914,46 +1016,134 @@ private: get_logger(), "%s | consecutive dynamic replan failures=%d", reason.c_str(), dynamic_replan_consecutive_failures_); if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) { - handleRouteExecutionFailure(reason); + onRoutePlanningFailed(reason); } } - void handleRouteExecutionFailure(const std::string & reason) + void resetRouteRecoveryState() { - startRecovery( - 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"); - }); + route_recovery_phase_ = RouteRecoveryPhase::Initial; + planning_backup_attempted_ = false; + final_recovery_attempts_ = 0; } - void startRecovery(const std::string & reason, std::function on_recovered) + void prepareRouteRecoveryReplan() { - if (!enable_recovery_ || recovery_attempts_ >= max_recovery_attempts_) { + 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; + } + + void onRoutePlanningFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Planning, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::PlanningBackup) { + planning_backup_attempted_ = true; + route_recovery_phase_ = RouteRecoveryPhase::AfterPlanningBackup; + RCLCPP_WARN( + get_logger(), "%s; planning failed, backing up at %.2f m/s for %.2f s before one replan", + reason.c_str(), planning_failure_backup_speed_, planning_failure_backup_timeout_sec_); + startBackupRecovery( + reason, planning_failure_backup_speed_, planning_failure_backup_distance_, + planning_failure_backup_timeout_sec_, [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after planning-failure backup"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void onRouteFollowFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Following, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::ClearLocalAndReplan) { + route_recovery_phase_ = RouteRecoveryPhase::AfterLocalClearReplan; + RCLCPP_WARN( + get_logger(), "%s; clearing local costmap, waiting for update, then replanning once", + reason.c_str()); + clearLocalCostmapBeforeRecoveryReplan( + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after local costmap clear"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void startBackupRecovery( + const std::string & reason, + const double speed, + const double distance, + const double timeout_sec, + std::function on_recovered) + { + if (!enable_recovery_) { failRace(reason); return; } - ++recovery_attempts_; recovery_in_progress_ = true; recovery_done_callback_ = std::move(on_recovered); recovery_start_time_ = now(); recovery_start_pose_ = currentPoseFromOdom(); + recovery_active_speed_ = speed; + recovery_active_distance_ = distance; + recovery_active_timeout_sec_ = timeout_sec; + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; cancelActiveFollowGoal(); RCLCPP_WARN( - get_logger(), "%s; recovery backup attempt %d/%d", - reason.c_str(), recovery_attempts_, max_recovery_attempts_); + get_logger(), "%s; backup recovery speed=%.2f m/s distance=%.2f m timeout=%.2f s", + reason.c_str(), speed, distance, timeout_sec); if (recovery_timer_) { recovery_timer_->cancel(); @@ -966,9 +1156,9 @@ private: const auto elapsed = (now() - recovery_start_time_).seconds(); const auto backup_distance = recoveryBackupDistance(); if (!recoveryBackupComplete( - backup_distance, recovery_backup_distance_, elapsed, recovery_backup_timeout_sec_)) + backup_distance, recovery_active_distance_, elapsed, recovery_active_timeout_sec_)) { - publishRecoveryVelocity(recovery_backup_speed_); + publishRecoveryVelocity(recovery_active_speed_); return; } @@ -984,6 +1174,79 @@ private: } } + void clearLocalCostmapBeforeRecoveryReplan(std::function on_cleared) + { + recovery_after_clear_callback_ = std::move(on_cleared); + recovery_costmap_clear_pending_ = 0; + const auto requested = requestEntireCostmapClear( + clear_local_costmap_client_, local_costmap_clear_service_); + if (!requested) { + startRecoveryReplanAfterClearWait(); + } + } + + bool requestEntireCostmapClear( + const rclcpp::Client::SharedPtr & client, + const std::string & service_name) + { + if (!client) { + return false; + } + if (!client->wait_for_service(200ms)) { + RCLCPP_WARN( + get_logger(), "costmap clear service not available: %s", service_name.c_str()); + return false; + } + + ++recovery_costmap_clear_pending_; + auto request = std::make_shared(); + client->async_send_request( + request, + [this, service_name](rclcpp::Client::SharedFuture) { + if (recovery_costmap_clear_pending_ > 0) { + --recovery_costmap_clear_pending_; + } + RCLCPP_INFO(get_logger(), "cleared costmap via %s", service_name.c_str()); + if (recovery_costmap_clear_pending_ == 0) { + startRecoveryReplanAfterClearWait(); + } + }); + return true; + } + + void startRecoveryReplanAfterClearWait() + { + if (!recovery_after_clear_callback_) { + return; + } + + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + + if (recovery_clear_wait_sec_ <= 0.0) { + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + return; + } + + recovery_clear_wait_timer_ = create_wall_timer( + std::chrono::duration(recovery_clear_wait_sec_), + [this]() { + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + }); + } + double recoveryBackupDistance() const { if (!recovery_start_pose_) { @@ -1106,7 +1369,7 @@ private: void sendFollowPath(const nav_msgs::msg::Path & path, std::function on_done) { if (!follow_path_client_->wait_for_action_server(2s)) { - failRace("FollowPath action server is not available"); + onRouteFollowFailed("FollowPath action server is not available"); return; } @@ -1211,6 +1474,17 @@ private: throw std::runtime_error("route direction is not selected"); } + std::string preferredCandidateSourceFile() const + { + if (selected_direction_ == RouteDirection::Clockwise) { + return clockwise_candidate_source_file_; + } + if (selected_direction_ == RouteDirection::Counterclockwise) { + return counterclockwise_candidate_source_file_; + } + return ""; + } + bool routeSegmentReached() const { std::lock_guard lock(odom_mutex_); @@ -1287,6 +1561,7 @@ private: bool enable_vlm_image_relay_{false}; bool enable_dynamic_replanning_{true}; bool enable_recovery_{true}; + bool enable_candidate_waypoint_selection_{true}; bool qr_detection_disabled_{false}; bool dynamic_replan_in_flight_{false}; bool recovery_in_progress_{false}; @@ -1302,6 +1577,9 @@ private: std::string vlm_result_topic_; std::string odom_topic_; std::string recovery_cmd_vel_topic_; + std::string global_costmap_topic_; + std::string global_costmap_clear_service_; + std::string local_costmap_clear_service_; std::string vlm_image_input_topic_; std::string vlm_image_output_topic_; std::string navigate_action_; @@ -1311,6 +1589,12 @@ private: std::string planner_id_; std::string controller_id_; std::string goal_checker_id_; + std::string candidate_waypoint_json_dir_; + std::string qr_candidate_group_name_; + std::string entry_candidate_group_name_; + std::string vlm_candidate_group_name_; + std::string clockwise_candidate_source_file_; + std::string counterclockwise_candidate_source_file_; double navigation_timeout_sec_{120.0}; double path_planning_timeout_sec_{30.0}; @@ -1324,6 +1608,15 @@ private: double recovery_backup_speed_{-0.2}; double recovery_backup_distance_{0.04}; double recovery_backup_timeout_sec_{0.2}; + double planning_failure_backup_speed_{-0.1}; + double planning_failure_backup_distance_{0.03}; + double planning_failure_backup_timeout_sec_{0.3}; + double recovery_clear_wait_sec_{1.0}; + double recovery_active_speed_{0.0}; + double recovery_active_distance_{0.0}; + double recovery_active_timeout_sec_{0.0}; + double recovery_search_radius_m_{1.0}; + double recovery_rear_clear_distance_m_{0.5}; double pass_through_vlm_trigger_radius_{0.35}; double circle_goal_tolerance_{0.30}; @@ -1336,7 +1629,7 @@ private: int dynamic_replan_max_consecutive_failures_{3}; int dynamic_replan_consecutive_failures_{0}; int max_recovery_attempts_{2}; - int recovery_attempts_{0}; + int final_recovery_attempts_{0}; geometry_msgs::msg::PoseStamped qr_pose_; geometry_msgs::msg::PoseStamped post_qr_pose_; @@ -1346,20 +1639,26 @@ private: RouteDirection selected_direction_{RouteDirection::Unknown}; VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop}; RouteSegment active_segment_{RouteSegment::None}; + RouteRecoveryPhase route_recovery_phase_{RouteRecoveryPhase::Initial}; + bool planning_backup_attempted_{false}; std::vector active_segment_waypoints_; std::size_t active_segment_next_waypoint_index_{0}; nav_msgs::msg::Path active_path_; bool vlm_capture_triggered_{false}; std::optional recovery_start_pose_; std::function recovery_done_callback_; + std::function recovery_after_clear_callback_; + std::size_t recovery_costmap_clear_pending_{0}; std::string latest_qr_result_; std::string latest_vlm_result_; rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME}; rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME}; nav_msgs::msg::Odometry::SharedPtr latest_odom_; + nav_msgs::msg::OccupancyGrid::SharedPtr latest_global_costmap_; sensor_msgs::msg::CompressedImage::SharedPtr latest_image_; mutable std::mutex odom_mutex_; + mutable std::mutex costmap_mutex_; mutable std::mutex image_mutex_; rclcpp::Publisher::SharedPtr sign_pub_; @@ -1369,14 +1668,19 @@ private: rclcpp::Subscription::SharedPtr qr_sub_; rclcpp::Subscription::SharedPtr vlm_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr global_costmap_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp_action::Client::SharedPtr navigate_client_; rclcpp_action::Client::SharedPtr compute_path_client_; rclcpp_action::Client::SharedPtr follow_path_client_; + rclcpp::Client::SharedPtr clear_global_costmap_client_; + rclcpp::Client::SharedPtr clear_local_costmap_client_; FollowGoalHandle::SharedPtr active_follow_goal_handle_; rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_; rclcpp::TimerBase::SharedPtr recovery_timer_; + rclcpp::TimerBase::SharedPtr recovery_clear_wait_timer_; rclcpp::TimerBase::SharedPtr tick_timer_; + CandidateWaypointSelector candidate_selector_; int startup_qr_enable_publish_count_{0}; std::uint64_t follow_goal_generation_{0}; diff --git a/src/racing_control/src/racing_control.cpp.unc-backup.md5~ b/src/racing_control/src/racing_control.cpp.unc-backup.md5~ new file mode 100644 index 0000000..9c5da8d --- /dev/null +++ b/src/racing_control/src/racing_control.cpp.unc-backup.md5~ @@ -0,0 +1 @@ +32e8eedad71eea774e96a762a1cfd3ae racing_control.cpp diff --git a/src/racing_control/src/racing_control.cpp.unc-backup~ b/src/racing_control/src/racing_control.cpp.unc-backup~ new file mode 100644 index 0000000..a131342 --- /dev/null +++ b/src/racing_control/src/racing_control.cpp.unc-backup~ @@ -0,0 +1,1706 @@ +#include "racing_control/racing_control.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "nav2_msgs/action/compute_path_through_poses.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/srv/clear_entire_costmap.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "std_msgs/msg/int32.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +namespace racing_control +{ + +namespace +{ + +enum class Stage +{ + Idle, + NavigateToQr, + NavigatePostQr, + WaitForQr, + NavigateToEntry, + SwitchToTask2Profile, + ComputeCirclePath, + ExecuteCirclePath, + SwitchToNormalProfile, + WaitForVlm, + ReturnOrigin, + Finished, + Failed +}; + +enum class RouteSegment +{ + None, + ToQr, + ToVlm, + AfterVlm, + FullRoute +}; + +struct RouteConfig +{ + std::string label; + std::vector waypoints; + geometry_msgs::msg::PoseStamped home_pose; +}; + +const char * stageName(const Stage stage) +{ + switch (stage) { + case Stage::Idle: + return "等待启动"; + case Stage::NavigateToQr: + return "二维码点导航"; + case Stage::NavigatePostQr: + return "二维码后置点导航"; + case Stage::WaitForQr: + return "二维码识别/TTS"; + case Stage::NavigateToEntry: + return "通道入口导航"; + case Stage::SwitchToTask2Profile: + return "任务二参数切换"; + case Stage::ComputeCirclePath: + return "任务二轨迹规划"; + case Stage::ExecuteCirclePath: + return "任务二轨迹执行"; + case Stage::SwitchToNormalProfile: + return "恢复导航参数"; + case Stage::WaitForVlm: + return "图生文/TTS"; + case Stage::ReturnOrigin: + return "返回原点"; + case Stage::Finished: + return "比赛完成"; + case Stage::Failed: + return "比赛失败"; + } + return "未知阶段"; +} + +double distance2d( + const geometry_msgs::msg::PoseStamped & a, + const geometry_msgs::msg::PoseStamped & b) +{ + const double dx = a.pose.position.x - b.pose.position.x; + const double dy = a.pose.position.y - b.pose.position.y; + return std::hypot(dx, dy); +} + +std::string poseSummary(const geometry_msgs::msg::PoseStamped & pose) +{ + std::ostringstream out; + out << "(" << pose.pose.position.x << ", " << pose.pose.position.y << ")"; + return out.str(); +} + +} // namespace + +class RacingControl : public rclcpp::Node +{ +public: + using NavigateToPose = nav2_msgs::action::NavigateToPose; + using ComputePathThroughPoses = nav2_msgs::action::ComputePathThroughPoses; + using FollowPath = nav2_msgs::action::FollowPath; + using ClearEntireCostmap = nav2_msgs::srv::ClearEntireCostmap; + using NavigateGoalHandle = rclcpp_action::ClientGoalHandle; + using ComputeGoalHandle = rclcpp_action::ClientGoalHandle; + using FollowGoalHandle = rclcpp_action::ClientGoalHandle; + + RacingControl() + : Node("racing_control") + { + loadParameters(); + + sign_pub_ = create_publisher(sign_topic_, 10); + startStartupQrEnablePublisher(); + recovery_cmd_vel_pub_ = create_publisher( + recovery_cmd_vel_topic_, 10); + guard_path_pub_ = create_publisher(guard_input_topic_, 1); + global_costmap_sub_ = create_subscription( + global_costmap_topic_, 10, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + onGlobalCostmap(std::move(msg)); + }); + if (enable_vlm_image_relay_) { + vlm_image_pub_ = + create_publisher(vlm_image_output_topic_, 1); + image_sub_ = create_subscription( + vlm_image_input_topic_, 10, + [this](sensor_msgs::msg::CompressedImage::SharedPtr msg) { + onImage(std::move(msg)); + }); + } + + qr_sub_ = create_subscription( + qr_result_topic_, 10, + [this](std_msgs::msg::String::SharedPtr msg) {onQrResult(std::move(msg));}); + vlm_sub_ = create_subscription( + vlm_result_topic_, 10, + [this](std_msgs::msg::String::SharedPtr msg) {onVlmResult(std::move(msg));}); + odom_sub_ = create_subscription( + odom_topic_, 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) {onOdom(std::move(msg));}); + + navigate_client_ = rclcpp_action::create_client(this, navigate_action_); + compute_path_client_ = + rclcpp_action::create_client(this, compute_path_action_); + follow_path_client_ = rclcpp_action::create_client(this, follow_path_action_); + clear_global_costmap_client_ = + create_client(global_costmap_clear_service_); + clear_local_costmap_client_ = + create_client(local_costmap_clear_service_); + + tick_timer_ = create_wall_timer(200ms, [this]() {tick();}); + startKeyboardThread(); + + RCLCPP_INFO( + get_logger(), + "racing_control ready. Press SPACE to start, or set auto_start:=true."); + + if (auto_start_) { + startRace(); + } + } + + ~RacingControl() override + { + stop_keyboard_.store(true); + if (keyboard_thread_.joinable()) { + keyboard_thread_.join(); + } + } + +private: + void loadParameters() + { + frame_id_ = declare_parameter("frame_id", "odom"); + sign_topic_ = declare_parameter("sign_topic", "/sign4return"); + qr_result_topic_ = declare_parameter("qr_result_topic", "/qr_results"); + vlm_result_topic_ = declare_parameter("vlm_result_topic", "/vlm_result"); + odom_topic_ = declare_parameter("odom_topic", "/odom_combined"); + recovery_cmd_vel_topic_ = + declare_parameter("recovery_cmd_vel_topic", "/cmd_vel"); + global_costmap_topic_ = declare_parameter( + "global_costmap_topic", "/global_costmap/costmap"); + global_costmap_clear_service_ = declare_parameter( + "global_costmap_clear_service", "global_costmap/clear_entirely_global_costmap"); + local_costmap_clear_service_ = declare_parameter( + "local_costmap_clear_service", "local_costmap/clear_entirely_local_costmap"); + navigate_action_ = declare_parameter("navigate_action", "/navigate_to_pose"); + compute_path_action_ = + declare_parameter("compute_path_action", "/compute_path_through_poses"); + follow_path_action_ = declare_parameter("follow_path_action", "/follow_path"); + guard_input_topic_ = + declare_parameter( + "trajectory_guard_input_topic", + "/trajectory_guard/input_path"); + planner_id_ = declare_parameter("planner_id", "GridBased"); + controller_id_ = declare_parameter("controller_id", "FollowPath"); + goal_checker_id_ = declare_parameter("goal_checker_id", ""); + + auto_start_ = declare_parameter("auto_start", false); + 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); + enable_candidate_waypoint_selection_ = + declare_parameter("enable_candidate_waypoint_selection", true); + vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); + vlm_image_output_topic_ = + declare_parameter("vlm_image_output_topic", "/vlm_image"); + navigation_timeout_sec_ = declare_parameter("navigation_timeout_sec", 120.0); + path_planning_timeout_sec_ = declare_parameter("path_planning_timeout_sec", 30.0); + circle_timeout_sec_ = declare_parameter("circle_timeout_sec", 120.0); + qr_result_timeout_sec_ = declare_parameter("qr_result_timeout_sec", 8.0); + profile_switch_wait_sec_ = declare_parameter("profile_switch_wait_sec", 1.0); + post_qr_wait_sec_ = declare_parameter("post_qr_wait_sec", 1.0); + vlm_capture_wait_sec_ = declare_parameter("vlm_capture_wait_sec", 0.5); + dynamic_replan_interval_sec_ = + declare_parameter("dynamic_replan_interval_sec", 1.0); + dynamic_replan_stop_distance_ = + declare_parameter("dynamic_replan_stop_distance", 0.5); + recovery_backup_speed_ = declare_parameter("recovery_backup_speed", -0.2); + recovery_backup_distance_ = declare_parameter("recovery_backup_distance", 0.04); + recovery_backup_timeout_sec_ = + declare_parameter("recovery_backup_timeout_sec", 0.2); + planning_failure_backup_speed_ = + declare_parameter("planning_failure_backup_speed", -0.1); + planning_failure_backup_distance_ = + declare_parameter("planning_failure_backup_distance", 0.03); + planning_failure_backup_timeout_sec_ = + declare_parameter("planning_failure_backup_timeout_sec", 0.3); + recovery_clear_wait_sec_ = declare_parameter("recovery_clear_wait_sec", 1.0); + pass_through_vlm_trigger_radius_ = + declare_parameter("pass_through_vlm_trigger_radius", 0.35); + 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); + recovery_search_radius_m_ = declare_parameter("recovery_search_radius_m", 1.0); + recovery_rear_clear_distance_m_ = + declare_parameter("recovery_rear_clear_distance_m", 0.5); + + const auto vlm_capture_mode = + declare_parameter("vlm_capture_mode", "stop"); + vlm_capture_mode_ = vlmCaptureModeFromString(vlm_capture_mode); + if (vlm_capture_mode_ == VlmCaptureMode::Unknown) { + throw std::invalid_argument("vlm_capture_mode must be 'stop' or 'pass_through'"); + } + + sign_qr_enable_ = declare_parameter("sign_qr_enable", 0); + sign_qr_disable_ = declare_parameter("sign_qr_disable", 5); + sign_vlm_trigger_ = declare_parameter("sign_vlm_trigger", 9); + sign_profile_normal_ = declare_parameter("sign_profile_normal", 10); + sign_profile_task2_ = declare_parameter("sign_profile_task2", 11); + + qr_pose_ = singlePoseFromParameter( + "qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392}); + entry_pose_ = singlePoseFromParameter( + "entry_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562}); + post_qr_pose_ = singlePoseFromParameter( + "post_qr_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562}); + vlm_waypoint_number_ = declare_parameter("vlm_waypoint_number", 2); + + const auto clockwise_defaults = std::vector{ + 0.7534956931109804, 3.766501234923627, 1.5592359900104606, + 3.8567885105264077, 4.329424195458402, 0.03997856199797794, + 2.5144339572664096, 2.4337692660037766, -1.6101462328054222}; + const auto counterclockwise_defaults = std::vector{ + 4.2128251001861265, 3.7376334819031842, 1.5450283923222128, + 1.1720794040064113, 4.319801449605877, 3.1112984779040144, + 2.4951887885861144, 2.318297930897253, -1.539556591616333}; + + clockwise_route_.label = "顺时针"; + clockwise_route_.waypoints = posesFromFlatDoubles( + declare_parameter>("clockwise_waypoints", clockwise_defaults), frame_id_); + clockwise_route_.home_pose = + singlePoseFromParameter( + "clockwise_home_pose", {0.536986899408154, 0.1772662932069835, + -1.652041913138445}); + + counterclockwise_route_.label = "逆时针"; + counterclockwise_route_.waypoints = posesFromFlatDoubles( + declare_parameter>( + "counterclockwise_waypoints", + counterclockwise_defaults), + frame_id_); + counterclockwise_route_.home_pose = + singlePoseFromParameter( + "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835, + -1.652041913138445}); + + candidate_waypoint_json_dir_ = declare_parameter( + "candidate_waypoint_json_dir", "/home/sunrise/yiliao_ws/src/racing_control/config/waypoints"); + qr_candidate_group_name_ = declare_parameter("qr_candidate_group_name", "qr"); + entry_candidate_group_name_ = + declare_parameter("entry_candidate_group_name", "entry"); + vlm_candidate_group_name_ = + declare_parameter("vlm_candidate_group_name", "goal_005"); + clockwise_candidate_source_file_ = declare_parameter( + "clockwise_candidate_source_file", "main_1.json"); + counterclockwise_candidate_source_file_ = declare_parameter( + "counterclockwise_candidate_source_file", "main_2.json"); + candidate_selector_.setGroups( + loadCandidateWaypointGroups(candidate_waypoint_json_dir_, frame_id_)); + } + + geometry_msgs::msg::PoseStamped singlePoseFromParameter( + const std::string & name, const std::vector & defaults) + { + const auto poses = posesFromFlatDoubles( + declare_parameter>(name, defaults), frame_id_); + if (poses.size() != 1) { + throw std::invalid_argument(name + " must contain exactly one x/y/yaw triple"); + } + return poses.front(); + } + + void startKeyboardThread() + { + 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(), + "no usable TTY for SPACE start; run from an interactive ssh tty or set auto_start:=true"); + return; + } + + keyboard_thread_ = std::thread( + [this, keyboard_fd, close_keyboard_fd]() { + termios old_termios {}; + 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(keyboard_fd, TCSANOW, &raw); + + while (!stop_keyboard_.load()) { + fd_set read_set; + FD_ZERO(&read_set); + FD_SET(keyboard_fd, &read_set); + timeval timeout {}; + timeout.tv_sec = 0; + timeout.tv_usec = 200000; + 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(keyboard_fd, &c, 1) == 1 && c == ' ') { + start_requested_.store(true); + } + } + } + + tcsetattr(keyboard_fd, TCSANOW, &old_termios); + if (close_keyboard_fd) { + close(keyboard_fd); + } + }); + } + + void tick() + { + if (start_requested_.exchange(false)) { + startRace(); + } + + if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) { + return; + } + + if (recovery_in_progress_) { + return; + } + + const auto elapsed = (now() - stage_start_).seconds(); + if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) { + if (stage_ == Stage::WaitForQr) { + finishStage("QR wait timeout"); + failRace("QR wait timed out before route direction was selected"); + return; + } + if (stage_ == Stage::WaitForVlm) { + RCLCPP_WARN(get_logger(), "VLM capture wait timed out; continuing route"); + finishStage("VLM capture wait timeout"); + runRemainingRouteSegment(); + return; + } + failRace("stage timed out: " + std::string(stageName(stage_))); + return; + } + + if (stage_ == Stage::SwitchToTask2Profile && elapsed >= profile_switch_wait_sec_) { + finishStage("profile switch wait complete"); + runFirstRouteSegment(); + return; + } + + if (stage_ == Stage::SwitchToNormalProfile && elapsed >= profile_switch_wait_sec_) { + finishStage("normal profile restored"); + runReturnOrigin(); + return; + } + + if (stage_ == Stage::WaitForQr && selected_direction_ != RouteDirection::Unknown && + (now() - qr_result_time_).seconds() >= post_qr_wait_sec_) + { + finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label); + runQrTransitNavigation(); + return; + } + + if (stage_ == Stage::WaitForVlm && elapsed >= vlm_capture_wait_sec_) { + finishStage("VLM capture window elapsed"); + runRemainingRouteSegment(); + return; + } + + if (stage_ == Stage::ExecuteCirclePath) { + maybeTriggerPassThroughVlmCapture(); + maybeRunDynamicReplanning(); + } + + if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) { + finishStage("route segment final pose reached"); + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { + runVlmWait(); + } else if (active_segment_ == RouteSegment::AfterVlm || + active_segment_ == RouteSegment::FullRoute) + { + finishRace(); + } + } + } + + void startRace() + { + if (race_started_) { + RCLCPP_WARN(get_logger(), "race already started"); + return; + } + race_started_ = true; + race_start_ = now(); + RCLCPP_INFO(get_logger(), "race started"); + runQrNavigation(); + } + + void startStage(const Stage stage, const double timeout_sec) + { + stage_ = stage; + stage_start_ = now(); + stage_timeout_sec_ = timeout_sec; + RCLCPP_INFO(get_logger(), "%s task started", stageName(stage_)); + } + + void finishStage(const std::string & detail) + { + const auto stage_elapsed = (now() - stage_start_).seconds(); + const auto total_elapsed = (now() - race_start_).seconds(); + RCLCPP_INFO( + get_logger(), "%s task finished: %s | task %.2fs | total %.2fs", + stageName(stage_), detail.c_str(), stage_elapsed, total_elapsed); + } + + void failRace(const std::string & reason) + { + stage_ = Stage::Failed; + recovery_in_progress_ = false; + if (recovery_timer_) { + recovery_timer_->cancel(); + } + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; + cancelActiveFollowGoal(); + publishRecoveryVelocity(0.0); + publishSign(sign_qr_disable_); + RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str()); + } + + void finishRace() + { + finishStage("origin reached"); + stage_ = Stage::Finished; + publishSign(sign_qr_disable_); + RCLCPP_INFO(get_logger(), "race finished | total %.2fs", (now() - race_start_).seconds()); + } + + void publishSign(const int value) + { + std_msgs::msg::Int32 msg; + msg.data = value; + for (int i = 0; i < 3; ++i) { + sign_pub_->publish(msg); + } + RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value); + } + + void startStartupQrEnablePublisher() + { + startup_qr_enable_publish_count_ = 0; + publishStartupQrEnableOnce(); + startup_qr_enable_timer_ = create_wall_timer( + std::chrono::milliseconds(defaultStartupQrEnableIntervalMs()), + [this]() {publishStartupQrEnableOnce();}); + } + + void publishStartupQrEnableOnce() + { + if (race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) { + if (startup_qr_enable_timer_) { + startup_qr_enable_timer_->cancel(); + } + return; + } + if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats()) { + if (startup_qr_enable_timer_) { + startup_qr_enable_timer_->cancel(); + } + return; + } + publishSign(sign_qr_enable_); + ++startup_qr_enable_publish_count_; + if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats() && + startup_qr_enable_timer_) + { + startup_qr_enable_timer_->cancel(); + } + } + + void disableQrDetectionOnce() + { + if (qr_detection_disabled_) { + return; + } + qr_detection_disabled_ = true; + publishSign(sign_qr_disable_); + } + + void runQrNavigation() + { + latest_qr_result_.clear(); + selected_direction_ = RouteDirection::Unknown; + qr_detection_disabled_ = false; + resetRouteRecoveryState(); + active_segment_ = RouteSegment::ToQr; + active_segment_waypoints_ = { + selectCandidatePose(qr_candidate_group_name_, "", qr_pose_).value_or(qr_pose_)}; + active_segment_next_waypoint_index_ = 0; + publishSign(sign_profile_normal_); + publishSign(sign_qr_enable_); + runRouteSegmentPlanning("start to QR pose"); + } + + void runQrWait() + { + startStage(Stage::WaitForQr, qr_result_timeout_sec_); + if (!latest_qr_result_.empty()) { + qr_result_time_ = now(); + } + if (post_qr_wait_sec_ > 0.0) { + stage_timeout_sec_ += post_qr_wait_sec_; + } + } + + void runQrTransitNavigation() + { + disableQrDetectionOnce(); + cancelActiveFollowGoal(); + runSwitchToTask2Profile(); + } + + void runPostQrNavigation() + { + disableQrDetectionOnce(); + startStage(Stage::NavigatePostQr, navigation_timeout_sec_); + sendNavigateGoal( + post_qr_pose_, [this](const bool ok) { + if (stage_ != Stage::NavigatePostQr) { + RCLCPP_DEBUG(get_logger(), "stale post-QR navigation result ignored"); + return; + } + finishStage(ok ? "reached " + poseSummary(post_qr_pose_) : "navigation failed"); + if (!ok) { + failRace("failed to reach post-QR pose"); + return; + } + runEntryNavigation(); + }); + } + + void runEntryNavigation() + { + disableQrDetectionOnce(); + startStage(Stage::NavigateToEntry, navigation_timeout_sec_); + sendNavigateGoal( + entry_pose_, [this](const bool ok) { + if (stage_ != Stage::NavigateToEntry) { + RCLCPP_DEBUG(get_logger(), "stale entry navigation result ignored"); + return; + } + finishStage(ok ? "reached " + poseSummary(entry_pose_) : "navigation failed"); + if (!ok) { + failRace("failed to reach entry pose"); + return; + } + runSwitchToTask2Profile(); + }); + } + + void runSwitchToTask2Profile() + { + if (selected_direction_ == RouteDirection::Unknown) { + failRace("cannot switch to task two before QR route direction is known"); + return; + } + publishSign(sign_profile_task2_); + startStage(Stage::SwitchToTask2Profile, profile_switch_wait_sec_ + 2.0); + } + + void runFirstRouteSegment() + { + const auto & route = selectedRoute(); + const auto vlm_index = vlmWaypointIndex(route); + latest_vlm_result_.clear(); + vlm_capture_triggered_ = false; + resetRouteRecoveryState(); + 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_); + } + + auto route_waypoints = route.waypoints; + if (vlm_index < route_waypoints.size()) { + route_waypoints[vlm_index] = selectCandidatePose( + vlm_candidate_group_name_, preferredCandidateSourceFile(), + route_waypoints[vlm_index]).value_or(route_waypoints[vlm_index]); + } + const auto selected_entry_pose = selectCandidatePose( + entry_candidate_group_name_, preferredCandidateSourceFile(), + entry_pose_).value_or(entry_pose_); + const auto qr_segment = routeWaypointsAfterQr( + selected_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(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); + resetRouteRecoveryState(); + active_segment_ = RouteSegment::AfterVlm; + active_segment_waypoints_ = remainingWaypoints(route.waypoints, vlm_index + 1); + active_segment_waypoints_.push_back(route.home_pose); + active_segment_next_waypoint_index_ = 0; + publishSign(sign_profile_normal_); + runRouteSegmentPlanning("VLM to home"); + } + + void runRouteSegmentPlanning(const std::string & label) + { + if (active_segment_waypoints_.empty()) { + failRace("route segment must contain at least one pose"); + return; + } + startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_); + + if (!compute_path_client_->wait_for_action_server(2s)) { + onRoutePlanningFailed("ComputePathThroughPoses action server is not available"); + return; + } + + ComputePathThroughPoses::Goal goal; + goal.goals = stampPoses(active_segment_waypoints_); + goal.planner_id = planner_id_; + goal.use_start = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (stage_ != Stage::ComputeCirclePath) { + RCLCPP_DEBUG(get_logger(), "stale route planning goal response ignored"); + return; + } + if (!goal_handle) { + onRoutePlanningFailed("circle path planning goal was rejected"); + } + }; + options.result_callback = + [this](const ComputeGoalHandle::WrappedResult & result) { + if (stage_ != Stage::ComputeCirclePath) { + return; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || + !result.result || result.result->path.poses.empty()) + { + finishStage("planning failed"); + onRoutePlanningFailed("circle path planning failed"); + return; + } + active_path_ = result.result->path; + finishStage( + "planned " + std::to_string(active_path_.poses.size()) + " path poses"); + runRouteSegmentExecution(); + }; + + compute_path_client_->async_send_goal(goal, options); + RCLCPP_INFO(get_logger(), "planning selected route segment: %s", label.c_str()); + } + + void runRouteSegmentExecution() + { + startStage(Stage::ExecuteCirclePath, circle_timeout_sec_); + dynamic_replan_in_flight_ = false; + dynamic_replan_consecutive_failures_ = 0; + last_dynamic_replan_time_ = now(); + if (use_trajectory_guard_) { + auto path = stampPath(active_path_); + guard_path_pub_->publish(path); + RCLCPP_INFO( + get_logger(), "published route path poses=%zu to %s", + path.poses.size(), guard_input_topic_.c_str()); + return; + } + sendActiveRouteFollowPath(); + } + + void sendActiveRouteFollowPath() + { + sendFollowPath( + active_path_, [this](const bool ok) { + finishStage(ok ? "FollowPath succeeded" : "FollowPath failed"); + if (!ok) { + onRouteFollowFailed("route segment FollowPath failed"); + return; + } + if (active_segment_ == RouteSegment::ToQr) { + runQrWait(); + } else if (active_segment_ == RouteSegment::ToVlm) { + runVlmWait(); + } else if (active_segment_ == RouteSegment::AfterVlm || + active_segment_ == RouteSegment::FullRoute) + { + finishRace(); + } + }); + } + + void runSwitchToNormalProfile() + { + publishSign(sign_profile_normal_); + startStage(Stage::SwitchToNormalProfile, profile_switch_wait_sec_ + 2.0); + } + + void runVlmWait() + { + triggerVlmCaptureOnce("stopped at VLM waypoint"); + startStage(Stage::WaitForVlm, vlm_capture_wait_sec_ + 2.0); + } + + void triggerVlmCaptureOnce(const std::string & reason) + { + if (vlm_capture_triggered_) { + return; + } + vlm_capture_triggered_ = true; + publishSingleVlmImageFrame(); + publishSign(sign_vlm_trigger_); + RCLCPP_INFO(get_logger(), "VLM capture triggered: %s", reason.c_str()); + } + + void publishSingleVlmImageFrame() + { + sensor_msgs::msg::CompressedImage::SharedPtr image; + { + std::lock_guard lock(image_mutex_); + if (latest_image_) { + image = std::make_shared(*latest_image_); + } + } + + if (!shouldPublishVlmImageFrame(enable_vlm_image_relay_, static_cast(image))) { + if (enable_vlm_image_relay_) { + RCLCPP_WARN( + get_logger(), "VLM image relay enabled but no image has been received from %s", + vlm_image_input_topic_.c_str()); + } + return; + } + + image->header.stamp = now(); + vlm_image_pub_->publish(*image); + RCLCPP_INFO( + get_logger(), "published one VLM image frame %s -> %s", + vlm_image_input_topic_.c_str(), vlm_image_output_topic_.c_str()); + } + + void onGlobalCostmap(nav_msgs::msg::OccupancyGrid::SharedPtr msg) + { + std::lock_guard lock(costmap_mutex_); + latest_global_costmap_ = std::move(msg); + candidate_selector_.setCostmap(*latest_global_costmap_); + } + + std::optional selectCandidatePose( + const std::string & group_name, + const std::string & preferred_source_file, + const geometry_msgs::msg::PoseStamped & fallback_pose) + { + if (!enable_candidate_waypoint_selection_) { + return fallback_pose; + } + { + std::lock_guard lock(costmap_mutex_); + if (!latest_global_costmap_) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no global costmap yet", + group_name.c_str()); + return fallback_pose; + } + } + const auto selected = candidate_selector_.select( + group_name, 50, false, preferred_source_file); + if (!selected) { + RCLCPP_WARN( + get_logger(), "candidate selection fallback for %s: no usable candidate", + group_name.c_str()); + return fallback_pose; + } + RCLCPP_INFO( + get_logger(), "candidate selected for %s from %s -> %s", + group_name.c_str(), selected->source_file.c_str(), selected->point_name.c_str()); + return selected->pose; + } + + void maybeTriggerPassThroughVlmCapture() + { + if (active_segment_ != RouteSegment::FullRoute || vlm_capture_triggered_) { + return; + } + + const auto & route = selectedRoute(); + const auto vlm_index = vlmWaypointIndex(route); + geometry_msgs::msg::PoseStamped current; + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_) { + return; + } + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + } + + if (distance2d(current, route.waypoints[vlm_index]) <= pass_through_vlm_trigger_radius_) { + triggerVlmCaptureOnce("passing VLM waypoint"); + } + } + + void maybeRunDynamicReplanning() + { + if (use_trajectory_guard_ || active_segment_waypoints_.empty()) { + return; + } + const auto current = currentPoseFromOdom(); + if (!current) { + return; + } + updateActiveSegmentWaypointProgress(*current); + if (active_segment_next_waypoint_index_ >= active_segment_waypoints_.size()) { + return; + } + const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back()); + const auto elapsed = (now() - last_dynamic_replan_time_).seconds(); + if (!shouldDynamicReplan( + enable_dynamic_replanning_, dynamic_replan_in_flight_, distance_to_goal, + dynamic_replan_stop_distance_, elapsed, dynamic_replan_interval_sec_)) + { + return; + } + + last_dynamic_replan_time_ = now(); + dynamic_replan_in_flight_ = true; + runDynamicRouteReplanning(*current); + } + + 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 = remainingWaypointsAfterProgress( + active_segment_waypoints_, current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + if (replan_goals.empty()) { + dynamic_replan_in_flight_ = false; + return; + } + + ComputePathThroughPoses::Goal goal; + goal.goals = stampPoses(replan_goals); + goal.planner_id = planner_id_; + goal.use_start = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) { + onDynamicReplanFailed("dynamic route planning goal was rejected"); + } + }; + options.result_callback = + [this](const ComputeGoalHandle::WrappedResult & result) { + if (stage_ != Stage::ExecuteCirclePath) { + dynamic_replan_in_flight_ = false; + return; + } + dynamic_replan_in_flight_ = false; + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || + result.result->path.poses.empty()) + { + onDynamicReplanFailed("dynamic route planning failed"); + return; + } + dynamic_replan_consecutive_failures_ = 0; + active_path_ = result.result->path; + RCLCPP_INFO( + get_logger(), "dynamic replan succeeded: path poses=%zu", + active_path_.poses.size()); + sendActiveRouteFollowPath(); + }; + + compute_path_client_->async_send_goal(goal, options); + RCLCPP_INFO( + get_logger(), "dynamic route replanning through %zu remaining waypoint(s), next index=%zu", + replan_goals.size(), active_segment_next_waypoint_index_); + } + + void updateActiveSegmentWaypointProgress(const geometry_msgs::msg::PoseStamped & current) + { + const auto previous_index = active_segment_next_waypoint_index_; + active_segment_next_waypoint_index_ = advanceReachedWaypointIndex( + active_segment_waypoints_, current, active_segment_next_waypoint_index_, + circle_goal_tolerance_); + if (active_segment_next_waypoint_index_ != previous_index) { + RCLCPP_INFO( + get_logger(), "route waypoint progress advanced: next index %zu -> %zu", + previous_index, active_segment_next_waypoint_index_); + } + } + + void onDynamicReplanFailed(const std::string & reason) + { + dynamic_replan_in_flight_ = false; + ++dynamic_replan_consecutive_failures_; + RCLCPP_WARN( + get_logger(), "%s | consecutive dynamic replan failures=%d", + reason.c_str(), dynamic_replan_consecutive_failures_); + if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) { + onRoutePlanningFailed(reason); + } + } + + void resetRouteRecoveryState() + { + route_recovery_phase_ = RouteRecoveryPhase::Initial; + planning_backup_attempted_ = false; + final_recovery_attempts_ = 0; + } + + void prepareRouteRecoveryReplan() + { + 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; + } + + void onRoutePlanningFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Planning, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::PlanningBackup) { + planning_backup_attempted_ = true; + route_recovery_phase_ = RouteRecoveryPhase::AfterPlanningBackup; + RCLCPP_WARN( + get_logger(), "%s; planning failed, backing up at %.2f m/s for %.2f s before one replan", + reason.c_str(), planning_failure_backup_speed_, planning_failure_backup_timeout_sec_); + startBackupRecovery( + reason, planning_failure_backup_speed_, planning_failure_backup_distance_, + planning_failure_backup_timeout_sec_, [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after planning-failure backup"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void onRouteFollowFailed(const std::string & reason) + { + const auto action = routeRecoveryAction( + route_recovery_phase_, RouteFailureKind::Following, + final_recovery_attempts_, max_recovery_attempts_); + if (action == RouteRecoveryAction::Fail) { + failRace(reason); + return; + } + + if (action == RouteRecoveryAction::ClearLocalAndReplan) { + route_recovery_phase_ = RouteRecoveryPhase::AfterLocalClearReplan; + RCLCPP_WARN( + get_logger(), "%s; clearing local costmap, waiting for update, then replanning once", + reason.c_str()); + clearLocalCostmapBeforeRecoveryReplan( + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after local costmap clear"); + }); + return; + } + + ++final_recovery_attempts_; + route_recovery_phase_ = RouteRecoveryPhase::AfterFinalBackupReplan; + RCLCPP_WARN( + get_logger(), "%s; final backup/replan attempt %d/%d", + reason.c_str(), final_recovery_attempts_, max_recovery_attempts_); + startBackupRecovery( + reason, recovery_backup_speed_, recovery_backup_distance_, recovery_backup_timeout_sec_, + [this]() { + prepareRouteRecoveryReplan(); + runRouteSegmentPlanning("after final backup"); + }); + } + + void startBackupRecovery( + const std::string & reason, + const double speed, + const double distance, + const double timeout_sec, + std::function on_recovered) + { + if (!enable_recovery_) { + failRace(reason); + return; + } + recovery_in_progress_ = true; + recovery_done_callback_ = std::move(on_recovered); + recovery_start_time_ = now(); + recovery_start_pose_ = currentPoseFromOdom(); + recovery_active_speed_ = speed; + recovery_active_distance_ = distance; + recovery_active_timeout_sec_ = timeout_sec; + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + recovery_after_clear_callback_ = nullptr; + recovery_costmap_clear_pending_ = 0; + cancelActiveFollowGoal(); + + RCLCPP_WARN( + get_logger(), "%s; backup recovery speed=%.2f m/s distance=%.2f m timeout=%.2f s", + reason.c_str(), speed, distance, timeout_sec); + + if (recovery_timer_) { + recovery_timer_->cancel(); + } + recovery_timer_ = create_wall_timer(20ms, [this]() {tickRecoveryBackup();}); + } + + void tickRecoveryBackup() + { + const auto elapsed = (now() - recovery_start_time_).seconds(); + const auto backup_distance = recoveryBackupDistance(); + if (!recoveryBackupComplete( + backup_distance, recovery_active_distance_, elapsed, recovery_active_timeout_sec_)) + { + publishRecoveryVelocity(recovery_active_speed_); + return; + } + + publishRecoveryVelocity(0.0); + if (recovery_timer_) { + recovery_timer_->cancel(); + } + recovery_in_progress_ = false; + auto callback = std::move(recovery_done_callback_); + recovery_done_callback_ = nullptr; + if (callback) { + callback(); + } + } + + void clearLocalCostmapBeforeRecoveryReplan(std::function on_cleared) + { + recovery_after_clear_callback_ = std::move(on_cleared); + recovery_costmap_clear_pending_ = 0; + const auto requested = requestEntireCostmapClear( + clear_local_costmap_client_, local_costmap_clear_service_); + if (!requested) { + startRecoveryReplanAfterClearWait(); + } + } + + bool requestEntireCostmapClear( + const rclcpp::Client::SharedPtr & client, + const std::string & service_name) + { + if (!client) { + return false; + } + if (!client->wait_for_service(200ms)) { + RCLCPP_WARN( + get_logger(), "costmap clear service not available: %s", service_name.c_str()); + return false; + } + + ++recovery_costmap_clear_pending_; + auto request = std::make_shared(); + client->async_send_request( + request, + [this, service_name](rclcpp::Client::SharedFuture) { + if (recovery_costmap_clear_pending_ > 0) { + --recovery_costmap_clear_pending_; + } + RCLCPP_INFO(get_logger(), "cleared costmap via %s", service_name.c_str()); + if (recovery_costmap_clear_pending_ == 0) { + startRecoveryReplanAfterClearWait(); + } + }); + return true; + } + + void startRecoveryReplanAfterClearWait() + { + if (!recovery_after_clear_callback_) { + return; + } + + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + + if (recovery_clear_wait_sec_ <= 0.0) { + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + return; + } + + recovery_clear_wait_timer_ = create_wall_timer( + std::chrono::duration(recovery_clear_wait_sec_), + [this]() { + if (recovery_clear_wait_timer_) { + recovery_clear_wait_timer_->cancel(); + } + auto callback = std::move(recovery_after_clear_callback_); + recovery_after_clear_callback_ = nullptr; + if (callback) { + callback(); + } + }); + } + + double recoveryBackupDistance() const + { + if (!recovery_start_pose_) { + return 0.0; + } + const auto current = currentPoseFromOdom(); + if (!current) { + return 0.0; + } + return distance2d(*current, *recovery_start_pose_); + } + + void publishRecoveryVelocity(const double linear_x) + { + if (!recovery_cmd_vel_pub_) { + return; + } + geometry_msgs::msg::Twist cmd; + cmd.linear.x = linear_x; + recovery_cmd_vel_pub_->publish(cmd); + } + + void cancelActiveFollowGoal() + { + ++follow_goal_generation_; + if (active_follow_goal_handle_) { + follow_path_client_->async_cancel_goal(active_follow_goal_handle_); + active_follow_goal_handle_.reset(); + } + } + + void runReturnOrigin() + { + startStage(Stage::ReturnOrigin, navigation_timeout_sec_); + sendNavigateGoal( + selectedRoute().home_pose, [this](const bool ok) { + if (!ok) { + finishStage("navigation failed"); + failRace("failed to return origin"); + return; + } + finishRace(); + }); + } + + void sendNavigateGoal( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done) + { + sendNavigateGoalAttempt(pose, std::move(on_done), 3, stage_); + } + + void sendNavigateGoalAttempt( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done, + const int retries_remaining, + const Stage expected_stage) + { + if (!retryActionServerWait( + [this]() {return navigate_client_->wait_for_action_server(800ms);}, 3)) + { + failRace("NavigateToPose action server is not available"); + return; + } + + NavigateToPose::Goal goal; + goal.pose = stampPose(pose); + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this, pose, on_done, retries_remaining, expected_stage]( + NavigateGoalHandle::SharedPtr goal_handle) mutable { + if (!goal_handle) { + retryNavigateGoalOrFinish( + pose, std::move(on_done), retries_remaining, expected_stage, + "NavigateToPose goal was rejected"); + } + }; + options.result_callback = + [this, pose, on_done, retries_remaining, expected_stage]( + const NavigateGoalHandle::WrappedResult & result) mutable { + if (stage_ != expected_stage) { + RCLCPP_DEBUG(get_logger(), "stale NavigateToPose result ignored"); + return; + } + const bool succeeded = result.code == rclcpp_action::ResultCode::SUCCEEDED; + if (shouldRetryNavigateGoal(succeeded, retries_remaining)) { + retryNavigateGoalOrFinish( + pose, std::move(on_done), retries_remaining, expected_stage, + "NavigateToPose result was not successful"); + return; + } + on_done(succeeded); + }; + + navigate_client_->async_send_goal(goal, options); + } + + void retryNavigateGoalOrFinish( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done, + const int retries_remaining, + const Stage expected_stage, + const std::string & reason) + { + if (stage_ != expected_stage) { + RCLCPP_DEBUG(get_logger(), "stale NavigateToPose retry ignored: %s", reason.c_str()); + return; + } + if (!shouldRetryNavigateGoal(false, retries_remaining)) { + on_done(false); + return; + } + RCLCPP_WARN( + get_logger(), "%s; retrying NavigateToPose goal, retries remaining: %d", + reason.c_str(), retries_remaining); + sendNavigateGoalAttempt(pose, std::move(on_done), retries_remaining - 1, expected_stage); + } + + void sendFollowPath(const nav_msgs::msg::Path & path, std::function on_done) + { + if (!follow_path_client_->wait_for_action_server(2s)) { + onRouteFollowFailed("FollowPath action server is not available"); + return; + } + + cancelActiveFollowGoal(); + const auto generation = follow_goal_generation_; + FollowPath::Goal goal; + goal.path = stampPath(path); + goal.controller_id = controller_id_; + goal.goal_checker_id = goal_checker_id_; + + auto callback = std::move(on_done); + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable { + if (generation != follow_goal_generation_) { + return; + } + if (!goal_handle) { + active_follow_goal_handle_.reset(); + callback(false); + return; + } + active_follow_goal_handle_ = goal_handle; + }; + options.result_callback = + [this, callback, generation]( + const FollowGoalHandle::WrappedResult & result) mutable { + if (generation != follow_goal_generation_) { + RCLCPP_DEBUG(get_logger(), "stale FollowPath result ignored"); + return; + } + active_follow_goal_handle_.reset(); + callback(result.code == rclcpp_action::ResultCode::SUCCEEDED); + }; + + follow_path_client_->async_send_goal(goal, options); + } + + geometry_msgs::msg::PoseStamped stampPose(geometry_msgs::msg::PoseStamped pose) + { + pose.header.stamp = now(); + if (pose.header.frame_id.empty()) { + pose.header.frame_id = frame_id_; + } + return pose; + } + + std::vector stampPoses( + std::vector poses) + { + for (auto & pose : poses) { + pose = stampPose(pose); + } + return poses; + } + + nav_msgs::msg::Path stampPath(nav_msgs::msg::Path path) + { + path.header.frame_id = path.header.frame_id.empty() ? frame_id_ : path.header.frame_id; + path.header.stamp = now(); + for (auto & pose : path.poses) { + pose.header.stamp = path.header.stamp; + if (pose.header.frame_id.empty()) { + pose.header.frame_id = path.header.frame_id; + } + } + return path; + } + + std::optional currentPoseFromOdom() const + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_) { + return std::nullopt; + } + geometry_msgs::msg::PoseStamped current; + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + return current; + } + + std::size_t vlmWaypointIndex(const RouteConfig & route) const + { + if (vlm_waypoint_number_ <= 0) { + throw std::runtime_error("vlm_waypoint_number must be >= 1"); + } + const auto index = static_cast(vlm_waypoint_number_ - 1); + if (index >= route.waypoints.size()) { + throw std::runtime_error("vlm_waypoint_number exceeds selected route waypoint count"); + } + return index; + } + + const RouteConfig & selectedRoute() const + { + if (selected_direction_ == RouteDirection::Clockwise) { + return clockwise_route_; + } + if (selected_direction_ == RouteDirection::Counterclockwise) { + return counterclockwise_route_; + } + throw std::runtime_error("route direction is not selected"); + } + + std::string preferredCandidateSourceFile() const + { + if (selected_direction_ == RouteDirection::Clockwise) { + return clockwise_candidate_source_file_; + } + if (selected_direction_ == RouteDirection::Counterclockwise) { + return counterclockwise_candidate_source_file_; + } + return ""; + } + + bool routeSegmentReached() const + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_ || active_segment_waypoints_.empty()) { + return false; + } + geometry_msgs::msg::PoseStamped current; + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + return distance2d(current, active_segment_waypoints_.back()) <= circle_goal_tolerance_; + } + + void onOdom(nav_msgs::msg::Odometry::SharedPtr msg) + { + std::lock_guard lock(odom_mutex_); + latest_odom_ = std::move(msg); + } + + void onImage(sensor_msgs::msg::CompressedImage::SharedPtr msg) + { + std::lock_guard lock(image_mutex_); + latest_image_ = std::move(msg); + } + + void onQrResult(std_msgs::msg::String::SharedPtr msg) + { + if (msg->data.empty()) { + return; + } + const auto direction = directionFromQrResult(msg->data); + if (direction == RouteDirection::Unknown) { + RCLCPP_WARN( + get_logger(), "QR result received but route direction is unknown: %s", + msg->data.c_str()); + return; + } + if (!shouldAcceptQrDirection(selected_direction_, direction)) { + RCLCPP_DEBUG( + get_logger(), "QR result ignored after route direction was selected: %s", + msg->data.c_str()); + return; + } + latest_qr_result_ = msg->data; + qr_result_time_ = now(); + selected_direction_ = direction; + 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 || + ((stage_ == Stage::ComputeCirclePath || stage_ == Stage::ExecuteCirclePath) && + active_segment_ == RouteSegment::ToQr)) + { + finishStage("QR result: " + latest_qr_result_ + " -> " + selectedRoute().label); + runQrTransitNavigation(); + } + } + + void onVlmResult(std_msgs::msg::String::SharedPtr msg) + { + if (msg->data.empty()) { + return; + } + latest_vlm_result_ = msg->data; + vlm_result_time_ = now(); + RCLCPP_INFO(get_logger(), "VLM result received: %s", latest_vlm_result_.c_str()); + } + + Stage stage_{Stage::Idle}; + bool race_started_{false}; + 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}; + bool enable_candidate_waypoint_selection_{true}; + bool qr_detection_disabled_{false}; + bool dynamic_replan_in_flight_{false}; + bool recovery_in_progress_{false}; + rclcpp::Time race_start_{0, 0, RCL_ROS_TIME}; + rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME}; + rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME}; + double stage_timeout_sec_{0.0}; + + std::string frame_id_; + std::string sign_topic_; + std::string qr_result_topic_; + std::string vlm_result_topic_; + std::string odom_topic_; + std::string recovery_cmd_vel_topic_; + std::string global_costmap_topic_; + std::string global_costmap_clear_service_; + std::string local_costmap_clear_service_; + std::string vlm_image_input_topic_; + std::string vlm_image_output_topic_; + std::string navigate_action_; + std::string compute_path_action_; + std::string follow_path_action_; + std::string guard_input_topic_; + std::string planner_id_; + std::string controller_id_; + std::string goal_checker_id_; + std::string candidate_waypoint_json_dir_; + std::string qr_candidate_group_name_; + std::string entry_candidate_group_name_; + std::string vlm_candidate_group_name_; + std::string clockwise_candidate_source_file_; + std::string counterclockwise_candidate_source_file_; + + double navigation_timeout_sec_{120.0}; + double path_planning_timeout_sec_{30.0}; + double circle_timeout_sec_{120.0}; + double qr_result_timeout_sec_{8.0}; + double profile_switch_wait_sec_{1.0}; + double post_qr_wait_sec_{1.0}; + double vlm_capture_wait_sec_{0.5}; + double dynamic_replan_interval_sec_{1.0}; + double dynamic_replan_stop_distance_{0.5}; + double recovery_backup_speed_{-0.2}; + double recovery_backup_distance_{0.04}; + double recovery_backup_timeout_sec_{0.2}; + double planning_failure_backup_speed_{-0.1}; + double planning_failure_backup_distance_{0.03}; + double planning_failure_backup_timeout_sec_{0.3}; + double recovery_clear_wait_sec_{1.0}; + double recovery_active_speed_{0.0}; + double recovery_active_distance_{0.0}; + double recovery_active_timeout_sec_{0.0}; + double recovery_search_radius_m_{1.0}; + double recovery_rear_clear_distance_m_{0.5}; + double pass_through_vlm_trigger_radius_{0.35}; + double circle_goal_tolerance_{0.30}; + + int sign_qr_enable_{0}; + int sign_qr_disable_{5}; + int sign_vlm_trigger_{9}; + int sign_profile_normal_{10}; + int sign_profile_task2_{11}; + int vlm_waypoint_number_{2}; + int dynamic_replan_max_consecutive_failures_{3}; + int dynamic_replan_consecutive_failures_{0}; + int max_recovery_attempts_{2}; + int final_recovery_attempts_{0}; + + geometry_msgs::msg::PoseStamped qr_pose_; + geometry_msgs::msg::PoseStamped post_qr_pose_; + geometry_msgs::msg::PoseStamped entry_pose_; + RouteConfig clockwise_route_; + RouteConfig counterclockwise_route_; + RouteDirection selected_direction_{RouteDirection::Unknown}; + VlmCaptureMode vlm_capture_mode_{VlmCaptureMode::Stop}; + RouteSegment active_segment_{RouteSegment::None}; + RouteRecoveryPhase route_recovery_phase_{RouteRecoveryPhase::Initial}; + bool planning_backup_attempted_{false}; + std::vector active_segment_waypoints_; + std::size_t active_segment_next_waypoint_index_{0}; + nav_msgs::msg::Path active_path_; + bool vlm_capture_triggered_{false}; + std::optional recovery_start_pose_; + std::function recovery_done_callback_; + std::function recovery_after_clear_callback_; + std::size_t recovery_costmap_clear_pending_{0}; + + std::string latest_qr_result_; + std::string latest_vlm_result_; + rclcpp::Time qr_result_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time vlm_result_time_{0, 0, RCL_ROS_TIME}; + nav_msgs::msg::Odometry::SharedPtr latest_odom_; + nav_msgs::msg::OccupancyGrid::SharedPtr latest_global_costmap_; + sensor_msgs::msg::CompressedImage::SharedPtr latest_image_; + mutable std::mutex odom_mutex_; + mutable std::mutex costmap_mutex_; + mutable std::mutex image_mutex_; + + rclcpp::Publisher::SharedPtr sign_pub_; + rclcpp::Publisher::SharedPtr guard_path_pub_; + rclcpp::Publisher::SharedPtr vlm_image_pub_; + rclcpp::Publisher::SharedPtr recovery_cmd_vel_pub_; + rclcpp::Subscription::SharedPtr qr_sub_; + rclcpp::Subscription::SharedPtr vlm_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr global_costmap_sub_; + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp_action::Client::SharedPtr navigate_client_; + rclcpp_action::Client::SharedPtr compute_path_client_; + rclcpp_action::Client::SharedPtr follow_path_client_; + rclcpp::Client::SharedPtr clear_global_costmap_client_; + rclcpp::Client::SharedPtr clear_local_costmap_client_; + FollowGoalHandle::SharedPtr active_follow_goal_handle_; + rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_; + rclcpp::TimerBase::SharedPtr recovery_timer_; + rclcpp::TimerBase::SharedPtr recovery_clear_wait_timer_; + rclcpp::TimerBase::SharedPtr tick_timer_; + CandidateWaypointSelector candidate_selector_; + + int startup_qr_enable_publish_count_{0}; + std::uint64_t follow_goal_generation_{0}; + std::atomic start_requested_{false}; + std::atomic stop_keyboard_{false}; + std::thread keyboard_thread_; +}; + +} // namespace racing_control + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + try { + rclcpp::spin(std::make_shared()); + } catch (const std::exception & e) { + RCLCPP_FATAL(rclcpp::get_logger("racing_control"), "%s", e.what()); + rclcpp::shutdown(); + return 1; + } + rclcpp::shutdown(); + return 0; +} diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index 9972c94..d87365a 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/src/racing_control/test/test_racing_control_helpers.cpp @@ -1,7 +1,11 @@ #include +#include +#include #include #include "gtest/gtest.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" #include "racing_control/racing_control.hpp" namespace @@ -216,6 +220,57 @@ TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout) EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2)); } +TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::PlanningBackup); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterPlanningBackup, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::Fail); +} + +TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::ClearLocalAndReplan); +} + +TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); +} + +TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Following, 1, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Planning, 2, 2), + racing_control::RouteRecoveryAction::Fail); +} + TEST(RacingControlHelpers, ParsesVlmCaptureMode) { EXPECT_EQ( @@ -228,3 +283,106 @@ TEST(RacingControlHelpers, ParsesVlmCaptureMode) racing_control::vlmCaptureModeFromString("unknown"), racing_control::VlmCaptureMode::Unknown); } + +TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson) +{ + const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test"; + std::filesystem::remove_all(dir); + std::filesystem::create_directories(dir); + { + std::ofstream json(dir / "main.json"); + json << + R"json({ + "points": [ + { + "name": "qr", + "yaw_degrees": 90.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 1.0, "y": 2.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + }, + { + "name": "entry", + "yaw_degrees": 0.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 3.0, "y": 4.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + } + ] + })json"; + } + + const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom"); + + ASSERT_EQ(groups.size(), 2U); + ASSERT_EQ(groups.at("qr").size(), 1U); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance); + EXPECT_EQ(groups.at("entry")[0].source_file, "main.json"); +} + +TEST(CandidateWaypointSelector, SkipsOccupiedCandidates) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 1.0; + costmap.info.width = 5; + costmap.info.height = 5; + costmap.info.origin.position.x = 0.0; + costmap.info.origin.position.y = 0.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(25, 0); + costmap.data[1 * 5 + 1] = 80; + + racing_control::CandidateWaypointSelector selector; + selector.setGroups( + { + {"qr", { + {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")}, + {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")}, + }}, + }); + selector.setCostmap(costmap); + + const auto selected = selector.select("qr", 50, false); + + ASSERT_TRUE(selected.has_value()); + EXPECT_EQ(selected->source_file, "free.json"); + EXPECT_EQ(selected->point_name, "qr_free"); +} + +TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 0.25; + costmap.info.width = 9; + costmap.info.height = 9; + costmap.info.origin.position.x = -1.0; + costmap.info.origin.position.y = -1.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(81, 100); + const auto set_free = [&](const int mx, const int my) { + costmap.data[my * static_cast(costmap.info.width) + mx] = 0; + }; + set_free(6, 4); + set_free(7, 4); + + const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom"); + const auto command = racing_control::nearestFreeRecoveryCommand( + costmap, current, 50, false, 1.0); + + ASSERT_TRUE(command.has_value()); + EXPECT_GT(command->linear_x, 0.0); + EXPECT_DOUBLE_EQ(command->angular_z, 0.0); +} diff --git a/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ new file mode 100644 index 0000000..a92d3d5 --- /dev/null +++ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup.md5~ @@ -0,0 +1 @@ +0af1c0d18ed8a7db4d7f58cb8d1f948a test_racing_control_helpers.cpp diff --git a/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ new file mode 100644 index 0000000..d23c65e --- /dev/null +++ b/src/racing_control/test/test_racing_control_helpers.cpp.unc-backup~ @@ -0,0 +1,386 @@ +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "racing_control/candidate_waypoint_selector.hpp" +#include "racing_control/racing_control.hpp" + +namespace +{ + +constexpr double kTolerance = 1e-6; + +} // namespace + +TEST(RacingControlHelpers, ConvertsFlatTriplesToStampedPoses) +{ + const auto poses = racing_control::posesFromFlatDoubles( + {1.0, 2.0, M_PI_2, -0.5, 0.25, -M_PI}, + "odom"); + + ASSERT_EQ(poses.size(), 2U); + EXPECT_EQ(poses[0].header.frame_id, "odom"); + EXPECT_DOUBLE_EQ(poses[0].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(poses[0].pose.position.y, 2.0); + EXPECT_NEAR(poses[0].pose.orientation.z, std::sin(M_PI_4), kTolerance); + EXPECT_NEAR(poses[0].pose.orientation.w, std::cos(M_PI_4), kTolerance); + EXPECT_DOUBLE_EQ(poses[1].pose.position.x, -0.5); + EXPECT_DOUBLE_EQ(poses[1].pose.position.y, 0.25); +} + +TEST(RacingControlHelpers, RejectsIncompletePoseTriples) +{ + EXPECT_THROW( + racing_control::posesFromFlatDoubles({1.0, 2.0, 0.0, 3.0}, "odom"), + std::invalid_argument); +} + +TEST(RacingControlHelpers, ParsesQrDirectionFromText) +{ + EXPECT_EQ( + racing_control::directionFromQrResult("7 顺时针"), + racing_control::RouteDirection::Clockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("8 逆时针"), + racing_control::RouteDirection::Counterclockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("5"), + racing_control::RouteDirection::Clockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("6"), + racing_control::RouteDirection::Counterclockwise); + EXPECT_EQ( + racing_control::directionFromQrResult("未识别"), + racing_control::RouteDirection::Unknown); +} + +TEST(RacingControlHelpers, LatchesFirstKnownQrDirection) +{ + EXPECT_TRUE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Unknown, + racing_control::RouteDirection::Clockwise)); + EXPECT_FALSE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Unknown, + racing_control::RouteDirection::Unknown)); + EXPECT_FALSE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Clockwise, + racing_control::RouteDirection::Clockwise)); + EXPECT_FALSE( + racing_control::shouldAcceptQrDirection( + racing_control::RouteDirection::Clockwise, + racing_control::RouteDirection::Counterclockwise)); +} + +TEST(RacingControlHelpers, SelectsPostQrTransitTargetWhenEnabled) +{ + EXPECT_EQ( + racing_control::qrTransitTargetAfterRecognition(true), + racing_control::QrTransitTarget::PostQr); + EXPECT_EQ( + racing_control::qrTransitTargetAfterRecognition(false), + racing_control::QrTransitTarget::Entry); +} + +TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable) +{ + EXPECT_TRUE(racing_control::shouldPublishVlmImageFrame(true, true)); + EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, true)); + EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(true, false)); + EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false)); +} + +TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution) +{ + EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard()); +} + +TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing) +{ + int wait_calls = 0; + const bool result = racing_control::retryActionServerWait( + [&]() { + ++wait_calls; + return false; + }, + 3); + + EXPECT_FALSE(result); + EXPECT_EQ(wait_calls, 4); +} + +TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure) +{ + EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3)); + EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3)); + EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0)); +} + +TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst) +{ + EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5); + EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200); +} + +TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal) +{ + EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0)); +} + +TEST(RacingControlHelpers, AdvancesNextWaypointOnlyAfterItIsReached) +{ + 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"); + + EXPECT_EQ( + racing_control::advanceReachedWaypointIndex( + 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.50), + 2U); +} + +TEST(RacingControlHelpers, DynamicReplanningKeepsRemainingUnreachedWaypoints) +{ + 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 remaining = racing_control::remainingWaypoints(waypoints, 1); + + ASSERT_EQ(remaining.size(), 2U); + EXPECT_DOUBLE_EQ(remaining[0].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(remaining[1].pose.position.x, 2.0); + EXPECT_TRUE(racing_control::remainingWaypoints(waypoints, waypoints.size()).empty()); + 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)); + EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2)); + EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2)); +} + +TEST(RacingControlHelpers, InitialPlanningFailureUsesOnePlanningBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::PlanningBackup); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterPlanningBackup, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::Fail); +} + +TEST(RacingControlHelpers, FollowFailureClearsOnlyLocalCostmapBeforeReplanning) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::Initial, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::ClearLocalAndReplan); +} + +TEST(RacingControlHelpers, FailedLocalClearReplanUsesFinalBackup) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Planning, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterLocalClearReplan, + racing_control::RouteFailureKind::Following, 0, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); +} + +TEST(RacingControlHelpers, FinalRecoveryFailsAfterConfiguredAttemptLimit) +{ + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Following, 1, 2), + racing_control::RouteRecoveryAction::FinalBackupReplan); + EXPECT_EQ( + racing_control::routeRecoveryAction( + racing_control::RouteRecoveryPhase::AfterFinalBackupReplan, + racing_control::RouteFailureKind::Planning, 2, 2), + racing_control::RouteRecoveryAction::Fail); +} + +TEST(RacingControlHelpers, ParsesVlmCaptureMode) +{ + EXPECT_EQ( + racing_control::vlmCaptureModeFromString("stop"), + racing_control::VlmCaptureMode::Stop); + EXPECT_EQ( + racing_control::vlmCaptureModeFromString("pass_through"), + racing_control::VlmCaptureMode::PassThrough); + EXPECT_EQ( + racing_control::vlmCaptureModeFromString("unknown"), + racing_control::VlmCaptureMode::Unknown); +} + +TEST(CandidateWaypointSelector, LoadsNamedCandidatesFromSavedPointJson) +{ + const auto dir = std::filesystem::temp_directory_path() / "racing_control_candidates_test"; + std::filesystem::remove_all(dir); + std::filesystem::create_directories(dir); + { + std::ofstream json(dir / "main.json"); + json << R"json({ + "points": [ + { + "name": "qr", + "yaw_degrees": 90.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 1.0, "y": 2.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + }, + { + "name": "entry", + "yaw_degrees": 0.0, + "odom": { + "pose": { + "pose": { + "position": {"x": 3.0, "y": 4.0, "z": 0.0}, + "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0} + } + } + } + } + ] + })json"; + } + + const auto groups = racing_control::loadCandidateWaypointGroups(dir.string(), "odom"); + + ASSERT_EQ(groups.size(), 2U); + ASSERT_EQ(groups.at("qr").size(), 1U); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(groups.at("qr")[0].pose.pose.position.y, 2.0); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.z, std::sin(M_PI_4), kTolerance); + EXPECT_NEAR(groups.at("qr")[0].pose.pose.orientation.w, std::cos(M_PI_4), kTolerance); + EXPECT_EQ(groups.at("entry")[0].source_file, "main.json"); +} + +TEST(CandidateWaypointSelector, SkipsOccupiedCandidates) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 1.0; + costmap.info.width = 5; + costmap.info.height = 5; + costmap.info.origin.position.x = 0.0; + costmap.info.origin.position.y = 0.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(25, 0); + costmap.data[1 * 5 + 1] = 80; + + racing_control::CandidateWaypointSelector selector; + selector.setGroups({ + {"qr", { + {"qr", "blocked.json", "qr_blocked", racing_control::poseFromXYYaw(1.2, 1.2, 0.0, "odom")}, + {"qr", "free.json", "qr_free", racing_control::poseFromXYYaw(3.2, 1.2, 0.0, "odom")}, + }}, + }); + selector.setCostmap(costmap); + + const auto selected = selector.select("qr", 50, false); + + ASSERT_TRUE(selected.has_value()); + EXPECT_EQ(selected->source_file, "free.json"); + EXPECT_EQ(selected->point_name, "qr_free"); +} + +TEST(CandidateWaypointSelector, FindsNearestFreeRecoveryCommand) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.info.resolution = 0.25; + costmap.info.width = 9; + costmap.info.height = 9; + costmap.info.origin.position.x = -1.0; + costmap.info.origin.position.y = -1.0; + costmap.info.origin.orientation.w = 1.0; + costmap.data.assign(81, 100); + const auto set_free = [&](const int mx, const int my) { + costmap.data[my * static_cast(costmap.info.width) + mx] = 0; + }; + set_free(6, 4); + set_free(7, 4); + + const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom"); + const auto command = racing_control::nearestFreeRecoveryCommand( + costmap, current, 50, false, 1.0); + + ASSERT_TRUE(command.has_value()); + EXPECT_GT(command->linear_x, 0.0); + EXPECT_DOUBLE_EQ(command->angular_z, 0.0); +} diff --git a/src/vlm_detect/launch/vlm_detect.launch.py b/src/vlm_detect/launch/vlm_detect.launch.py index 34ae249..e743efb 100644 --- a/src/vlm_detect/launch/vlm_detect.launch.py +++ b/src/vlm_detect/launch/vlm_detect.launch.py @@ -24,6 +24,12 @@ def generate_launch_description(): prompt_text = LaunchConfiguration('prompt_text') max_tokens = LaunchConfiguration('max_tokens') image_max_dim = LaunchConfiguration('image_max_dim') + use_tcp = LaunchConfiguration('use_tcp') + use_serial = LaunchConfiguration('use_serial') + tcp_host = LaunchConfiguration('tcp_host') + tcp_port = LaunchConfiguration('tcp_port') + serial_port = LaunchConfiguration('serial_port') + serial_baud = LaunchConfiguration('serial_baud') audio_sink = LaunchConfiguration('audio_sink') tts_speed = LaunchConfiguration('tts_speed') @@ -45,6 +51,12 @@ def generate_launch_description(): 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_use_tcp = DeclareLaunchArgument('use_tcp', default_value='true') + declare_use_serial = DeclareLaunchArgument('use_serial', default_value='false') + declare_tcp_host = DeclareLaunchArgument('tcp_host', default_value='192.168.127.1') + declare_tcp_port = DeclareLaunchArgument('tcp_port', default_value='9216') + declare_serial_port = DeclareLaunchArgument('serial_port', default_value='/dev/ttyUSB0') + declare_serial_baud = DeclareLaunchArgument('serial_baud', default_value='921600') 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') @@ -66,6 +78,12 @@ def generate_launch_description(): 'prompt_text': prompt_text, 'max_tokens': max_tokens, 'image_max_dim': image_max_dim, + 'use_tcp': use_tcp, + 'use_serial': use_serial, + 'tcp_host': tcp_host, + 'tcp_port': tcp_port, + 'serial_port': serial_port, + 'serial_baud': serial_baud, }], ) @@ -104,6 +122,12 @@ def generate_launch_description(): declare_prompt_text, declare_max_tokens, declare_image_max_dim, + declare_use_tcp, + declare_use_serial, + declare_tcp_host, + declare_tcp_port, + declare_serial_port, + declare_serial_baud, declare_audio_sink, declare_tts_speed, LogInfo(msg=['Config: ', config_file]), diff --git a/src/vlm_detect/vlm_detect/__pycache__/tcp_vlm_client.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/tcp_vlm_client.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b30e22c7f91a7249375281446c5f715057d9bfe3 GIT binary patch literal 5919 zcma)AUu+b|8K2qR+uPeaAI33`fsmXiO>#|Q0!m1V(iG|dA_)!!8j72Ab-r2KhtGG% z>>kFKy_5oN66Fu66eVh)t=%>fC~eX{Rf(cT>U-4pd2D3Bedz;gr4m)`@0-0lpKU~+(3&1WsK^cu;qfBsNl=&#vZ)4QZZ*$bt zZ)?;N=4isVg>{yXCh<<-?ciKee_W3J0npybZy(@2hu<-l$uKnTc z)psuGl~>-nboK3@etG(gi;;zkmli(y^_5ruboIlJbN6ri=Jug&J02g}_SmCCJ08vd z3ovcp#TZ4KF{sm8u?d{asZ7ktSVQUuL1J5JI--Z97k={D!bhL{ zpOTK%3CgkX*?GwOMo&j`wm&wsZINwZY|+C(MdDlgM}5+WVLZVAnutftW08TDMttvy63`^5~wI1dzWd(kDT=rtKTuV(KH65W#vQ^TQQl%7n9-R@8 zY;10Gmp9gRGh(Q{YLNuIj8F#y^k_nVhfc`m*gT8OJ6Y2byqO4j1iOulDJG9ZMhp$I zrXBG)BQ%8(S@DiRcGj5ZoHde|u_Ak(HGsbs; z2`e%#uwTGaoZ-jV31@~y$>3?Mz}aU&L6m%qb<+}LqXYzQk6Z`IY~uZ&)9;gSH~OSs zJeh0paA$6!8s={Bt&NSvYNY}TfCa;_tAQ_aW3#!ymnVIhca%jNlv(yG$_@OmwOyIh zg;UBRjLHtG#VJ3GWk8macjO&-K}eWTyrO56Bm9CWmnuLuRv%Zk^rs7@ib@nG>y@dM zvP*k}+Vf8pis44!3O1w3aEoW~ALItl;!iz`8~?E^hdVs;PbqS!iLUBs22a$P8vsK=WzOu&4g$ZSeLReVIyNpK|J=*7o4`Q))t1dL(<-L3W zjBBi4&Ze&QMY7EHk9gGK(;EO%EMmvO4`B#XSZ|t5BQiu{%8(m6;|OUO2PZ_5#w+{; ztb0!HnKy*99;Oq*JVdHB>VkXmcp^%OG?afQOeYg1pz?-RaD5`?dNgJ`f&q$OHFyI9Z1lrgqlZqB5BXwN#@f5p+W`p|CD1MH6V-zce~oJX*EqaC!17Av@0T zfYBrvs3|+H6}$1r#&N(2z&4)bxK`xQnM=W&xe?S|@QWC0S`bzmCX$J)c@qW@Lp>2E zD9p}f=iz}cx`qtk1*2&LT=lVrro{TaTrEalnwtE-wtebr=9>JMV$%}@~j@|sccUSD} zoBf=#rVDSq2?jn8x!{7PmN1B9(|y^!DczrT(977X2XDyfZget(G6m zZ7x)T8D9d1rvT`e3pF@z7=ICC-$5{#nhH7Ne{@qQDr$8^wUxMTEj!=;#nIP~A%f7#qfwxP_w zv)r)SQ1+`0+yI7nrE}W&$}rndqi>lz*2flc;<6tZ`7S&<61HZwfwCqJ{3TReY0dZ*;p5Pm3+9nnom^Lml zFl}TP6S|x;sEUVeBpaJnQ#rg;7q4O&v72;B9;K#j5UqN%Wf01a%g?TBbrk3XZ6lx5 zxc9aPFL5ZJPze;vepxv!?F^KQ6y6J~Q+_2->Cm69`Lcj~!q(0j6w}1=h?w4=Q0&_wh+(Ugw*ki&Q5o)(|EL zw|7LRkcvt2P$-9HYd%{4RM?KHby-#>nnr@M@trsLgK|5THu@{ooQ}J>QYF{sx{fE3 zgs+mQdn(nLin8NKp3f+ItPuD+9#Q55>Z$wmdF2x7Ul)ag41<|cIH^)&J06MSo=OC@ zaw&|%-xyZQ?@^;IjTAh;CU&hpjglXb=D zX34rp4WXU-TaQFvn~Q@87*}U5)nYoSb$CDGhi~BWCBUmJo(yppp0?i`xCv?a$hFFVP3$2V>R{0Jc{lZV zbgW8ueI%1a#0qX|SNh1mq?uszKvP^@sV;yd4|v! zOAO9u<&S9lK5D3Vjq~VD)O!GpGOJR^{RBQu%`?;-poT6KlSVkZ&hM~IIZwo2S)U}O U2`I>0JZD)Z*)$4S6MSp_53g!fd;kCd literal 0 HcmV?d00001 diff --git a/src/vlm_detect/vlm_detect/serial_vlm_client.py b/src/vlm_detect/vlm_detect/serial_vlm_client.py new file mode 100644 index 0000000..3585175 --- /dev/null +++ b/src/vlm_detect/vlm_detect/serial_vlm_client.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python3 +"""Serial VLM Client — 通过串口调用 VLM 推理。 +用在客户端 (192.168.175.65)，模拟 OpenAI client.chat.completions.create 接口。 +""" + +import json, struct, time, serial + +MAGIC = b'\xAB\xCD' +FLAG_CMD = ord('C') +FLAG_IMG = ord('I') +FLAG_RESP = ord('R') + + +class SerialVLMError(Exception): + pass + + +class SerialVLMClient: + """串口 VLM 客户端，兼容 OpenAI client.chat.completions.create 调用模式。""" + + def __init__(self, port='/dev/ttyUSB0', baud=921600, timeout=95): + self.port = port + self.baud = baud + self.timeout = timeout + self._ser = None + + def _connect(self): + if self._ser is None or not self._ser.is_open: + self._ser = serial.Serial(self.port, self.baud, timeout=0.1) + self._ser.reset_input_buffer() + self._ser.reset_output_buffer() + + def _send_packet(self, flag, data): + if isinstance(data, str): + data = data.encode('utf-8') + self._ser.write(MAGIC) + self._ser.write(bytes([flag])) + self._ser.write(struct.pack('>I', len(data))) + self._ser.write(data) + self._ser.flush() + + def _recv_exact(self, n, timeout=15): + deadline = time.time() + timeout + buf = b'' + while len(buf) < n: + remain = n - len(buf) + chunk = self._ser.read(remain) + if chunk: + buf += chunk + if time.time() > deadline: + raise SerialVLMError(f"recv timeout: got {len(buf)}/{n}") + if not chunk: + time.sleep(0.005) + return buf + + def _recv_packet(self): + while True: + b = self._ser.read(1) + if not b: + continue + if b == b'\xAB': + b2 = self._ser.read(1) + if b2 == b'\xCD': + break + flag = self._recv_exact(1)[0] + length = struct.unpack('>I', self._recv_exact(4))[0] + data = self._recv_exact(length) + return flag, data + + def infer(self, image_bytes: bytes, prompt: str) -> dict: + """发送图片+提示词，返回 {"success": bool, "answer": str, "elapsed_sec": float}""" + self._connect() + try: + # Reset buffers + self._ser.reset_input_buffer() + self._ser.reset_output_buffer() + + # Send command + cmd = json.dumps({"prompt": prompt, "image_len": len(image_bytes)}) + self._send_packet(FLAG_CMD, cmd) + + # Send image + self._send_packet(FLAG_IMG, image_bytes) + + # Receive response + t0 = time.time() + flag, data = self._recv_packet() + elapsed = time.time() - t0 + + if flag != FLAG_RESP: + return {"success": False, "error": f"unexpected flag: {flag}"} + + result = json.loads(data.decode('utf-8')) + return result + + except SerialVLMError: + return {"success": False, "error": "serial timeout"} + except Exception as e: + return {"success": False, "error": str(e)} + + def close(self): + if self._ser and self._ser.is_open: + self._ser.close() + self._ser = None + + # ---- OpenAI-compatible interface ---- + class _FakeChoices: + class _Msg: + def __init__(self, content): self.content = content + def __init__(self, content): + self.message = self._Msg(content) + + class _FakeResponse: + def __init__(self, content): + self.choices = [SerialVLMClient._FakeChoices(content)] + + class _FakeCompletions: + def __init__(self, client): self._client = client + def create(self, *, model=None, messages=None, max_tokens=None, temperature=None, timeout=None): + # Extract image and text from OpenAI-format messages + import base64 + image_bytes = None + prompt = "" + for msg in (messages or []): + content = msg.get("content", "") + if isinstance(content, list): + for item in content: + t = item.get("type", "") + if t == "text": + prompt = item.get("text", "") + elif t == "image_url": + url = item.get("image_url", {}).get("url", "") + if url.startswith("data:") and "," in url: + image_bytes = base64.b64decode(url.split(",", 1)[1]) + if image_bytes is None: + raise SerialVLMError("no image in messages") + + result = self._client.infer(image_bytes, prompt) + if not result.get("success"): + raise SerialVLMError(result.get("error", "unknown")) + return SerialVLMClient._FakeResponse(result["answer"]) + + class _FakeChat: + def __init__(self, client): self.completions = SerialVLMClient._FakeCompletions(client) + + @property + def chat(self): + return self._FakeChat(self) + + +# Convenience function +def serial_infer(image_bytes, prompt, port='/dev/ttyUSB0', baud=921600): + client = SerialVLMClient(port=port, baud=baud) + try: + return client.infer(image_bytes, prompt) + finally: + client.close() diff --git a/src/vlm_detect/vlm_detect/tcp_vlm_client.py b/src/vlm_detect/vlm_detect/tcp_vlm_client.py new file mode 100644 index 0000000..30ebd0a --- /dev/null +++ b/src/vlm_detect/vlm_detect/tcp_vlm_client.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +"""TCP VLM Client — 通过 TCP 套接字调用 VLM 推理。 +用在客户端 (192.168.175.65)，模拟 OpenAI client.chat.completions.create 接口。 +""" + +import json, struct, time, socket + + +MAGIC = b'\xAB\xCD' +FLAG_CMD = ord('C') +FLAG_IMG = ord('I') +FLAG_RESP = ord('R') + + +class TCPVLMError(Exception): + pass + + +class TCPVLMClient: + """TCP VLM 客户端，兼容 OpenAI client.chat.completions.create 调用模式。""" + + def __init__(self, host='192.168.127.1', port=9216, timeout=95): + self.host = host + self.port = port + self.timeout = timeout + + def _recv_exact(self, sock, n, timeout=15): + deadline = time.time() + timeout + buf = b'' + while len(buf) < n: + remain = n - len(buf) + sock.settimeout(max(0.1, deadline - time.time())) + try: + chunk = sock.recv(remain) + except socket.timeout: + if time.time() > deadline: + raise TCPVLMError(f"recv timeout: got {len(buf)}/{n}") + continue + if not chunk: + raise TCPVLMError("connection closed by server") + buf += chunk + return buf + + def _send_packet(self, sock, flag, data): + if isinstance(data, str): + data = data.encode('utf-8') + sock.sendall(MAGIC) + sock.sendall(bytes([flag])) + sock.sendall(struct.pack('>I', len(data))) + sock.sendall(data) + + def _recv_packet(self, sock): + while True: + b = self._recv_exact(sock, 1, timeout=60) + if b == b'\xAB': + b2 = self._recv_exact(sock, 1, timeout=5) + if b2 == b'\xCD': + break + flag = self._recv_exact(sock, 1)[0] + length = struct.unpack('>I', self._recv_exact(sock, 4))[0] + if length > 1024 * 1024: + raise TCPVLMError(f"packet too large: {length}") + data = self._recv_exact(sock, length, timeout=30) + return flag, data + + def infer(self, image_bytes: bytes, prompt: str) -> dict: + """发送图片+提示词，返回 {"success": bool, "answer": str, "elapsed_sec": float}""" + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.settimeout(self.timeout) + try: + sock.connect((self.host, self.port)) + + # Send command + cmd = json.dumps({"prompt": prompt, "image_len": len(image_bytes)}) + self._send_packet(sock, FLAG_CMD, cmd) + + # Send image + self._send_packet(sock, FLAG_IMG, image_bytes) + + # Receive response + t0 = time.time() + flag, data = self._recv_packet(sock) + elapsed = time.time() - t0 + + if flag != FLAG_RESP: + return {"success": False, "error": f"unexpected flag: {flag}"} + + result = json.loads(data.decode('utf-8')) + return result + + except TCPVLMError: + return {"success": False, "error": "TCP timeout"} + except Exception as e: + return {"success": False, "error": str(e)} + finally: + try: + sock.close() + except Exception: + pass + + # ---- OpenAI-compatible interface ---- + class _FakeChoices: + class _Msg: + def __init__(self, content): self.content = content + def __init__(self, content): + self.message = self._Msg(content) + + class _FakeResponse: + def __init__(self, content): + self.choices = [TCPVLMClient._FakeChoices(content)] + + class _FakeCompletions: + def __init__(self, client): self._client = client + def create(self, *, model=None, messages=None, max_tokens=None, temperature=None, timeout=None): + import base64 + image_bytes = None + prompt = "" + for msg in (messages or []): + content = msg.get("content", "") + if isinstance(content, list): + for item in content: + t = item.get("type", "") + if t == "text": + prompt = item.get("text", "") + elif t == "image_url": + url = item.get("image_url", {}).get("url", "") + if url.startswith("data:") and "," in url: + image_bytes = base64.b64decode(url.split(",", 1)[1]) + if image_bytes is None: + raise TCPVLMError("no image in messages") + result = self._client.infer(image_bytes, prompt) + if not result.get("success"): + raise TCPVLMError(result.get("error", "unknown")) + return TCPVLMClient._FakeResponse(result["answer"]) + + class _FakeChat: + def __init__(self, client): self.completions = TCPVLMClient._FakeCompletions(client) + + @property + def chat(self): + return self._FakeChat(self) + + +def tcp_infer(image_bytes, prompt, host='192.168.127.1', port=9216): + client = TCPVLMClient(host=host, port=port) + return client.infer(image_bytes, prompt) diff --git a/src/vlm_detect/vlm_detect/vlm_node.py b/src/vlm_detect/vlm_detect/vlm_node.py index da61829..f1b8571 100644 --- a/src/vlm_detect/vlm_detect/vlm_node.py +++ b/src/vlm_detect/vlm_detect/vlm_node.py @@ -12,11 +12,25 @@ import time import numpy as np from origincar_msg.srv import Speak +# TCP VLM client (optional) +try: + from .tcp_vlm_client import TCPVLMClient, TCPVLMError + HAS_TCP = True +except ImportError: + HAS_TCP = False + +# Serial VLM client (optional) +try: + from .serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + class VLMProcessor(Node): def __init__(self): super().__init__('vlm_detect') - self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_host', 'http://192.168.175.64:8000') self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') self.declare_parameter('image_topic', '/image_mjpeg') self.declare_parameter('trigger_topic', '/sign4return') @@ -28,6 +42,12 @@ class VLMProcessor(Node): self.declare_parameter('temperature', 0.1) self.declare_parameter('crop_ratio', 0.45) self.declare_parameter('auto_crop', False) + self.declare_parameter('use_tcp', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('tcp_host', '192.168.127.1') + self.declare_parameter('tcp_port', 9216) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) vlm_host = self.get_parameter('vlm_host').value vlm_model = self.get_parameter('vlm_model').value @@ -41,12 +61,39 @@ class VLMProcessor(Node): self.temperature = self.get_parameter('temperature').value self.crop_ratio = self.get_parameter('crop_ratio').value self.auto_crop = self.get_parameter('auto_crop').value + self.use_tcp = self.get_parameter('use_tcp').value + self.use_serial = self.get_parameter('use_serial').value + self.tcp_host = self.get_parameter('tcp_host').value + self.tcp_port = self.get_parameter('tcp_port').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: TCP > Serial > HTTP + self.tcp_client = None + self.serial_client = None + self.client = None + + if self.use_tcp: + if not HAS_TCP: + self.get_logger().fatal("tcp_vlm_client not found") + raise RuntimeError("tcp_vlm_client module required for use_tcp=True") + self.tcp_client = TCPVLMClient(host=self.tcp_host, port=self.tcp_port) + self.get_logger().info(f"VLM using TCP | {self.tcp_host}:{self.tcp_port}") + elif self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") - self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") self.vlm_model = vlm_model self.latest_image = None self.image_lock = threading.Lock() self._busy = False + self._vlm_done = False # one-shot: ignore triggers after first self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) @@ -56,7 +103,8 @@ class VLMProcessor(Node): while not self.tts_client.wait_for_service(timeout_sec=5.0): self.get_logger().info('Waiting for TTS service...') - self.get_logger().info(f"VLM ready | host={vlm_host} | dim={self.image_max_dim} | crop={self.auto_crop}") + mode = "TCP" if self.use_tcp else ("SERIAL" if self.use_serial else "HTTP") + self.get_logger().info(f"VLM ready | mode={mode} | dim={self.image_max_dim} | crop={self.auto_crop}") def image_callback(self, msg): with self.image_lock: @@ -69,10 +117,14 @@ class VLMProcessor(Node): def sign_callback(self, msg): if msg.data != self.trigger_sign: return + if self._vlm_done: + self.get_logger().warn("VLM already triggered, skip") + return if self._busy: self.get_logger().warn("Busy, skip") return - self.get_logger().info(f"Trigger {msg.data}") + self._vlm_done = True + self.get_logger().info(f"Trigger {msg.data} (first and only)") with self.image_lock: if self.latest_image is None: self.get_logger().warning("No image, using fallback") @@ -115,17 +167,36 @@ class VLMProcessor(Node): else: new_w, new_h = w, h _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) - b64 = base64.b64encode(jpeg.tobytes()).decode('utf-8') + jpeg_bytes = jpeg.tobytes() t0 = time.time() - resp = self.client.chat.completions.create( - model=self.vlm_model, - messages=[{"role": "user", "content": [ - {"type": "text", "text": self.prompt_text}, - {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} - ]}], - max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) - self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg)//1024}KB") - return resp.choices[0].message.content + + if self.use_tcp: + result = self.tcp_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"TCP inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"TCP {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + elif self.use_serial: + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + else: + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"HTTP {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content def _detect_and_crop(self, img): h, w = img.shape[:2] diff --git a/src/vlm_detect/vlm_detect/vlm_node_http.py.bak b/src/vlm_detect/vlm_detect/vlm_node_http.py.bak new file mode 100644 index 0000000..da61829 --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_http.py.bak @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + self.get_logger().info(f"VLM ready | host={vlm_host} | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + b64 = base64.b64encode(jpeg.tobytes()).decode('utf-8') + t0 = time.time() + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vlm_detect/vlm_detect/vlm_node_serial.py b/src/vlm_detect/vlm_detect/vlm_node_serial.py new file mode 100644 index 0000000..fbc86a3 --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_serial.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + +# Serial VLM client (optional) +try: + from serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + self.use_serial = self.get_parameter('use_serial').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: serial or HTTP + if self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found, falling back to serial") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.client = None # not needed for serial mode + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.serial_client = None + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") + + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + self.get_logger().info(f"VLM ready | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + jpeg_bytes = jpeg.tobytes() + t0 = time.time() + + if self.use_serial: + # Serial mode + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + else: + # HTTP mode (unchanged) + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vlm_detect/vlm_detect/vlm_node_serial.py.bak b/src/vlm_detect/vlm_detect/vlm_node_serial.py.bak new file mode 100644 index 0000000..fbc86a3 --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_serial.py.bak @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + +# Serial VLM client (optional) +try: + from serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.10.189:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + self.use_serial = self.get_parameter('use_serial').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: serial or HTTP + if self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found, falling back to serial") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.client = None # not needed for serial mode + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.serial_client = None + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") + + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + self.get_logger().info(f"VLM ready | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + jpeg_bytes = jpeg.tobytes() + t0 = time.time() + + if self.use_serial: + # Serial mode + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + else: + # HTTP mode (unchanged) + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"API {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vlm_detect/vlm_detect/vlm_node_tcp.py b/src/vlm_detect/vlm_detect/vlm_node_tcp.py new file mode 100644 index 0000000..10e76de --- /dev/null +++ b/src/vlm_detect/vlm_detect/vlm_node_tcp.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32, String +from sensor_msgs.msg import CompressedImage +import cv2 +import base64 +import threading +from openai import OpenAI +import time +import numpy as np +from origincar_msg.srv import Speak + +# TCP VLM client (optional) +try: + from tcp_vlm_client import TCPVLMClient, TCPVLMError + HAS_TCP = True +except ImportError: + HAS_TCP = False + +# Serial VLM client (optional) +try: + from serial_vlm_client import SerialVLMClient, SerialVLMError + HAS_SERIAL = True +except ImportError: + HAS_SERIAL = False + + +class VLMProcessor(Node): + def __init__(self): + super().__init__('vlm_detect') + self.declare_parameter('vlm_host', 'http://192.168.175.64:8000') + self.declare_parameter('vlm_model', '/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + self.declare_parameter('image_topic', '/image_mjpeg') + self.declare_parameter('trigger_topic', '/sign4return') + self.declare_parameter('trigger_sign', 9) + self.declare_parameter('result_topic', '/vlm_result') + self.declare_parameter('prompt_text', '') + self.declare_parameter('max_tokens', 30) + self.declare_parameter('image_max_dim', 96) + self.declare_parameter('temperature', 0.1) + self.declare_parameter('crop_ratio', 0.45) + self.declare_parameter('auto_crop', False) + self.declare_parameter('use_tcp', False) + self.declare_parameter('use_serial', False) + self.declare_parameter('tcp_host', '192.168.127.1') + self.declare_parameter('tcp_port', 9216) + self.declare_parameter('serial_port', '/dev/ttyUSB0') + self.declare_parameter('serial_baud', 921600) + + vlm_host = self.get_parameter('vlm_host').value + vlm_model = self.get_parameter('vlm_model').value + image_topic = self.get_parameter('image_topic').value + trigger_topic = self.get_parameter('trigger_topic').value + self.trigger_sign = self.get_parameter('trigger_sign').value + result_topic = self.get_parameter('result_topic').value + self.prompt_text = self.get_parameter('prompt_text').value + self.max_tokens = self.get_parameter('max_tokens').value + self.image_max_dim = self.get_parameter('image_max_dim').value + self.temperature = self.get_parameter('temperature').value + self.crop_ratio = self.get_parameter('crop_ratio').value + self.auto_crop = self.get_parameter('auto_crop').value + self.use_tcp = self.get_parameter('use_tcp').value + self.use_serial = self.get_parameter('use_serial').value + self.tcp_host = self.get_parameter('tcp_host').value + self.tcp_port = self.get_parameter('tcp_port').value + self.serial_port = self.get_parameter('serial_port').value + self.serial_baud = self.get_parameter('serial_baud').value + + # Init client: TCP > Serial > HTTP + self.tcp_client = None + self.serial_client = None + self.client = None + + if self.use_tcp: + if not HAS_TCP: + self.get_logger().fatal("tcp_vlm_client not found") + raise RuntimeError("tcp_vlm_client module required for use_tcp=True") + self.tcp_client = TCPVLMClient(host=self.tcp_host, port=self.tcp_port) + self.get_logger().info(f"VLM using TCP | {self.tcp_host}:{self.tcp_port}") + elif self.use_serial: + if not HAS_SERIAL: + self.get_logger().fatal("serial_vlm_client not found") + raise RuntimeError("serial_vlm_client module required for use_serial=True") + self.serial_client = SerialVLMClient(port=self.serial_port, baud=self.serial_baud) + self.get_logger().info(f"VLM using SERIAL | port={self.serial_port} baud={self.serial_baud}") + else: + self.client = OpenAI(base_url=f"{vlm_host}/v1", api_key="EMPTY") + self.get_logger().info(f"VLM using HTTP | host={vlm_host}") + + self.vlm_model = vlm_model + self.latest_image = None + self.image_lock = threading.Lock() + self._busy = False + + self.image_sub = self.create_subscription(CompressedImage, image_topic, self.image_callback, 10) + self.sign_sub = self.create_subscription(Int32, trigger_topic, self.sign_callback, 10) + self.result_pub = self.create_publisher(String, result_topic, 10) + + self.tts_client = self.create_client(Speak, '/tts/speak') + while not self.tts_client.wait_for_service(timeout_sec=5.0): + self.get_logger().info('Waiting for TTS service...') + + mode = "TCP" if self.use_tcp else ("SERIAL" if self.use_serial else "HTTP") + self.get_logger().info(f"VLM ready | mode={mode} | dim={self.image_max_dim} | crop={self.auto_crop}") + + def image_callback(self, msg): + with self.image_lock: + try: + np_arr = np.frombuffer(msg.data, np.uint8) + self.latest_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + except Exception as e: + self.get_logger().error(f"Decode error: {e}") + + def sign_callback(self, msg): + if msg.data != self.trigger_sign: + return + if self._busy: + self.get_logger().warn("Busy, skip") + return + self.get_logger().info(f"Trigger {msg.data}") + with self.image_lock: + if self.latest_image is None: + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + return + img = self.latest_image.copy() + self._busy = True + try: + t0 = time.time() + desc = self.process_image(img) + self.get_logger().info(f"Result({time.time()-t0:.1f}s): {desc}") + msg = String() + msg.data = desc + self.result_pub.publish(msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) + except Exception as e: + self.get_logger().error(f"Inference: {e}") + finally: + self._busy = False + + def process_image(self, img): + if self.auto_crop: + img = self._detect_and_crop(img) + h, w = img.shape[:2] + max_dim = self.image_max_dim + if max(h, w) > max_dim: + scale = max_dim / max(h, w) + new_w, new_h = int(w * scale), int(h * scale) + img = cv2.resize(img, (new_w, new_h), interpolation=cv2.INTER_AREA) + else: + new_w, new_h = w, h + _, jpeg = cv2.imencode('.jpg', img, [cv2.IMWRITE_JPEG_QUALITY, 60]) + jpeg_bytes = jpeg.tobytes() + t0 = time.time() + + if self.use_tcp: + result = self.tcp_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"TCP inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"TCP {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + elif self.use_serial: + result = self.serial_client.infer(jpeg_bytes, self.prompt_text) + if not result.get("success"): + raise RuntimeError(f"Serial inference failed: {result.get('error', 'unknown')}") + elapsed = result.get("elapsed_sec", time.time() - t0) + self.get_logger().info(f"Serial {elapsed:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return result["answer"] + + else: + b64 = base64.b64encode(jpeg_bytes).decode('utf-8') + resp = self.client.chat.completions.create( + model=self.vlm_model, + messages=[{"role": "user", "content": [ + {"type": "text", "text": self.prompt_text}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{b64}"}} + ]}], + max_tokens=self.max_tokens, temperature=self.temperature, timeout=60) + self.get_logger().info(f"HTTP {time.time()-t0:.1f}s {new_w}x{new_h} {len(jpeg_bytes)//1024}KB") + return resp.choices[0].message.content + + def _detect_and_crop(self, img): + h, w = img.shape[:2] + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blurred = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blurred, 50, 150) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) + edges = cv2.dilate(edges, kernel, iterations=1) + contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + best_rect, best_area = None, 0 + for cnt in contours: + peri = cv2.arcLength(cnt, True) + approx = cv2.approxPolyDP(cnt, 0.02 * peri, True) + area = cv2.contourArea(approx) + if len(approx) == 4 and area > w * h * 0.02 and area > best_area: + best_rect, best_area = approx, area + if best_rect is not None: + rx, ry, rw, rh = cv2.boundingRect(best_rect) + self.get_logger().info(f'Crop: {rw}x{rh}') + return img[ry:ry + rh, rx:rx + rw] + return img + + +def main(args=None): + rclpy.init(args=args) + node = VLMProcessor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()