From 6ec778dc6d8562d17cf4be416b2bbabaf246bafb Mon Sep 17 00:00:00 2001 From: ywb <347742090@qq.com> Date: Thu, 28 May 2026 10:05:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=BB=E5=8A=A1=E6=89=A7=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- agv_app/__pycache__/app.cpython-312.pyc | Bin 0 -> 73434 bytes agv_app/mission_executor.py | 650 ++++++++++++++++++ agv_app/running.html | 122 ++++ agv_app/running.js | 130 ++++ agv_app/scripts/fix_scan_timestamp.py | 67 ++ agv_app/scripts/start_agv.sh | 17 + agv_app/start_agv.sh | 17 + agv_app/start_all.sh.bak | 177 +++++ agv_app/start_all.sh.bak.234249 | 165 +++++ agv_pro_ros2/nav2_params.yaml | 350 ++++++++++ agv_pro_ros2/param/agvpro.yaml | 362 ++++++++++ .../src/agv_pro_base/src/agv_pro_ros.cpp | 624 +++++++++++++++++ 12 files changed, 2681 insertions(+) create mode 100644 agv_app/__pycache__/app.cpython-312.pyc create mode 100644 agv_app/mission_executor.py create mode 100644 agv_app/running.html create mode 100644 agv_app/running.js create mode 100644 agv_app/scripts/fix_scan_timestamp.py create mode 100644 agv_app/scripts/start_agv.sh create mode 100755 agv_app/start_agv.sh create mode 100755 agv_app/start_all.sh.bak create mode 100755 agv_app/start_all.sh.bak.234249 create mode 100644 agv_pro_ros2/nav2_params.yaml create mode 100644 agv_pro_ros2/param/agvpro.yaml create mode 100644 agv_pro_ros2/src/agv_pro_base/src/agv_pro_ros.cpp diff --git a/agv_app/__pycache__/app.cpython-312.pyc b/agv_app/__pycache__/app.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..91b98c3088323c4b57d38c1b50d2e6ef9da2e562 GIT binary patch literal 73434 zcmeFa33yc3nIKyGqN-9!D(wrDHb4R`?E50n0@%nb!Zxz8T<8{B(8jHj0i|Ti4v8fj zN0Ln}VdEfdrx7-e<&G1}TY?iOCe!m?S5b0QuGZ`JbRz*X(;aL`+fBZCJ^z2s-Kum; z!bzrI_xt*SZr$bl=bU?%e?R~ED~lyc0msEbb7RXV3dKL*4S(pli<_P}gS{Il)l_T9ueMrCes$G4_*FOQn+?^5W@ELnIjTCU*;H+6HdmXQ zE!7q!=BH_jZnjoi$+fmAra87cwmGgkuGv;?YmTpuZ%(LAXilt7Y)-09YEG_BZceFA zA!&6@sm*ECY2;epl-`_Cok6Y*O>>&(R?j8Z#wL4nW_2dHj%vzk&aTdG&Z*8J_ok-Y z=6Tign&(%~S1MErr>1cy|KZ{Jz{0+`U zjFpLDVlTwKuHl{+IAa-G3fwpg8twF%^vuMsgIj>LIt%!`u)GQdlW<(YB=U8rE`&6N zuXCW)MNHChMRhTg4ClqnyG+V)W%Uv!74DWAvBlG%N0wDDQ(~!yq~+0TKA+_Z@@-4` zd(vvEChXmG$ZG|ZscmeT4)0vSWL%iT*RApJ^c0dDRSf*&(~=q;nVz1p^Iy+o9#`m#Jj5mM($Qg4WmdY+Jaz9jX=2&oq^d3DMQ`TX}W3neg{BET#P zgDH@}lth3j41+0>z-*2HQyd1fSOT*p0?d*yn57b!(g-ligce&aNnI8p^$H>NN=fSS z2&q?v)n~N?rXm8&nlPBP5}2(KVAh4fte3!SivY8M*%;FInaaF=Kmv1L1egbz{ULq&kOb!bh%lk!phg0-F9OWNVRdszV5%d)90+TJS_#Yp z5n!0Ga-0&F2P49S;`M_PnEer8>cYyYm%uy}0j41grcnY@69J|!tPKuHU>=SD(-a2N zEP-)EfN2OD2Q3nq0})|D$3d$ErZxghTUgx=OJJA?Fl<=6xg;>oh%ljj>XyJ9i~!Rf zHcuasz|=*6ITBX4qY{|<2r$ROU^*l)4H02Nzvo*Ln8pY&p0IK{B`}8~z&sk(KaWXZ znj*k-g~2>7foYBa^F$cTwSu-2&q-205+U_TA@%nq zsgFcR{d`zI3`k&(Mt~U%YoAjRm}3!OUI;7aMF~tt1enud<-8<;`Bns&AB4f2k-&H& zzzl`KoRz?IMu2%a4CWOH%%c%tUJZl!p#B7Srzk>7A5o37Zoq5_&T8!4rN-!RQwg+4tyKH?>qQ4An%X9&gnFiiesu{s%*t# zg-dxvrBv(#ErL?fr$6yeN7efjN0ixc2R+U5d5XWxTuJ|~*{A3;oG?(By!YBmAzf5S z#l7~ar=OYXKYsPc?_YiYjJ?2K-sEr{vR@f}|LXA5lkfFLUHf_8wX-KCPY>HM@s-i1 ze)qwNtFON~+53upd2w-Z)O9ScGEW!O63C#Hb+#~0w#MyjZfkP5ok4x0tF@)!;IW{d zbslbay4*p-Zl|lQwZ-KO+FX#h<{;bJT*EZ5&RTaXdn`{G)NHwD_ntgs(6Xm&_q}Di zYqs1|S-!0z7*(=+hj?9aFMn;>yYudQN=o_Ls2wFc`RiB$QnI6LcM1Q4fTcMjU{aJmZk=0i~GDXXo1h((%Rx?Tbr7kEWX{fdrz&SrG?yBwlzEI zoO|1vS{?Awd8(jwM}y1N(ArXV)LGjOpA(F#g7(Cm+gf!qa6&PSY0Ee^chZ_iyouhzg-v(x2*Lh~>`ElCyB9c*Qr9qyps zQTIrVqpdAyhMm$AmmQK1fYFW*Fg$Kb6NwbEPM_m zRWyG`Y9-(UcP9V5F^j(ve|fwFxq;T09>)}U+RF0l*aFw_!=YIlkv8;hVp&9>6J%Ys_$U1!kTOh;-B<8V8I(fl3jY-?rR!6*mYTvJOX zK`DRr8W)`cK}#)+P}Wh?+U9I=GQn5^7V;>n+0lm32OH{wmbTW07Pm{d zZEj_pO~S2E2QIa>*-=~H(Bc#l7+ZvyV|3TEP6yM_QWwuo_ z zH_Qq<0Z$L4WB^$7M100TWq+kVKBrsrjPWK^Nf9zdaOjdLasn;}U>G)HlCT5S%x+g4yOwP6Np!~0wGd_lc8e@N>I)#fdsIdpa?S3*dn|t#?xXvEx}U>oM^|e zTi`ONWt}dkyCYWGCFId+Oo*p0MZgl87Yem;{zS^0GpVOiyGu`O57=|MOUKL^ zfvkD>JNGL+y#ERlk(9SxICG!LS)+bWTcXxi=4o*VAO@|e2}ae_z_4p?!fSI)&Ea+j z!Ll_qY$*V-Tj6wG!It4QR+F92pKxxvaH1bqe5s1p=H6VS(axs{F-=HEVWX3d+71`9 z;j%^nKS5&da0^(I z2a@48xsu=ngWY85UD>m;cU{lA(}tlOziGZ#LqDii7(S2?F(mnx@4*Fp3(hTaxbZ1D zppM*Av{PMBzs|i`uIN;`g|BNAzKMqeJg)Sp7!9MnpcDBW9CUX!?DF)LLFJL#+IIKB zf>qD}24_ocD-(xY}7~4UpD`hM=wic#{@4qDw?rq(OojWKe<{roFi> zPtEQIIJiNas66Px@37nLbif*2jz^p|n3{bUo|nU)3++kJl19sk=D|{bbmq{~k)$!> z>Kh8BcJ=7434^)U*kkO=8B`8yM;xCTR(++07hmDqFYrW1>R{?x_50cqjXtE^g~>h! zWk}myY;9;=p{L{~x*Qkp5%^NJz%5WX=!Ni}Q@aHk*C^08s0w#@)O?@Us=|A05A^Hf zDvxrHqCv4|8qzpn<+xA{^d8IJ3mLNa!6{EqI)tsn2lv1!sB<1|aJgOV1Nh`Yd;)0R z!Q^RZH|ZJYK}UO&nN=i5cF+ zyzZ4=&EjDePG4!^ApyAU!iIY{p=6HwqdD4=`TAP86%K5299B2eq3fjG$|(5;?F7uu z+8LEc@rZ&=_eik0$n>~zT9SiNdlZZc#fDI!L!79LrcOX2ol1|A&Gsn8fm4HRMh;1y zTCwvzLcKs-)4e2gJ5U(`6)<{92`Y~YN{|fA8e!r~N-#)D5QZd_V2n^gRD<{pYLA*- z?@>#>L1+z02_}!a5iw)f!I&Ar*bL=GM@VH2Nfq;wmhYXA?~avxx5lGkb37VJt%T_% zsa2fhy9I$4>S2?9_p$h6@p%dD^P%cgAe-!abMmR-sezwPJ~edh!w)Cl`O7Ome`)fK z!K*JEzy6(Pu73EV+rTaHd{!aBXa;<6%+cJ`o&niBKy%n{=Q21wGWp`qDCipd?YI!e zjKIr5U47!+D<3|?L$xDE3?kuc^zrSu5OywNCR3;1oBYAk(=wrtZ`*iAlrn)4bGh>V z`;$+d<#S;dLRDDAh3!agKw|yn0J{ipajH#r58RFqV}V9kNAU%p8*6q80wg2g5#-S&&m}kud3SdVpHGg0P3}}mQtCX)Q0lP{Sg=mz zqsn94?-At@j@JjKi*Q$Dy0P()W&2z>2?z%DMAx2t4bauKpZ{R$dq14&?wfq-Ogjn! z5z|b*`qs6#e+a6jYj3_Y)$_Cf+nzZi)CW&Yj*hOegQ`e_iW3lRo1D(JU`o5Yp~+Pk zDz?_Jt*#|(0_4Y{j}5yFPC-3jnHJDHv8{M#EIV53Y(tSbPeoD&l};BT6@KbjYaF1| zsR2zD2i65IzXg9TL<+!Fm}7dk_iXPwd}7B9mBF&{|B2c1S&Zdcv0er9x@hI1!WJOFf>7DbnARDZNl3)fZWRY9HG!2
KKQ! zxwR!^Y~vS^u}w8KA{FPy7LFn+YA6X>VebYAQ6vhQL4&}$+uLdo6S3ccmp$<3!tvdu zxMIlo)G(K%zO5_@DHsctEm)jDdw^-L?1(}>{b*smySeGSl12U`yb4;9g2i%CEOFsX zfwE1hgX{dJ1zybpQh^Rb5r~*>5D5ObdYCzAQWLgYrXG9>5U}c;?9-6qS@^ptR%e6X zl<(E#^VMP7TU!43>Tu9AREJAJtAj1`JXZPyp330VVJdR8H4sf;yDOwk5H*m#8G$n( zJh-KY9Ku5t1EcJerov7NJJ>o&E;8@s}*ipQomrsqepi<-_N$KK0Y7uE(i1q*7#(32U5Z zA{x4&g`<;52SJ5vAu3!--<%A)0`g^7!U?!lkS7r1Ln1TJ#kRJ$xPe{e^|$dcuI>mVIfPg5N(YMwYgw$0JPogY;6ZsG%B$iEp<&!7bV)}2BHzB z^<|M$qpC)+s+t3>txbeKXWQ^Ka_1sr&K|~F!kBB=ez*pG+M%GbhV6svFbzT!pvxLm zn76ywGw}K}CMoAR>?m_@Qcuzs3cJ?yd2&X#1vP5xpIU!<_nG~t_K#;2U(P7Lp)y*E zuEfL*So$sFG1a*O4 zQKZZvIgXpsFPqXQ(&vn)7x>Z(#?#mM(%1OY*LCl>VojV#NE*1m|NcNmKK!4^m^+?P z;L9kO$jlzkT<*(UK9M@-G<&A=RHr|+U?P3)tEzE(vCm%YPhT8z67cZ|i)|6gCFLBbo@Xt!NC(4PGT6=wC%OU&gNs$ZB&4RG`8jJyh?_7j~B zu0JtrD-!e_HcGgTfymQb#IavN&CYxFQ~@imZ+5!tTbWRbhDrxQ(c)o}dP zN}&~HYm8M1JH%3;)zbKv?sWc&F_Tu}=(?DHjUWn%&l+VYR3nNLVFzC(w2p;qB@31! z=2RolWf1PHjIJpT_yJWTDrdqD_X7R_6N7m$u}s_r8?O!T1hrLTKA%GTW#ZBALQC!Yk441tBo3CQ7{HkH4BuMU{S>{mGj(Sv@*i0 z4uw~soEI5#C5o@#FmKZgdGGlX^NyY&?-kSYM$SUgrot#89D+`sLy&J@j%!G0M?(;m z(usS=+mR=_D};G9=nlzBjW6Eso20Q5)T zLqS+1ZEaJl%gJs+Fl5|=`aNY;Rof~nD1(A?neZ#95Cl^vm@ZuGW0+0@I)%eQ6}tqg z-PBamdMI4!XF@)N*E+lpARob>D*-TUm*R#_k(54EHnR8P0blgCu8JEvjka(iJ@3VP zsAerCZ9FC4my$o8vc#9N#GkUPTi>VciM|nK)GnJy&U`h0_+FspZ#P~{^sU(HPu@lp zgehrfGEQZTrxf~93jHa?6eM7Z>s$WRhAZai-W@$V#?4tibJmzShgW;JPTVsQ_UcBI zmKIVl679>{=uh56a!;RgX2+==i)YE7S|8OgrOS~*B@!H|T!?oVbCF6#rmkmov>Scep74uC}--uIca|3g8PN>EW z>9@av{tm6`ZI=UvN6)f~B*o7*r*73Mewk7dwRNfDEnHvH8n+g!FGVM9U8ugaP>a_~joap^FRe@3mZJVmiWaValWEzuSpA!2 z+Ug#(2f!iK#EWP3Kn0-Glh=LGga$V(w-Di_HiphqET4U|R? z@tK_JAf!ZO8N&`a)k1_4@hiz8DN#nN8&aao`)}}#DEp9uQHfip;W!#YCGvtgJ=7DDqMT}tijhsOsDgF57F-`o0HNLl_ zr{nnM<69>H{gn>p^nr@e?$y|-MvV$4l%(;3iTnh34Tj{CnkThS>Smh;N0lz{^lVU| zAy{rWpsk+gp`Oz8XrQyei{i8zRQiHMBIo(a58S7;4T`69-I{J~x9*^t(H=K|CxHkl zGblYO@X+9|=+VlWVGVqKF3^T}lvGAxqTtGee7IYX zLPP`f9z~B%wmpoZ7KYC$LQ0d^ax<5dXKu%8&_O==%=f|0dF@9(n|$?2uu{<8USkLL z$nLQNe*|aBAko_zo?D^`gNkzSEV&4|k*N~Y5+*8Skl+l!559Q|u*mn-10Xi`z#?$s zkX`bBT2$@PGAjI5{6!|aPw8r5)NrT4r+vznI;e}5)a9(`GU67738Ols3 z%SX_bQKT*Rx{>`Rha1&*q)hUolq9g4xAdC-21F;chLYL~t8;6{?V!w&%l%*infEHN-mlO4rnxA71^i z|LXgvr@EiF@4jcx680C6oCsa)IlLN%Q{7T>_&2s=;|Ssxht7z(m55&KYw)@q`wGj0 zl>k%*o9cNE=sYc$a|R|}z0~N7ol$k2Q0)tF$-aqsq`^rjN6ctoX!5ydCf_^Rj?Bo^ z>DR8F`60!lV7ELkkV@EiIcnwkry~&bai!6cR*7O)5?ov z9rFpdLgiYH3ocv-F3FB2*M>q#LI}~JaE-v}YmnPiS;mZjHDNH}smCTnGjVqAc=j@1 z_OkKpb-wI%{_G8XQG?5TG4?=GCOBy(%@5??+qZr2h(CUQAU0`m`LlB;?0FaRNAC6I zulu0!Qlf9ePQQKEN!3ZqM0W1E^=H?QXRq>QukvTF;q$R&3^JE(*%R@JsF59y&-caW zkHs&Vh)*8a-oJg&b@HB&SD(f&`aHh`i@Z#WTy;4%8_O&hjrQeN`0ZOm3R~{WUhdCc z$rqMgFwFR}m!bune!*q@f-*^V>n_L6|C^Zk0VsZJ|JJdL!r^vb#`00-Qc2%de|%-1 zGLVydZpYajBYS)~tN3hVVh8m7`jf`M{KX?VzWFOL{~XdnB>xI7|IZh!7q~6M&%1_PdU*FOzVp2e!Gv-ZXb5SNU=KTTWY9Z}y z7f#7W|WN7h8mKv{f<0@9F^OWop08n)ui6o)= z?TFOq^t<-c?|>)Q?>^{WTzvK1v%mYG2OS>>Z!yKs7BZ#5*$4+6@qDC%dIQ`BwPj>% z>Z*tfJo|0(jF{wunj?+|H&rp{*@y6jj_MiM|AIG4H|0W5cf$S#PA+8nU{KpPZQ4Xf zb=>rU%wC2>Y8<$~hKt*T_cq(mZTPY&V{l1VDJP=$)eousmh7(bK=Pa+=g0w{Wp!8i zmDtpQx&3qfu~}W)1F^QQ?fZc@`IDCLNpsE=o+=zp8p-mfE~ijefPtqlly|of`4#M1MQGQmg|8D1L~cm7ZoUm4 zJ{!LtnNN3P2a!$gU-0fE&-1XiiL*?_fRt(+mw-$D7y~3># z*wC|KaKV@Trynj@D}d&CGGKBtGxH_^IG?L4f{T~&mXz}(kfqk6^P3_ zCNvgr^xRkD&Sjj<7}FGi^gNjJDtoT;Y^Tq*+;3Xp)vO4ghUj4*G7SaoQ6qfUolHYF zLXbl=p^GvLF6)vEEfb#>efC1M9U3xtsE;ij7t|w^bT#ScDqtT(>>S#Gq*K9j342kx z9z*Ce{7!ZP-tNRcD};;CK8sHl8*nIoOfyeb^$;zfuLfL(7}9Wv*a`m1f%9w)$Hqxj zXL24tk0@EIxRN8Zxfl)3d^Z$G4>@>>{lH^Lz=w?jHsj&stvjM!s8b2{H!axTbbt#5 zYz3AfJ>R9U-b4+SIRRscIg6OAQxk53dQ8(O=o&-_#=sc)d^@$X)LRfhM0mi2f{#k4 zZWee!5R||R`W9DQNXtigbSTV-!=STdejGa;9+D1=QV|Ew--11@_W4#c=!^*4} zxy)O)L;;i71x?bHanI-T_Xs{Zq2Exe0#QxZX?oNIUqgOHQ7tc)vxc3VeDMc#%7ceI zNJ9&DR#xOuJdbkL?{VaDq~bJqxuQG%^^-7-iD*ND4ozWOOI?SNiiEBM@%neRCk79Ai8UocL~(ytZfLG!e?YDdv>zmb7+GeAY$<ZujO^c+j_qyiX&g)*S~{M+(wB`?yIUPFMR%{I zz_}Z|X&e2vO&7Dhwlc4|3}h8+T;GEG3z?7Vcl2rq5imHDfbVz7I5aLBGO{7+J<($lITrZ;J4-tFZEgrKQ$B* zLsijj*A{5?k8)$maugpek1I=7|3Y7q4>!MDQVi#hvzNj7SIOG4O#QDi^mv_PEVrtE zRghF3rT(=>3D>`l(!#kzPk!$VEU3bZk@qIw90m0N!K&r#-$DlLZ}IeZc*1s|fdpJu!LzA(!u@{ z0{jk7H}FI<`2w&01y5h&=`lD(pcewWjTnag10?(wl8a?Ck_)G|aHB72liyl$5ino5 z*HAtyxzO0O>;8=7;vWog5%7QRf}t9~;>H(@qw{(!-MT(4xPS)Y5+OFhnFXg746Pf9 z_ouE1B&H73_ty^|eJ#bGxHOQE8c0n4!lXktO0_LLKTy^I}SuDLz^eSGqv`3&ZBQaP#p3ZRukD#|3)4 zUS=$_t3O_!T$ZN(Re}<(f0d?%b4Lu3Rk#2S&LVef2*yW-f?)hYIR6`A{23(?qR|n= zL#Q2hfbSZN9z2K!M=ph4NC69YW&PCzvl2)vxK3Kt6-u9?LNH4>eUdRh{VD4XT%l!~ zrk5SEA|r-=z4Y8=6K%CTKQBsyq6e!-{kTbVFc2j&paDkkQWUb3b5>}Sk&q;L362Vq zXM$$Rqk)(NW2vbU53Y5;HrYLL^~E1e z4Lv#eQXiZt;b;g6dbZqN=q@5x48I_ zSe-a38~) zcKNNly;b*lt@n8i_ubH#wd-(=1XRLnnmduU;KhyLYj^kOxdq2OjoX^8FS^x9T&K1!SS29PiRnUs{>p zR_-;IUx5d?-X$%*v{t{Z&C9lXZS7uj`_~hu_%9VIDBKX;Ywj`kJv!tV+T%^g8#Cn3 zK=rf$aod#(J@QLq$%?WV#jg}rJjWQzDTO`4V$kc@$a@GSL`vQi+aYPRE6=KvO_)UBV>!=u=_>R$|ER<(xM;bHv z`01%PUZ!&=e5#UHEchrX%871+n_;JhVd718zchL7>B$q%izwv0hR!yeN)Vw5+J&mU z?}mG#0;WSfHe{%iWU!5a;(Z+Kn;|KY@xklM^MnVzzMvziXBR|g!d_&&MVov{C4TE> zuVFK4n{&PSB|h6`ziG?GJzmpRuVyRNH^+ze6NIv=bvDWO)bmj6zl)xNeH}u35ghVR zUICTS)K9x7KYVVo=NZ}yM4WhivhNv?9K!pJBqseva4#yJ-i9~|!xWN5>vZ@^TU$t{ z(6(Y@A>qf^E!d;qv|Dn#3)lH<>;0w;Ud;yC9-)raL3Kr0RYn4B@1|<0`!nma?j)j#}j{GFNEbl7%Gm@ysRXXP>8ESmOlv0 zDN%DumP5!gl)z|CTfGkpRv%TIXu)N=k3yJ8!OuibEE{yGAnkU#NU)Nu;zwEve>6U! zU6{(0=z{OQrh$Plh+jb)fJ+;VE!ge~#?-d65ckoI)R2Un3q}LQT2kY9#L>{?IMC$G zGl&R)&{8V->7W~f`Xde&*F+PPKv(Hn#7`U=zyN%PGBtQ6^`M`7?%~YuWH|b*HDg~W=JcBM*7}2ubv#{1PusMyZhdT4hTu~2KvaIewx;ng0M-D5m?q= z!mQDog-yg0K5fUAmuAePJ1Qzf&mt-y4Wt4>JT6rZ$Av6xuS8wXSj2Y{`9Md_{Ca$9 zfD?oxp?H){z()l6;lQTyfE1-GMwXvj}iQ6D`ZpUx|w! zSk%90JZ`BkZmBwcHnd5M$1d>2E&!`Uw+?VHs;-jm3)tre=H#4dJk{vUTRpnm zKc@sjWTLi8i`pu!(eyR0!-0i2A_MPDS~y$+mWhO-F+=gJ96U(w8tic43iKVG85>s4AfbNVT|ti7lSA}fU;XAR^)7!Oo@0&<3iriXMzp4t;VY`2ARl$+|qljBbx~oxeb3eQ9h+zZ|mE7`oQ>{ zLf@Q1udT>$D)wrMDU)R-Y%lkT4kpy!NgzMvcR+q3`#Zrx6hXR}+22S&OX?1&J3(a} z^VJEwZiu&&3^`!eNphEYKNBkk+A_iS;2RSO6{8ikU81&0)`tnc3lIKF@f*=U3RZV$ zlo%se_i`jJFg>`QbFC4!J|9o$M!6x<~2-Ab}1zJS4{RlAvbPXbDEpjgn{J# z z$1~RYGS-e|td~TiTI{ne_S=?r8vrQQc5>-S_rS6KW6yodZ_D<^<#?lW12L(AxYWyW z1uwMnwgu1deg4GdoX_rH_3-?W72dox{`j?EXt3zI%5Fs~wEA&PyiXH9u1WK0(gw@M zG+D$WEDO~|%X~S@#x%=`PuOsRFBDN_ND)>_=#DP z3B=6>E{8QxGhFI?vpak`OvI#|&Kgh6_oe3hV-|j?&}j2|G~n*AIAAh&SM{vytA`k` zuqJujobEHH5AO1r=Z>4_`ONc%++*fN6XpbOV*Z8A$7-F^s zm(2@qz#8B@;%SVkq_sX9Ec{zP>hhX4do`OWO$`~U5X{^eGE{@Q(z3hDs>evyQgc^DE??5JW_;!s-+r;zA+(w?VH z$!>r&cI6Nu(cM-zLTW7SjBz%(u2T)VLOn1`= zOn(E#Nj+huoGfvMErl}XLr=TzhLoWsFDY%{X#de6!*G^AsUTt(59VC3UD!Qdu-;d& z-k-O@XWQsEZSrb1(RoVE&lIi9;Oq~`K8(E=dklM(HO(508FX=U`7SVcA!_$L4OQ4q z3*bv959ePjU# zca?vpw*=BMNwft@i08ewXk@d`1_2LNc{Qt~Ek`4K{o4sfu#gOb1P67r+z`QN4Ymf3 z5>h{jk&KH%Mg${RU!qtIAI@gs*e!$99aQ|`J|v>}^8uv)kcr2yESOqRE$~VZUBo}C}%^24myfBQGoP(4Vw!c98@*ez@|4&wgC*YZLITP z!%_AlXxgBOak^^R1`-1{sBLKl_f~N8C!7%qDQqPIgVf*xw^xV;qF~W~pFISxM45t= z1JMu!_K2cJQKldRxEKNDWCt~6jc9CWB0I3VA#LbZ1Vmuq#ybiORghu>&hvMnDxui( z4h4qKW8xxk{wWz}l20WM6+z7AlohaqE5PAn%PxiLdG}_WRCm)J(fY=&BFb&%W)_Z%1=f%+U7!;e!lal zx^or04=S7G*nVzAb8P=UNO%w8@U@sD!QqtM=EerkZu8kkkPP30519?4{|8~S<@<_f zvq|eej9rKv0DCqqfmp!e-C3q^e$CwMxdyY{U{=&00*@?B@@#D{mN6p2q_`p;4CR@ zOgx{0NdWo{3#MqjOqm}CeGXoeExu`#NJE-?MbposRd1im_o&*g!FeR(Nv_h z7eF&oCF10{pF=oXaB`y)0knzNPj^k7cyjVbFW7l3fC@z%_?-TN_^JsnMVAyp)F~;i zAq>LrkzUgR*j+dc2&2HM8*!ZtZ2kv`D+31LF7DONbbd#e}}-+7zmuy5;Q9^5QMW3G?1L~QJnm&6Y>nh zwqzv@nbkGOJ{*J;4w%F)J6wda5Bx$=zcssS>t`k#F}6%|kp_>gaZ85JlJTX&tXQB`Vc}DK@&si_t)sE~PUHle##_=ZehnMUyA2Y0Le#6_?E`Zlu6h+)9Nd z<`ES?=~Dc-jvss*ib$h|=zEYoCTQ{`T+D_h0fFU(>3OKC(JZPiVPhcq{aHaz#7{lZ z?j%e!96#h>Af5OgSY|be-9cSK=*)s`W%#)^aP90c<$!TkgwPr?A`e5J>;pK&U;qh3 zhB#`_R-wZT6z43i(>j8~Y!{@Y(r64nY*gAcp~Hr(a~j%pZtvN>!z)KN`*T-%(^mOy zt1p{Z-vGzFHMeva0ia@VcOQ5XWqLK4ltY*|Q@@}V*#&_onGHep&b?J3+if8pBHe^j zaJFs&sYe6p$sre24!ZFubmMf5u~}S;3}LfOd>-O2b`IZ(JSBnL?%}l18gyUgRDe8f zvjS!%nr06Tkt%3}#9TupE(!NI^H=co06Z>8aTt1C96VGVNOjI9IzaC%O{Wvng*X;c ztuu#e3ExTZt%Uic7|-&^_%V6=~ zpr#+T;Q{3`YzEm^2eDQTWBio`zW4>h@ni9@A4rCG&XTt`k1s3rEh`<1FPngXn5Dxx zL+ku$#bfcsUl<{?8(JtA%quYXO9ykw;2&D*HO=#C=8+i?GN|!;$)M)8u@Oduocd} zbej!2JC5_BvNoh>#HG-Fzpp2xbcDiDF7v{#vOT_m1+HLF;_or4bq#od@Ej zV5}4#(E}|uv@eIx<5B8f@_+cA-VW4Lz1e&vIqspDs0cQjaZ zG1>PQQ*XQi+b>nYN)*uYG`jR&Its5`eyw*~$p>!h3JRX(i zi%J`hTHuRX;E&4h+VYtxnlQCDKviC*`a+>dE?2?Q@|X%0JlUqY8-HTR*?6}K7K@KZ z&-F#m9gkk30nKXh7Z3Z-r zQ6CxHXa-DqjJgrNHT)2_T$OV+;s+T&oPbLaq$@zV8Xk#~g~n>RX&|*rgI6;A<6$89 zIM|UfCKK@s`!Mzf-g--1$_aN2=8S=b@1h)L~kZY#pg z-sq;^IOPgqwK1BJ@p?^^-Chp?E}Sf0+3ha3qq&VI%F4$Qt`%=rL~akhKeBk>0mD3rXOVy)YCjJ51S` zMF$#MiViqj^&N@Mqqx@r2H>nIy=OiNx^pG3p*^HYN)lh zwI8q-6u`!QP!LiUFl6C)wAVQyOE-J0rrA~JD%uH;?rU(@ZxP1Y9vtmX#-5e4pw{8G zcc}8RZY!f2{t;+R)pbOuK}*O71(l0oABgB$N1GeB(T4q)+F(m2jT`pMWPgOo)y|ej zI-&*Q2FojJ*LN5f!L(u^@p=*(l)Hzuk?YB>vUFJqoy)M3ZRm8StCNy&?(+-DxRl)b z&2q}r{5_<|v#}ZQL*lBz@=B+>4Lpk;z-noaunoY6>?GH<^`MD>J*CM8PM~NQMuFyP+{~v`q~yU^Db0;A3zS0xt>@LV}dx5!#dOgPwU6P?zf&xWK(0 z7HZc8V)MFnH#9Mpj2mi&G2zBMMMA-N{8C^1(ziFiv+b>IKfcQ!zp2}Z;cM~+_xLTD z7~~~uXji~&>8fW_9mHIO!Uh#A=&h*@@W$-tWaHQtzIgO81R!`;f~U?;bO>BR>bL0V{n;_xPJ?3E8PAn4ys5#yqwad>R&`{PKTS1=W9!g z^&cD&%cM0^UV3{ zgFnILhTy1yvH)xsL+XwZ7HjKq%z&aZq&($Mn-IYEW@_62|_g zI0>=LZ-NmZW_H$Ml;-&gMr*37+hHi+NNeWkQ6M!ro$x=@E!=5taG?*WbrGl zqngX})?JMKB;liki;w&=!@IlIS5o^WP}rz15niatA)hAemO-J7_C{yFx|>p|G0jS% z2H;(8!k|<(Jq8$d-^k09BP`uFo8N4GEkvvRE{D0!OnFW=~{; zXUOmrx+Hi9T@sm`ibMrFOhm8$jy$66@PS!VJ?~zBvX?qW7g6h#bQ-ZZeb{D5S;cOK zOX<=GiCZ-3;)fv><%HVc3h*^SwwOUkFU!w0;wb$pq`i&(ybCf2#jd=qerU7bIv>Kf zq-3JEbRt=Glk@6<@p-F!^Hz;%R#Qwxyrnn#3O4%lHu-FTki}IEDe_Y^+yMyXMKUx0 z7V0zG%#1t?z@9D7CrNlIP>X3Jq|%8T7dYF9+?QlP$#G18DPT+!S0QTcqUO}C5o}!o ze;lC}!uvxM-xNj2M^N5H$VYh3<+H~vaxFytD&lu03RP&-_zLR^XKlj>DJ1O|A%zKC z+08HG-nkYi9r_xN2x+6gL0XK}2>F6?71XOBfp9r9ap;yLq64|!YqZc>NY5e8N}`e5 zHgisVsle-rEgcUg^#rk{fs!VY9kw|)BzfG=?m#^7Jf84NNK9LH0B&iFoNI6XP*8cm z?r9W*hz1(UxqStIfE5Zb6JixdRxYSI%o6`(Fty;Dpy4oE^N6FV-APQ|yq^&%8OxI} zfq#Lw6g9mBmouRzq(0(8M&6C+EM|aIi2VVa!rX{ti4i&!orkn)Kn<>~a1n~6IDg@s zp{${*bN8RUf7m&)3!OW1R*fo0bAG_Cims9JUCMI&i!xQKkEE(jX$^aQtV$R{3hX2`(LGd_w66w=kV=wcn{S1 z_tpEi)g$?XFj3olY1{p_yDshV+IC(x??ktfT}s#?#G1m#$JY)X_FFTLY;eg~VqCCl%)_&7Ofyp)>jK;AzMt z6P=?8{SWh5(XeEG5hh?2Wr`^tqeenZe_AE7?tucSmLjs|R$&RqJSpER3OZ4OB;Q z^W{5W`CwCVNJ@=hN+)2<;0hs_C>#ikDJ0ZsG&~AB zpghpKpr4=IMivl3NKlt`>O4B|ZBGVwP+=97n3-sIg*5pdu+=LSElLe;_HKfZsPrrN zx>>Mpl66(lv$PGzssYm^*#8-;_^q-vw}>@|I%uH|G1x8;{?!2NpWp!?U~w^LB@>4c z&KTQEYN_DP#FNnN(sZzuXc*g;2}Mlxygt?2d*!_oSKoj8>d@np@BVc1J3qem_({%_ z{M2xHI|f@Kt~0_8c&A^Zo8U=UG=lgEOND$IVaeEYFihBCI8~C>VT2>&I4)c=21tN- z$6yNjZVz0wW1T4nXm3X!5QtgCFR@1w!fo338am(nHj#)!M+V&w-6B)dYw~>L^fr3RdvpC)-rBia@oFw^C2(y zB~|#XTQ6mMtvkGi9T@dr=19XJ9m&V&9x%fBfPv74b$}PboOL z9S&Gy`r>3oIQOdeN2BvVBR8dyv*s>4@SY(Z=c(9 z`YaO}bH_6Zd>Kd)W9Rgh1v0YE>^rq@=&@0iKVxlQc`zwsXx@0%QeW26k@8Qo)&>?7 z3|c-*&VWz=v2(tNQp9Hl@>h;(eEI8X5rf-(@$-S!dQ*$v&Kh4*>RVDe7E?BnGzUU* z2ki3#cG$D;tp1$otZ8^V6k%Wc+1jl`1tYr4St~CcfG|h)I0$o;Z2MB7jkVpF3tx9D zUlEfu=p50HMvZtbuJIP~My%a?z1F>6!(J5LAsPV}*wU-nLY<8`zh_RXhPGf=2o^6| zhYI2iA>N?|7zhNJe*hP=ljZ#|oFwux9|R-RGhHROd8A@Me(xO&heT}STk~uU8=_uodjVbVLAqHL2Aw*C2Q%HR`nIMpu zOc1G_u1YH02#UJccM{%87;%s8{_))L#ihQ*r5EqJbiluOmoII%-?rznc@K&%RZ32Q zH=O0O75GhsBWkZ{saLa<$_*UGCaow?am^i2@jLK!|G!~ih%Udvz(9Hclrcf{dB;$+ z9wn}c3h6|M07$!<(MhPYAmYmfY2s-*Po>GqXO&#oC-hq52Nx)11jPVp335W8Ht+>A zg{-fVPbpoiC7%NP$@tQk7zoM)TjhC(f{BSmY9~h-MejGDrvfEs;_lcxU*LJb>rBJM zixg0pKw=6NlMvGOiDZ>kQu_+1oXjdKaP}nYt6(P0ICaX~V=)6d_rQttjaLBzyg@Kt z20Mzkchk|x)G|^VDJr* zz=-fA-7}Nex5P;+E}6pVE75gmB@>y*Vkm~u6$3ciVM_DDn9QU`ea9%Iufj>KfQwK$ zOjKQ1!nN?AR~TH}(+SjK$mw0R?NZLAMDL27zPY>nNhD}?(tVd>?wfV_ls|dtH(ncM z(vh`M(YQ8>0gsRhh>kOE&i0wJhf2rHd2-@k=mM!U_?TPiGuR6{ibDj8hy+Pol2eP= zlRpFt!<}e0fMbTCFarjPl`fu=?(~Ne1)U?pEG2xg#K;X;2#ws_nj4j{oB@r`H0+Kz z!>M^)4;q^dG{y*7)DVdf z(piNhj#4+OXe5&)sj`9+Jx-E+58&!?ikycW z!|0w)y3zp^6Ox&s8FCJGLg1HSmp5t2<(MUbg^Px(&aWI<>07w=w{v$7IYtj(jDPpY zrTPAiyIxjap1T{kLw-ohjpLAY5Edv|ek~2IgRt*Wx=<(lfn{@y?xQFToIB=(4C^{) z3kiEw!-t@&h2+A4usEo1ZiS|NQvamkN#kr>maH!Yta0Fn3hbaLWG^dH3j;++24QGK z3yCyF)E&c1{8gUPOUf5~D8#a!GISdbDjD5zLno{b0xiBkc*K1iOBH>p{qYLLQ^p>n zNbY-teNzs>RDty&F!AWot?xD-glTcyAjwVG@uO4_?|D`L1xlQ|L-xP=;CY=ok~0jk!XoVPHtW^F**3DHSK`U>Tm+>Pp6ndp4qt<#i6h z0=oS7^1ZQq3*O=P#>#(hA1FAa-^}OwANorz@U8!Gd-j`;WZCcSQ=LeH@f^dU0tjxH z7uUWK?xu#`g{{-5J0cC81@4HH;?diyC(lfE_f0+h%oJ#J-s_$09svIJ+KW$4e)uwq zdkY~CHV;q4$tfSN3g8qXAZy?O^5Zv=k^H`LK+&UyAyov(0wPSvty-#}vU4CoU(BX6K zXWNJOzTz2i`RA_*Bqcv<03pd@5>Mr1u2SA6qAM~!B${tmZ z8c`aRlb%-&C;^Q{YZS*6Z>Wzb*$Q|?oRPp2T~$~dRMt|Tv;***Ge~yPkH2kPhaY*g zjor{ue21SdKztWJ9INs5HT?F=2`h$n^)Z8s`|AcB-*5CKtoRpF*tg>nl;zaGivb}B zGZj=du|XBv1eRFBD2bL>WSN3`aMi294ad#g1+0FnJwyUbT>n=%h`(N*k$M6V=U5`2 zpx_eJvJh;-NrLDF)sB{9RBCw;Ugbqeob*1#svxf%)Ym&)5TN21dl4TIMh(X`by6Ws zM)Zv^VBpxaX%iha88fyP`(Ge0TzTa>3zY6Qr`CgocRo}4xXJDVBW&VutlzY-s}#2P zxx?pie$ygIZ;c(dX8Np|U0ZKyG}`zZ(TbQj2wZE4>s$8p12+{bwQ1eT&)2N|p!Q11dNap$bUQhSuE+5SqJ^b$ci<$4|f0mp*R5EnfpPbiM z5==@S+;wK}sl7uhUwSBzY(HZ?Wewyn94yep8iFfe~%U{NsyyUk1ey+W6g zcEboI-AYy@rVTC|$~?7dX!mfOKVi|IT!9rY?U$`Ues~4GBo#;{75FoJ_#Kz~f9k{U zxaG-*-*`X?1LYR0RG4GB?xGse(B+pPi;y*1u3DH3!hzf}qNbZD_5X3axv04W-dq&@ zz^5t96rmv=BvN0@kid_K7zKw+yy0ar5g}l0ffV3N7feWnoPk*QPU@NYUm@K^oVZBk z$h?iUjmKpAVlu~La(ywmLx&)&Tg-~CZFCXqzR=*0A#EjSi*=CSY;6T?fwd(EcnS&a z!aoC!R3g>z{F*dCa|9>|b-~L*0u=g>$&~;`EO5*L6TdSY@o?}e z5s;t~=o9HVv>yBjfm~zxRLmb)6DB&mCcukYiQ05}$XsAG!9U>2=xcO}|L%hqCc9sp zeE&VzosfE+APY*k1fZu1;v`13D}<#W-e1JRNYtDJ8E7S}A2JmZ7qKSDLckoj3ZMNo zoI(Of5zQxIDq65SL@8hrbh;VmK}UO&o0<$C##cCN36BBn1yO~D@-mWQv06d(A?Gm~ zUxjFcB&-E+7nHRKac>aO6LDtsLPdX#=n0j>p?cFOOE=S7-&5b$KIrgA+x=0QU0WvP z9*T?vjm^dB-F7m}J8h&6j%OA78 zYuo3h=stB{l{YFC3W`Y?%=5?Opx?|EB@HP)0lTjsrcCf)$G0t< z+t0T{P^yyGAM-EVcyXzJVfm%Sf4%%S%e}kq`>XZd2Osv|^>842-mq)5^isKZ{{dfG zZTGf5N6+0vKeZ5}7H{wsY#1|dq<-1kMjdb6?Mo~1+csY|Z^kI(Ta-7VV8HorI=xfil3b|I`!L z&OJN%-d~jeJ`O%Y*b5Y-!>N}?u08pLXq4c+^>wr*c#?~f3Y`fr;FTW+XNbp69CFJT z2u^H7_ESjoIS%Ct0O2^~-t|wd=ZxK(eMwvV*3yf$Uh6imVH<9TlhL0s9-HTj&GW}D ze0+P?X4n!3Y`a;*y0GmK#6E)=J7b@LhsUga7TFJ5LZKpg6NCAV$JAgJ6!d?M$Qy(m zFg8&CNe+zEGku@Qnc?NAC}MY^e6=KhgGZtihjC$~JMEB-ZPC?(9}(;rw2)|PDrYa_ zy=bFCrFev8Qo@)Zh?DCE7%&7$irHcgVP3MrXc4=Q4M4fqaI~~S`Vhp&Z^v3cr1D#` zrRb~HZ_er}!~Ihu4=baoH=`$G+?eSzW)968&h;CMyKpZ|@c+u{157Z-tC>SFL7adI zglfqnf(3ulpc2`&KW)jR7F^6`8*%@OplbW#pq8KunG6o_usfS@@>F|bWj493X!9(y!XAS=ii;|>%V@I?8Zb!(ci$J2pU0W<*dbU z8dTr)#|=~HLy9d!hUw=s4pS3PU;&u@dr11PcQ!ydKm4>Ihd*XP*S0@lfEq`Z`;Dt3 z3{a6GBLhJ9n36}HKY)gxm5Z{9JPvYEB0^!*1YtoQ^pLgKeRCd45>y#b7TxxP>LsMX z#)uPCIq}`#*d)UIPvep3maW`ITUVg3BC|?dqA5sZ(m=v8_^dEiX&U1A28=Dmd}{wH|CJ}0xazA;Y6RU*l$|w z)hwp$My!C-+ggu6P!#Y~h3tg`%A<<7=i%btN5?S01dCH~kK(+hQZgl`wKe+|)SUhA zIR4>}*UiQ7h>ZVg_MhNw8hq}}4Uno!aYbb3rVV|*ap(GsbN$B52*^lmS=!nE@miMs zM`)Aj4f>ZeHYoqG>=yu>1r5sn4lbtgwq#(p9z6ITZM4a^3AlOlA2e;KiL9NFm*$z$? zf)fa%z91|CCWsVWD0Xb$Syo}ccJ8~^U+$m!$>Zl$9kKKq?6Ad)y|oQ&h72Ke)4qk+ zEPo1FP>W^AF4d^I3p?bTI(UT!3f^7HW2pF6G^uedid+OnTnx>NxWc}JDmEGXW@Y0= z^SO6GO&ATHIs55ew4LT@D+%j%TT|HBR_Drv*98S6kewxJ3+$Z<;nE7KAh!J)dqcCM z&RNvh=B%?sS)vFDw?u(GljsY{Vgm?`;PGu|6%fykU8_Y>bUD*ch{g z#VlSSKo&?smd1rhfP-1`*(Sz;)U-*l6DPLYbTAWlP^JrN>Q3CIi`%3VCdstiu=)3Z=PDoiCNzpmd{@g~#z^`s_Zpae4$__vX+Hx2- zBq}=>9cbzFxED#D!>zj>@vL_3Mx-l1UW}i}VUn9pb^DnfV{&wbY-T z{9`*S{V&Qap-xtzj)wWsUXTbmN-rC&ro~Vw+S3PXzgh1uzGK36r{8#|R*J^~SK@Yt z&P6#=%VmJhCb%PwE;7wh z$WblB;#)|`(A^0mX1V@$-BA%zA<}}y-14DYd}C)_L?3V(I((=Ve%kiBF8W@Ye!ZV$ z^naw;r8aECt$=^QH@9W}6MnvGRo9Hf&_dsph%mJ9Chpp^3!dM2JP6zdRn`zk7Pff) zOn})6BHW2CBfhS#(xcBPiL;2bC>5P~NrwhD5K6fL>iQ`4o-qlbF#EUmZk@Cj2kgZ` zdkN0;W_$PX5cKDc)c#|=$A%ib^+Csi6HVQI0~e0%WR+)nmMf(s1Hy8L`SZ4XxJaek&l@+MV*7)rimWT-C}W zAZyWZ0nh$zRsKsF)f^d;`b~z^nieXjQ?~^dA<2IP5Su>OoRgT8mYZlzjdf4I?!EN% z*-O1A8~E=xx{1e@B#ThXU1ehtPc#3JxUNP4qT6*@2}*D_6eY za|fD%;Lo#`C&`%jLS$=n8~sTonKMa=^oZovZodb@kv2)WQV=$`9AG{tu{p2>POCU< z;nAwe_9q%i=J2leeY5sO?UIhRQ?j6gJ7`J&2JgGhl8&!gxc(}SmR}cet`9mkoU8Ub z8vJ4d)M2O0gXUBALHqDdZ)Y%hF^mC>2`6tqar=P9TlcQ9jC2*wLFcL5!Q7!1-^!oX zzHWTO@=MEWw&2n_e|7!29saV-!Iay(H;0U7R?W(TJwusM?6bzYFX(czb6|AqqN8WL z?Te6xZC~k(aF9@>6&#-h9Mq%q`>c<8PsYvc$VlBVlAwsjh|^MJzG_vsnQZwb;VACw z!k#ZaH)b8N$p{Oz{MR7;Meu@ShAu2qW&;?9aJcPq4SsK6Ka91cIj{nZS2hth3zCRy zla~yPmmRqKi~9zw9`5ZPGw!bggn4M8LLAgy$cq%jfH+QoX3f6_(A6K?7;JAp;Zind75-A|s- z6P`@K46$#Gcj)wvt?261YtJjvk&DmvavCCU=3w-(iGw^`8X3V8DC4L9sqf;qehN|n zm(d^n{Au{kpYA#R@#*I-{R+SI@@-zC0o@_}D^h8Ml2?$(dp}(;VtZeU4i>4PFxE%* zbsXH^+}6_NVJ!1sLj3DIH!USSy!01Jrckp5e&lr>Fnrf(@ZQDA)+$$-U3!(kGc7$$ zrSwxIVLQ38Rou!N_p}^zOFyHhjBPkfxhE+}ptrn<9P|bZ&TjhkVI*M_Yh+kxW1eTj zdQa!IcX^rTO=?z@Ta{Uh6f8iukq`s$yLDF*bTHA=&oMb+Z`Z%2cgsZ90^hPgR@GSX zJ3`NvpnWs*&C4+rNgy{xlY3c$zPlLzX1 zxAxS12(hhY&@xmWOfG?`Y1NX^T|dbfF!JBLkNK)z>X=AgG=`fUbnAD+Ge?FBhj#rv z!qTS@<$A$#!CBxndhbHXl$YAPw$auJXYF`$?UaLT2#*e}4mwJdvge)o5hq?B$%J3O zesiz6&jRk}PZw4YvVC@x z&~}&}3CuaN_VmP*0P?-^G$3L(A!0oWYc@pM)UFYnU2~Hh8qne5p(8IY9EEZ{CcJSX zmrL$FQhDIek=WP_$}Tj5*}2)gTwkqzD;iMMa4?=v4BF3R3NC+xRaBJjkCNHVtPd{G z-0=C0_7bS>mA!;VeNfppu0zF6EV!*?{3W?=@*ZNCW_{FM2OcT@8<1?b1SxG682lLn z=^a3_-x4Ik+gYC&NZ$dZ#9M--u+l2H7)Vb7QqnC!BI~kQpBP9mv2#0a2~s?!T-oO_ z;!6REnfOTsH<`P4E1z?(VLf{>Aq{`t?ZVbpNaD>%{QIAF<0JOgTv*vzQ_s4mm*@nt6!F9{_NJ zbeNtgW+P$qo8=HTgrzK6!DgZb-%_Vvv0<@1F4iRjbdDc_bpWPzJs z$US)c$4UEwfE_H$yY{M3V#d&d@x)@yv+{tw+*kjueF;4)8BcU+o|Ojdr9RWU_6mAd zI-aP7$^2%`c^6*v@vLGzalWQrWx!rJih9fGS><>lE>|mUSsbu09!0$sao$%Huot~+ zFP?N<*pc zH`#VdOD&CQhTkmCY)mrzRuqu`ZITITS5&KneNi^qjNw`6JjHmHm6*|+?T!B*>!{oJe5j3m?0 zaH=Rv-WlyVfpw9Zl_&CGm zY`4a!l{>V7o(b^yCmu%_PjjU1%%l&C#}m4hE7q8R8^c!wi*hUIp5CnZ+y?mVx~Aoj z3n)9eT32ox$xPbzqD&mFP%a}toPC}4TV!d4K=Oy1yhobrw%)jx%@(=Wg4G|*H*tcK6gjws+iqGwVN&gbZ7@j1!t z`oYC-{KKW*A58uFo8W4uzxy1hhij3GubhwG7y#cx{?10f{n0Dm`uOB?7yoW_YV4Uy zFZ_&)I~qEO?_??w#w}5Bzu}ja$U7>JBN5}%Q6Lu{ei`Ou7oYzgNm5?<1&7OjVUmv> z+zkb53~u30KXV2J5$Y$l0ia18a{-YzhzmkEKuyrWsMX7V(Cc%N@E$5asp$aBy9*f# z$nDLIE)U9 zq)m~V7;TWsv8;Yc#HIk9lPc{ULa6@u?cAxH(Zv3 zy%Iw1#R=)Oc5}wZ%>k2(5#4WR55zI znby=$T7Og{dXJ~si&qD|j3YScRXmH3LsCsWrFE!xLoopq5e^!2C^Sf1l>JxsMiCpx zX>Oppe&i2z5xH&nVEDa(m5mu^5^y(p=f&)Y!7t5U4q)*Bhlf#!!6;E-m-q9 zL&p>WJjR{vz^+cv_jZIAY=d~6fJhZUTIsvUg{>WjTMxl9rbB8CCq?t^EeAXlI!^vO zS~+Zwiq|2<_jI!A()R#^$pHBF$#+3C`b~muc6-`kO3)Tg;1&gC$t;qOrbmxaW2oB_ zxh4+VqWf4gVh8eVa0xvlU9(h4$zm#HzWbhS+cs@$3_CoXo<}-hDjOxI;UsrUn+8NU z;m!^BY_G3Fua!ZRD;;8L2}CF(Ju5*u1P2FHm$pLK)1c8XqCtp6iRipW<4zD`1CO3MIC#*z(_h&b%xDT_mJC_X zB%Mz3c8#tGX0E=FmKVy#F+Gr75y~(3Z3^Tsr|+$S{8gdcycvg>W9@N#lB!Fe>&5O^ z-6LYJ%ydmU%LC4G_?YUkha5SRj)H)rz-yXtl=xaFD{2B2HDjF<6-}W9E6+U?aNXUv zal)Q=!I^uiYp|(Bn*>kDET)^{vC-d#Gy$DC%`EzY`ge8 zVZ?Q~F0!5!utj{m0?^E;W5JwJ4BUG-EQPCvh}I5pP+ zh8acMR^b(Vk{x|t@!BTL3onSyNwF{>7EX#K0kLF4oPWWQ-rv>R_0-0vn&O_IV_CMt zyEuT3~QsPIAcuaNRD*`agzjSWe%ACd3ln*I zqeQt071OClv~Q3ddg?$ExmvCa?>@8h^iF?TNzhj6H(?V zMK$Y%E)ocF4vS87oUc+CA1R4`BdL=_=QCys#%M}7xuYFSS_QM8Xr&m` z5*=)$W0a7tQ7#ZwX2WNsM3@>*=k=H!X)|XX@rVv;;SByrUbHfY#R)v|({g1NtQtwn z-d*6pq-;Erh>;K55No>Sky;*z)wpR}!ySl9>}lVEQNl}@y~rjiXV`W4uvALb7**U( zxi3?)hmyxB=|mESI{q*>L_zH5Zu(i_Cc^2vJ@c$vwc zMD{ik**Y+t8I#{L0X7pXG1iOi%xpiBh=eoR$B9D^fm%kR_8=w8DIxxD76H8~u0c5L zj#f{N^ko1*0bSc4ae{~#1VNb58wBGOQ72g6)7jX+r1x~m@99!L(A__&yZ?P%&ZkD5 z(5U|u6&v+`VCnn1lJ|AhEUoxJSMz~x6}W04z1uvgOAF}ILe`Z2g}n~E&Phvlz>+<5ThKDEJK=W;ww@;M{&SiB2i+41o^C@ZXTC2tkh8qoaMF6h+OsC0 z%RH|$c5i>m@@YbjP&%WdWR)%<QzV(IhOnoSH9e6(So2r{H?u zIPXJ?y}zIr_ouRFu!A=U=fylkrT$Mb^RnJ*Sr^IB!M&YjQp^m9nO?`3aY9@>Db@zW z+6l4#(*mjxseob7$*dDuJ=I^!pV13K8E#AKlOvQvN2AYH1%(WHSWXX}^sxNK53O>| zwLu|kMqeb@sAhJAa*-xTKiE6{HkWU?Z+>@HP^g~KR|>gQXlaCUk=<&cxxPKVom6P~ zj6P8)q(Unql#ArY1$3K`69t4^q)P_Wv(9_3w~62^n$g>Z6;!4oLb*t;++1xD6Mq$o zAP3KFa+`C4Lhg*-B~%iCya?qYSe((JS{D>@X7oA28hV-=ptf2(`&LjH z46jXSW{@zxlvVodw`t#(#T$;-As1Ptye-dX@;*+GDrfY1p^DzNI6^sKygV?u-Xq?9 z(K2aD8RQ}hlrjrO?;T}xuy#f-3bm|7H4&DLq-#p;9NkQi*3IaRd>YmZ5z0pFN+In~ znuHAu6vmRWn%UAe+=NBM8;)EgRRihsbuLNi9C8tt0_Shoxgy`aJ~mFvX7p*%aas0vVvKoZaQU`}nLrn+vA{Qx++bDe$3(zRaY9LjP%^P#l`!>)(akSfr4pLq- zR@zZdP)D7znpIUYnlUO8u-j(zslqa9)~X2QBJ-7IX<%WooSP>RGw*MHC?P8+cU<9X~T>@CECSC0Hb`Q zL}~xpcf@mD)Udl}^cIc~+ffQxP3y}?J);b@u$Y?!A(`HY#Z1}Ay?O=KT%X6siUF4m zdOK@??>(@+;Jy|eH#fPIVr+PhR7zFu<*eJTOQ?j3Coa+)UR`!alNdaDQr*t?bx3KVE) z-K~_-b}I@8-6A;Zrr`<-g*5MXF(fLYX}?Q5?fkh-a8XO~7`aHcIsvS9B1d^VMNYE? z7YBuTGx|)yNp(vil#47^>&A>0qr+_SR5~29KWB3sz~+lwWTgTo-#5o+CcMP{DbpYj zeMW=i{UZiYbo&6H*hQ7&-Q#8JpkhW}z+YAwpo{!Nn;0pLc8fm->In7ey{D~kmvN;BW zLRmaF+Izaj4Ot{=y#1@U4~P>+Iqy%NKVdABACwL)f|suBN`KbkQDJn^xrD%yMt@b4 zKY7!H5w~E}+P_QB8#>_I7RX;6OkdNz@hK=>vrZlV#_>sK*|@VT=q&F>fadgQ4S(u_ zQE75TV{k>|4_3aj)4$+efAW13#+`Jnrtzzdp~UQ=<$=T^`xz@M~o}ycR!1`jDqV!lQ?P03z+gSC?-PPg#0U%yV_O^{{OAw40o~a dtyNWSr{V2#5&0>n$z5ok%4PZDboXk*{{jDm4C4R* literal 0 HcmV?d00001 diff --git a/agv_app/mission_executor.py b/agv_app/mission_executor.py new file mode 100644 index 0000000..efc3f0f --- /dev/null +++ b/agv_app/mission_executor.py @@ -0,0 +1,650 @@ +# -*- coding: utf-8 -*- +""" +任务执行器 V3 — M×N 网格蛇形路径拍摄 + +工作流： +1. 根据 grid 生成蛇形路径（奇数行左→右，偶数行右→左） +2. 逐台机器： + - 正面：导航 → 扫码(多姿态重试) → 查机型 → 按姿态拍照 + - 背面：导航 → 按姿态拍照 +3. 回到 (0,0) +""" +import os +import time +import json +import logging +import threading +import subprocess +import math +from typing import Optional, Dict, List +from enum import Enum + +import requests +import cv2 +import numpy as np + +logger = logging.getLogger(__name__) + +ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash" +from config import ARM_CAMERA_CONFIG +ARM_CAMERA_SNAPSHOT = ARM_CAMERA_CONFIG["snapshot_url"] +PHOTOS_DIR = "/home/elephant/photos" + +# 二维码扫描重试参数 +QR_SCAN_TIMEOUT = 5 # 单次扫描超时 +QR_POSE_WAIT = 1.5 # 调整姿态后等待时间 +MANUAL_QR_TIMEOUT = 300 # 5分钟超时 + + +class MissionStatus(str, Enum): + IDLE = "idle" + RUNNING = "running" + PAUSED = "paused" + COMPLETED = "completed" + WAITING_QR = "waiting_qr" + + +class MissionExecutorV3: + """任务执行器 V3 — M×N 网格蛇形路径""" + + _instance = None # 单例，供外部停止用 + + def __init__(self, config: dict): + self.config = config + self.status = MissionStatus.IDLE + MissionExecutorV3._instance = self + + # 实时状态报告 + self.report = { + "status": "idle", + "step": "", + "progress": 0, + "total": 0, + "log": [], + "error": None, + } + + # 线程同步 + self._stop = threading.Event() + self._pause = threading.Event() + self._pause.set() # 初始不暂停 + self._qr_event = threading.Event() + self._qr_value: Optional[str] = None + + # 设备 + from .arm_client import ArmClient + self.arm_client = ArmClient( + config["arm"]["host"], + config["arm"]["port"] + ) + + from .agv_controller_ros2 import AGVController + self.agv = AGVController( + device=config.get("device", "/dev/agvpro_controller"), + baudrate=config.get("baudrate", 1000000) + ) + + # ==================== 连接 ==================== + + def connect_all(self) -> Dict[str, bool]: + results = {} + results["agv"] = self.agv.connect() + results["arm"] = self.arm_client.connect() + return results + + def disconnect_all(self): + if self.arm_client: + self.arm_client.close() + self.agv.disconnect() + + # ==================== 主任务流程 ==================== + + def execute_mission( + self, + mission_config: dict, + machines: list, + qr_configs: list, + models: list, + ) -> dict: + """ + 执行完整拍摄任务。 + + Args: + mission_config: {rows, cols, grid, positions} + machines: [{id, row, col, front: {coords}, back: {coords}}] + qr_configs: [{id, name, joint_angles: [a1..a6]}] + models: [{id, name, poses: [{name, arm_angles}], poses_back: [...]}] + + Returns: + 执行报告 dict + """ + self.status = MissionStatus.RUNNING + self.report = {"status": "running", "step": "初始化", "progress": 0, "total": 0, "log": [], "error": None} + self._stop.clear() + self._pause.set() + + start_time = time.time() + + try: + rows = int(mission_config.get("rows", 1)) + cols = int(mission_config.get("cols", 1)) + grid = mission_config.get("grid", []) + positions = mission_config.get("positions", []) + + # 1. 生成蛇形路径 + path = self._build_snake_path(rows, cols, grid) + if not path: + self._log("❌ 网格中没有机器，任务终止") + self.report["error"] = "No machines in grid" + return self._finish(0) + + self.report["total"] = len(path) + self._log(f"📍 蛇形路径生成: {len(path)} 台机器") + + # 初始化任务列表 + self.report["tasks"] = [{ + "row": r, "col": c, + "machine_id": f"m_{r}_{c}", + "label": f"{r+1}-{c+1}", + "status": "pending", + "step": "等待", + "qr_value": None, + "photos_front": 0, + "photos_back": 0, + } for (r, c) in path] + + # 初始化任务列表 + self.report["tasks"] = [{ + "row": r, "col": c, + "machine_id": f"m_{r}_{c}", + "label": f"{r+1}-{c+1}", + "status": "pending", + "step": "等待", + "qr_value": None, + "photos_front": 0, + "photos_back": 0, + } for (r, c) in path] + + # 2. 逐台执行 + for idx, (r, c) in enumerate(path): + if self._stop.is_set(): + self._log("⏹️ 任务已停止") + break + + self._wait_pause() + + # 更新任务状态 → 正面开始 + task = self._get_task(r, c) + if task: + task["status"] = "active" + task["step"] = "正面扫码" + + machine_id = f"m_{r}_{c}" + machine = next((m for m in machines if m.get("id") == machine_id), None) + if not machine: + self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过") + continue + + rl, cl = r + 1, c + 1 # 显示用的 1-based + + # --- 正面 --- + self._log(f"📍 机器 {rl}-{cl} 进入正面点位") + self._step(f"机器 {rl}-{cl} 正面") + + # 导航到正面点位 + front_pt = self._find_point(positions, r, c, "front") + if front_pt and self._has_coords(front_pt): + if not self._navigate(front_pt, "正面"): + self._log(f"⚠️ 导航失败，尝试继续") + else: + self._log(f"⚠️ 无正面点位坐标") + + # 扫描二维码 + qr_value = self._scan_qr_with_poses(qr_configs) + if self._stop.is_set(): + break + + # 查机型 + 更新任务步骤 + model_name = self._lookup_model(qr_value) + self._log(f" 🏷️ 机型: {model_name}") + if task and qr_value: + task["qr_value"] = qr_value + if task: + task["step"] = "正面拍照" + + # 正面拍照 + model = self._find_model(models, model_name) + if model: + self._shoot(model, "front", rl, cl, qr_value or "unknown") + else: + self._log(f" ⚠️ 未找到机型 {model_name}") + + self._progress(idx, 1) + + # --- 背面 --- + if task: + task["step"] = "背面拍照" + self._log(f"📍 机器 {rl}-{cl} 进入背面点位") + self._step(f"机器 {rl}-{cl} 背面") + + # 导航到背面点位 + back_pt = self._find_point(positions, r + 1, c, "back") + if back_pt and self._has_coords(back_pt): + if not self._navigate(back_pt, "背面"): + self._log(f"⚠️ 导航失败，尝试继续") + else: + self._log(f"⚠️ 无背面点位坐标") + + # 背面拍照 + if model: + self._shoot(model, "back", rl, cl, qr_value or "unknown") + + # 标记任务完成 + if task: + task["status"] = "completed" + task["step"] = "完成" + self._progress(idx, 2) + + # 3. 回到出发点 + if not self._stop.is_set(): + self._step("返回出发点") + self._log("→ 返回 (0, 0)") + self._nav2_go_to_point(0, 0, 0, timeout_sec=60) + + elapsed = time.time() - start_time + return self._finish(elapsed) + + except Exception as e: + self._log(f"❌ 任务异常: {e}") + logger.exception("execute_mission 崩溃") + self.report["error"] = str(e) + self.status = MissionStatus.IDLE + self.report["status"] = "idle" + return self.report + + def _finish(self, elapsed: float) -> dict: + if self._stop.is_set(): + self._step("已停止") + else: + self._step("完成") + self._log(f"✅ 任务完成 ({elapsed:.0f}s)") + self.report["progress"] = 100 + self.status = MissionStatus.IDLE + self.report["status"] = "idle" + return self.report + + # ==================== 蛇形路径 ==================== + + def _build_snake_path(self, rows: int, cols: int, grid: list) -> list: + """奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左""" + path = [] + for r in range(rows): + if r % 2 == 0: + for c in range(cols): + if self._has_machine(grid, r, c): + path.append((r, c)) + else: + for c in range(cols - 1, -1, -1): + if self._has_machine(grid, r, c): + path.append((r, c)) + return path + + @staticmethod + def _has_machine(grid: list, r: int, c: int) -> bool: + if not grid or r >= len(grid): + return False + row = grid[r] + if isinstance(row, list): + return c < len(row) and bool(row[c]) + return False + + # ==================== 点位查找 ==================== + + @staticmethod + def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]: + for p in positions: + if p.get("row") == row and p.get("col") == col and p.get("side") == side: + return p + return None + + @staticmethod + def _has_coords(point: dict) -> bool: + coords = point.get("coords", []) + return len(coords) >= 2 and (coords[0] != 0 or coords[1] != 0) + + # ==================== 导航 ==================== + + def _navigate(self, point: dict, label: str) -> bool: + coords = point["coords"] + x, y = float(coords[0]), float(coords[1]) + yaw = float(coords[2]) if len(coords) >= 3 else 0.0 + self._log(f" 🧭 导航到{label}点位 ({x:.2f}, {y:.2f}, yaw={math.degrees(yaw):.0f}°)") + return self._nav2_go_to_point(x, y, yaw) + + # ==================== 二维码扫描 ==================== + + def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]: + """用二维码配置中的姿态依次尝试""" + if not qr_configs: + self._log(f" ⚠️ 无二维码配置") + return self._request_manual_qr() + + self._log(f" 🔍 尝试 {len(qr_configs)} 个二维码姿态...") + for i, qc in enumerate(qr_configs): + if self._stop.is_set(): + return None + self._wait_pause() + + angles = qc.get("joint_angles", []) + if not angles or len(angles) < 6: + continue + + name = qc.get("name", f"姿态{i+1}") + self._log(f" [{i+1}/{len(qr_configs)}] {name}") + + # 调整机械臂 + if self.arm_client: + self.arm_client.set_angles(angles, speed=500) + time.sleep(QR_POSE_WAIT) + + # 扫码 + qr = self._decode_qr_from_arm() + if qr: + self._log(f" ✅ 识别成功: {qr}") + return qr + + time.sleep(0.3) + + self._log(f" ⚠️ 全部 {len(qr_configs)} 个姿态均未识别到二维码") + return self._request_manual_qr() + + def _decode_qr_from_arm(self) -> Optional[str]: + """从机械臂摄像头取一帧，识别二维码""" + for attempt in range(3): + try: + resp = requests.get(ARM_CAMERA_SNAPSHOT, timeout=QR_SCAN_TIMEOUT) + if resp.status_code != 200 or not resp.content: + continue + + arr = np.frombuffer(resp.content, dtype=np.uint8) + frame = cv2.imdecode(arr, cv2.IMREAD_COLOR) + if frame is None: + continue + + detector = cv2.QRCodeDetector() + data, bbox, _ = detector.detectAndDecode(frame) + if data and len(data) > 0: + return data + except Exception: + pass + time.sleep(0.5) + return None + + def _request_manual_qr(self) -> Optional[str]: + """暂停任务，等待手动输入""" + self.status = MissionStatus.WAITING_QR + self.report["status"] = "waiting_qr" + self.report["step"] = "等待手动输入二维码" + self._log(" ⌨️ 弹窗等待手动输入二维码...") + + self._qr_event.clear() + if self._qr_event.wait(timeout=MANUAL_QR_TIMEOUT): + self.status = MissionStatus.RUNNING + self.report["status"] = "running" + self._log(f" ✏️ 手动输入: {self._qr_value}") + return self._qr_value + else: + self.status = MissionStatus.RUNNING + self.report["status"] = "running" + self._log(f" ⚠️ 等待超时（{MANUAL_QR_TIMEOUT}s），跳过") + return None + + def set_manual_qr(self, value: str): + self._qr_value = value.strip() + self._qr_event.set() + + # ==================== 机型查询 ==================== + + def _lookup_model(self, qr_value: Optional[str]) -> str: + """TODO: 后续通过 HTTP 接口查询机型""" + return "机器1" + + @staticmethod + def _find_model(models: list, name: str) -> Optional[dict]: + """在机型列表中找到匹配的机型""" + for m in models: + if m.get("name") == name or m.get("id") == name: + return m + # fallback: 第一个机型 + return models[0] if models else None + + # ==================== 姿态拍照 ==================== + + def _shoot(self, model: dict, side: str, row: int, col: int, qr_value: str): + """按机型配置的所有姿态依次拍照""" + # 更新任务照片计数 + task = self._get_task(row - 1, col - 1) + side_label = "正面" if side == "front" else "背面" + poses = model.get("poses", []) if side == "front" else model.get("poses_back", []) + if not poses: + self._log(f" ⚠️ 机型无{side_label}姿态配置") + return + + self._log(f" 📷 {side_label}拍照 ({len(poses)} 个姿态)") + for pi, pose in enumerate(poses): + if self._stop.is_set(): + break + self._wait_pause() + + angles = pose.get("arm_angles", []) + if not angles or len(angles) < 6: + self._log(f" 跳过 {pose.get('name', f'姿态{pi+1}')}: 无效角度") + continue + + name = pose.get("name", f"{side_label}-{pi+1}") + self._log(f" 🎯 {name}") + + # 调整机械臂 + if self.arm_client: + self.arm_client.set_angles(angles, speed=500) + time.sleep(QR_POSE_WAIT) + + # 拍照 + path = self._capture_arm_photo(row, col, side, pi + 1, qr_value) + if path: + self._log(f" 💾 {os.path.basename(path)}") + + def _capture_arm_photo(self, row: int, col: int, side: str, + pose_idx: int, qr_value: str) -> Optional[str]: + """从机械臂摄像头拍照存本地""" + try: + resp = requests.get(ARM_CAMERA_SNAPSHOT, timeout=10) + if resp.status_code != 200 or not resp.content: + logger.error("arm snapshot 请求失败") + return None + + os.makedirs(PHOTOS_DIR, exist_ok=True) + ts = time.strftime("%Y%m%d_%H%M%S") + fname = f"{ts}_r{row}c{col}_{side}_p{pose_idx}_{qr_value[:20]}.jpg" + fpath = os.path.join(PHOTOS_DIR, fname) + with open(fpath, "wb") as f: + f.write(resp.content) + return fpath + except Exception as e: + logger.error(f"拍照异常: {e}") + return None + + # ==================== 控制 ==================== + + def _wait_pause(self): + """等待暂停状态解除""" + self._pause.wait() + + def pause(self): + self._pause.clear() + self.status = MissionStatus.PAUSED + self.report["status"] = "paused" + self.report["step"] = "已暂停" + self._log("⏸️ 任务已暂停") + + def resume(self): + self._pause.set() + self.status = MissionStatus.RUNNING + self.report["status"] = "running" + self._log("▶️ 任务已恢复") + + def stop(self): + self._stop.set() + self._pause.set() # 解除暂停 + self._qr_event.set() # 解除 QR 等待 + if self.arm_client: + try: + self.arm_client.task_stop() + except Exception: + pass + self.agv.stop() + self.status = MissionStatus.IDLE + self.report["status"] = "idle" + + def get_status(self) -> dict: + return { + "status": self.report["status"], + "step": self.report["step"], + "progress": self.report["progress"], + "total": self.report["total"], + } + + def get_logs(self) -> dict: + """返回实时日志和完整状态""" + return self.report + + # ==================== 状态报告 ==================== + + def _log(self, msg: str): + self.report["log"].append(msg) + # Keep last 500 entries + if len(self.report["log"]) > 500: + self.report["log"] = self.report["log"][-500:] + logger.info(msg) + + def _step(self, text: str): + self.report["step"] = text + + def _get_task(self, row: int, col: int) -> Optional[dict]: + """获取指定行列的任务记录""" + for t in self.report.get("tasks", []): + if t["row"] == row and t["col"] == col: + return t + return None + + def _progress(self, machine_idx: int, side_code: int): + """side_code: 1=正面完成, 2=背面完成""" + if self.report["total"]: + self.report["progress"] = min( + int((machine_idx * 2 + side_code) / (self.report["total"] * 2) * 100), + 99 + ) + + # ==================== Nav2 导航 ==================== + # (保留原实现) + + def _nav2_check_available(self) -> bool: + try: + rc, out, err = self._run_ros2_cmd("ros2 action list") + if rc != 0: + return False + return "/navigate_to_pose" in out + except: + return False + + def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0, + timeout_sec: float = 120.0) -> bool: + if not self._nav2_check_available(): + logger.error("Nav2 action server 不可用") + return False + + qz = math.sin(yaw / 2.0) + qw = math.cos(yaw / 2.0) + + pose_yaml = ( + f"pose:\n" + f" header:\n" + f" stamp:\n" + f" sec: 0\n" + f" nanosec: 0\n" + f" frame_id: 'map'\n" + f" pose:\n" + f" position:\n" + f" x: {x}\n" + f" y: {y}\n" + f" z: 0.0\n" + f" orientation:\n" + f" x: 0.0\n" + f" y: 0.0\n" + f" z: {qz}\n" + f" w: {qw}" + ) + + cmd = ( + f"ros2 action send_goal /navigate_to_pose " + f"nav2_msgs/action/NavigateToPose " + f"'{pose_yaml}' --feedback" + ) + full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'" + + try: + process = subprocess.Popen( + full_cmd, shell=True, + stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True + ) + succeeded = False + elapsed = 0.0 + while elapsed < timeout_sec: + import select + reads = [process.stdout] + ready, _, _ = select.select(reads, [], [], 1.0) + elapsed += 1.0 + if process.stdout in ready: + line = process.stdout.readline() + if not line: + break + ls = line.strip() + if "succeeded" in ls.lower(): + succeeded = True + break + elif "failed" in ls.lower() or "aborted" in ls.lower(): + break + elif "canceled" in ls.lower() or "cancelled" in ls.lower(): + break + if process.poll() is not None: + break + if process.poll() is None: + process.terminate() + try: + process.wait(timeout=3) + except subprocess.TimeoutExpired: + process.kill() + return succeeded + except Exception as e: + logger.error(f"Nav2 异常: {e}") + return False + + def _nav2_cancel(self): + cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || true'" + try: + subprocess.run(cancel_cmd, shell=True, timeout=3) + except: + pass + + def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple: + full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'" + try: + result = subprocess.run( + full_cmd, shell=True, + capture_output=True, text=True, timeout=timeout + ) + return result.returncode, result.stdout.strip(), result.stderr.strip() + except subprocess.TimeoutExpired: + return -1, "", "Timeout" + except Exception as e: + return -1, "", str(e) \ No newline at end of file diff --git a/agv_app/running.html b/agv_app/running.html new file mode 100644 index 0000000..5536159 --- /dev/null +++ b/agv_app/running.html @@ -0,0 +1,122 @@ + + + + + + 运行监控 - AGV 拍摄系统 + + + + + + + + + \ No newline at end of file diff --git a/agv_app/running.js b/agv_app/running.js new file mode 100644 index 0000000..aa01478 --- /dev/null +++ b/agv_app/running.js @@ -0,0 +1,130 @@ +const { createApp } = Vue +const API = '' + +createApp({ + delimiters: ['[[', ']]'], + + data() { + return { + missionState: 'idle', + progress: 0, + tasks: [], + report: null, + previewUrl: API + '/api/camera/preview', + polling: null, + logs: [], + showQrModal: false, + qrValue: '', + } + }, + computed: { + missionStateText() { + const map = { + idle: '空闲', + running: '任务运行中', + paused: '已暂停', + completed: '已完成', + waiting_qr: '等待输入二维码' + } + return map[this.missionState] || '未知' + }, + }, + mounted() { + this.poll() + }, + beforeUnmount() { + if (this.polling) clearInterval(this.polling) + }, + methods: { + poll() { + this.refresh() + this.pollLogs() + this.polling = setInterval(() => { + this.refresh() + this.pollLogs() + }, 2000) + }, + async refresh() { + try { + const res = await fetch(API + '/api/mission/state') + const data = await res.json() + this.missionState = data.status || 'idle' + this.progress = data.progress || 0 + if (data.tasks) this.tasks = data.tasks + + // QR 弹窗 + if (this.missionState === 'waiting_qr' && !this.showQrModal) { + this.showQrModal = true + this.qrValue = '' + } + + // 完成后获取报告 + if (this.missionState === 'idle' && this.tasks.length > 0) { + const reportRes = await fetch(API + '/api/mission/report') + const reportData = await reportRes.json() + this.report = reportData.report + } + } catch (e) {} + }, + async pollLogs() { + if (this.missionState !== 'running' && this.missionState !== 'waiting_qr') return + try { + const res = await fetch(API + '/api/mission/log') + const data = await res.json() + if (data.log) this.logs = data.log + if (data.progress != null) this.progress = data.progress + if (data.tasks) this.tasks = data.tasks + // 自动滚到底 + this.$nextTick(() => { + const box = this.$refs.logBox + if (box) box.scrollTop = box.scrollHeight + }) + } catch (e) {} + }, + async startMission() { + if (this.missionState !== 'idle') return + this.logs = [] + this.progress = 0 + this.report = null + this.showQrModal = false + await fetch(API + '/api/mission/start', { method: 'POST' }) + this.missionState = 'running' + }, + async pauseMission() { + await fetch(API + '/api/mission/pause', { method: 'POST' }) + this.missionState = 'paused' + }, + async resumeMission() { + await fetch(API + '/api/mission/resume', { method: 'POST' }) + this.missionState = 'running' + this.showQrModal = false + }, + async stopMission() { + await fetch(API + '/api/mission/stop', { method: 'POST' }) + this.missionState = 'idle' + this.showQrModal = false + }, + async submitQr() { + const val = this.qrValue.trim() + await fetch(API + '/api/mission/manual-qr', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ qr: val || ' ' }) + }) + this.showQrModal = false + this.qrValue = '' + }, + cancelQr() { + this.showQrModal = false + this.qrValue = '' + fetch(API + '/api/mission/manual-qr', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ qr: 'SKIP' }) + }) + }, + onPreviewError(e) { + e.target.style.display = 'none' + } + } +}).mount('#app') \ No newline at end of file diff --git a/agv_app/scripts/fix_scan_timestamp.py b/agv_app/scripts/fix_scan_timestamp.py new file mode 100644 index 0000000..479190d --- /dev/null +++ b/agv_app/scripts/fix_scan_timestamp.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +"""修复激光雷达时间戳偏移的修正器 v5""" +import os, sys, rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from builtin_interfaces.msg import Time + +LOCKFILE = "/tmp/scan_fixer.lock" + +if os.path.exists(LOCKFILE): + with open(LOCKFILE) as f: + old_pid = int(f.read().strip()) + try: + os.kill(old_pid, 0) + print(f"Another fixer running PID {old_pid}, exit.", file=sys.stderr) + sys.exit(1) + except (OSError, ProcessLookupError): + print(f"Stale lock removed (PID {old_pid} dead)", file=sys.stderr) + +with open(LOCKFILE, "w") as f: + f.write(str(os.getpid())) + +def main(): + rclpy.init(args=sys.argv[1:]) + node = Node('scan_timestamp_fixer') + offset = 2.0 + pub = node.create_publisher(LaserScan, '/scan_corrected', 10) + count = [0] + + def cb(msg: LaserScan): + count[0] += 1 + s, ns = msg.header.stamp.sec, msg.header.stamp.nanosec + s2 = s - int(offset) + ns2 = ns - int((offset % 1) * 1e9) + if ns2 < 0: + ns2 += 1000000000 + s2 -= 1 + out = LaserScan() + out.header.frame_id = msg.header.frame_id + out.header.stamp = Time(sec=s2, nanosec=ns2) + out.angle_min = msg.angle_min + out.angle_max = msg.angle_max + out.angle_increment = msg.angle_increment + out.time_increment = msg.time_increment + out.scan_time = msg.scan_time + out.range_min = msg.range_min + out.range_max = msg.range_max + out.ranges = msg.ranges + out.intensities = msg.intensities + pub.publish(out) + if count[0] % 200 == 0: + node.get_logger().info(f'#{count[0]} /scan={s} -> /scan_corrected={s2}') + + node.create_subscription(LaserScan, '/scan', cb, 10) + node.get_logger().info(f'Fixer PID={os.getpid()}, offset={offset}s') + + try: + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.5) + finally: + node.destroy_node() + rclpy.shutdown() + if os.path.exists(LOCKFILE): + os.unlink(LOCKFILE) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/agv_app/scripts/start_agv.sh b/agv_app/scripts/start_agv.sh new file mode 100644 index 0000000..f2c548a --- /dev/null +++ b/agv_app/scripts/start_agv.sh @@ -0,0 +1,17 @@ +#!/bin/bash +source /opt/ros/humble/setup.bash +source /home/elephant/agv_pro_ros2/install/setup.bash +export ROS_DOMAIN_ID=1 +cd /home/elephant/agv_pro_ros2 +nohup ros2 daemon start >/dev/null 2>&1 & +sleep 5 +nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 & +sleep 8 +nohup python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py > /tmp/scan_fixer.log 2>&1 & +sleep 5 +nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True > /tmp/ros2_nav2.log 2>&1 & +sleep 15 +cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/agv_flask.log 2>&1 & +sleep 5 +echo "ALL_STARTED" +ps aux | grep -E 'lslidar|agv_pro_node|nav2_container|scan_timestamp_fixer|ros2-daemon|app.py' | grep -v grep \ No newline at end of file diff --git a/agv_app/start_agv.sh b/agv_app/start_agv.sh new file mode 100755 index 0000000..f2c548a --- /dev/null +++ b/agv_app/start_agv.sh @@ -0,0 +1,17 @@ +#!/bin/bash +source /opt/ros/humble/setup.bash +source /home/elephant/agv_pro_ros2/install/setup.bash +export ROS_DOMAIN_ID=1 +cd /home/elephant/agv_pro_ros2 +nohup ros2 daemon start >/dev/null 2>&1 & +sleep 5 +nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 & +sleep 8 +nohup python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py > /tmp/scan_fixer.log 2>&1 & +sleep 5 +nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True > /tmp/ros2_nav2.log 2>&1 & +sleep 15 +cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/agv_flask.log 2>&1 & +sleep 5 +echo "ALL_STARTED" +ps aux | grep -E 'lslidar|agv_pro_node|nav2_container|scan_timestamp_fixer|ros2-daemon|app.py' | grep -v grep \ No newline at end of file diff --git a/agv_app/start_all.sh.bak b/agv_app/start_all.sh.bak new file mode 100755 index 0000000..ad3c346 --- /dev/null +++ b/agv_app/start_all.sh.bak @@ -0,0 +1,177 @@ +#!/bin/bash +# ============================================================ +# Robot AGV 全量启动脚本 v2.6 +# 修复： +# - 清理 scan_fixer lock 文件防残留 +# - Nav2 节点检测 grep -c 改为单行输出 +# - nohup 启动 Nav2 用 bash -c 包裹（确保 source 环境） +# ============================================================ +set -e + +AGV_APP_DIR="/home/elephant/work/agv_app" +AGV_ROS2_DIR="/home/elephant/agv_pro_ros2" +ROS_DOMAIN_ID_VAL=1 + +echo "==========================================" +echo " Robot AGV 全量启动 v2.6" +echo "==========================================" +echo "" + +# ---------- 1. 清理旧进程（不杀 ros2-daemon） ---------- +echo "[1/7] 清理旧进程..." +pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true +pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true +pkill -f "agv_pro_node" 2>/dev/null || true +pkill -f "lslidar_driver_node" 2>/dev/null || true +pkill -f "fix_scan_timestamp" 2>/dev/null || true +pkill -f "python.*app.py" 2>/dev/null || true +sleep 4 + +# 清理 scan_fixer 锁文件（防残留 PID 导致启动失败） +rm -f /tmp/scan_fixer.lock + +echo " 清理完成" + +# ---------- 2. 重启 ros2 daemon ---------- +echo "[2/7] 重启 ros2 daemon..." +pkill -f "ros2-daemon" 2>/dev/null || true +sleep 2 +nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 & +sleep 5 +echo " ros2 daemon 已就绪" + +# ---------- 3. 启动 bringup (含激光雷达) ---------- +echo "[3/7] 启动 AGV Bringup..." +source /opt/ros/humble/setup.bash +cd "$AGV_ROS2_DIR" +source install/setup.bash +nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \ + port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 & +BRINGUP_PID=$! +echo " bringup PID: $BRINGUP_PID" + +echo " 等待 bringup 就绪..." +for i in $(seq 1 20); do + if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then + echo " ✅ bringup 已就绪" + break + fi + sleep 2 +done + +# ---------- 4. 启动激光时间戳修正节点 ---------- +echo "[4/7] 启动激光时间戳修正节点..." +pkill -f "fix_scan_timestamp" 2>/dev/null || true +sleep 2 + +# 确保 /scan 存在 +for i in $(seq 1 10); do + if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then + echo " /scan 话题已上线" + break + fi + sleep 2 +done + +nohup bash -c "source /opt/ros/humble/setup.bash && \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \ + > /tmp/scan_fixer.log 2>&1 & +FIXER_PID=$! +echo " fix_scan_timestamp PID: $FIXER_PID" +sleep 5 + +# 验证 fixer 进程和 scan_corrected +FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0) +if [ "$FIXER_COUNT" -gt 1 ]; then + echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..." + pkill -f "fix_scan_timestamp" 2>/dev/null || true + sleep 2 + rm -f /tmp/scan_fixer.lock + nohup bash -c "source /opt/ros/humble/setup.bash && \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \ + > /tmp/scan_fixer.log 2>&1 & + FIXER_PID=$! + sleep 3 +fi + +if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then + echo " ✅ /scan_corrected 已上线" +else + echo " ⚠️ /scan_corrected 未上线，检查日志：" + tail -5 /tmp/scan_fixer.log +fi + +# ---------- 5. 启动 Nav2 ---------- +echo "[5/7] 启动 Nav2 导航..." +source /opt/ros/humble/setup.bash +cd "$AGV_ROS2_DIR" +source install/setup.bash +# 使用 bash -c 确保 source 环境变量传递到 nohup +nohup bash -c "source /opt/ros/humble/setup.bash && \ + source /home/elephant/agv_pro_ros2/install/setup.bash && \ + export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && \ + ros2 launch agv_pro_navigation2 navigation2_active.launch.py \ + autostart:=True" > /tmp/ros2_nav2.log 2>&1 & +NAV2_PID=$! +echo " Nav2 PID: $NAV2_PID" +sleep 12 + +echo " 等待 Nav2 节点就绪..." +for i in $(seq 1 20); do + NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \ + grep -cE 'lifecycle_manager_navigation|bt_navigator|controller_server' 2>/dev/null || echo 0) + # 去除可能的换行符，确保是单个数字 + NODES=$(echo "$NODES" | tr -d '\n' | awk '{print $1}') + if [ "$NODES" -ge 3 ] 2>/dev/null; then + echo " ✅ Nav2 节点已就绪 ($NODES 个)" + break + fi + sleep 3 +done + +# ---------- 6. 设置精度参数 ---------- +echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..." +source /opt/ros/humble/setup.bash +cd "$AGV_ROS2_DIR" +source install/setup.bash + +for NODE in /controller_server /bt_navigator /planner_server; do + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true +done +ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true +ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true +ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true +echo " 精度参数已设置" + +# ---------- 7. 启动 Flask ---------- +echo "[7/7] 启动 Flask API..." +cd "$AGV_APP_DIR" +nohup python3 app.py > /tmp/agv_flask.log 2>&1 & +FLASK_PID=$! +echo " Flask PID: $FLASK_PID" +sleep 4 + +# ---------- 完成 ---------- +echo "" +echo "==========================================" +echo " ✅ 启动完成" +echo "==========================================" +echo "" +echo " 进程状态:" +for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do + NAME="${PROC%%:*}" + PID="${PROC##*:}" + echo " $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')" +done +echo "" +echo " 日志文件:" +echo " bringup : /tmp/ros2_bringup.log" +echo " Nav2 : /tmp/ros2_nav2.log" +echo " fixer : /tmp/scan_fixer.log" +echo " Flask : /tmp/agv_flask.log" +echo "" +echo " 关键验证命令:" +echo " curl http://localhost:5000/api/navigate/status" +echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once" +echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once" diff --git a/agv_app/start_all.sh.bak.234249 b/agv_app/start_all.sh.bak.234249 new file mode 100755 index 0000000..33d2c0b --- /dev/null +++ b/agv_app/start_all.sh.bak.234249 @@ -0,0 +1,165 @@ +#!/bin/bash +# ============================================================ +# Robot AGV 全量启动脚本 v2.2 +# 完整流程： +# 清理旧进程（不杀 daemon） -> 启动 bringup -> +# 启动激光时间戳修正节点 -> 启动 Nav2 -> +# 设置导航精度参数 -> 启动 Flask +# ============================================================ +set -e + +AGV_APP_DIR="/home/elephant/work/agv_app" +AGV_ROS2_DIR="/home/elephant/agv_pro_ros2" +ROS_DOMAIN_ID_VAL=1 + +echo "==========================================" +echo " Robot AGV 全量启动 v2.2" +echo "==========================================" +echo "" + +# ---------- 1. 清理旧进程（不杀 ros2-daemon） ---------- +echo "[1/7] 清理旧进程..." +pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true +pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true +pkill -f "agv_pro_node" 2>/dev/null || true +pkill -f "lslidar_driver_node" 2>/dev/null || true +pkill -f "scan_timestamp_fixer" 2>/dev/null || true +pkill -f "python.*app.py" 2>/dev/null || true +sleep 4 +echo " 清理完成" + +# ---------- 2. 重启 ros2 daemon（仅杀 daemon进程本身，不杀整个环境） ---------- +echo "[2/7] 重启 ros2 daemon..." +pkill -f "ros2-daemon" 2>/dev/null || true +sleep 2 +nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 & +sleep 5 +echo " ros2 daemon 已就绪" + +# ---------- 3. 启动 bringup (含激光雷达) ---------- +echo "[3/7] 启动 AGV Bringup..." +source /opt/ros/humble/setup.bash +cd "$AGV_ROS2_DIR" +source install/setup.bash +nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \ + port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 & +BRINGUP_PID=$! +echo " bringup PID: $BRINGUP_PID" + +echo " 等待 bringup 就绪..." +for i in $(seq 1 20); do + if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then + echo " ✅ bringup 已就绪" + break + fi + sleep 2 +done + +# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ---------- +echo "[4/7] 启动激光时间戳修正节点..." +# 确保只有1个 fixer 进程在运行 +pkill -f "scan_timestamp_fixer" 2>/dev/null || true +sleep 2 + +for i in $(seq 1 10); do + if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then + echo " /scan 话题已上线" + break + fi + sleep 2 +done + +nohup bash -c "source /opt/ros/humble/setup.bash && \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \ + > /tmp/scan_fixer.log 2>&1 & +FIXER_PID=$! +echo " scan_timestamp_fixer PID: $FIXER_PID" +sleep 5 + +# 验证只有1个 fixer 进程 +FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0) +if [ "$FIXER_COUNT" -gt 1 ]; then + echo " ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..." + pkill -f "scan_timestamp_fixer" 2>/dev/null || true + sleep 2 + nohup bash -c "source /opt/ros/humble/setup.bash && \ + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \ + > /tmp/scan_fixer.log 2>&1 & + sleep 3 +fi + +if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then + echo " ✅ /scan_corrected 已上线" +else + echo " ⚠️ /scan_corrected 未上线，检查日志：" + cat /tmp/scan_fixer.log +fi + +# ---------- 5. 启动 Nav2 ---------- +echo "[5/7] 启动 Nav2 导航..." +source /opt/ros/humble/setup.bash +cd "$AGV_ROS2_DIR" +source install/setup.bash +nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py \ + autostart:=True > /tmp/ros2_nav2.log 2>&1 & +NAV2_PID=$! +echo " Nav2 PID: $NAV2_PID" +sleep 12 + +echo " 等待 Nav2 节点就绪..." +for i in $(seq 1 15); do + NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \ + grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" 2>/dev/null || echo 0) + if [ "$NODES" -ge 3 ]; then + echo " ✅ Nav2 节点已就绪 ($NODES 个)" + break + fi + sleep 3 +done + +# ---------- 6. 设置精度参数 ---------- +echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..." +source /opt/ros/humble/setup.bash +cd "$AGV_ROS2_DIR" +source install/setup.bash + +for NODE in /controller_server /bt_navigator /planner_server; do + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true + ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true +done +ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true +ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true +ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true +echo " 精度参数已设置" + +# ---------- 7. 启动 Flask ---------- +echo "[7/7] 启动 Flask API..." +cd "$AGV_APP_DIR" +nohup python3 app.py > /tmp/agv_flask.log 2>&1 & +FLASK_PID=$! +echo " Flask PID: $FLASK_PID" +sleep 4 + +# ---------- 完成 ---------- +echo "" +echo "==========================================" +echo " ✅ 启动完成" +echo "==========================================" +echo "" +echo " 进程状态:" +for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do + NAME="${PROC%%:*}" + PID="${PROC##*:}" + echo " $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')" +done +echo "" +echo " 日志文件:" +echo " bringup : /tmp/ros2_bringup.log" +echo " Nav2 : /tmp/ros2_nav2.log" +echo " fixer : /tmp/scan_fixer.log" +echo " Flask : /tmp/agv_flask.log" +echo "" +echo " 关键验证命令:" +echo " curl http://localhost:5000/api/navigate/status" +echo " ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once" +echo " ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once" \ No newline at end of file diff --git a/agv_pro_ros2/nav2_params.yaml b/agv_pro_ros2/nav2_params.yaml new file mode 100644 index 0000000..3e1933a --- /dev/null +++ b/agv_pro_ros2/nav2_params.yaml @@ -0,0 +1,350 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 5.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan_corrected + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + 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_assisted_teleop_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_goal_updated_controller_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_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.05 + # yaw_goal_tolerance: 0.05 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.05 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 5.0 + xy_goal_tolerance: 0.05 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan_corrected + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan_corrected + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + 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", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 5.0 + use_sim_time: true + 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: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + 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: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/agv_pro_ros2/param/agvpro.yaml b/agv_pro_ros2/param/agvpro.yaml new file mode 100644 index 0000000..3a3b844 --- /dev/null +++ b/agv_pro_ros2/param/agvpro.yaml @@ -0,0 +1,362 @@ +amcl: + ros__parameters: + use_sim_time: False + alpha1: 0.4 + alpha2: 0.3 + alpha3: 0.1 + alpha4: 0.1 + alpha5: 0.04 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 2 + robot_model_type: "nav2_amcl::OmniMotionModel" + scan_topic: scan_corrected + save_pose_rate: 0.5 + sigma_hit: 0.02 + tf_broadcast: true + transform_tolerance: 5.0 + update_min_a: 0.06 + update_min_d: 0.025 + z_hit: 0.7 + z_max: 0.001 + z_rand: 0.059 + z_short: 0.24 + + # Initial Pose + set_initial_pose: True + initial_pose.x: 0.0 + initial_pose.y: 0.0 + initial_pose.z: 0.0 + initial_pose.yaw: 0.0 + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_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_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_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_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_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 + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 10.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 3.0 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.15 + movement_time_allowance: 8.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.05 + # yaw_goal_tolerance: 0.05 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.05 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 0.5 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 0.5 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -0.5 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 40 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 5.0 + xy_goal_tolerance: 0.05 + trans_stopped_velocity: 0.1 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 23.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 18.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 4.0 + inflation_radius: 0.30 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan_corrected_corrected + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + 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 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: False + footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]" + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan_corrected_corrected + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 4.0 + inflation_radius: 0.30 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +map_server: + ros__parameters: + use_sim_time: False + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the navigation2_active.launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: False + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 1.0 + use_sim_time: False + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 2.0 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + recovery_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_recoveries/Spin" + backup: + plugin: "nav2_recoveries/BackUp" + wait: + plugin: "nav2_recoveries/Wait" + global_frame: odom + robot_base_frame: base_footprint + transform_timeout: 0.1 + 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 + 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: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 0.5] + min_velocity: [-0.26, 0.0, -0.5] + max_accel: [2.5, 0.0, 0.5] + max_decel: [-2.5, 0.0, -0.5] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/agv_pro_ros2/src/agv_pro_base/src/agv_pro_ros.cpp b/agv_pro_ros2/src/agv_pro_base/src/agv_pro_ros.cpp new file mode 100644 index 0000000..21c887a --- /dev/null +++ b/agv_pro_ros2/src/agv_pro_base/src/agv_pro_ros.cpp @@ -0,0 +1,624 @@ +#include "agv_pro_base/agv_pro_driver.h" + +uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) { + uint16_t crc = 0xFFFF; + for (size_t i = 0; i < length; ++i) { + crc ^= static_cast(data[i]); + for (int j = 0; j < 8; ++j) { + if (crc & 0x0001) + crc = (crc >> 1) ^ 0xA001; + else + crc = crc >> 1; + } + } + return crc; +} + +std::vector AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector& payload) +{ + std::vector frame(SEND_DATA_SIZE, 0x00); + frame[0] = 0xFE; + frame[1] = 0xFE; + frame[2] = 0x0B; + frame[3] = cmd_id; + + for (size_t i = 0; i < payload.size() && i < 8; ++i) { + frame[4 + i] = payload[i]; + } + + uint16_t crc = crc16_ibm(frame.data(), 12); + frame[12] = (crc >> 8) & 0xff; + frame[13] = crc & 0xff; + + return frame; +} + +void AGV_PRO::print_hex(const std::string& label, const std::vector& data, std::optional override_size) { + std::stringstream ss; + for (auto b : data) { + ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2) + << static_cast(b) << " "; + } + size_t len = override_size.value_or(data.size()); + RCLCPP_INFO(this->get_logger(), "%s (%zu bytes): [%s]", label.c_str(), len, ss.str().c_str()); +} + +void AGV_PRO::send_serial_frame(const std::vector& frame, bool debug) +{ + try { + size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame)); + if (debug) { + print_hex("Sent", frame, bytes_transmit_size); + } + } catch (const std::exception &ex) { + RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port: %s", ex.what()); + } +} + +std::vector AGV_PRO::read_serial_response( + const std::vector& expected_header, + size_t payload_size, + double timeout_sec) +{ + std::vector sliding_buf; + uint8_t byte = 0; + + rclcpp::Time start_time = this->now(); + rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec); + + while ((this->now() - start_time) < timeout) { + boost::asio::mutable_buffers_1 buf(&byte, 1); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + if (n == 1) { + sliding_buf.push_back(byte); + if (sliding_buf.size() > expected_header.size()) { + sliding_buf.erase(sliding_buf.begin()); + } + if (sliding_buf == expected_header) { + break; + } + } + } + + if (sliding_buf != expected_header) { + RCLCPP_WARN(this->get_logger(), "Timeout waiting for header"); + return {}; + } + + size_t remain_len = payload_size + 2; + std::vector remain_buf(remain_len); + size_t total_read = 0; + + while (total_read < remain_len && (this->now() - start_time) < timeout) { + boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read); + boost::system::error_code ec; + size_t n = serial_port_->read_some(buf, ec); + if (ec) { + RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return {}; + } + total_read += n; + } + + if (total_read != remain_len) { + RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload"); + return {}; + } + + std::vector full_buf = expected_header; + full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end()); + + return full_buf; +} + +bool AGV_PRO::is_power_on(){ + auto power_query_frame = build_serial_frame(GET_POWER_STATE, {}); + send_serial_frame(power_query_frame,true); + + const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x12}; + auto power_query_response = read_serial_response(expected_header, 8, 12.0); + + print_hex("recv_buf", power_query_response); + + if (power_query_response.size() != 14) return false; + + uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13]; + uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12); + if (received_crc != computed_crc) { + RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc); + return false; + } + + int is_poweron_status = static_cast(power_query_response[4]); + RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status); + + if (is_poweron_status == 0){ + auto status_query_frame = build_serial_frame(POWER_ON, {}); + send_serial_frame(status_query_frame,true); + + rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command + + const std::vector expected_header = {0xFE, 0xFE, 0x0B, 0x10}; + auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds + print_hex("recv_buf", status_query_response); + + if (status_query_response.size() != 14) return false; + + uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13]; + uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12); + if (received_crc != computed_crc) { + RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc); + return false; + } + + int poweron_status = static_cast(status_query_response[4]); + std::string status_msg; + + switch (poweron_status) { + case 1: + status_msg = "Motor is operating normally."; + RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); + return true; + case 2: + status_msg = "Emergency stop button is not released."; + RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); + return false; + case 3: + status_msg = "Battery voltage is below 19.5V."; + RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); + return false; + case 4: + status_msg = "CAN initialization error."; + RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); + return false; + case 5: + status_msg = "Motor initialization error."; + RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str()); + return false; + default: + RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status); + return false; + } + } + else{ + RCLCPP_INFO(this->get_logger(), "Motor is operating normally."); + return true; + } +} + +void AGV_PRO::set_auto_report(bool enable){ + auto frame = build_serial_frame(0x23, {static_cast(enable)}); + send_serial_frame(frame,true); +} + +void AGV_PRO::clearSerialBuffer(int fd) { + if (tcflush(fd, TCIOFLUSH) < 0) { + RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno)); + } else { + RCLCPP_INFO(this->get_logger(), "Serial buffer flushed."); + } +} + +void AGV_PRO::disableDTR_RTS(int fd) { + int status; + if (::ioctl(fd, TIOCMGET, &status) == 0) { + status &= ~(TIOCM_DTR | TIOCM_RTS); + if (::ioctl(fd, TIOCMSET, &status) != 0) { + RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno)); + } else { + RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully."); + } + } else { + RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno)); + } +} + +void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + linearX = std::clamp(msg->linear.x, -1.5, 1.5); + linearY = std::clamp(msg->linear.y, -1.0, 1.0); + angularZ = std::clamp(msg->angular.z, -1.0, 1.0); + + int16_t x_send = static_cast(linearX * 100); + int16_t y_send = static_cast(linearY * 100); + int16_t rot_send = static_cast(angularZ * 100); + + uint8_t buf[14] = { 0xfe,0xfe,0x0b,0x21 }; + + buf[4] = (x_send >> 8) & 0xff; + buf[5] = x_send & 0xff; + buf[6] = (y_send >> 8) & 0xff; + buf[7] = y_send & 0xff; + buf[8] = (rot_send >> 8) & 0xff; + buf[9] = rot_send & 0xff; + buf[10] = 0x00; + buf[11] = 0x00; + + uint16_t crc = crc16_ibm(buf, 12); + buf[12] = (crc >> 8) & 0xff; + buf[13] = crc & 0xff; + + std::vector data_vec(buf, buf + sizeof(buf)); + + try + { + boost::asio::write(*serial_port_,boost::asio::buffer(data_vec)); + // print_hex("Sent", data_vec);//debug + } + catch(const std::exception &ex) + { + RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what()); + } +} + +void AGV_PRO::handleSetDigitalOutput( + const std::shared_ptr request, + std::shared_ptr response) +{ + uint8_t output_number = request->pin; + uint8_t output_state = request->state; + + if (output_number < 1 || output_number > 6){ + RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number); + response->success = false; + response->message = "Invalid output pin number"; + return; + } + + auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state}); + send_serial_frame(frame, true); + + const std::vector expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO}; + auto response_frame = read_serial_response(expected_header, 8, 5.0); + + // print_hex("recv_buf", response_frame); //debug + + uint8_t status = response_frame[4]; + if (status == 0x01) { + RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded"); + response->success = true; + response->message = "Success"; + } else { + RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status); + response->success = false; + response->message = "Failed with status code"; + } +} + +void AGV_PRO::handleGetDigitalInput( + const std::shared_ptr request, + std::shared_ptr response) +{ + uint8_t input_number = request->pin; + + if (input_number < 1 || input_number > 6){ + RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number); + response->success = false; + response->message = "Invalid input pin number"; + return; + } + + auto frame = build_serial_frame(GET_INPUT_IO, {input_number}); + send_serial_frame(frame, true); + + const std::vector expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO}; + auto response_frame = read_serial_response(expected_header, 8, 5.0); + // print_hex("recv_buf", response_frame); //debug + + uint8_t status = response_frame[5]; + if (status == 0xff) { + RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status); + response->success = false; + } else { + RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status); + response->state = static_cast(status); + response->success = true; + response->message = "Success"; + } +} + +bool AGV_PRO::readData() +{ + std::vector buf_length(1); + std::vector data_buf(RECEIVE_PAYLOAD_SIZE); + + uint8_t byte = 0; + boost::system::error_code ec; + + while (true) + { + size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + if (ret != 1 || byte != 0xfe) { + continue; + } + + ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + if (ret == 1 && byte == 0xfe) { + break; + } + } + + size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec); + if (ec) { + RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str()); + return false; + } + + if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) { + //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]); + return false; + } + + ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec); + if (ec || ret != data_buf.size()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload"); + return false; + } + + std::vector recv_buf; + recv_buf.push_back(0xFE); + recv_buf.push_back(0xFE); + recv_buf.push_back(0x1C); + recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end()); + + // print_hex("recv_buf", recv_buf); //debug + + if (recv_buf[3] != 0x25) { + //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug + return false; + } + + uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8); + uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2); + + if (received_crc != computed_crc) { + RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc); + return false; + } + + vx = static_cast(static_cast(recv_buf[4])) * 0.01; + vy = static_cast(static_cast(recv_buf[5])) * 0.01; + vtheta = static_cast(static_cast(recv_buf[6])) * 0.01; + + motor_status = recv_buf[7]; + motor_error = recv_buf[8]; + battery_voltage = static_cast(recv_buf[9]) / 10.0f; + enable_status = recv_buf[10]; + + imu_data.linear_acceleration.x = static_cast(static_cast((recv_buf[11] << 8) | recv_buf[12])) * 0.01; + imu_data.linear_acceleration.y = static_cast(static_cast((recv_buf[13] << 8) | recv_buf[14])) * 0.01; + imu_data.linear_acceleration.z = static_cast(static_cast((recv_buf[15] << 8) | recv_buf[16])) * 0.01; + + imu_data.angular_velocity.x = static_cast(static_cast((recv_buf[17] << 8) | recv_buf[18])) * 0.01; + imu_data.angular_velocity.y = static_cast(static_cast((recv_buf[19] << 8) | recv_buf[20])) * 0.01; + imu_data.angular_velocity.z = static_cast(static_cast((recv_buf[21] << 8) | recv_buf[22])) * 0.01; + + roll = static_cast(static_cast((recv_buf[23] << 8) | recv_buf[24])) * 0.01; + pitch = static_cast(static_cast((recv_buf[25] << 8) | recv_buf[26])) * 0.01; + yaw = static_cast(static_cast((recv_buf[27] << 8) | recv_buf[28])) * 0.01; + + // RCLCPP_INFO(this->get_logger(), + // "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], " + // "Gyro[x: %.2f, y: %.2f, z: %.2f], " + // "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]", + // imu_data.linear_acceleration.x, + // imu_data.linear_acceleration.y, + // imu_data.linear_acceleration.z, + // imu_data.angular_velocity.x, + // imu_data.angular_velocity.y, + // imu_data.angular_velocity.z, + // roll, pitch, yaw); + + return true; +} + +void AGV_PRO::publisherVoltage() +{ + std_msgs::msg::Float32 voltage_msg,voltage_backup_msg; + voltage_msg.data = battery_voltage; + pub_voltage->publish(voltage_msg); +} + +void AGV_PRO::publisherImuSensor() +{ + sensor_msgs::msg::Imu ImuSensor; + + ImuSensor.header.stamp = this->get_clock()->now(); + ImuSensor.header.frame_id = "imu_link"; + + tf2::Quaternion qua; + qua.setRPY(0, 0, yaw * M_PI / 180.0); + + ImuSensor.orientation.x = qua[0]; + ImuSensor.orientation.y = qua[1]; + ImuSensor.orientation.z = qua[2]; + ImuSensor.orientation.w = qua[3]; + + ImuSensor.angular_velocity.x = imu_data.angular_velocity.x; + ImuSensor.angular_velocity.y = imu_data.angular_velocity.y; + ImuSensor.angular_velocity.z = imu_data.angular_velocity.z; + + ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x; + ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y; + ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z; + + ImuSensor.orientation_covariance[0] = 1e6; + ImuSensor.orientation_covariance[4] = 1e6; + ImuSensor.orientation_covariance[8] = 1e-6; + + ImuSensor.angular_velocity_covariance[0] = 1e6; + ImuSensor.angular_velocity_covariance[4] = 1e6; + ImuSensor.angular_velocity_covariance[8] = 1e-6; + + pub_imu->publish(ImuSensor); +} + +void AGV_PRO::publisherOdom(double dt) +{ + currentTime = this->get_clock()->now(); + + double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt; + double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt; + double delta_th = vtheta * dt; + + x += delta_x; + y += delta_y; + theta += delta_th; + + geometry_msgs::msg::TransformStamped odom_trans; + odom_trans.header.stamp = currentTime; + odom_trans.header.frame_id = frame_id_of_odometry_; + odom_trans.child_frame_id = child_frame_id_of_odometry_; + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat); + + odom_trans.transform.translation.x = x; + odom_trans.transform.translation.y = y; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation = odom_quat; + + odomBroadcaster->sendTransform(odom_trans); + + nav_msgs::msg::Odometry odom; + odom.header.stamp = currentTime; + odom.header.frame_id = frame_id_of_odometry_; + odom.child_frame_id = child_frame_id_of_odometry_; + + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; + odom.pose.pose.orientation = odom_quat; + odom.pose.covariance = this->odom_pose_covariance; + + odom.twist.twist.linear.x = vx; + odom.twist.twist.linear.y = vy; + odom.twist.twist.angular.z = vtheta; + odom.twist.covariance = this->odom_twist_covariance; + + pub_odom->publish(odom); +} + +void AGV_PRO::Control() +{ + if (true == readData()) + { + currentTime = this->get_clock()->now(); + double dt = 0.0; + if (lastTime.nanoseconds() != 0) { + dt = (currentTime - lastTime).seconds(); + } + + lastTime = currentTime; + publisherOdom(dt); + // RCLCPP_INFO(this->get_logger(), "dt:%f", dt); + publisherVoltage(); + publisherImuSensor(); + } +} + +AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name) +{ + this->declare_parameter("port_name","/dev/agvpro_controller"); + this->declare_parameter("odometry.frame_id", "odom"); + this->declare_parameter("odometry.child_frame_id", "base_footprint"); + this->declare_parameter("imu.frame_id", "imu_link"); + this->declare_parameter("namespace", ""); + + this->get_parameter_or("port_name",device_name_,std::string("/dev/agvpro_controller")); + this->get_parameter_or("odometry.frame_id",frame_id_of_odometry_,std::string("odom")); + this->get_parameter_or("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint")); + this->get_parameter_or("imu.frame_id",frame_id_of_imu_,std::string("imu_link")); + this->get_parameter_or("namespace",name_space_,std::string("")); + + if (name_space_ != "") { + frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_; + child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_; + frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_; + } + + odomBroadcaster = std::make_unique(this); + pub_imu = this->create_publisher("imu", 20); + pub_odom = this->create_publisher("odom", rclcpp::SensorDataQoS()); + pub_voltage = create_publisher("voltage", 10); + cmd_sub = this->create_subscription( + "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1)); + + set_output_service = this->create_service( + "set_digital_output", + std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2) + ); + + get_input_service = this->create_service( + "get_digital_input", + std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2) + ); + + lastTime = this->get_clock()->now(); + + try{ + serial_port_ = std::make_unique(io_); + + serial_port_->open(device_name_); + serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000)); + serial_port_->set_option(boost::asio::serial_port_base::character_size(8)); + serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); + serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); + serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); + + int fd = serial_port_->native_handle(); + this->clearSerialBuffer(fd); + this->disableDTR_RTS(fd); + + rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time + + RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully"); + RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str()); + + boost::asio::serial_port_base::baud_rate baud_option; + serial_port_->get_option(baud_option); + unsigned int current_baud = baud_option.value(); + RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud); + } + catch (const std::exception &ex){ + RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what()); + return; + } + + if (this->is_power_on()) { + this->set_auto_report(1); + + control_timer_ = this->create_wall_timer( + std::chrono::milliseconds(20), + std::bind(&AGV_PRO::Control, this) + ); + RCLCPP_INFO(this->get_logger(), "Control timer started"); + } + else { + RCLCPP_WARN(this->get_logger(), "Control timer not started."); + } +} + +AGV_PRO::~AGV_PRO() +{ + if (serial_port_ && serial_port_->is_open()) { + this->set_auto_report(0); + serial_port_->cancel(); + serial_port_->close(); + } +} \ No newline at end of file
+
+ + +
+ +
+ +
+
+
+ + [[ missionStateText ]] +
+
+ 进度 [[ Math.round(progress) ]]% +
+
+
+
+
+
+ + + + +
+
+ + +
+

📋 任务清单 ([[ tasks.length ]] 台机器)

+
+
+
[[ task.label ]]
+
+ ⏳ + 🔄 + ✅ + ❓ +
+
[[ task.step ]]
+
+
🏷 [[ task.qr_value.substring(0,8) ]]
+
+ 📷 [[ task.photos_front ]]正 [[ task.photos_back ]]背 +
+
+
+
+
+ + +
+

📜 实时日志

+
+
[[ log ]]
+
等待任务开始...
+
+
+ + +
+

📷 摄像头预览

+
+ +
+
+ + +
+

📊 任务报告

+
+
✅ 完成: [[ report.completed ]]
+
❌ 失败: [[ report.failed ]]
+
总计: [[ report.total_points ]]
+
+
+ + +
+
+

⌨️ 手动输入二维码

+

所有姿态均未识别到二维码，请手动输入：

+ +
+ + +
+
+
+ +
+