From 04cbb03e79431b92eaf63c5db9e9fb14bb9a574e Mon Sep 17 00:00:00 2001 From: lyq Date: Fri, 2 Jan 1970 12:46:42 +0800 Subject: [PATCH] 6003 --- config.json | 6 +- ...ons_2.2.1-1focal.20230606.034520_arm64.deb | Bin 0 -> 12444 bytes run.sh | 48 ++++--- .../src/rs_driver/cmake/rs_driverConfig.cmake | 4 +- src/fu/CMakeLists.txt | 2 +- src/fu/config/fu_params.yaml | 2 +- src/fu/src/fu_node.cpp | 7 +- src/sub/include/sub/sub_node.hpp | 39 +----- src/sub/src/sub_node.cpp | 119 ++++++++---------- src/sweeper_interfaces/CMakeLists.txt | 6 + start_all.sh | 10 +- 11 files changed, 109 insertions(+), 134 deletions(-) create mode 100644 ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb mode change 100644 => 100755 start_all.sh diff --git a/config.json b/config.json index b199dea..3a3d229 100644 --- a/config.json +++ b/config.json @@ -1,13 +1,13 @@ { "mqtt": { - "vid": "V060002", + "vid": "V060003", "inter_net_address": "tcp://192.168.4.196", "inter_net_port": 11883, "external_net_address": "tcp://36.153.162.171", "external_net_port": 19683, "mqtt_user": "zxwl", "mqtt_password": "zxwl1234@", - "pub_gps_topic": "/zxwl/vehicle/V060002/gps", - "remote_topic": "/zxwl/vehicle/V060002/ctrl" + "pub_gps_topic": "/zxwl/vehicle/V060003/gps", + "remote_topic": "/zxwl/vehicle/V060003/ctrl" } } \ No newline at end of file diff --git a/ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb b/ros-foxy-pcl-conversions_2.2.1-1focal.20230606.034520_arm64.deb new file mode 100644 index 0000000000000000000000000000000000000000..e395b206af580ddce3333749926da37c87310e36 GIT binary patch literal 12444 zcmbu_Ly#t1)Gg?=ZQH7}ZQHhOo0Yb0+xezhY1_7~{%-#rH#&N92fbp)ne8)t20Iop zuaUEfB|nU*nX#3TJ-xA&y^*sQ5fKp+J109MGYbNtVKt*C; z450r>X8j+KJ<4A&K|LT$*d9DT;M9J6X?`#C!6}MjeRM`5)}ssqe+bcwsSPm0E}c?g zW)CqFk!#I1F1I~U<_K_q-{gn`u5126;PQ5#mGXDxPih0})>d^Bn_KM_%PvOj6cqxL zQ~vI~`wbj>su1}+m1N+{uuM%cRw&)b%O|I6pn!_@^$ilY!`O{OzLW@$&sw=p)H&$S5O=B2f zc}Ay!ua0V?Y*M!K5=kf{YX>@Ll8HnhgRRH}|1@rP;|S#+XSFKw9w(doHT2LY)3xs| zKPT2;K!K+{i0kKH3R2L`W<`QiL5p-8zLlcY>Gq@7*XEm!KT0U$iP3adb>p4Ls~JGgUh@$IJlB*+dbayb-L`vpbq zexC!u7Y?vrRkEo^>N=7f(*5ZzKe*)_jd1nH@K)|Jo~|fn-CpG+q>)%EC?&PuOooK{ z9x|a}Mtu@lB;4pFE{oAk>Y@(-wET+*uG$^ke$Y)G4L&3gBb_h_@$CBzeiL4bO;}&v z{BqYM>F6KN%qWU5z8aL!(bN2izIhUbbno1P7O3@|Kxa1&-GKfeSBeRDGE){2@h6oqr8qv#n?+mDCr)d$FI>_$&DKx`244icinYaO5fpt#h-iMe{;B=)N-U zUk(z7Ym<#Gy(3@J9cgdBMpisqP(=}m>&l(D^iL~jF{-0)N}`lo{`@i32eX%ub(2Fv zsrE6*Q!SUy$uZ51>;ed8pqg{EK#txPnIJ7ynaIZ*R zYdPi03$xB6`STQ!F#KY$lh_KfBeY0VD&xW+@E?Aso|>yrfJWPR?=ySnd}%~s;z86L zfZvNu1^z-sdcgxF?gh2ZxE94>*n$O+nr)7ZQ$@fN#1T`rf%?g`rcX$i!rUy7-hZj* z_!Lvi!(2A~4V*C1qv7%jr*BA`nSLnR8j^KlbPS`W2wQr62{@2GJXF5j!=D$=~0IDXy|WMZ73*H9%FiXx?^0loJ&TWgC4XNgF}Tc_Z+*TT15thn5j;Z1AWkRP3RK6mj}d)cr#Ta&~|%cFGNf{Xd;5?S6#FT+K`{ zd-sBhJ#ox^x39i$jOGx{y97ZI}B5GGP9Q+ICt zon)2a9>{I7I&~>&UfGfN8abGE2GtR=FGC|_s(OSOhA-w2QAKYc-tIFyVGs8B{V>$a zhiMu`uM@o(+&zD&??_hf!L_Ud8(%bBhmcyNj)HEdV|-$F=AE!U(4xom$Y`>WFKFEL z%UX|Eu+rYl1EKkF?2R`qp!A>e1);}{AOWt{6 zsl7Y7e==$&VF|<>a7McPh+3-RS zZS(w&5|KsRpWc6$6Z=oTf2W1|lb~Bt9ODtF0ynP1uky5d?GF0@!y7_yU(GDF8()FO zB`f*tLnhwlD`v*vhv5%`?VA9EG&ULjqWdV@?ywl6y>p0>BS3->Kpi_NZmAq&>f=Ra z&hJkPCSXFp>d7&kb};{mu&z@zs6K2C4!3GxOTF~i4aZ9>gR%e@9VRdls-`-R&l(a- zR%QkU%n{To_sI4@iZ`Z2=k+nJ*YT=#cm&r`h9ya6gvdns3CKa57}L=+3ESH;Ir~4; zuAf@;v(MDr36Y%-YTAR}yQB3O}!lY1hR()jK@-?iXuZOoxoqlH)(8F26ZNXZ_B1aq?k(CN47$ zvID$CNCbAGE~rs}+#uXybRe%i_bG&UHW&C83CGZp&MSU;nkF0&>l$jg8OU8@pErMH zJd3lB2crgTA+SD#c>RoE7|1k^gRzHgjpJYU<0>QVM75;q$pmAG2xU#L)qp~-veJ7~ z-*Wzx>;0r`PI)ADjX()ZEGc3^(_yuMdIVPxypDbrXP|^y|D_(@M~V`&(tw$2VBuA& zQ!}+Ya2atpzyn=^=tnb*uYWJlhs+8&YdER$StK+}M&c504iD99-GJ06>S~r}7>dx* z9A+TFr<>w{{ebD&r;IHG8;;T9!TV>>7TfCZFXc_gfYTW@?>?5!QNSO(sblV)4shaD zNmn%XLfLe7rfcm`YaI#fU(FXxhsy7@>{560$+U-Yr0IoY8qedc`2^Zgc9j@UJ%H(2 zV;y2g{zA}JBnoGb9qF0z$9e`!Vc!G&!?LO3F#Tp{GBZtan3|%V#DvSA2Pl6~OcZtZK*A&~PnTrR49K#L^K z8q4zbm4K#oNpx=aa-T`>EPh;StnY0CcIC4HL1Eh%2Pa1e#%I_e6UD0i6%Y7{j$vWN z-&v{C_KGUXD6T=px{4dnw74UaqkO8IOj`b$oA|QwbM_`ZY2+I^)k~v(%YDV$pRbAZ z%qS_=+p`lM;gGu`sLZpBG)=_T!K8Bywd*!DAC@L&n25P)#A_04pIoy@IVT=$T+jhi zA9fMdT{p}u&4a1*E0_sU3*eyv^uw*UL+xoHDV*vw@O*H4r-fI2;mNQB&t{A}r=mj8 zQS99w4=9F0&c(qc1>r)O6T)EI85>}Z-K9!wF4)Vq7Dy6UVq{iK=(IV< z?ZMTqny1)5C`T8_8X?UW;_(#20`z<7YE;V63G5LXN*sjHh$Fu~@ zQK@8uqgH8OvwnkN&xQIP!?JFI7Q%GB!5p-H9+MJ8bEB9y3J<&t1C?r~5mA)}o_^i) z7lqJt-;Xgjv`YtbYQcmeKb~FL<02YAvx}}dNd6_bfm7h!P$4x&r3GLp`}pql@$YnO zafMMFzd@YUnYEGM3uJUt3G%hNnCAOPL#n+JF;+=n$QMG8vE+5B01NF+p8Jx6Pf>3V ztR-lx1>4dSV~V2O&9l`JQFrYR0QEF-YLZhy*I~$v*X`Y z9Qd_OgP9VD$-dxCR#WkLg80#p?`#)_pUIsEzT=P?))TXv6QvZ@oto|Kiqlq~Nei=F z6gSVi@U=0-V@fooWIC_4GoLip=r0$HH}AwFIv?L3zN`M(ynDT6iC;&@vI;PS6e`?% zH#;Z>S#=8ia(+fp>GQPGkF{hRI<uvlm$7849_NSWNnl2Fcv!Q__np%;D2BL0V-ML&(eU~wn4jThEbaaIDaFr zSB(1f4CJXXkLP*hxAk9eI7;^1$#3vU;JC&|&$K-cTUY@{nj$V$eCmxjPvx5|pjd8q z6{3PXN5Ase*QZ#incbpodPJBAGqBe^N|+YNWVl!L_e1nLNv9e4K!{w=HV2$T-~EwT z*t3wD*_YuD0f$dA<78lW>zU8tMryY1*|O_$u=Fx#l@BkYk6|OXa(z!Ho)f4sAdHCy zyF>% zr(_qME&cA82e8{Z<$+_1VU+}KZt9$FUocGAQgdeRF$n2TbjZ(1l9l7W_4=)b@%jQ# z6>BMOsL2)SJ^nY@_`+r$l_Gl%P1bLXj?=F8w^RON43!MX3|&O9&T3vdrq37AF=;M~ z7iRx}n+=d<89f=MO>$TU}5>Gxe;pe3a0dk!5#OIS&n!XIqknJun%W4b4m4*GxT6v8NDT`OJTT~354X}`<$6h^gPhr8iY7tSYSTNSDXzDG|vb3_(qT!uyc06! z9EyiRjKDLL2(xZ*+ZQW_N=V2C6X(t$C7vohfM2YHD?4KK#LuxS4_I#kkm0toQd>Gb zB#Z(XAocL1CPD`YE)kK{)N}7s9MOGZT47z2A6&)lDz>oaEx)=8_?Y9k>x*)}QLHWU z`zRyCw^HrLi5%q1U~7sOr?nDD|Fn-)S_T*=@I2rMyD7ZR==~9S-WDFot59p)ksB-Y zX~KM-A~|CUvKV9`UyyvwTRna*t8@RmmF1&j`Q9N}#9+~~@mVE*tCf}}Ts*G*)SES!Ef=Oged1z#j; zpD1h-zofpij*>|d0*%WbSx_i0c{ozb6q!esA7QBObIPEdoUiQBB*_ER!;93NkAfg3 z+;T|C`3fTZ5uoaNTlSmh^?Z3ZBuv`nIAdh+gzeuv(*JZ`dc3qCBTl*Wr{bwLMDS(u z!*0Oz_l=4&WXfbCXL(E7mVWzc-TbCUIsni1d4qF;E5k_8r5cWL(g-Qpy|*a!D~nI{ z^IE`_vUe&|5AD-Wxh%G}(eJT@woKGyNstzzT>b(^eZ;D|+nN94%JJAP7%REXLEQEo zs!uUYq)!;ZlTjMNh8~bj`KFH7OwX2>1#y#(zd8bsC4QNk0jrUM^oI;^-LXMf9KcQM zj}`bzWIHi|^%gD^NE)f^K40jfPUaddXCqiTjfmIsN>Vj9?UC+-D1Z`X`NUD4=IiVA zdIC_>(!kuO&}^C$MNl8$nW+f6s;F_j>qO@19(l`L&xcp-xUDmih?w2dD`Y;h9+mf zs=17w5%oGZo%U4ge<F6bd^$utP~tQ>d1~e z$7zlb?umJNbe?_37U{Di1QPX1RBh8Kb@rC$q`I3haT91`BSVwWrIpFTJvv>F;7L1} zu*VzRq~|q7h;x1mY_2msTyr}hxviM^Z!UF28z>&qK8w!P%h=pCHM-M>iC+Z4&}^86 zsp0;b5qvPfQpI?cbtN*9Nf|Y{3=<>l&h6~d_{DR`= zhM%UI`Hg;_&~!4m#L&E~;{H(~=kPOD!I!c)sRqWCK#rXBQQT1Z)_BHI+3F+kTvKNj z8c$9yl$jhM8Xwfc8Zu^-w9rTPP6p zt{LJcZSIJ|#2ew>UGz^%RyxlW!}~MEb->!_M?@#|PusVXnNo-S8jydWj4Z0`S3|rZ z+qs{1FiWY;L#l^lQ@i=SgA9zeO@T9wl{q5cXCoQ~8ciyB6Z*0rnQ0aPk}np}UNN46 zyBCvSs9!XY*F-sCi!%NSD~1FpKtHy1SS<2R4`%-L2Xx#ev#s!&j{(cKO}~W zkVfY0`c!CMx6~IluNCuLWoFO2$c?%J>J;Bx;S|b+k`ygQTGAM-{3Wa@F0B+ohy-)+ zD{U{>o#|6AEQ)5n1AorQ-d7%}*+n6Pr*5jjwJSgL zlLM?-h%E?MF?(iIgxv|=2Zj}~AuxQ^X2ZfWBs``z#WVk3ACfE=b#{_>K z+WvhDtL*RXY4!DJL>m7n@=r!t43T+o#c6wcm*uzoAphxL+J?)!>;&7U#a@SaG6jVvjmbSpep=u5NJ!$`e0W@;6q)6l zGDK)Oiu}D~(-V`Gl;K?~@f5xEQsHg*W6Qh;w`w|+sq#`Au0yB5R#WOpV@{9iYrE0f zKjF3Do${x2nlqET73k9GMPGDr;SXQN?$rV3K&A#A|AZ}W`CZbL>w$xroYor83 z4YhOtPiU7_NJBUug!c$3-r>++$%I|8XM7IIn;dDdb1jPM^`muXi$FbEJax}9ZZq#w ztfvEXB#o-{R`sY>gUv{7{cbY_Ph9BaVa=OIh>7N89STvQ$jZvsvflj4g>mhsv+da< z-zxf>vqXNy)Af>e80?cvj0+Y{HH~J)8Bu z3|?n7{ke5qltYGWb39-&aJ{g;)^btGk`G+w)WZ>$h zN5^qAfUi6}Z&9?A++JzuguMWHlcon3p+z%qTmFp$q}o<8f7uAHo21ooi7kl2%`enZ zBX6%oU4cGr-!&8hUJfx~W5^{U(TX-!oAbTjn}NCeiL)@`42CfGfH6>CRV*rBIO+E1 zShVj26TXwIqfAktAS+U>pcTs9X8=hE^o%kORPF#FE&D=ABCmvd;_9B{?@`4%;+D?M ziI$xt$_q=wqbRxWsEVW5;EF4yqa?y?Wi7s3?JzSW31kXI=v^y;05umJ;vk_37fY*# z1hp_^zm+xkof1&-KtfbM3-wEqY1E{Z%Cd6?7`4Y3ysfQsZH6h&b`vv}z~l6Z@vCJG z>txo0U_!IF?>GL9U23p7yP$;sKiT9o@hJ?mpQ*db&Lg4E`#LRDI?;d1Lo^kw+rt0y zmo@c;!C67hJ3ALx%gXZAo1;d6BeY-mQXf#W?{Kf|?aSu9igbOXast+nsW?s`E5RsU z?&H_DXe{CH=NocvTS>r4*TMr9w7SzBKVEolvzjDISx+#SSXHE<0miyoB?gXFNRLpj zV-TcCu`*ZS^=a_1k{4<{`p?H{dmy^%HV5Rg@e+jxV2c zOFAuY{O3fjOqS`2#BHp5!(!DMo!&Z0s+5j>f6#!`qx`pPpw6xqacE_U(Np?V{pAIv z%&dBAatY34+3Y~07Tog*mvv`kkk8ymIqMvL zi+n=u#eAp5e1MkK)Pf-`b0qyco-UR?t2HiUmsp{#5@C9{3%e3h zZ2GlO*hoA2DZ{tJ(czo)N1s8+@J&B9B=^>P4qlS5f+|n9r8{_7pB1qj`4vN_l&dZD zwuND_480Qg&gKh8`G)#O5Quf`JvCA@Fqn;KfivxLRdxlU_o5Cfp?H&N z+YQIIFp{-1&)>X&3$vCgcn%Qccl-=HU4MHno}%ksUG&*5vS|B)9w41YS^k>sl=)qa zVEbgRvjsN}MHC$3bO0BP#ENysph&2j3>EXq=$m>B9<>{>D0YSbm$Y< zj-5)8Q`bPI`w#U~$;kdBUQy5Z5ar%chemCw>_uAKBP{*fOdBH$ivsd+V$HmVU&GWoZ2k=m4jO z);ykH#hcF(<}HCOx6c>rD(|5$H@G>4*8lMXP?^x|;=bG5cmnyzfCxMv3g{b4r-TS6 zUY0|ADR!}ni9-zRlLmEnm`eQU3=d)>s6A27VZ+(*O2W`TIvE~pW|L9^QhsA-gl2w! z9j7VKZ?_7jXz@yc{^~dz1sX3a*vJ~RlVz(Xw?yPW?IFS&(ftB3a)FFSd}3VkDgOBb z=I4ql{yaGRCs0KlU*aGy{L&)R%~jZ7H37@REckQwxdv&+WW})S2KE75}i3Oab3~ZlN+kmg@^i>`0=n!{Icl8DV?Kz;tW;mUyL zQJ%hQeo@a2){_-vYw#umu5_8#cq~0q$^`o7qWZ?Q{8PkCtwo4%0Z)o5n^TxiBjqQM zT$jbh1^NXvWv6y6DNQ>y$3o%By%brRT_ps>0JT}FI-wcKkx^V)+5Ez=?c87KEe};p zTb0sOj{4{+*RIfVmDwjPmzMe-+G}^E=(v$}b zg`$)wxDU)a**cAR2a{!Ms5$69mONwGr~E+|eX{f#8gFUdE10neH)ZpqGXA2$@WpEJ z=__Lr5oB1ga{4tIPsb6J>9C4I-aw^Vx(dDIBh#eKWA$Ee*A(4-SJ=+tG=YTd)P*6& zplYob0W&%Wq)#E4WSC#J}3x)1;S!W@W=3;MT9@` z7M7|IVZ)<-z0f=^nBpKA*Lj zC~XKKA#_!9mjrx&81Bf9NyHfF#_KbAB_>7I=sNg@&E!F~|M0}3O#M#O^z#Y36Vrhx zse~qo_$~2PH^jZsW{!Zg*Lb{SrsNzCP?x0VIyMUs&dNT~pXMt7)VlFLVXLwH-2kS{>Qiv$hkr=l{SIz@gXgA+BdF0_s`$HGwCqOL2p>;xg$n-pjAq}U1pU`1!Z zNGG2mR*6(&Mp3ER*htfIxVE;Qplh|FsBhYHo`;$MCo`j+%O*36pn1Q6e{};v6Ihv$}`CZ29lY|@f?Zn0)r$ydF_}Fa`2j} zz@^{meH=>vG{EyV0*KuJkaZs5%%Tl%|^O~Wu z%E;kF^PZ(%!u*hBoi*~bpHb-Re#oOZI%cf_#lGX>R!k4dcUFml0(vW|1oQ873^D@D zaF^JB@&AHN++}n>46bge8|pGA_DuX7V9H*9ua{Q)U~=9sZ9t!P!!^F4(dG(}-KocE zKWjYbnlR{Bl1wqZMG1r>t6kUK?-j&`)tvV&8g*K76ZDJw(UQmH2;>UOG z#;OcZxEKScV1>{W2j5<;-_QrJ4heQxlmfi#HFBH=k~)dA!tmP_&IvqWObxUnR|v(K z0+?n+c&1w;nY;I13(ZEP2=e_9vz|~p@Zm}?UpSyj4v*6ojh)@phM(+OWc#(Xf^&R` z{rbw^G=^|oEQv>(vuF5CsnDAQ=PnRF+g!RbeyERYA;MVJa7nxp>7^+{kvd(Pp3`sw zmE5(XQsSW&jU+u;qfwlgC=v;u8B}k(Y!onS=f)vx_6(e-(b`%|-($#QB zl?&0g|8jVYygZM@@?p!64WVTL4~`-Xu@mCdxXo z3w)u)3;0d<_6RykN9un_ zrL6$ndS(K4HORND0m_5pwf##^o8OqKa$KpZ@!HEPFDT`XfMg7e*0@J zF5tb^AVg7e9oE>-wnBG`hH35{1UPELv+ftT;$8Ma*_dsuql*`oZWo7{K>o?AaVD)A z?^%^Rk4#v&4AQ>xF)|XLouPj$VZGO)K?)O4j+?IL7eN+CGi6F6h;#T6SU?1GUform zV8IMt827ZaUU(t>_3Ak;g>40>A<)R+&QzT@4$~0O8g^-NW!d1@c=N7jk6WDW*oafy zhup=lXyCnQ<-9iHsN{T3MVl<8*21~dHD9+1f_HWy5~q3)3Prs)9<0h;I`uK2==G#! z;1r`qiaRk6Wp&CQ{ItmRm2WF{2XNYxzMs&weKbu8opGUOIu$`JzF>E{Nu2QZ#_1shPks)?&G|Pg#N0qhY zL^JT$?fT*}L&Bg=L>S+9mt zxa|?QmOLJ}y1`_!fziwefY;%IKE{4L4VmX}nAF0m`R00r$0D|-2yW3c8JEmQ@y_l3 zY5fK5orw~DzGX9PE${XZL@|^LQ?$m+q`it5*aBR}&$?r@3l^8|r5hk8CyF=?ghG`_ znJ@Y5h30zvsQOHX1*u(05o0S6>y7)O)yst<=8dH*#3MP(i(t3Q2U3;c1Y4!Di}fD| zCs7?mJG=(f%{$6tT(M~HYZ(S@b^(*K^*Qh3u>n> zk&DZ2!>#ZJAdU-0Z;FDY6-jEztRKoC(#3qe8sQ1YD}e!L>yCY#S|H-z0C#+VKL!_> z@wnC_W0T#edIZTwmEf2Vq;c1$>D;a`R7(j$4b9@v{C0h1cF&+nbwepfsa9Usf48v; z#!qqDHWJMvlhLeOwHu=R~U z%>i@siY(F->uny`41|gG+KUcg60`Jy1S?j5Ra!quEVG(?t+y4}TZn3r6m3_yjV2M& zGX*8HbiFBgcKIB{h}R>g)fYY+JZuhqL4d1R1e9UCICx1Vb8pt#*|UI5cr_MJ)R|~+ zUZlGyTa%^yIE853(FC=XWCPAD%!5HMdd(vgSbgLtL82+ojTa0ZerLQjh4_U3aFrTg zXk2KZPB1GJ*9#A%B#9^e?osRJS+;d7>vK5%2yFI}ALX6XVJm#+%v)<7fm=we8)wC< z_>&E@9DD${GUWRlbE}!R=+-dbBW~rtdbVyvpuHpK?L0axz-0m~^P2CnahEhghZ5C* zq0zP7Zz@4A7o#ud0!@S$;G)L0ov+&+RMi?4{ppEuv92BL=YlvzZZfh7|C~0kN;rJl zMh;|${r)Zrv2Z*7?b|V^sgkyE@jO~>lie3T)eW%VLb$Nz+92~{l*@YkUFmMHhz|v( zPgwc~1Pp?|bUrwkg8-mi|GAxs%OAl)Hn>TzoRhu^0vDy-jzYa@-ce3DFO6T9xicu4 z!*j(E*6ZkttXfRl!*zFNQc6|Stq^-A%?#ss5coNS3F0hX)6g?F>dj(rt={3>mHs|# zD-|Wdp_J>fr--~T+HJqs{dq}NuHTaY;gxp=xj*fG2_J&zB(0mR-)G0aWwEiW+sx2s zu?qW&FfO&rr-1se8nM${R=&@Gn)+4ZvBaTyaX@uZw!xH)38BUETxQhse8bJqbs&w@ zMx7;C++vvi-vRG8pc+$$e#p=byD=}xwU1f-o{Ww4z4cHzT!^|ZD$VyCzS5I1j)g6u z%^x2ay>12|I$JujvjN(55qm%D9+2?`5TJmrjKtd_ene=tEHvj8TaWC2Z@HLAloHXO zTcWb`BMogi^O>x@;GjS?Pd=Ef{--GCOSdTZFp&tQS#Yuj1bq%z9;m@~ocz!C!!#G_1=LEo-4BY`DP< zQj`aPH|qNf-Wu3!%gP?kuq;aHB8rD$7571y4mHAhc8VhEdmiWFWqDZg_}B3Pw;Sls zq`&MU1&?tDA?Y{Py4B+dI6}BBb!Ke*w`fHN>&JpRyX%EtoGDGwC^U}u`^Wp4k4F4# zg^4|`!5QSttYlbTXIQN1+&U^z0c`Ab^T;ClokaIhLAM9}(aP^^d6!Q;jX@vZgaGKv z+dJP{HaqvXFZs=9(|1_5Z6c3slrkIrsnW G 100 ? 1 : 0; - msg.ehb_brake_pressure = brake_pressure; + if (car_ctrl_mes.gear == 0) // N档 + msg.gear = 0; + else if (car_ctrl_mes.gear == 1) // D档 + msg.gear = 2; + else if (car_ctrl_mes.gear == 2) // R档 + msg.gear = 1; - // eps部分 - msg.angle = car_ctrl_mes.steering; + msg.rpm = car_ctrl_mes.throttle / 65535 * 1000; + + // 转向 + // 0~32200 -> -40~0 33200~65535 -> 0~40 + int steering = 0; + if (car_ctrl_mes.steering < 32200) + { + const int originalWidth1 = 32200; + const int targetWidth1 = 40; + + double unitLength1 = targetWidth1 / originalWidth1; + + steering = -40 + static_cast(car_ctrl_mes.steering * unitLength1); + } + else if (car_ctrl_mes.steering > 33200) + { + const int originalWidth2 = 32355; // 注意这里是从 33200 到 65535，共计 32355 个数 + const int targetWidth2 = 40; + + double unitLength2 = targetWidth2 / originalWidth2; + + steering = static_cast((car_ctrl_mes.steering - 32200) * unitLength2); + } + else + { + steering = 0; // 朝向前方 + } + msg.angle = steering; msg.angle_speed = 120; } else @@ -370,53 +377,25 @@ private: msg.gear = 0; msg.rpm = 0; - // ehb部分 - msg.ehb_anable = 1; - msg.ehb_brake_pressure = 8; - // eps部分 msg.angle = 0; msg.angle_speed = 120; } - // vcu部分 - // 非0即true 0:关, 1:开 - msg.edge_brush_lift = (car_ctrl_mes.sweepCtrl != 0); // 和sweepCtrl<一键清扫>协同作业 - msg.sweep_ctrl = (car_ctrl_mes.sweepCtrl != 0); - msg.spray = (car_ctrl_mes.waterPump != 0); - msg.mud_flap = (car_ctrl_mes.flap != 0); - msg.dust_shake = (car_ctrl_mes.dustShaking != 0); - - msg.left_light = (car_ctrl_mes.turnLight == 1); // 1:左转时左灯亮 - msg.right_light = (car_ctrl_mes.turnLight == 2); // 2:右转时右灯亮 - msg.brake_light = msg.ehb_anable; // 0:关, 1:开 - msg.headlight = car_ctrl_mes.headLight; // 0:关, 1:开 - - // 倒灰 - // 0:全部收回, 1:斗升, 2:倒斗 - msg.dump = car_ctrl_mes.dump; + msg.sweep = car_ctrl_mes.sweepCtrl; pub_mc->publish(msg); - RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" - << "\n brake: " << static_cast(msg.brake) // uint8 - << "\n gear: " << static_cast(msg.gear) // uint8 - << "\n rpm: " << static_cast(msg.rpm) // uint8 - << "\n ehb_enable: " << static_cast(msg.ehb_anable) // bool - << "\n ehb_brake_pressure: " << static_cast(msg.ehb_brake_pressure) // float32 - << "\n angle: " << msg.angle // float32 - << "\n angle_speed: " << msg.angle_speed // uint16 - << "\n edge_brush_lift: " << static_cast(msg.edge_brush_lift) // bool - << "\n sweep_ctrl: " << static_cast(msg.sweep_ctrl) // bool - << "\n spray: " << static_cast(msg.spray) // bool - << "\n mud_flap: " << static_cast(msg.mud_flap) // bool - << "\n dust_shake: " << static_cast(msg.dust_shake) // bool - << "\n left_light: " << static_cast(msg.left_light) // bool - << "\n right_light: " << static_cast(msg.right_light) // bool - << "\n brake_light: " << static_cast(msg.brake_light) // bool - << "\n headlight: " << static_cast(msg.headlight) // bool - << "\n dump: " << static_cast(msg.dump) // uint8 - ); + // RCLCPP_INFO_STREAM(this->get_logger(), "Publishing ControlMsg:" << "\n 档位: " << [&]() + // { + // switch(msg.gear) { + // case 0: return "空挡"; + // case 1: return "后退"; + // case 2: return "前进"; + // default: return "未知档位"; // 添加默认分支 + // } }() << "\n 油门转速: " << msg.rpm << " rpm" + // << "\n 转向角度: " << msg.angle << "°" + // << "\n 刹车: " << (msg.brake == 0 ? "开(释放)" : "关(刹住)") << "\n 清扫: " << (msg.sweep ? "开启清扫" : "关闭清扫")); } } @@ -465,7 +444,7 @@ int main(int argc, char **argv) { init_main(); memset(&car_ctrl_mes, 0, sizeof(car_ctrl_mes)); - car_ctrl_mes.steering = 32767.0; + car_ctrl_mes.steering = 32767; pthread_create(&mqtt_sub_thread_t, NULL, mqtt_sub, NULL); rclcpp::init(argc, argv); diff --git a/src/sweeper_interfaces/CMakeLists.txt b/src/sweeper_interfaces/CMakeLists.txt index 1dc4f17..79c87e7 100644 --- a/src/sweeper_interfaces/CMakeLists.txt +++ b/src/sweeper_interfaces/CMakeLists.txt @@ -13,7 +13,13 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/McCtrl.msg" "msg/CanFrame.msg" + "msg/Sub.msg" "msg/Rtk.msg" + "msg/Pl.msg" + "msg/DetectLine.msg" + "msg/Route.msg" + "msg/Fu.msg" + "msg/Task.msg" DEPENDENCIES std_msgs ) diff --git a/start_all.sh b/start_all.sh old mode 100644 new mode 100755 index e32b2f5..1f99e43 --- a/start_all.sh +++ b/start_all.sh @@ -2,10 +2,10 @@ set -e # 加载 ROS2 系统环境 -source /opt/ros/humble/setup.bash +source /opt/ros/foxy/setup.bash # 加载你的工作空间环境 -source /home/lyq/project/minibus/install/setup.bash +source ~/sweeper_whu/install/setup.bash # 启动节点 ros2 run radio_ctrl radio_ctrl_node & @@ -14,5 +14,11 @@ ros2 run ctrl_arbiter ctrl_arbiter_node & ros2 run mqtt_report mqtt_report_node & ros2 run rtk rtk_node & ros2 run pub_gps pub_gps_node & +ros2 launch rslidar_sdk start_double.launch.py & +ros2 run route route_node & +ros2 run sub sub_node & +ros2 run task_manager task_manager_node & +ros2 run pl pl_node & +ros2 launch fu fu.launch.py & wait