From 173670e194d4ad14d4ea34530cd800a671cce1fc Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Thu, 6 Aug 2026 18:57:54 +0800 Subject: [PATCH] =?UTF-8?q?gc=E6=9B=B4=E6=94=B9=E5=89=8D=E7=9A=84=E6=8E=A8?= =?UTF-8?q?=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/lslidar_driver/src/lslidar_driver.cc | 8 +- src/map/nav2_costmap_binary.png | Bin 0 -> 20611 bytes src/map/nav2_costmap_map.yaml | 7 + .../obstacle_nav2/config/nav2_params.yaml | 6 +- .../src/obstacle_array_layer.cpp | 8 +- .../src/obstacle_scanner_node.cpp | 2 +- src/origincar_base/CMakeLists.txt | 10 + .../include/origincar_base/command_frame.hpp | 43 +++ .../include/origincar_base/command_gate.hpp | 63 ++++ .../include/origincar_base/origincar_base.h | 21 ++ .../origincar_base/serial_frame_parser.hpp | 56 ++++ .../base_serial.launch.cpython-310.pyc | Bin 0 -> 1245 bytes .../launch/base_serial.launch.py | 4 + src/origincar_base/package.xml | 1 + src/origincar_base/src/origincar_base.cpp | 290 ++++++++++-------- .../test/command_frame_test.cpp | 64 ++++ src/origincar_base/test/command_gate_test.cpp | 42 +++ .../test/serial_frame_parser_test.cpp | 66 ++++ 18 files changed, 542 insertions(+), 149 deletions(-) create mode 100644 src/map/nav2_costmap_binary.png create mode 100644 src/map/nav2_costmap_map.yaml create mode 100644 src/origincar_base/include/origincar_base/command_frame.hpp create mode 100644 src/origincar_base/include/origincar_base/command_gate.hpp create mode 100644 src/origincar_base/include/origincar_base/serial_frame_parser.hpp create mode 100644 src/origincar_base/launch/__pycache__/base_serial.launch.cpython-310.pyc create mode 100644 src/origincar_base/test/command_frame_test.cpp create mode 100644 src/origincar_base/test/command_gate_test.cpp create mode 100644 src/origincar_base/test/serial_frame_parser_test.cpp diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc index a141253..53375b5 100644 --- a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc @@ -1009,8 +1009,8 @@ namespace lslidar_driver scan->ranges.assign(scan_num, std::numeric_limits::infinity()); scan->intensities.reserve(scan_num); scan->intensities.assign(scan_num, std::numeric_limits::infinity()); - // scan->scan_time = scan_time; - // scan->time_increment = scan_time / (double)(count_num); + scan->scan_time = scan_time; + scan->time_increment = scan_time / static_cast(scan_num); for (int k = 0; k < scan_num; k++) { @@ -1168,8 +1168,8 @@ namespace lslidar_driver scan->ranges.assign(scan_num, std::numeric_limits::infinity()); scan->intensities.reserve(scan_num); scan->intensities.assign(scan_num, std::numeric_limits::infinity()); - scan->scan_time = 0.1; - scan->time_increment = 0.1 / (double)(scan_num - 1); + scan->scan_time = scan_time; + scan->time_increment = scan_time / static_cast(scan_num - 1); int start_num = floor(angle_able_min * count_num / 360); int end_num = floor(angle_able_max * count_num / 360); diff --git a/src/map/nav2_costmap_binary.png b/src/map/nav2_costmap_binary.png new file mode 100644 index 0000000000000000000000000000000000000000..3ada2bcf636358257828b38c290e1b6bf4bb6d8c GIT binary patch literal 20611 zcmb@uXH-*B*DZW#(n0AWMWuHTQ92k@ngRx;cce(91_&(_0g<98U8xZwAiWbHG$~R9 zM2gZv5kimD03mS0^St-_#<)N3ulpn8Bs)91th4ssYt1=NVvP*68R@v`003ar)wvG_ z0J8my7tJM7$-o3Aj&z~*(lLhu08`h+i!4=w=^6lB2Xyb>dm50nmHoIxM%Sc50E+vC z9K9*UaXaM=1-c=qpf-|Ek&~K<=Y;XpE|l)d&yTG4DjV_2u3mY_+rN!CqzX^Q{(OXa ziQ{8=Rct2W%&t_K{KZ9lG29Ase*V0rlzl_!W&;k63&u9Xa6F_z0GriHSOaXR>C`tVKFa`mFHa-b%f>EccEW{t$fHQYaMgj6A%DjY;j`p5y$ z@sciPfT%C&Vn_CWd`Ue(Dh22P`u|&{|7#2XkG~AavW{ROv^MXl&VKtAsq*d{Z|~0T zt1_eZKV@#pqUbPBy)BjI(2^P|#*lLLc?#JxP5P*{Y8?O8oau@7)lB2^-FkN@Gc)tB z9+61EZ|_Gochl;2m;4Ry}OKw zPF2r=sj-}sHMnwAN5XdqE%0wzrDv1fz4%8EKJ1NeZrwZDtu5oA&^7!V^!Yx|OE_Nx zx^YP>NcCVFGhntqp!9Aj)gkQsj97E8`09Z)46E+DUhlrWHQn$a+WOUJO`(D0m9M#% zoVI5|A}m17oo>t9bm#-PW~tCe}A7x|H1`>pn=h4d9C$?wXEqo_{ut=Zx;?M<`C5VN!tt! z!el|2(v(Og@4FKQF5HkSmVMxq{D1t4l@2~#zYfQe$~9>r69Ejax}>-N+mC4`lB&jC zPz0Tf7R0e zYf8OX#x$+G;Z;zy2k#>zV4HrWP2FUhwd=pjN7fS|utM1me?bgY{@aZ_PIYHRl!3U!!p(d4wL|;34h;nxn$aBGi{MQK~nmDeE zsBMTjJRt)%D^ox*UMT{pj8ksj&=>Gf8QBTVgyyT4OY$Gg8JU9+*!r7xAasGkisH3> zk&%Rhoj*;(*9cu$hlhyCtmUCusG0HEAeqHpNdEzPn(?;)59?EAgSC^V$5MWo-7f>L z-z(OzRD+c%DW&)jH3#86w86A4dis9 zWos?D&5vb1h{j|4%DLeQ=`c{<$hD7Z5lE@rq{?k6pzfz-zz;qRA&X?T;>nn_(}Qz_ z%l6ck&+IpAE3b{rjdL^GV>-{S(+}722B^cvIfc4kqV-&q@CXoR&!_OT*06obZK>N9 zm~_iFlj!2vMZ+gP<1t}|9nW^_+6vdNh&O@*x&KCDoX1XH#HJ>4SD0{!=!@<@9W21+ zechVzqB4w?`w|-exNC~mES@9cHXVJkaugVAPW(Nvn0^x}>#(==DG{YS5d{?ZPzug0 zi$QFHnqNqb?rPMH{Oo6;gA1xrULOBE!qGPRFJx=bHTRQC`Dh|q>t@;TQd*&XM;2&0 z!~BL=6}#v@txgZoLGEE*@)dGw;`tQ`4{+|sB>_%Cp-st;`NnHTtB({TZBEQn`i8QN zMyHicGDg%+0v@WooL+gi6mWL;fR7(Sb_xWx_0t+@#qX^A#9>6hLvQ9g}rg2C=A7aFcDS2 z-u>(P?0wzxA-Ac5E=;Oz(nzm*NpN9&KD`Rp>}so1XDZ{FAcL>{UeB}6uAIvE>bFvk z=BGakD9Y3)!-hhGf9EHj1x0{}%}kFcXKty7o{`meAa{W-a=b zr}j79rD_j*5pn|wo2W8>VQacnAVsql(dqXQT5-*OM^`;}S6!;g(j&Nm?#j>+ z?QF3aB6GKC%n0JGC)FA(Aa~{vf9I>VxT(owxS-b6D%4oD(IYhx6TYMNT{`%4a{A@t zELuTx?4K*R18oIZN(b^A|9)<;-m@gRZy_q%l7%WOs8 zqCgea-rX#D1(cvt$>IW$caW#oM(ILQG%Nf*=ttjjt8ho>Hrrh-Gs2a(2gj@y=w=6d z_359VL;iZd_~4bgAuoWl;}B+RG7+3LO^UmbZZ;mQsh^JC{rgK3mqmL$UV#d?Br!de z6_M1B@o3r`BpePMM0%c;XZ1|@tA`~ymoqY=Q0U%I z5V6B5&2&|Mt*Vo>n!CenwWYwbFY6B0>Snbv-zUvrJ+IJ~pkB{yKU;sLa++1{n{;FR zEq~LQ<%;b94!L1^?RxEP98elZty5~U-dxlYV#3`kV|aw091c^d4tC+g#eXmQd1 zQ{n(ZKpqBp8HsqfEsixxLY08g6v_}8W{NuX(Mf!$hDTuWMsb*@Uu@!G_`cbge%fTo zqy8GvVTr!zkNw_$%T^R@K@m^Lqt{tB*^QIMC6*cI#aZ-@6-v)kJpxBLN^h49*Ua00 zuyUnvN65YRNME%wCW1&eNDpgFF((LLB=1ZJ!_av5C5})|2^_EZ8ijJD_ zyBC?Pp6R{6byIoYT)MVZ8|pKVZ_ckH|?Lx1Hr{oZ4cIb{%*Xf zvUA_Le9o=5$|QmsR0^h`2kbc4{_4nH8s|`^f=KwgG4vZgr2xXO=zPyixpDKxaKV5j z;~t&0O7SD#@{7oSqLCq)gHtuHrWRD_?2)$3OuO_5#b7Mz1hiX!=xLOT7)~ORIZtiEd4IBBZ&*f36t$fC{wND7lUGV`1R-pxS;u-z zittAL!`AXQb+^~n*2V*I*g*Aj;t9r^r^28>qC3JNIc&dI);Wxi{kQYst1`a`sjVk* zshs`H=T`~pE4RHgVnh#y6!;6p7c>%$dp5lvkU}tO8PuhHQbskCZX3E5x}VRFB1OjX zkxYPWx_%Y>WUX*JUZ&Nn?Krb-t=|3Gml2tVAzjM`q#67zsc7}lI9*`2ee@b~^Q2dD zNC))O89~wB@o6B3pn5Jk;XXYx9Cil9YlrP?Z&#e3RLmxZ?hBawk*dvEq+Psdf?DG! zg0Pbqwt2Red)7ya!IiKx^rpev6td{#E(?*O`jhbBpTR9DEBU?#kHiw{-7^Wp=ck5v z&afTM?cDR@TyrOVBS$;4KcG7ltaLSZPk$ zot3nmNZ^mLxf&8Xl}t(!v)a>LBPRTS;8nF&DE%c&CVt%17Txp^l;dr^RE4WdFW)fd zKM&-1o=h9fr0!$Bm>YUh3}vFb3>Xa~$FzHg?(vVrU)quX{eg!ZC|U-uw5BuvrlOm@ zP73>fnsEVuSCwzA*muXy<+IXKVYsnr{^IH1WW${E?0)B7N-E%QBPaleX)_catxpTP zvK>6ECQ@%`{nz__O=%>)naqgkuij@)xfn6 zgbVvOsGZ|p-L#cKWhBR*nRB6_e~4k&p`RHy!pZ_+;}X9AxX<$?0YH{jqJNvIec7O( zw&{`eFZ#`ERKTF{pu;flGPg%!re998LP;itr0NKs^E;bf#c{gvY7h!|2#Vo6#<*JnM z>(^S<(YK@_!^+zcX+Z$+l9h0HRD|vM`}?3_##5?PQiWtfQVIswObC|B8=)VxK$8RQ zIzblYh{@WC_&E4#;D0a%1_3OX+MRT-Yw&Q}bD5p0h$*ZJ-rKLq-E!yD=@AD2M1QC* zW8*CRUdHjKzRBt~;v(r+5?W6=|;s>;@+oLhOS!^yT<~K-5rt-{V z-SRV{FDu8%Y^sT5ETu~m1z-`VzT}8+)!5#}hSbgvASFcJ2$Yx@w~w}i?$TYl1LURZ zBlvpd(0oG&>5!6v$E+O60kL77qw5rGzzFB&mJPMpYEOF?1)$>{z3sN#zb^Lzl3~0$ z31lgFIWzxWQnaa!zbTribU?U{5#`9Jj#>`tD{J7mMf=l>g?F! ze-cc5-N6ie_r6Lqgj%$JrY0&y9>Pri*PIk!I37_<=O$DUKV^_C$R9S0KWJN0oWZx9 z!{sXYx~F0~eo~A2MrL8x&XzRJmd2xg*Z)vcRc*<;4>d91svt*DJr06gJL*&UF6A!+@y} zrKH2PDcms%ub#z{wm(bzm6#f=U#i;WlF4y`rZ0u%MO@LyF|qUbW^8_yRO zI!HU$q;1&N*-_&8Z}?6(XUnddV$QubQXK6K=3!=~;|=+%YKY%zz%*gbwlMzZXSnn2 zxmGx#lM0s=?1UiTh1YJI`-4UY2HgIwKLP(3GAhEreCGa4?MkFdm1QFHdkjnAw&3U+CY= z!A*(NPN$xShkEN=2QM^$ph!8@oaXw#O|)Zwqq9hN5v4jLp0q?r&9##kHH>Hj^dyk- z#Qw)<9i8NVam6vqRi2WMgtA#aKhplv{9buA8cJ>@#wUHO=1U)gt|3IFBfOGcB-p1w*xBI8`gd4A#ec$-sENOKv&QFM2SpE=e?Xc9n2nPGni>zE zR~4#LSX%bPUBB2!H5Kw33he2vxc>udJ{vSDM^@;~UBG96CU+N4hjoWp*bf%EY!<_{ zz@FfWU9vYHrrYSLJn0#U&#oo%*=MKs1U$|*&0!@W4Sz~iHN(}*J?~*U4vOwA96YbX zlR+auo`QodU6k3wy#5NMrylCCKtlENNn;5KhSjN#0x^pm z7wIOZuA5a*9+;ybjl^*k{AVmwFmFP?Q-USUo7yn`lC+YEa<9Mv2EERDrb5-zr=4cB z`YHUu&r3+6o~|5<)@D6%NzrU_^0LbHNb!`?w=$3ev_vbI`J8DJh@QLpymNpBV%W!i z(u!C6gg8uqV0{yF`E2hbINgPKP`owbXq&_+1n>}>0Cl6)$aVnv#I4XIo$%6P)RS%lSt_PEq9g1u=tc<}Dq z_JMz7qWd&yOLGFIQ)9QP;tYOS&cBu$9jyVwZ|hyzuMen`sGY!%bFg|wScZxEiSCQ0 ziGcdu)qNo5rRoc9(6E8jUUb{xl+Awq8^^zr3USdHDsJ`nJ>zrYeVHgGQ>D=HH%xM8 zZkF$>+FjeDQ*&{u6|Ekf8yr)!&=DJ*q5RS{TjcZv_aZj4r~ijFe>~jyM3RR>jq}Rb ziSe0%yGmQu%x4KdL#F;$l68I+PkXryxn0y7Hg&#_mpEBE(Z=q4dC+IPj?i6kYi|Ly z=_lyWtJU6KBeh@%{h{P(2>kGR6%m=S( zrbtLM3#nvky(GIL+*Wr>o>JhzYV8hcUUVU;SJPqU2*=eb&q3f{Rp1Ef=p#d^tVsFh zWtvN@*bHaixG%j?=WqX$g#uPC)g>!_quR7>|4l?MNaLxXnxMjPuF_d{JkErS-`oH( z*`o|E0t;S zaN?Y!aVLeWaTUa1>K6@LP}EbCJ5j9JDVOJ)B%vRNQ^o^4uBAP&U|(5!jzwDwj`p>jp(0`+cNhQ@-+;yo-e=+Oh_@s>oD79e|AWB&yYpw+Yj$ z-px^zJE)X&QxskP*ss>s<1_3I8JkFib!UNOx^;!vJ&RREj}xt8#h>cmIE2O7J#uP! zJ-NL8C&}&(dD%gdruY>Jd;?hLwa>6Z4jwMj$G#1!-ESV_lKRm+=5W*BT8vuYAZw!F zQCm*2zgS!g!)6_H1mw7Nu?>wd8tMyBnLe@>2D`?Y(}OkFHbVdVy^JP zpA}V3*-~{9cvIoGM$XNHzaIW-;`JTotfvg2x7Ap%pu1t>iC;u4rqcU&>Ckr-++AT2nRP?xIb}3fe{fVE^{fnKDV8wAUEzl^PPWC}v@Mp*A2E)s9{&MNg{QXR z2qh3u$Mq}npGOCXU47Nd;t;n#Y;IElUkaV~J6|``=iRNNXDfb*e?0 z|Fa;~7Y^2{XjY-ymy^-Rjc+b#>lJDkVG{DkuVAm3{b_(^8O)-oHmAa1Y?PZj@Tv@X zd&jM`=p*ZGlQ7y2rS;GkMF+98-rMjoY#~d#&vHtCnS$bNe0tR82cKN%^48Y{(>*#k zr4W@=S(l=YQZ23s0S+*6fC-YNVZ_YeQ6vH+C_!)P!cZz zV|IzqjD0Xs`bkXJXhxA>{qy%7uH9DS^hZUu{}b)U$qNro?7{f6dU~ zFody0VmIl4Ge{yxH4{uNE{6k4Dvh?Ox5$_{m!8J+(Pec?4y0wKd}qAp#pz%bn0t+R z5;q!1`H4iy=)2Yk1~u2}QCLmSZ}2v!*^In^v|a6L5$06me3fK0fR%2n>$^sm^%uOx z^J?O-xXww zCZlvLSf{hDiHvA#cygev&V{H{Rf>l!+CT`Ihn~!24?28UhAgBs^@xso*-*5T8`bC_ zo>`v6qtV$z$kiSW!?-@&R!sggj3x1VI1ecE^mhHPc&s9J2zQ9Y4Vgqj=bOGgY=6?O z@TVC)_aRfZ@s&i5+ty*gX9VnJT)Hm-M&28(_GzA{v z&gCT83W#JCC?V9T^njX88K88lt`5FzQ8qzo5IaVAQk^eVAKk4?f!0JsT2{RR+~3M! zM=ra~XE9fT!pN^jyG4iQ6skm?_fTE}7gW!DgPgW`PWL7@jU{x0S?rG*eiH?-njW68 zP5kdVWLtXmmK?T&#s~U~4r=h&ZoXm^_s>r!1F^#TC(#2 zIgrQnj3a2MyWV}e);JURx6l))@6>&^l`j;^&yrceCX^c&3~k^f2W&n+L|~aX8ikdG z6*Jy?TH~$k+)NgS;4fpQDkBeBnaC>Na7X7PG`J7(Zu?@?(P3xPD>Lyp1ci~7xUu$5?kcGpWgv!3QpQ5yb*4m+QV zatbB+vI^clb^quH@pT~$F;UZoY?~O5Lw4j}dE2{O{-`7dQTy14dqc(Pb{6Zr@mJ-Qkuqltf$SSsDNMP08%@LH`tHdO#&|a<}vA&9KJ-ky_V9qw~Y#TRo z0Vp*$cw$-=*=PW|^j9$;&c%g=h3-2D5+sJT3UmtghNZXEvj@joBst2E@7~y>IsVGcFI^?mkzTZUD=g-#j_;UWbQ?IuDyO@yt&s(+hQ9rv@EaS01 zU?t}yoUE8|F;Ex8>A!#fUS8H2PuF^R`oh=??YN(hDO_oxzzBuCMUqC$zMCHv6dj6E zc3`cFiI#@9-JV*{&xS0(X!k!DQ69~nBnNRZJ_~rwvEuX$4JS+x@nPrqnp?0Twexey z+lO^M%5~R?>Y;CXK$gX(t3WM_aQBPO&aW8mNq+&X&30VX_4j11_X<-eG-)(S*vaAW za;njn5T5X>q_w%+VSgWgBLV(8jM~mAZ=_??s(j+p^P;OJ#Pv9rvCj3Wb-MQK3-yKM zx4Q6OpXA9j4Q1EKfPr~WdGdJ=Qu-1e+b$)^Ea~fID!bk>y6FLJp3YKf!Nsd!nP5%% zyYKJE{thZ<5Ivs!J0;<-s3$(a+xOwWJ-M>Y3ozX$2WFTXXUMo{w4d}EJd?T0&QD4_OGS?NQRhOS)&?df8Atz};9{g<~Lj~ux0D#dd@cds}g23yxnarM~=Nj&t~dB^+_jqdQ&90JgJR?drCLoXFf09|!^zCj|l8_bfg(2>3 zN;s?4$0A$PdXD3!cv~AED&?vjzV+3+3X}q*PW6hO-7zAGhOqNK)a|NX(-c5fB&k>K zybLlxiqz}n`QChO@jt&~9+?>LDl9~pCGY_M{+puOkzLlJ)|8tuUIGC81@iWw9mxO` zQAxwRt`+s)VsB1+_oDFdT?Lm4tURR^}}mtD)TjcRr1s+`ccTu+fiLv}>-|5XD-SD0*{PypXat&n~x zNt%k;CkZBNt>H$${`JlFe$R3}X8 zbq^PhjRgySs{a)oNHtxLiiwF?t$Po%bFw43mS!d>`@yCJgnRm-V>YnLNV0R>&mgy% z2ZQl)tY-7`L!>q}@7Fl+K5&abJcvC(T-oQm5D(bBaAW)N68TIyF+^A{y%~CoE~QCB zqxV@N9FsqpI1~-o36fSD!>gw2?F*tLnD+ukeePg;hXPn7iMYba+Lft+{Y!Mbl7{*# zH7O40bt7>Z_U6fgd1K52{)3q@O!~aKbA|Sfy&OcjH~p zIff^jpW5En-#nxBbM++EH(OP*k4n|3jFS1=;0`r3H2h{&?L{O9oc}fAnuCLaf~rQ* zhd!ND%#usJW#{wEXLjG^E!mGZ31xfx`+u|kbL<_f;^F7>Z&cq=>@rCHI9&OWxu?j~ z;GRi;jzLX|n4(#@R1CxS&(eH~Pd`PfS^TEkUD}iiO9TbW<_@#JSs*|Ok<+#dll3Hp zn8tCR;SA=Sh*8021@EmeA|{AjYG>O$$dTNfoSBN#{)(1=V?4b(>p`Lcl`M&^erJ6m zxsCr3rG0}s|NC_u#A0<+Vk^Et=^#P#rJa6&dBf5INXhAihi&g!#?s0$mC+SFi%2gI z3kaa0_rk)$ND41B;WUisiN71Rch>`hUy$3MARuOrk8Y%4h_e_!+tB0X8mCb6O`}D- zXZ5zkaa*;Eu}n93+Lk(3+zV?=Pdk=~D6v|Ns-rL5Ex~;2oBJ1PS#bLPj-Sf>v3;0! z&Id?5R;3w(8W{Crc2WLXi^4n-1F{!L%1-)%Nh&=J1SR*vT&} z#-^_V6u3$)o3y8@@qNraSSYvx1AU1(Kf@1uwjTEochlQSPrk@aG`$d&`cp%cR1H%Q zC3G(MVV%7<^uWCPhsO?ZML|B zKGy$2!EkdZQs5G5Hh4BwL7Y0?o>jqb&o(;-Q8UQ{i;#Bg=~GvfYObT9H6@;Sz(kbMH73h(On+q_ZXpOqoyH>Nu9zw zokMwRE8J`Y&LF$=uM-b~w!|>%jW=g6Q18%WImlZyYl^qGp@tFQa*Ne&k1soG>j1=p1v!51dS!^PoyZ)<2KXKPaw0!j} znvc|oBBGk}&h_b2!cDi%*N;_g-Zl(;^kwHc#e3&$|H!;MjQG+kQA`u;Lgq@HyFJ$3 ziW~OmUnRROM31V}&ljwU=WsStH%0PQa|FpF@jHp@evifUeqy6rCK}*`iv81@$Q^&{ z!`Xn=q{#|vp2Q)z4#$EsgaWWxlz2OKXZW{5e%mFO2p_*jR(9%1nlj8Zx~bc&L|5(8 z(DZX#jdC`J)fqnCIQ(*t-@C!O%(WfGci2rs8a7~xR)eb(&AjKuQ|4aw2rZ)x9c()d zpJ3VT#*51DmBne(Fav*$Tsk7kHG_tbb;L5ft?L_{9OrewPV&rzZBPpU*u$ES^Ut*b zOWXMFJKsd*LxFjI5<>csh0f~*e zWweC-cNy@~09#t&H?&E*UElEMihw4gN)$D4pfjAyiGA<*ff2C#XVyvyych6~a+NSi z2{be0q5%e%F5G{Ae<8mXHcbd>z*G62ay<+1zoq@hCspmBrX&jzbc=8d%%tjv9p9oL zoitZYV;6I?X9!RdU1Kz`#jdRWhAafkucpJBI0Kz&kE8_Vy@I2eG-;lF=#e~}fmdPd zIw*jG0K^E+j?t?PR$BMCC@1?dF!qaWGn z@l14zeRT6gHzqrXZ_JmPZ+OqQ*SqESCkjoevnaK9-?j*RL^Xw>PK*-W?HLQ_D#PebWpGul(){oTCiX)#|h&L;*>R_u5!bJ?iqf8K#5}I`#0+;v`P%TB zH8QT=Ew&(uR?GC8m$h>xCxWkKJ{>gBdt$rrc}9NQAil7G5_U}#$m35OIyi2OGSS2J z$A0uF8<~Ivr-&S!KHbcmdN?CLNQBgp^i-fo6_cW?e;44z)fTbnf$xe+3L=pfY4=%N z6b~&IkBSLCD0Wm?X5FRx8{}3#JYEg!CwT8Yx5izkhe@B$8O&D6N@QMI)YR&aiLbDf zNGe-ek8n)o6<*F4q+Q|opx~EmbNk4sSK;8(454li@$}D3!4ul(ySEa)J=V*1Cq-@7 z91p{Fe@aa7S_Dgxi%B{GU(# z?JWv!_O-~JJPBsTd&O&JE`ahs|AG80u}L}P&rZMTB)2lttAJ>$w2C8`HI)_g-V?hT z*cCF;VuVJ55+A>|fLXkNJcArtQ28_TYs!dy9}SD~q6~R#)gZhw;n>a3YBHuud5$q< zv1g9Bf2Bx1%KkC`$@$8@2$H#T!w2uP=j%`%1GbUIXid2;v}d1N!2-%Xm39K9Br8UW z={|4^h^NY=zW*RzAvk6!wK+ki8nla#`-8vl^MPZ-^M_pPI6|7c*yl4W%sY2-T1zQ1 zb!A|eE?esnmmVGFS#n;81?TmY-Lkq^X5_ZiF^!n%rjC&PM-yg!w^QQ*?(9e}d{)f( zT$oa{^hc!H(wRx4bVzx8^T9C->qC8qOy|xPGB*bH?=1BielsV;Uc>}N@6Pk`@8US5 z0Yb|tEU!l|zbhn&Y32j`i+4!Wwn-nmhLAJ2#lpfKB$?dOhp98e*In)5d!O`%vGMaX zGA#Pro%}g%>&ik7hAgT;vrBhRjkP?9M*S)yiXQMWvHkdG>?$y-9}!DKRKWLN5-vy+ z>8^hM7jF-B#Py?78}VuVd9Uph`kwmHq3gY!QI8rqWG97YZ;>f5);7=6M_k?FmyY_j zBOi=-sYVTY^$~|z@1XC+ufkqDS&=ij7QfDR@-o8s^PS+jgO4cxpRH~!^4yfK3+C(U z-@4pRIh^krMU}%mG#cK~pIbX{Y|%^xBoY*Zbc|`%exfD`QZC?W*|C#`=a&<&E4=q% zwH&Y`UkhDKX!xY)yxD5@NUbGpaWR#}OkRzX+x26O)$vMlbH#H+Zz!{4e=!Up{%s6i zxA1D_Rje|zBi!jmTKN!&zeCb#hb!LhO+oZ@EnG{M`NtNsUbT7ja)vtGV!IH<(1%F>hAg%ojD) zd#RvMc^WL)<-Pq#&%Q^}Rxjb7axn1(onqNBducqFVHTe7%r6*S3XKrbK2=+8! zXl3Ic4tx&X4)VB_Kk)zurbE9rGOzb%x$BJ-l6bn2M}9CS$W{0J4y0kVGH?_DRv*%B?$H?qtgln9SuDX=QI zAxA$Kv^cJpQ14~`WEwk^<|KVTWttpo`^(tGGS_yP4*pE&N!+Z_l9X7ZWs2J+hHmmm zCk5uPgykMhv7yIT_Jtx$;yS<_?@Rj08E2e4(o&=qbZ?s&h%$fs?O$FB28#+wSA3kd zTwT!oxOFD>M846)N9;#We^+aziEZU2aP=2|*c)H9(#muh6leN$Op+YN(rnIM)p%L9=eq3+D~=AjM~XoPDG14Zc!-ZaEDM@ zJN|Gn(2lnj4;x^j!%pF$*!ts%6wYRwX4AAt@n}BCh!?-~a_Cp^qwc4j9m(J$ma5c= z3}x)y*bUr6_ppK?rZ@Lun~gU>Qt3hG>r44~mDjrkWw*zC9>ES8SQ_Xw&(+bj{29`@msSy1)O?L6->?wkSKWF?TSL!vr8`O~f!*8Rl z9+@^7y7>g?Ej3^p`|85Jq7|W{lQJAe$PvgTP#HvUC5dieQSi&MvIf>9ndj7G_I}zQ ze7Zq-R%J<*u*FHTcnWwj)vD8?ms_Ojc4}7!0qv{NP?|D2w7lEJLSlBSs;V6)ZF2<~ zdxO4ajVj743qRS&`Kp~!qxmC3Z~1BWI`Y|{67VXUsPvCd35zBR_pkD>43jl2Sj^?C zJt%ucvt$~Rc~=Nq@a^>i&Vv09!)CWWX6cKOj3k=HuiVGSK8&S z&bW_%p7s1(kGvfXejrdlp}7yen7nBSD#@3)hYGQ_f7or|Eo>bVsj0 zky(dN^-v;1CB*!obZSxg^TytvZRkR8r>(oCjyBE%s!K*~=_WV^~G#?l|F@w&jp^_Tu)muVn5BfQ6pKf!17`h^M z#RKu{4&N9~i~n)_#tZfaMja@g64BhIG4l9_w2;Ri`;`{qw3-piJP%ACLhTFX^Y=#1 zX=n|jmD?6cV-EO8rN8wDFC4_8`3A<8eF2x-$=#D@A{k-rECb@cm@Qrp`Mj=1WP|Wn zmZ#*w%j?uIX1Bh`rNjpYsN!8Yo9ALe#oms8SNX5M)%cS3Y*k6naR%%%An@9a)2(fs zC)wOsc$K;V{ZqqEuwgbv-A(2OHstxEP@?NxBpuLrx1n|fUi5K;Q?1nX>jp+ilxPxE z>r&1TU=0BIW)_I_>KNlrG}h&reyunO%-OB&QAJgG)M~88M%Nr@``{^mA=#TKS?JQ1 zs5Bq>Nslq#2J)y9w7tBixtcgGC)ZI8-J%9uos{UU7lXfz9(l-#Mkr`%kTbS(0qS4b zn(nKL`g+Fb-4=PHFXwmOhQf@zB@%|r5{ctuDjb-aIVQjr8kq;g_piKS=w#UkTv)@OwVUw`RMT0rY}@#0I(E|<%pP)_<@lhD zeLyBJwJSNe@*M+w4GbOeMbrln*Q$+9XgzYniBx6hkquCE?Zz{7VE$_Zxr$7PfaP-9U((HF3&J0L{~{7Muf^96#IZF{!9Ue z(Km~F*U~B{`10SorQ5_$jERI3E6Gnd!Y}F}>yvex-aBF(VB1c_pe$Y)Ms?1Q!VMLw z(U|ipf>ud-wO9Y!+}}j7<#Oj9yMq$>0<`S!n{dJhmFBK~MQad(;2O74FDPkkc2#Gd zVoa7K!R=!Ctr#P4P~dzcZihirBh7eqq8&eq)7_#DzAa^z9lYgSXoZWEV+`lfm)O7h z@i#Kx8A8QHiTRXrqA;^~Czz*eg{{zL%$gqn)V0`~YBd+l8svgRL;B^iE?YoG93`1d&qJP6<#ghyR*!M0$U~2qZ?2+O|BW8d%P)PyfJLzOe1j1;<4vkJ%lI=$Q5Z(y3kO}3 zdE|Qe7KXm8^Pcz$Z*!~Dy||q($tt3I^aFY%?E$|x%e6v*hcJt^tnI6XmEL=oO)^{1 z#V;zd4*1?(@pxqcIY)Bjh(X#?jKS6IVuXKk!cIj>HGq3XjQjMy`KncEc|mPBtx~!L zeSHC-&9`{d&xo;bbIgH^enhF%wb+tGVN=f=xUzmqN#3&9rP51Nge8WSw)}?IZH3$& z?%FI&EB z{ah9GPZJ3yjJS58^aJ|4CRZ8%ETZT4;p>}g;HzLqNriXNrA!KKIpd!DdJ{*}cs-Xb zY>wQ?AS3BfP0PG~<{uL%Xyr&uP16I~6-`~)UvFxA_1wt35l*cuEtm4%j+yjvO-=Jt z4}=S?ddtR9)mk#h|1-?W_j${|{VETZnNOmQo~$hRaid>Vj&N$nN0Xh%IX35?zvS<8 z_fEm4=3p!|zJhULGte|8wlgz_-fNJ>pGs;~*r|*Kx-LpXE_xnzW-9;HGRE))o=4dLTj5|<;H<3c51rKuOnY~Wvqq%8QvPV7NdT<1(tE{JPNYa{YWB%e)i9* z(U1VF7eU`&&%A09oT+m`4u^&sZr{{`KN9!do-@|hVb(0^1H-7`soL>(e~J{2p>)IK zIGcK4tpXezG{WpBajyKL;P#t8jHjp>cj?M&oysPp^pB+`T7efK~;eUC=I+iw01ngKphrt?U$}j=2jbQ`IY%0 zy~;o*`+lLX2c<%DDEszpTgm+eAG%p`6#Mmqv72Q_^wjjpX#5KYITLm5d)Ho^>o zVm^*7Eb^}2bha`F_3{bZgLll|c2OGiDSP&H@kT`ZS1hmRR_3;ITS-ri-IG(WEB)O) z^;thVnoqnYYdpo`3MBFIvIEg-!({p%yT!CBRkuUjmcsTVKZo%gPc z2v*3d!$-#cj~)rt`RqEKge+5US68yH@DA}#OVUZE)9_En*i|A0(($==rAWJL1#FZg;-xw}A9oC>5rEe+x z=!i_E+tDMB`X0mZ>L0yF-U**Z6*Z$(Dig({rTw~!g-XXKSx`G{^l&3Q_KW22IYKbX z<+L{^vsq+tdpQmLihMA85tglGcL0aLW>=!MHUk)`^e<^r>p!)8g8TQ%VadF%N58H_^SmfC~*|(9+nED4m zZO^LS@Rtcj>19Q>2TrJ2i{7H6{7C5I026K+%a5z}n)GCWzc!I+rPnBpR zs_N5JJ;Nc4lj6yhLt}T|4{jh*{CHxf@u8T(^$*?INiM~Aq}9@cybO;#*U4wvTGJVF zkTp+9LDfq$FM*MII*;Wb{xw+Ar-b4@wPxlZnImTzQKc;jbJPBN67o~dhXD|wdLX{) zQwaS>h0rm$`T@&{T2$Mef<=VM$eH@o2K73v{8&X>v`$c%TO~&l!tAd)?88<~c(ghV zgw>C2iePCRHZHjOL`rx|XQKHz?@&Eo+2NIdz67}fJw)LQ+f-Bt;~1CCW-CX-a~V#U zn~aN?e^($bji5a;U6!W*@bilOe%yJ(W_JamAlz}XyT@f4KNjEtFB~NbHTdfJ*RHS_ z;S$Fqz@Zx+m))i(-z~B4>%9?=-k$mNcJr6zjTf#Oeej8?e^jaW)gaSfefgg){C;Ek z762~uUi>e>M5`7v!mLEqGI_W2esO9egn>|AWW1`WA%^5AKY>IoxsfYqzdb-arh!!O zQ~l_d;>`CbgZ&`N$y^yBk$IQxQ_b9Uw$T=nO%{mnesJ(c2RiY@>T9QK;P&SII|HSa z*D3fTK>S^LB~w@Bip;mPf6TbYYEK$X`J^@pJEb1DnYxj4!y-UenNj%t+`t@{`W!W> zqYNsWWO?Y@4wB#lU&Y_sAs(;XTNIm7M?Yt!62pG9Ub!=i)EgoZjHNM6@Kp)NpXk*1h17DKIp_P8 zYIUDB!nsVW#up^(3RuO?4Sy%`@atclaod^%t|tv87=3!h@2{ELLAg;Yv2?4`1*Q2v zJ}vc}xy0BBPD$1F6VJNv+xM@j=G(Gd4hupyY>Z=Ckw z{9M2REU=Iu5$`CQCbyMU`s#CvDM7XPPGLY^(QeVD{<+<(-M4iLNJ+iykOKKlF{|N! zPWsz$V|?$wf#sQA``40BNy53TtAp~8Lj5;ShJa>Q**_n!E-HcFu#ujieD z@2Tmm<>XBQcs3mC|KU3HY%bYRJYJH@IyYLW4+ojXebLPkSfSfveOB5MW9T8lj)#|8 zX4P=Ig9%%ZT$i%8Ml&YMSCgdY9;CeQ#M5;vp_+^b#JXqocDjEP)4{cZvTla}0dHYS zqePW6&BDGN!&I>|Fs>@{?U*GvSN3dbS3T<%iJ8D_oyX%uSl zX8g&kRP<$i4@*cN3t9mAzXGrgPx1l1(txj}v=wY6Qr?MN0$FS!cU?DPyI0M&H7#)J z%}F?v;4Wi+6--#@t22A0$jL;`)zPXzl_HiXXbq0SNLj9&Hjv&n*TW;k%{;FyJGMw_ zX`iqyMtCe!-VHE@uzeDL=V)+B9y}}yDC8b`uz0Z_wPNk%} zTn+{$Aur_-$i?_m^(`s&*!xO)7E*uD{g!(FTuuYk=$T>RU(cly${ot0rX+X6>!dv= zJyE65fxZC}gQ}b?CMD)3_G-lXr*)rw>H3_dDYfWHExt4^s%j}EsY0?~crDz5YA3$}K7TSraDp++1uo7Gw0UjkQ4-^Bg%|W9>_gHOJn8TKC$xb1wcH zD=YQy%TlMtRDJxt-Xr>oIzCd1O-jF|#&0g4!M`S~SLyl|D2uk--0cy9(5%!=D|F}X zvtk4`=^^A=pgfym)s0+?XlSolb>J@gN6EuGc|9y)UZIC*UqR(qX@aCOM6JQ?6H1mX z)uNg~XnD=flZe%aL#yk3)VtRtxlzW7=8Wu_P`nDW=VA>@DRh4|ZLu@J8gK-t)^Vz> zUfSw2id2YXW_v)s>P3}8zvDoO#bH-jNprV2jX+|Im8|emf|pNdt&d1TOQCt})mr9S z-{&Hi(~#@CTuz6t^i%Qf*t1LZo3WTkjY&U#zVCbVI?g;yl^?5%Ha+FnMU_HtB~1DN z`iNyLk7V}PkX%f}%9CnO%k#)my>khafXC>(4vl@a^geX$Xty5jfVMu!S^p_-fd9W# znbh~Rx?|+R!u-E1XA5-=(PpLg7^D8efRYoV3G)iwLL-BZXi^w6GMH;-h!3}26~Cbpl4dY7_YLl zC}@rHV|IOFIWH@FaKZ(RwRc@gp@TK)=|O4T=M3h+u|8MkZb_vvBo`X7odK3Guh8!} zi8aw`arVs8ZmGvA+|46Eb(hAjpFwHY0vJzfj#yCi>J)nG{?VHF*^EgPI|L_;>1(I8 z-BRzG+jrWFT$|^{B&WY}dt0S|LWp~GC|;gX^Nu3pD0QhFkF74{)FNn<>a^lFb(+3FPfU9hCMbBx^g>70(qq#5pk8FyevfLiA_e;$ItMBMzUZG#C&>4UZ`WpaUXr7a)y5(|lEG~N0x!k$r z&L!54@AkAfpi28-d&ZnnD{qNsNS%dHySE+4!pXzwbsAx%3B{ObOl(>IJPh$S|;wSZDv--!wU3VRB7GD*oxJ9ug0TudWCYWqSN|5 z6lPeEH7Bjb;*Jz6B)pINycpCNY54rqwP8Gsv(L3Q>mQc+JZA2k2*+1!JaOkIH$S5^ z_WT@xnPamy2O(GgQfxiEHuv8efQ7CAjRT`ZO3_IR#9jhcQaD-cKM+S-NTxTO1%h@` zbJ|M%C0IhCmsX^Ah+owbv#>QI8TXP<>ev_GhL*9Xr5>}NA3`~3#R`_{TFVj&-7?x@ zYMken>L$5bu64~zw_mFKS;~C&cxlCViPl()*4{V7di`gU3RzIDT@lp^y>_tIcHE2@ z5hpg>yMCqeW54J2ou^_}v48vbaO#$`kW1I6@Rdx8UW#0D@2v&)op!cWD$W`9!)Te& zN@>oOWI*nI39mz`Z0>?gn%wyvvG>pA2$cS&k+AJmEA$#Hbi&F=xUh3dz4cim55wR- z%erzN2_?A+w8KqY#TcH;atb|!nz4N6fw=0s_Yg3gjCvqPYM*|%uNc%VDz)SgfHsyn z%e@5%1U;0jaIUbD+77mDv=5rZ>g)dxwBfg-@Akd8wkKWxn~k>m#E7+H?02A_r7ZN^ zHY$oMcZ9$##iVlA&4}+);!oGL;XSE#%_-SZk_%_gXc!Bp#>qIbeQsPoMb9|s3WV*D zQ|c{kCGFLzNgSmTn{%b|b9pVL1l{oO*Zh%G_@#H@ zrO+iN_w1x6YXitRy34O7>)V>JF1hAg->1f#W68>}*yp{WEXJrIp%vQ}W9B2O6*^Y{ zB&z%cU%8eP_e*U%ExY~5|oKvmP zDU^h9i&ZE+SYmaxU^Pj@Roc|DoU4zCmlg)OYbfQU%4|ieK$D_%0R?Ep65K4brRF-$ zsGE6(z8abbD21HIfs#=!soEkp(|h|E1f%t zXXPZ+)>CsoX|CIy>6yb&%Bi4^bIXajT7iWgKy8rNb+JL2+j_xyUJKl9A=(KokKDG8 z7_An1ZY5m$`;7Q%%gQ|#t_lCUw1DZQP$XizoN_rF$Kjl#pBtZpGEY;v4Y3$>?=cut z)SUW32FMevV9R4Or&o)TOi&Yk)%UkGLXjQynIjh25ut+O_9`sbw)TB5o ztd?~Q)Z!L65M`}PhuXh6$&Y$fHz(3u3f;LmN?q$j?zO;>;>u#@B=XFza}3EkX%a%*9E{mH8?&+Y{@UvYULvBO%VwTp}Bo*#hqqcQz z{JOMvI{X{u+1KA<)_P69`S;f5d{CY(k@pdnN2y=$vFv-e%v=*(m5#CY zZp+p-;4WZBD2{M)UxF(oz$6y1l)7;6>nw@1kkt;?gK@QcEP*g~eeJHE)P3RN%YrQB z&_f=~Y$NjuJ*3o_;fht0@X%Ht3atwQ>G!oFbS(69s5B^<`jp=+ygP@U<88(^(izVj zaZl{N9`73SfQ(`}g-*eOz}JfPZA7EPLov5<3C{Jk78$2p&O)>2yJ{t+1SPiNr7Usk t;)-J6g!hGE6pA}uic#aidT1xb{|8+#9>w<8TR;E+002ovPDHLkV1g5@a$W!c literal 0 HcmV?d00001 diff --git a/src/map/nav2_costmap_map.yaml b/src/map/nav2_costmap_map.yaml new file mode 100644 index 0000000..1f1edcf --- /dev/null +++ b/src/map/nav2_costmap_map.yaml @@ -0,0 +1,7 @@ +image: nav2_costmap_binary.png +mode: trinary +resolution: 0.01 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/src/navigation/obstacle_nav2/config/nav2_params.yaml b/src/navigation/obstacle_nav2/config/nav2_params.yaml index bfd63a8..2e4f9c6 100755 --- a/src/navigation/obstacle_nav2/config/nav2_params.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_params.yaml @@ -170,7 +170,7 @@ controller_server_rclcpp_node: local_costmap: local_costmap: ros__parameters: - update_frequency: 8.0 + update_frequency: 10.0 publish_frequency: 4.0 transform_tolerance: 0.5 global_frame: odom @@ -189,7 +189,7 @@ local_costmap: enabled: true topic: /obstacles obstacle_timeout: 1.0 - transform_tolerance: 0.2 + transform_tolerance: 0.02 default_obstacle_radius: 0.05 minimum_obstacle_radius: 0.02 maximum_obstacle_radius: 0.06 @@ -229,7 +229,7 @@ global_costmap: enabled: true topic: /obstacles obstacle_timeout: 1.0 - transform_tolerance: 0.2 + transform_tolerance: 0.02 default_obstacle_radius: 0.05 minimum_obstacle_radius: 0.02 maximum_obstacle_radius: 0.50 diff --git a/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp index f5add7d..6057ea1 100755 --- a/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp +++ b/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp @@ -54,7 +54,7 @@ void ObstacleArrayLayer::onInitialize() node->declare_parameter(name_ + ".enabled", true); node->declare_parameter(name_ + ".topic", std::string("/obstacles")); node->declare_parameter(name_ + ".obstacle_timeout", 0.5); - node->declare_parameter(name_ + ".transform_tolerance", 0.2); + node->declare_parameter(name_ + ".transform_tolerance", 0.02); node->declare_parameter(name_ + ".default_obstacle_radius", 0.05); node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02); node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50); @@ -82,7 +82,7 @@ void ObstacleArrayLayer::onInitialize() rclcpp::SubscriptionOptions subscription_options; subscription_options.callback_group = callback_group_; obstacle_sub_ = node->create_subscription( - topic_, rclcpp::QoS(10).reliable(), + topic_, rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), std::bind(&ObstacleArrayLayer::obstacleCallback, this, std::placeholders::_1), subscription_options); callback_executor_ = std::make_unique(); @@ -91,9 +91,7 @@ void ObstacleArrayLayer::onInitialize() callback_stop_.store(false, std::memory_order_release); callback_thread_ = std::thread( [this]() { - while (!callback_stop_.load(std::memory_order_acquire)) { - callback_executor_->spin_once(std::chrono::milliseconds(100)); - } + callback_executor_->spin(); }); current_ = true; diff --git a/src/obstacle_scanner/src/obstacle_scanner_node.cpp b/src/obstacle_scanner/src/obstacle_scanner_node.cpp index e9a7d53..6bb958d 100644 --- a/src/obstacle_scanner/src/obstacle_scanner_node.cpp +++ b/src/obstacle_scanner/src/obstacle_scanner_node.cpp @@ -63,7 +63,7 @@ public: 1, static_cast(declare_parameter("debug_info_stride", 1))); obstacles_pub_ = create_publisher( - "/obstacles", 10); + "/obstacles", rclcpp::QoS(rclcpp::KeepLast(1)).reliable()); if (debug_) { debug_pub_ = create_publisher( diff --git a/src/origincar_base/CMakeLists.txt b/src/origincar_base/CMakeLists.txt index 050823f..15147d1 100644 --- a/src/origincar_base/CMakeLists.txt +++ b/src/origincar_base/CMakeLists.txt @@ -46,6 +46,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Position.msg" ) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights # uncomment the line when a copyright and license is not present in all source files @@ -112,6 +113,15 @@ if(BUILD_TESTING) add_executable(scan_odom_timing_logger_test test/scan_odom_timing_logger_test.cpp) target_link_libraries(scan_odom_timing_logger_test origincar_base_log) add_test(NAME scan_odom_timing_logger_test COMMAND scan_odom_timing_logger_test) + + ament_add_gtest(command_gate_test test/command_gate_test.cpp) + target_include_directories(command_gate_test PRIVATE include) + + ament_add_gtest(command_frame_test test/command_frame_test.cpp) + target_include_directories(command_frame_test PRIVATE include) + + ament_add_gtest(serial_frame_parser_test test/serial_frame_parser_test.cpp) + target_include_directories(serial_frame_parser_test PRIVATE include) endif() #add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp) diff --git a/src/origincar_base/include/origincar_base/command_frame.hpp b/src/origincar_base/include/origincar_base/command_frame.hpp new file mode 100644 index 0000000..b6dd863 --- /dev/null +++ b/src/origincar_base/include/origincar_base/command_frame.hpp @@ -0,0 +1,43 @@ +#ifndef ORIGINCAR_BASE__COMMAND_FRAME_HPP_ +#define ORIGINCAR_BASE__COMMAND_FRAME_HPP_ + +#include +#include +#include + +#include "origincar_base/command_gate.hpp" + +namespace origincar_base_core +{ + +inline void writeInt16(std::array * frame, std::size_t high_index, int16_t value) +{ + const auto bits = static_cast(value); + (*frame)[high_index] = static_cast(bits >> 8); + (*frame)[high_index + 1] = static_cast(bits & 0xFF); +} + +inline std::array encodeCommandFrame(const Command & command) +{ + std::array frame{}; + frame[0] = 0x7B; + + writeInt16(&frame, 3, static_cast(command.linear_x * 1000.0)); + if (command.ackermann) { + writeInt16(&frame, 5, 0); + writeInt16(&frame, 7, static_cast(command.steering_angle * 500.0)); + } else { + writeInt16(&frame, 5, static_cast(command.linear_y * 1000.0)); + writeInt16(&frame, 7, static_cast(command.angular_z * 1000.0)); + } + + for (std::size_t index = 0; index < 9; ++index) { + frame[9] ^= frame[index]; + } + frame[10] = 0x7D; + return frame; +} + +} // namespace origincar_base_core + +#endif // ORIGINCAR_BASE__COMMAND_FRAME_HPP_ diff --git a/src/origincar_base/include/origincar_base/command_gate.hpp b/src/origincar_base/include/origincar_base/command_gate.hpp new file mode 100644 index 0000000..4950700 --- /dev/null +++ b/src/origincar_base/include/origincar_base/command_gate.hpp @@ -0,0 +1,63 @@ +#ifndef ORIGINCAR_BASE__COMMAND_GATE_HPP_ +#define ORIGINCAR_BASE__COMMAND_GATE_HPP_ + +#include +#include + +namespace origincar_base_core +{ + +struct Command +{ + double linear_x{0.0}; + double linear_y{0.0}; + double angular_z{0.0}; + double steering_angle{0.0}; + bool ackermann{false}; +}; + +inline bool operator==(const Command & lhs, const Command & rhs) +{ + return lhs.linear_x == rhs.linear_x && + lhs.linear_y == rhs.linear_y && + lhs.angular_z == rhs.angular_z && + lhs.steering_angle == rhs.steering_angle && + lhs.ackermann == rhs.ackermann; +} + +class CommandGate +{ +public: + explicit CommandGate(std::chrono::milliseconds timeout) + : timeout_(timeout) + { + } + + void update(const Command & command, std::chrono::steady_clock::time_point now) + { + std::lock_guard lock(mutex_); + latest_command_ = command; + last_command_time_ = now; + has_command_ = true; + } + + Command commandAt(std::chrono::steady_clock::time_point now) const + { + std::lock_guard lock(mutex_); + if (!has_command_ || now - last_command_time_ >= timeout_) { + return {}; + } + return latest_command_; + } + +private: + const std::chrono::milliseconds timeout_; + mutable std::mutex mutex_; + Command latest_command_{}; + std::chrono::steady_clock::time_point last_command_time_{}; + bool has_command_{false}; +}; + +} // namespace origincar_base_core + +#endif // ORIGINCAR_BASE__COMMAND_GATE_HPP_ diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h index 2748ea1..9da67d4 100644 --- a/src/origincar_base/include/origincar_base/origincar_base.h +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -1,6 +1,8 @@ #ifndef _ORIGINCAR_BASE_H_ #define _ORIGINCAR_BASE_H_ +#include +#include #include #include #include @@ -10,6 +12,8 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "origincar_base/log.hpp" +#include "origincar_base/command_gate.hpp" +#include "origincar_base/serial_frame_parser.hpp" #include "origincar_base/wall_fit_core.hpp" #include "origincar_base/wall_kalman_filter.hpp" #include @@ -154,6 +158,10 @@ private: void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan); void Apply_Wall_Update(); void Print_Timing_Log_If_Due(); + void Sensor_Receive_Loop(); + void Control_Timer_Callback(); + void Tx_Timer_Callback(); + void Send_Command(const origincar_base_core::Command & command); bool Get_Sensor_Data(); unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode); @@ -215,6 +223,7 @@ private: rclcpp::TimerBase::SharedPtr test_timer; rclcpp::TimerBase::SharedPtr odom_timer; + rclcpp::TimerBase::SharedPtr tx_timer; rclcpp::TimerBase::SharedPtr imu_timer; rclcpp::TimerBase::SharedPtr voltage_timer; @@ -232,6 +241,10 @@ private: int wall_scan_stride_; std::string cmd_vel; int serial_baud_rate; + int cmd_watchdog_timeout_ms_; + int tx_period_ms_; + int serial_read_timeout_ms_; + int control_period_ms_; RECEIVE_DATA Receive_Data; SEND_DATA Send_Data; @@ -264,6 +277,14 @@ private: bool latest_scan_consumed_; bool has_latest_scan_timing_frame_; bool has_pending_odom_timing_frame_; + std::unique_ptr command_gate_; + origincar_base_core::SerialFrameParser serial_frame_parser_; + std::mutex serial_mutex_; + std::mutex sensor_data_mutex_; + std::thread sensor_thread_; + std::atomic sensor_thread_stop_{true}; + std::atomic sensor_sequence_{0}; + uint64_t last_processed_sensor_sequence_{0}; }; #endif //_ORIGINCAR_BASE_H_ diff --git a/src/origincar_base/include/origincar_base/serial_frame_parser.hpp b/src/origincar_base/include/origincar_base/serial_frame_parser.hpp new file mode 100644 index 0000000..c9fe1ad --- /dev/null +++ b/src/origincar_base/include/origincar_base/serial_frame_parser.hpp @@ -0,0 +1,56 @@ +#ifndef ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_ +#define ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_ + +#include +#include +#include +#include +#include + +namespace origincar_base_core +{ + +class SerialFrameParser +{ +public: + static constexpr std::size_t kFrameSize = 24; + + static constexpr uint8_t frameHeader() {return 0x7B;} + static constexpr uint8_t frameTail() {return 0x7D;} + + void append(const uint8_t * data, std::size_t size) + { + buffer_.insert(buffer_.end(), data, data + size); + } + + bool popFrame(std::array * frame) + { + while (!buffer_.empty()) { + const auto header = std::find(buffer_.begin(), buffer_.end(), frameHeader()); + if (header == buffer_.end()) { + buffer_.clear(); + return false; + } + buffer_.erase(buffer_.begin(), header); + if (buffer_.size() < kFrameSize) { + return false; + } + if (buffer_[kFrameSize - 1] != frameTail()) { + buffer_.erase(buffer_.begin()); + continue; + } + + std::copy_n(buffer_.begin(), kFrameSize, frame->begin()); + buffer_.erase(buffer_.begin(), buffer_.begin() + kFrameSize); + return true; + } + return false; + } + +private: + std::vector buffer_; +}; + +} // namespace origincar_base_core + +#endif // ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_ diff --git a/src/origincar_base/launch/__pycache__/base_serial.launch.cpython-310.pyc b/src/origincar_base/launch/__pycache__/base_serial.launch.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8fb17f8578bf2b9822371741e4925b568a7ca85f GIT binary patch literal 1245 zcmb7^&2Jnv6u@V^-j7apH@i*RK+8vI%g1uaAh-en6{!~p2RKDSmUo_a*6GZ6WP7sN z1y1yegoFeaZioXH{v9OWP{kn!B)&fiZFrvDCm7WUm1Ypm&2k3gn$&)I36~U=+IHo4hKL=^Mf% zT9#I-VmI-slNREjh5Z(G^9e$tYapM7-W{?DEo8f92QJ_^f;D6pddNPkBL^@ZhAreH@F?;MTtQxi$B>W16UZmw8uBUpuEUKl8efHJ_+elJ8}5_+2Hb?FKM3HN zj~i+DQHY^DyX)2~!?k6lLKHj~i*5|!Ahve?`mMKKa04T>|?`t(y zmQ6GsiAiw%F*kxul(MCkg>^jzDnC_IFS?CH0&3Is$9gTDzr2rp36*f#bzcpI*FB*sVow%F)65Au8V6Q6KS{i zjVfGJR^v>XDYKJzSN`jLjnxvwkuNyc_K7(VnOoy`a&{unLKVXGORbW<+%zXq46O zglT15np(w;(cZuPKki_;L0s3!vNE@qQ`_a-T}?$HeCe{) zIpFlcHgPAjhv*O*l4DB9L)st@C?)^UCi$DT$bH%-f6)&4lU^Wy(1`p_*T`?QOMaz2 z@(b;gpXoaJi4Mq*WJp31wtk?;WUGZE|Ir7nWBok3A2nTTx%)CD7gtBYlx(juqHm&I eZCuBH1|w9s&dWJgL?&MKrC|JTr5kj@ZsT7NBay-Y literal 0 HcmV?d00001 diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py index c8079ec..51448df 100644 --- a/src/origincar_base/launch/base_serial.launch.py +++ b/src/origincar_base/launch/base_serial.launch.py @@ -6,6 +6,10 @@ def generate_launch_description(): robot_parameters = [ {'usart_port_name': '/dev/ttyACM0', 'serial_baud_rate': 921600, + 'serial_read_timeout_ms': 20, + 'tx_period_ms': 20, + 'cmd_watchdog_timeout_ms': 150, + 'control_period_ms': 50, 'robot_frame_id': 'base_footprint', 'odom_frame_id': 'odom', 'combined_odom_topic': 'odom_combined', diff --git a/src/origincar_base/package.xml b/src/origincar_base/package.xml index 5e569f3..b55ea21 100644 --- a/src/origincar_base/package.xml +++ b/src/origincar_base/package.xml @@ -11,6 +11,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest tf2_geometry_msgs diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp index 985c265..9a5b980 100644 --- a/src/origincar_base/src/origincar_base.cpp +++ b/src/origincar_base/src/origincar_base.cpp @@ -1,4 +1,5 @@ #include "origincar_base/origincar_base.h" +#include "origincar_base/command_frame.hpp" #include "rclcpp/rclcpp.hpp" #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" #include "origincar_msg/msg/data.hpp" @@ -94,78 +95,26 @@ float origincar_base::Odom_Trans(uint8_t Data_High, uint8_t Data_Low) void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl) { - short transition; - std::cout << "linerx" << akm_ctl->drive.speed << std::endl; - std::cout << "angular" << akm_ctl->drive.steering_angle << std::endl; - - Send_Data.tx[0] = FRAME_HEADER; - Send_Data.tx[1] = 0; - Send_Data.tx[2] = 0; - - transition = 0; - transition = akm_ctl->drive.speed * 1000; - Send_Data.tx[4] = transition; - Send_Data.tx[3] = transition >> 8; - - transition = 0; - transition = akm_ctl->drive.steering_angle * 1000 / 2; - Send_Data.tx[8] = transition; - Send_Data.tx[7] = transition >> 8; - - Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK); - Send_Data.tx[10] = FRAME_TAIL; - - try - { - Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx)); - } - catch (serial::IOException &e) - { - RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port")); - } + origincar_base_core::Command command; + command.linear_x = akm_ctl->drive.speed; + command.steering_angle = akm_ctl->drive.steering_angle; + command.ackermann = true; + command_gate_->update(command, std::chrono::steady_clock::now()); } void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux) { -// RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z); - std::cout << "linerx" << twist_aux->linear.x << std::endl; - std::cout << "angular" << twist_aux->angular.z << std::endl; - short transition; - Send_Data.tx[0] = FRAME_HEADER; - Send_Data.tx[1] = 0; - Send_Data.tx[2] = 0; - - transition = 0; - transition = twist_aux->linear.x * 1000; - Send_Data.tx[4] = transition; - Send_Data.tx[3] = transition >> 8; - - transition = 0; - transition = twist_aux->linear.y * 1000; - Send_Data.tx[6] = transition; - Send_Data.tx[5] = transition >> 8; - - transition = 0; - transition = (twist_aux->angular.z) * 1000; - Send_Data.tx[8] = transition; - Send_Data.tx[7] = transition >> 8; - - Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK); - Send_Data.tx[10] = FRAME_TAIL; - - try - { - Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx)); - } - catch (serial::IOException &e) - { - RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port")); - } + origincar_base_core::Command command; + command.linear_x = twist_aux->linear.x; + command.linear_y = twist_aux->linear.y; + command.angular_z = twist_aux->angular.z; + command_gate_->update(command, std::chrono::steady_clock::now()); } void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch) { (void)sign_switch; + std::lock_guard lock(sensor_data_mutex_); if (sign_switch->data == -1) { memset(&Robot_Pos, 0, sizeof(Robot_Pos)); @@ -202,6 +151,7 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr void origincar_base::Publish_ImuSensor() { + std::lock_guard lock(sensor_data_mutex_); tf2::Quaternion q; q.setRPY(0.0, 0.0, Robot_Pos.Z); @@ -227,6 +177,7 @@ void origincar_base::Publish_ImuSensor() void origincar_base::Publish_Odom() { + std::lock_guard lock(sensor_data_mutex_); tf2::Quaternion q; q.setRPY(0, 0, Robot_Pos.Z); geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q); @@ -365,6 +316,7 @@ void origincar_base::Print_Timing_Log_If_Due() void origincar_base::Publish_Voltage() { + std::lock_guard lock(sensor_data_mutex_); std_msgs::msg::Float32 voltage_msgs; static float Count_Voltage_Pub = 0; @@ -378,6 +330,7 @@ void origincar_base::Publish_Voltage() void origincar_base::Publish_GyroDebug() { + std::lock_guard lock(sensor_data_mutex_); origincar_msg::msg::Data gyro_debug; gyro_debug.x = gyro_z_filtered_pre_bias_; gyro_debug.y = gyro_z_bias_model_; @@ -411,14 +364,37 @@ bool origincar_base::Get_Sensor_Data() { short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0; uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; - try - { - Stm32_Serial.read(Receive_Data_Pr, sizeof(Receive_Data_Pr)); - } - catch (const serial::SerialException &e) - { - return false; + std::array frame{}; + if (!serial_frame_parser_.popFrame(&frame)) { + try + { + std::string incoming; + { + std::lock_guard serial_lock(serial_mutex_); + const auto available = Stm32_Serial.available(); + if (available == 0) { + return false; + } + incoming = Stm32_Serial.read(std::min(available, 256)); + } + if (incoming.empty()) { + return false; + } + serial_frame_parser_.append( + reinterpret_cast(incoming.data()), incoming.size()); + } + catch (const std::exception &e) + { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Unable to read STM32 serial data: %s", e.what()); + return false; + } + if (!serial_frame_parser_.popFrame(&frame)) { + return false; + } } + std::copy(frame.begin(), frame.end(), Receive_Data_Pr); for (j = 0; j < 24; j++) { if (Receive_Data_Pr[j] == FRAME_HEADER) @@ -441,6 +417,7 @@ bool origincar_base::Get_Sensor_Data() return false; } + std::lock_guard sensor_lock(sensor_data_mutex_); Receive_Data.Frame_Header = Receive_Data.rx[0]; Receive_Data.Frame_Tail = Receive_Data.rx[23]; if (Receive_Data.Frame_Header == FRAME_HEADER) @@ -529,6 +506,7 @@ bool origincar_base::Get_Sensor_Data() transition_16 |= Receive_Data.rx[21]; Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001; + sensor_sequence_.fetch_add(1, std::memory_order_release); return true; } } @@ -539,31 +517,85 @@ bool origincar_base::Get_Sensor_Data() void origincar_base::Control() { - rclcpp::Time current_time, last_time; - current_time = rclcpp::Node::now(); - last_time = rclcpp::Node::now(); - while (rclcpp::ok()) - { - current_time = rclcpp::Node::now(); - Sampling_Time = (current_time - last_time).seconds(); - if (true == Get_Sensor_Data()) - { - rclcpp::spin_some(this->get_node_base_interface()); - if (wall_filter_) - { - wall_filter_->predict(1.03 * Robot_Vel.X, 1.01 * Robot_Vel.Y, Robot_Vel.Z, Sampling_Time); - Apply_Wall_Update(); - const auto fused_pose = wall_filter_->pose(); - Robot_Pos.X = static_cast(fused_pose.x); - Robot_Pos.Y = static_cast(fused_pose.y); - Robot_Pos.Z = static_cast(fused_pose.theta); - } - Publish_ImuSensor(); - Publish_GyroDebug(); - Publish_Voltage(); - Publish_Odom(); + sensor_thread_stop_.store(false, std::memory_order_release); + sensor_thread_ = std::thread(&origincar_base::Sensor_Receive_Loop, this); + rclcpp::spin(this->get_node_base_interface()); + sensor_thread_stop_.store(true, std::memory_order_release); + if (sensor_thread_.joinable()) { + sensor_thread_.join(); + } +} + +void origincar_base::Sensor_Receive_Loop() +{ + while (rclcpp::ok() && !sensor_thread_stop_.load(std::memory_order_acquire)) { + if (!Get_Sensor_Data()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - last_time = current_time; + } +} + +void origincar_base::Control_Timer_Callback() +{ + const auto sequence = sensor_sequence_.load(std::memory_order_acquire); + if (sequence == 0 || sequence == last_processed_sensor_sequence_) { + return; + } + + const auto current_time = rclcpp::Node::now(); + Sampling_Time = (current_time - _Last_Time).seconds(); + if (Sampling_Time <= 0.0f || Sampling_Time > 0.5f) { + Sampling_Time = static_cast(control_period_ms_) / 1000.0f; + } + _Last_Time = current_time; + + Vel_Pos_Data velocity; + { + std::lock_guard lock(sensor_data_mutex_); + velocity = Robot_Vel; + } + if (wall_filter_) + { + wall_filter_->predict( + 1.03 * velocity.X, 1.01 * velocity.Y, velocity.Z, Sampling_Time); + Apply_Wall_Update(); + const auto fused_pose = wall_filter_->pose(); + std::lock_guard lock(sensor_data_mutex_); + Robot_Pos.X = static_cast(fused_pose.x); + Robot_Pos.Y = static_cast(fused_pose.y); + Robot_Pos.Z = static_cast(fused_pose.theta); + } + + Publish_ImuSensor(); + Publish_GyroDebug(); + Publish_Voltage(); + Publish_Odom(); + last_processed_sensor_sequence_ = sequence; +} + +void origincar_base::Tx_Timer_Callback() +{ + if (!command_gate_) { + return; + } + Send_Command(command_gate_->commandAt(std::chrono::steady_clock::now())); +} + +void origincar_base::Send_Command(const origincar_base_core::Command & command) +{ + const auto frame = origincar_base_core::encodeCommandFrame(command); + try + { + std::lock_guard lock(serial_mutex_); + if (Stm32_Serial.isOpen()) { + Stm32_Serial.write(frame.data(), frame.size()); + } + } + catch (const std::exception &e) + { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, + "Unable to send STM32 command: %s", e.what()); } } @@ -612,6 +644,10 @@ origincar_base::origincar_base() this->declare_parameter("laser_y", 0.0); this->declare_parameter("laser_yaw", 0.0); this->declare_parameter("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha); + this->declare_parameter("cmd_watchdog_timeout_ms", 150); + this->declare_parameter("tx_period_ms", 20); + this->declare_parameter("serial_read_timeout_ms", 20); + this->declare_parameter("control_period_ms", 50); // Odom covariance parameters (tunable via YAML) this->declare_parameter("odom_pose_cov_x", 0.01); @@ -648,6 +684,16 @@ origincar_base::origincar_base() this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_); this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_); this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_); + this->get_parameter("cmd_watchdog_timeout_ms", cmd_watchdog_timeout_ms_); + this->get_parameter("tx_period_ms", tx_period_ms_); + this->get_parameter("serial_read_timeout_ms", serial_read_timeout_ms_); + this->get_parameter("control_period_ms", control_period_ms_); + cmd_watchdog_timeout_ms_ = std::max(1, cmd_watchdog_timeout_ms_); + tx_period_ms_ = std::max(1, tx_period_ms_); + serial_read_timeout_ms_ = std::max(1, serial_read_timeout_ms_); + control_period_ms_ = std::max(1, control_period_ms_); + command_gate_ = std::make_unique( + std::chrono::milliseconds(cmd_watchdog_timeout_ms_)); wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_); const auto initial_pose = ::origincar_wall::Pose2D{ @@ -691,7 +737,7 @@ origincar_base::origincar_base() { Stm32_Serial.setPort(usart_port_name); Stm32_Serial.setBaudrate(serial_baud_rate); - serial::Timeout _time = serial::Timeout::simpleTimeout(2000); + serial::Timeout _time = serial::Timeout::simpleTimeout(serial_read_timeout_ms_); Stm32_Serial.setTimeout(_time); Stm32_Serial.open(); } @@ -711,55 +757,27 @@ origincar_base::origincar_base() { RCLCPP_WARN(this->get_logger(), "scan_to_odom timing log file could not be opened: %s", scan_odom_timing_logger_.logPath().c_str()); } + _Last_Time = rclcpp::Node::now(); + tx_timer = create_wall_timer( + std::chrono::milliseconds(tx_period_ms_), + std::bind(&origincar_base::Tx_Timer_Callback, this)); + odom_timer = create_wall_timer( + std::chrono::milliseconds(control_period_ms_), + std::bind(&origincar_base::Control_Timer_Callback, this)); } void sigintHandler(int sig) { - sig = sig; - printf("OriginBot shutdown...\n"); - serial::Serial Stm32_Serial; - Stm32_Serial.setPort("/dev/ttyACM0"); - Stm32_Serial.setBaudrate(921600); - serial::Timeout _time = serial::Timeout::simpleTimeout(2000); - Stm32_Serial.setTimeout(_time); - Stm32_Serial.open(); - SEND_DATA Send_Data; - if (Stm32_Serial.isOpen()) - { - Send_Data.tx[0] = FRAME_HEADER; - Send_Data.tx[1] = 0; - Send_Data.tx[2] = 0; - - Send_Data.tx[4] = 0; - Send_Data.tx[3] = 0; - - Send_Data.tx[6] = 0; - Send_Data.tx[5] = 0; - - Send_Data.tx[7] = 0; - Send_Data.tx[8] = 0; - int check_sum = 0; - for (int k = 0; k < 9; k++) - { - check_sum = check_sum ^ Send_Data.tx[k]; - } - Send_Data.tx[9] = check_sum; - Send_Data.tx[10] = FRAME_TAIL; - - try - { - Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx)); - } - catch (serial::IOException &e) - { - } - } - // Shutdown ROS2 and release resources. + (void)sig; rclcpp::shutdown(); } origincar_base::~origincar_base() { + sensor_thread_stop_.store(true, std::memory_order_release); + if (sensor_thread_.joinable()) { + sensor_thread_.join(); + } + Send_Command(origincar_base_core::Command{}); RCLCPP_INFO(this->get_logger(), "Shutting down"); } - diff --git a/src/origincar_base/test/command_frame_test.cpp b/src/origincar_base/test/command_frame_test.cpp new file mode 100644 index 0000000..b12a8a5 --- /dev/null +++ b/src/origincar_base/test/command_frame_test.cpp @@ -0,0 +1,64 @@ +#include +#include + +#include + +#include "origincar_base/command_frame.hpp" + +using origincar_base_core::Command; +using origincar_base_core::encodeCommandFrame; + +namespace +{ +uint8_t checksum(const std::array & frame) +{ + uint8_t value = 0; + for (std::size_t index = 0; index < 9; ++index) { + value ^= frame[index]; + } + return value; +} +} // namespace + +TEST(CommandFrameTest, EncodesTwistCommandUsingExistingProtocol) +{ + const auto frame = encodeCommandFrame(Command{0.25, -0.1, 0.4}); + + EXPECT_EQ(frame[0], 0x7B); + EXPECT_EQ(frame[3], 0x00); + EXPECT_EQ(frame[4], 0xFA); + EXPECT_EQ(frame[5], 0xFF); + EXPECT_EQ(frame[6], 0x9C); + EXPECT_EQ(frame[7], 0x01); + EXPECT_EQ(frame[8], 0x90); + EXPECT_EQ(frame[9], checksum(frame)); + EXPECT_EQ(frame[10], 0x7D); +} + +TEST(CommandFrameTest, EncodesAckermannSteeringWithFirmwareScale) +{ + Command command; + command.linear_x = 0.25; + command.steering_angle = 0.4; + command.ackermann = true; + + const auto frame = encodeCommandFrame(command); + + EXPECT_EQ(frame[3], 0x00); + EXPECT_EQ(frame[4], 0xFA); + EXPECT_EQ(frame[5], 0x00); + EXPECT_EQ(frame[6], 0x00); + EXPECT_EQ(frame[7], 0x00); + EXPECT_EQ(frame[8], 0xC8); + EXPECT_EQ(frame[9], checksum(frame)); +} + +TEST(CommandFrameTest, ZeroCommandEncodesZeroPayload) +{ + const auto frame = encodeCommandFrame(Command{}); + + for (std::size_t index = 1; index <= 8; ++index) { + EXPECT_EQ(frame[index], 0x00); + } + EXPECT_EQ(frame[9], checksum(frame)); +} diff --git a/src/origincar_base/test/command_gate_test.cpp b/src/origincar_base/test/command_gate_test.cpp new file mode 100644 index 0000000..2b58d38 --- /dev/null +++ b/src/origincar_base/test/command_gate_test.cpp @@ -0,0 +1,42 @@ +#include + +#include + +#include "origincar_base/command_gate.hpp" + +using origincar_base_core::Command; +using origincar_base_core::CommandGate; + +TEST(CommandGateTest, ReturnsZeroBeforeFirstCommand) +{ + const auto t0 = std::chrono::steady_clock::time_point{}; + CommandGate gate(std::chrono::milliseconds(150)); + + EXPECT_EQ(gate.commandAt(t0), (Command{})); +} + +TEST(CommandGateTest, KeepsLatestCommandUntilWatchdogExpires) +{ + const auto t0 = std::chrono::steady_clock::time_point{}; + CommandGate gate(std::chrono::milliseconds(150)); + const Command command{0.25, 0.0, 0.4}; + + gate.update(command, t0); + + EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(149)), command); + EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(150)), (Command{})); +} + +TEST(CommandGateTest, NewCommandRefreshesWatchdog) +{ + const auto t0 = std::chrono::steady_clock::time_point{}; + CommandGate gate(std::chrono::milliseconds(150)); + const Command first{0.1, 0.0, 0.1}; + const Command second{0.3, 0.0, -0.2}; + + gate.update(first, t0); + gate.update(second, t0 + std::chrono::milliseconds(100)); + + EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(249)), second); + EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(250)), (Command{})); +} diff --git a/src/origincar_base/test/serial_frame_parser_test.cpp b/src/origincar_base/test/serial_frame_parser_test.cpp new file mode 100644 index 0000000..2c4ba2c --- /dev/null +++ b/src/origincar_base/test/serial_frame_parser_test.cpp @@ -0,0 +1,66 @@ +#include +#include +#include + +#include + +#include "origincar_base/serial_frame_parser.hpp" + +using origincar_base_core::SerialFrameParser; + +namespace +{ +std::array makeFrame(uint8_t seed) +{ + std::array frame{}; + frame[0] = 0x7B; + frame[23] = 0x7D; + for (std::size_t index = 1; index < 23; ++index) { + frame[index] = static_cast(seed + index); + } + return frame; +} +} // namespace + +TEST(SerialFrameParserTest, ReassemblesFrameAcrossPartialReads) +{ + SerialFrameParser parser; + const auto expected = makeFrame(10); + std::array actual{}; + + parser.append(expected.data(), 7); + EXPECT_FALSE(parser.popFrame(&actual)); + + parser.append(expected.data() + 7, expected.size() - 7); + ASSERT_TRUE(parser.popFrame(&actual)); + EXPECT_EQ(actual, expected); +} + +TEST(SerialFrameParserTest, DiscardsNoiseBeforeValidFrame) +{ + SerialFrameParser parser; + const auto expected = makeFrame(20); + const std::vector noise{0x00, 0x01, 0x7D, 0x55}; + std::array actual{}; + + parser.append(noise.data(), noise.size()); + parser.append(expected.data(), expected.size()); + + ASSERT_TRUE(parser.popFrame(&actual)); + EXPECT_EQ(actual, expected); +} + +TEST(SerialFrameParserTest, RejectsInvalidTailAndContinuesSearching) +{ + SerialFrameParser parser; + auto invalid = makeFrame(30); + invalid[23] = 0x00; + const auto expected = makeFrame(40); + std::array actual{}; + + parser.append(invalid.data(), invalid.size()); + parser.append(expected.data(), expected.size()); + + ASSERT_TRUE(parser.popFrame(&actual)); + EXPECT_EQ(actual, expected); +}