From 6d59baa30e034dffa50e2192ba5e551ccc306d02 Mon Sep 17 00:00:00 2001 From: sl-jetson Date: Fri, 20 Mar 2026 16:25:23 -0400 Subject: [PATCH] feat: End-to-end CAN integration tests (Issue #695) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add saltybot_can_e2e_test package with 64 tests covering the full Orin↔Mamba↔VESC CAN pipeline: drive commands, heartbeat timeout, e-stop escalation, mode switching, and FC_VESC status broadcasts. Tests run with plain pytest — no ROS2 or real CAN hardware required. Co-Authored-By: Claude Sonnet 4.6 --- .../src/saltybot_can_e2e_test/package.xml | 33 ++ .../resource/saltybot_can_e2e_test | 0 .../saltybot_can_e2e_test/__init__.py | 1 + .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 205 bytes .../__pycache__/can_mock.cpython-314.pyc | Bin 0 -> 9356 bytes .../__pycache__/protocol_defs.cpython-314.pyc | Bin 0 -> 13596 bytes .../saltybot_can_e2e_test/can_mock.py | 160 +++++++++ .../saltybot_can_e2e_test/protocol_defs.py | 314 +++++++++++++++++ .../src/saltybot_can_e2e_test/setup.cfg | 5 + .../src/saltybot_can_e2e_test/setup.py | 23 ++ .../conftest.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 3408 bytes ...drive_command.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 31776 bytes .../test_estop.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 33418 bytes ...esc_broadcast.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 34943 bytes ...tbeat_timeout.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 29111 bytes ...ode_switching.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 35720 bytes .../saltybot_can_e2e_test/test/conftest.py | 93 ++++++ .../test/test_drive_command.py | 193 +++++++++++ .../saltybot_can_e2e_test/test/test_estop.py | 264 +++++++++++++++ .../test/test_fc_vesc_broadcast.py | 315 ++++++++++++++++++ .../test/test_heartbeat_timeout.py | 238 +++++++++++++ .../test/test_mode_switching.py | 236 +++++++++++++ 22 files changed, 1875 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/resource/saltybot_can_e2e_test create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/can_mock.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/protocol_defs.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/can_mock.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/conftest.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_drive_command.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_estop.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_fc_vesc_broadcast.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_heartbeat_timeout.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_mode_switching.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py create mode 100644 jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml b/jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml new file mode 100644 index 0000000..a542bf8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml @@ -0,0 +1,33 @@ + + + + saltybot_can_e2e_test + 0.1.0 + + End-to-end CAN integration test suite for the SaltyBot Orin↔Mamba↔VESC full loop. + + Tests verify the complete CAN pipeline: drive commands, heartbeat timeout, + e-stop escalation, mode switching, and FC_VESC status broadcasts. + + No real hardware or a running ROS2 system is required. + Run with: python -m pytest test/ -v + + Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/695 + + sl-jetson + MIT + + + saltybot_can_bridge + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/resource/saltybot_can_e2e_test b/jetson/ros2_ws/src/saltybot_can_e2e_test/resource/saltybot_can_e2e_test new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py new file mode 100644 index 0000000..2d216a8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py @@ -0,0 +1 @@ +# saltybot_can_e2e_test — End-to-end CAN integration test helpers diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5dd04a3c4c8bc9db4c3fb70ca4bbda580b85e8d8 GIT binary patch literal 205 zcmdPqlZtIAuDt$G}i15SiM@2cGk;MatBKVH~SpE z)sT|msmae+G_4tGzL0)*s5q_s zZuT#g!?;&=kBll4B~zI!aHV8qt>LU;>1ViRWebL4>8524MWYAu#hjkk4a-!lDP1wO zysqT4MmDeI6q4tYT3S~$Qz=ecQw3uXZ`xogyD)4zr@iX*_dYBQdUMQC>2PtZ_zp z8w=4bT1jOxG-+>Q(Lt|86|rbYVs1mGZe%v*%`*dvmCfsT$Bxm0p(hn;v_IaI#4NWd z*-0f=C=@5O^t%I!mcyEK<7`PU>01*$OV$tWY~V(XPpH|vW}iG#22xj01Qwmg zM@Xx%kNKh{k#Vk=HD{QB{RqXR>Ka zRZV*D`dRy(;p3*x&0$lY7#=w=Y-%}cI;TwxPG)(YYcXE`Kp!>qfv91*us)~Vc-ul{klfgZ~-*3|l-k!}|;7E618PY<*`_zN$78#lA; zM6*F$(N5BDqK*4DyXQX63>U^3JMoH9l?dn&I`|nvjrg;q9q-+OFJvS-(RfWZG zCN26!YP_8W2&*d4A7^j3)NH-O65GBBQnRew#QMJUajfTlXYXJ4e?NY4&*cLP>t6w5 z;iXqUdg&|IzxMv;b6D8=JSV>i>(|Rta zs<$NGh7Km3-kW0Koc2MGb@W3A$Y+0wg)aF>KO7}13ndd16_RNPRhoC?i@yV_4pjzR zR3QxM6v#rD5>5qNWRX*=1sBl(xIvd2bh#nH4ZGZskcjY*7hU{cJ5q7r9+ytt~S z!%(!|1qGmhq>YRHAWwHeDgQ8?Wq=bL*PDy_b&5tH=oO8^fLlHQR@9ED-o^pD zT#d42p@O!2l;TJ`q|%Y9XKh)8R`AloR-g|ERbVQBIuUZ^=su!)K~K`p36A(6h#hi= zHfhH^z;=KyQFjUsT8k5S4WhGbVRi52riIR)%Z>N@lRrNA!-Icw_+sF4Y~h6sxJ2)- z>HA>ldqa2Eyo9It!s>Mw1MfFEXG1&g?KMIs;coJNPHE4CSko$tFXNO0Ry?&dFzEA^_1!M=X(U_an%Z;6Q&H@y@^myI9uO!@BWq`gdr= z%{BcqyHk3)z%$6|s>z%HBnPQHD+bZtakq8Lo!FM&G`7sCbLyX|3$f<8w=TVPFV;OD>%N}; zIJSPt&zia`o#^*Nmk!;FbtIf`NL4du8v4u zsz6{Q3~C>4zL-J5#2Eg3wPq%e+%J$^?1L~b><+y`SeJ-do^4`BA#e7BIKj2tiP&21 zys`i0@xMBP2uSYy>4n?j|9t&`U^{k3uRk6h%fb&c{!{oY9Q|Uv+1Ug(hjyv91 z1+u*tFzDDf)1s3sFeAU}R80ak`sMPLE6+akt~;IY&ZniW-L-oVb1Au8$$(k|$_faZ*WH zD88%HNWb~3n7l3T%@?g{9GGwcLbYkhUZfr%k5Z@E#1N;q7h;IfqYT<9bo0C&3T-$zS`@KZpe4_6s{XY-gN%Y-Kym=?~=4bcgZFBEkdhZdFBAqtG{kVQ^5`uiU z>s9s-?ISV&?FI?7PKRkiLWgONPtxI(Zx|=<_L(`>JHj!kl%K$Ez9%}Wbw9gi0;B{b$2;JcTv{Xbz|?)U z&bibYElmSUi!ENp%`7_!_v^G{(lOsL|9C(bAaE>jxb38C(r{e?PpW#YMrA4A>+bJG zlw_XU$;zG+hm(auCjDABCWC6Cs91UxHH7uBbEpWkfQo8PEwik zJu$Em zP;{cai%2&Sa=&~RNHXZa6Y2M|(gNQNo=M0o>I;ffU?$H~L2qHL2<$Ov*0>p?B*>ERz)iW>m> zX1?rtOHDBit!Pdaki|L`U^g$NBnHOA3#BttxFWs-&<>3v452vA4p5pcqB$-c6`hfb zq48y9XM0VJf-L(@>}?H1lfvYx2(;MuJZW zz$mdsJ~DWKPR90MKtAGkT+>HuBIGCHPbI2m43RrEUv%UbG!4GqYlqe znhH#!Q$=b10}o$BX;zN6n%Qw*$v4DGzI~{Hon60@I*m(j`~hQUph8{jAgYGrzER)7 z)r>{hI`k_d3tCCdO2{eb*X$QjxYt~TD^N8>D3_~f=cfuLzDw0r#PKZvH5R?9Z;qzX z*|=GPQ#q9;-5kM%r1oX%rA|y!eZBUB(uxw|;0Oe3)NrAv?}M4|&D>3l-I%%Fdv9>Z z{NRpXt(zY_a&6>0BR6K|6JtU-zpeg{?cUC*7m+CRVwkJ(k%auWts_n0e-47&mV9`= z-6RuDG>r7uYcDAQj({ybJM`kCkmE-{o*fEYgj6-^icsMOUx+P18DBlNI?J46b?j7q9O82NM* zJv8oMC{mS5qP|k)sWw$Ph*wrp75~hp-PO-zHJ{t)3u}gJ)dFZWW*HjYmyFaT&~lna zaMBD}r{uzpl42W$^BO)ZQPRQRB0^#AV*o9S&vn>T#YoGb^~njQ+1m*OIe z{@+K4HCJETc4Zrqlg3qcb32lg##Nu26p372JF-*0y?V{A7Iu4Q!>*>thb`^9Uh;pK zlt5QbRTOl48rqZJ=U;;m0)E2sXWPtQK&ompKJxV-H`3zrxXrkh8xtQOpJX#*WZTP{ zr-O)HrI{@*G>Xi%l%LQ!`C<7_l$9wO_`|gDM9$z%zf#52rGpY!d1B6M^BSnABvmt#Ki{ZeE~4M=5BI$@kMaNCesX-HE%-!EtpSI+J_@gK?W6Z zLRCAgUr4LgbP)wyx;v-k?4V#xnDnl?&u01s@!7041H^O%^9PVMDHFqN!?C%>OO2P~ zzmB(wNh5%!PiobwmgBTU*-O|Vg{@+zlC8Xt?)!FN0ui6Y9{6H%c&Q6@{D)M`U+P5J z^&!<>m)cMrdPpUrrA}n(WlGnVnvoPfq-=6&6M|lfX!@6;6y7+&%~BNJrA#hnX%%dS z`t(0+mgKgjpige6$9AzzVjjsDzd&;mNcB41b%EGHoDlTYiMhyZY(GBI*dg}=y$IOt z2K-N#0=Jx^qF5RELuYJKV>`Uv`PR6Te-n?SEaq_#V2va_2sBFpd{2=YKV{9IvWEX+ zy=Ab*rB4G*3-MMeoXqZDT5m~G`y&1`fz;dm zJEy9;sh3Kr5(Qib@BBXb_d5mQ->G3;9uryl!X*fC zVM>rAHGy%=~Q<3)N2aFOw1y%czm z@e;iZc!+V2UJks7@C$-o0lb)TmtG0HgmJfC1-z8;LcJPz8RH(k26#E+hxA(D6^tL# zHvz9CTrRB<5j|;S&ULTutlg;dCmNP%dgs;}g;sQ#YjU&XdxpqG|C`bNb0;AvY_(UolNB?PcG*0hQ6$x)hURLOx z>CwZYgpP(&nS|1u#!F(u@v){6N#YTkZ#~{EHS(Lok~J~MQ|S4P9&lg6Yqao<`O3rzar z&0k55!4IcLM(bjkx= zJ)HxR6ur7X8g>46Ozs`*fyi*te=USSIV}ezZCt@n8^3t#S1r+Kw5e$?-NF&;UtLku zRsTAB?muX&pyHDFPs85zq3(FXtvu!j++mnfu5@~5q zkhe!#+p$EX$4Sn1FSfx@FD$2AFVU;fmUenX6j=hMavPPaI>rODZhht<-?3K$7C>Ab ziX{_LyVOX}$4);#U>V4>&q{}AuKd6lESdO-@fpeb%cNqor;QctFgeZsR!KhHFEw`c ziUfqIvX0MxP)eYg%?~SF4=XC841X48*g?u z2)8|7p+6xTa!MlWR~0>@j@@=;J(x4b?Kf9%dgu0Ry7?@{hGtC}YVPc9=A|_iAKG^@ zsg7R3`riDCqHF1~W;Ly~#;$-*9X6P(^;cgsjkU?6=jwPg+kNpq?{ST%#ykp1`$>%V#W=hH7%AyE5hP&guqNH4o^eeGi;B8 zUHGtsu=rhU6;`?Nm1oXQ{ff|N$wMx2+ojT^HPY}dH3XO~W)@=(vEE~9IfyLxN+46h z*Tq6n{mg3%A!)X6AsC(WM%7~c@#Jh%MIs}pg#clf8pMqUx19&aM!+CWf7q7LO1f{o z)j*cedZ&R~_PD^y*Y~%C-+oU^=oyI7!eK7nwlS=udXwE+jPkU%O7l?k{@l8i#|lN4 z+KM(hRm&g(r=AT{IAi=vimnC&i`7>2mRZ+AXxr?$g<#8^x5XAHnG0QB{K$nKcM&25 z@I{MI*rWQ<$a*y8H8lY2wp*ngP1Z;Gh4v`zr@@xfzT4HjECAfZI)K})v)r-=3tqm+ zu6P|d3dFe%oX!o)U?jK=JETFZn-OcOptYHNH1x^dF3OqV@^*%1=DdeF_d&?OyfVtz zumbgGkTWCYvV&?wYBN%kaMPswXm5(7`0X3tzTtrsvVKihGsAk;J02f?6>_M>pLwd| zd_6Xz#8rJriRQ+a7fbhNV75fSe}mR!NBJb^wiJw98!1OAK4#WGps3noxJ*PZl~E4c7B=Aym%R zlDRoNyYt;w?lk;H`MW*y!2@&N1FKEuiFmx1GR4O;k0}h&;puzGxAz#HzUNjIx9mZJ zm#^>P=@-6T$J5=DjjMIfS+KRmZ8TEXaML&Lqg`q}MgV)?jhoYDiw!l<#90eQE&`q! zFgiVUVD{L1FV6?VbKbB$G0S0}i1hu&rtIWI>|uhJ zuO}-1Oc(lGtpZ~UVYweNDc)(exA5lM+H7)zH=Ips7~NRfi2!$M$8-|jsjzyQz`#>~ zSUrQYiTBRW2lvi-_uBKe)bqsG(}1;2%NFMLXRNodJL|iM0*kJaZXUJ1{v}Oy7XYQq zepjb!nc=OfCBA=fz_@>es7e1Clls43WUpa_)*|cJDCV}XIcGC>uo0=!ci0tRT5IR;POx$#emD%qM>*y zjueLLB?{SG{F3%~E62MSkai4!46oMZUnX#`qxy+z(Bs(u5s=5p<{WYUk+I*w=eF{J{5;UX;juXK5MKlTD3xF9V=p$b|Ow+FsiIn z5tMRz#wOAz5~5yX>Kh~~*MZH{yGb}CNqPFW$!rIb^fI=LCKp5I9WW2!KZF-9?wN5j7X(T(M52yq&G; zc_8qw?WTV%21@S*f^YR+FLvCXyB?apyb#zzr2hH9=9z(oz&2|1ToG#J z^qIy*i@3Fro5m%q4Dg$th%4sS)Mg))?l;^o#0$JSKFS_gWGwPlvy}fd3r);WW-vHN zkLQd3h-qjf=saI6uQGhGqT2Gsn$4Ck*4A0R2oEG*G(6CYKCFwj=44P9sn3A} z;Qz)4Cmb0u+|R3aKwD}j0ZOdYZUBwcYCB$$5!>K+BpCHL{&X17*E@1^OHy_w! zIT`MAq566Dy1CGn`M^^vK|gt!e=RSgJwFG$6F|O82@~5-JYif2c(!MGVG2IOvpn#k zY|!B;S&>S!+beWi`miHrT>Z-H2TkyoBNVpY8Bl11EHF%3*hEXzS+qk{^yITcNmU$zrvYf^gbQON^Hq5T!6TN@1P| zZ8glxDurFa60DZ1ZR0-KJLr;qgKpVB=#dKsy>ejCCl{WhAi>w5?}96Di*1fd7V~)W z3vMP)a{){_IQd(uv1kguTR$>QGHUj@DvX#&4(rmDq&_0Os!V8y?Mw)tU#$!%Vr4!s zYiGtA!43l%Zv@txe;HdvBj-U~FP%=|%MfQhW=ovhV&`&W_#CTQRvwWqrFBVdhgd#5 z`4z1UC)dr1vLWY-vOa!8*q8OBl(DQhNMWN#RnsbOTeAMl7&&#;%j5B@r_uMiU2CN3 zT|6r@-{Oz|c2cRn3 zmc>BHbk$;@`lfg0?CijM=jLnofOMmBTA4|_lbSDUSSksSn+BeUo2sWsC@%m!YS-+A z<>j2SikuVRCh8ryt#(fOG{w&OFD$6F9PYyZhjYTM$vM|>O_y94F|#KOS&&6IZXTtX z_K3mB5A$4Zos?=LhjS)Q5%WW{nd3%`57CY=z1r1Q+ zzU!~$eLa}9dWJv;fldOsd?;(wUGRec03Fwk4yynI+HJ2&-^0{DBf#Z?mw+MfV0Y^v zDs`7Na(hh8ncJI9W3NNbXZT)G&i6RwO5k}m!7D)4^1Oo|Xm{KGVdDK)=QefC7kAGE zy6^p<=MUFDyhhGcIX(1FEj+Ko1=o9as&BTQ9Is|3WV+ktnZCt9-OTyLK>2jvY}-QR zZt}?UvjYoN;iX6HioSD5d^}&Pclufx`5M5@9=@&iwIC$IzV^>7nm-v|!H+sNf$rV+{MRg#_j=Kio;CBlDNEpc)y`?*<$ zSj&0(Fdf5r@-wt-f%!@`29B(k-g$|q-to&0H4Y3Pir7o?pELF!IcF#{oPl>`W@?wj zvur0RAaICM;h|7;_@n)I_1`^nckkS$|yj*AV`j_P=U>zxrXoLtr(gLzQ1q6z7^s^5@nWEI`V|U9M`kM*Dt0agil@by!G+R>rAO>qC~QL6!ivm8 zd30ikgBQ8oiJfKH*nYP&t8#o5HN-^80cw(zc&XHn$~DPHd=`(AFE#MM9tfffpGyF4 zo?W8bt<0UXAN2g@+`{g|OkV9<=G43^Z*vcSD!zqCl zb#t5g=ZjCy1y0_>cir}1w9iz{dS~Uo4s7_9=tr>Awf5D`I7Dmjps5O%5P8>-v(+PR z*|iJw3ywoX>+rdm&vC$L9X=29Id<;r@OhcfQN~_}&&Pa@68AcMe&+LUINt*1E7*`P zz+k`NR$Rg3K4(kT1l1p$++pn6GF1}>Fep|?kWrpgV5DEFwi5v zWS$u^XcYZ>vW3(g&Ia}MWJN}ec^Ei%t(HOxY88O_I1WqWfmmP9@qug^B@!`{BKJOb zVgTPQ>tZn+K~E&vWS0`arAvOsJrFR4n&1?W9HPCfX^c;kb;!$FvuihP=g9|Cvxn z@2y)t&$J5`4ezL1jxlX0k2cJ-ghj)9>;6k28YdfJ*H_RV6#Nx5NIRy#z>Puj+ak|15c2 zF4up1A>WlQ*hKNa3AIZhp-!3;%IAfxi<`Gj6`S`WM;k2VHB*K2LiJ)<^;Cg*_dD9_ zbW(y&KChFeaF-V8Efp403Iq>TR8qGUi{%wlqT^28)zedTi<|4_1TfcqzR5%u2`K~R zz8{Bp?^ojd=7V}1 None: + self.arbitration_id = arbitration_id + self.data = bytearray(data) + self.is_extended_id = is_extended_id + self.timestamp = timestamp if timestamp is not None else time.monotonic() + + +class MockCANBus: + """ + Thread-safe mock CAN bus. + + Parameters + ---------- + loopback: bool + When True, every frame passed to send() is also placed in the recv + queue, simulating a loopback interface. + filters: list[dict] or None + Optional list of {"can_id": int, "can_mask": int} dicts. Only frames + matching at least one filter are returned by recv(). If None, all + frames are returned. + """ + + def __init__(self, loopback: bool = False, filters=None) -> None: + self._loopback = loopback + self._filters = filters + self._recv_q: queue.Queue = queue.Queue() + self._sent: List[_Message] = [] + self._sent_lock = threading.Lock() + self._shutdown = False + + # ------------------------------------------------------------------ + # python-can Bus interface (subset used by CanBridgeNode) + # ------------------------------------------------------------------ + + def send(self, msg, timeout: Optional[float] = None) -> None: + """Record an outbound frame. If loopback is enabled, also enqueue it.""" + if self._shutdown: + raise RuntimeError("MockCANBus is shut down") + # Normalise to _Message so callers can pass any object with the right attrs + with self._sent_lock: + self._sent.append(msg) + if self._loopback: + self._recv_q.put(msg) + + def recv(self, timeout: Optional[float] = None) -> Optional[_Message]: + """ + Return the next injected frame. Blocks up to timeout seconds. + Returns None if nothing arrives within the timeout. + """ + if self._shutdown: + return None + try: + return self._recv_q.get(block=True, timeout=timeout) + except queue.Empty: + return None + + def shutdown(self) -> None: + """Mark the bus as shut down; subsequent recv() returns None.""" + self._shutdown = True + + # ------------------------------------------------------------------ + # Test helpers + # ------------------------------------------------------------------ + + def inject( + self, + arbitration_id: int, + data: bytes, + is_extended_id: bool = False, + timestamp: Optional[float] = None, + ) -> None: + """ + Inject a frame into the receive queue as if it arrived from the bus. + + Parameters + ---------- + arbitration_id: CAN arbitration ID + data: frame payload bytes + is_extended_id: True for 29-bit extended frames (VESC style) + timestamp: optional monotonic timestamp; defaults to time.monotonic() + """ + msg = _Message( + arbitration_id=arbitration_id, + data=data, + is_extended_id=is_extended_id, + timestamp=timestamp if timestamp is not None else time.monotonic(), + ) + self._recv_q.put(msg) + + def get_sent_frames(self) -> List[_Message]: + """Return a snapshot of all frames sent through this bus.""" + with self._sent_lock: + return list(self._sent) + + def get_sent_frames_by_id(self, arbitration_id: int) -> List[_Message]: + """Return only sent frames whose arbitration_id matches.""" + with self._sent_lock: + return [f for f in self._sent if f.arbitration_id == arbitration_id] + + def reset(self) -> None: + """Clear all sent frames and drain the receive queue.""" + with self._sent_lock: + self._sent.clear() + while not self._recv_q.empty(): + try: + self._recv_q.get_nowait() + except queue.Empty: + break + self._shutdown = False + + def pending_recv(self) -> int: + """Return the number of frames waiting in the receive queue.""" + return self._recv_q.qsize() + + # ------------------------------------------------------------------ + # Context manager support + # ------------------------------------------------------------------ + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.shutdown() + return False diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py new file mode 100644 index 0000000..41bdd6c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py @@ -0,0 +1,314 @@ +#!/usr/bin/env python3 +""" +protocol_defs.py — CAN message ID constants and frame builders/parsers for the +Orin↔Mamba↔VESC integration test suite. + +All IDs and payload formats are derived from: + include/orin_can.h — Orin↔FC (Mamba) protocol + include/vesc_can.h — VESC CAN protocol + saltybot_can_bridge/mamba_protocol.py — existing bridge constants + +CAN IDs used in tests +--------------------- +Orin → FC (Mamba) commands (standard 11-bit, matching orin_can.h): + ORIN_CMD_HEARTBEAT 0x300 + ORIN_CMD_DRIVE 0x301 int16 speed (−1000..+1000), int16 steer (−1000..+1000) + ORIN_CMD_MODE 0x302 uint8 mode byte + ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR) + +FC (Mamba) → Orin telemetry (standard 11-bit, matching orin_can.h): + FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t) + FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t) + FC_IMU 0x402 8 bytes + FC_BARO 0x403 8 bytes + +Mamba ↔ VESC internal commands (matching mamba_protocol.py): + MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian + MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop) + MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop + +VESC STATUS (extended 29-bit, matching vesc_can.h): + arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id = (9 << 8) | node_id + Payload: int32 RPM (BE), int16 current×10 (BE), int16 duty×1000 (BE) +""" + +import struct +from typing import Tuple + +# --------------------------------------------------------------------------- +# Orin → FC (Mamba) command IDs (from orin_can.h) +# --------------------------------------------------------------------------- + +ORIN_CMD_HEARTBEAT: int = 0x300 +ORIN_CMD_DRIVE: int = 0x301 +ORIN_CMD_MODE: int = 0x302 +ORIN_CMD_ESTOP: int = 0x303 + +# --------------------------------------------------------------------------- +# FC (Mamba) → Orin telemetry IDs (from orin_can.h) +# --------------------------------------------------------------------------- + +FC_STATUS: int = 0x400 +FC_VESC: int = 0x401 +FC_IMU: int = 0x402 +FC_BARO: int = 0x403 + +# --------------------------------------------------------------------------- +# Mamba → VESC internal command IDs (from mamba_protocol.py) +# --------------------------------------------------------------------------- + +MAMBA_CMD_VELOCITY: int = 0x100 +MAMBA_CMD_MODE: int = 0x101 +MAMBA_CMD_ESTOP: int = 0x102 + +MAMBA_TELEM_IMU: int = 0x200 +MAMBA_TELEM_BATTERY: int = 0x201 +VESC_TELEM_STATE: int = 0x300 + +# --------------------------------------------------------------------------- +# Mode constants +# --------------------------------------------------------------------------- + +MODE_IDLE: int = 0 +MODE_DRIVE: int = 1 +MODE_ESTOP: int = 2 + +# --------------------------------------------------------------------------- +# VESC protocol constants (from vesc_can.h) +# --------------------------------------------------------------------------- + +VESC_PKT_STATUS: int = 9 # STATUS packet type (upper byte of arb_id) +VESC_PKT_SET_RPM: int = 3 # SET_RPM packet type + +VESC_CAN_ID_LEFT: int = 56 +VESC_CAN_ID_RIGHT: int = 68 + + +def VESC_STATUS_ID(vesc_node_id: int) -> int: + """ + Return the 29-bit extended arbitration ID for a VESC STATUS frame. + + Formula (from vesc_can.h): arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id + = (9 << 8) | vesc_node_id + """ + return (VESC_PKT_STATUS << 8) | vesc_node_id + + +def VESC_SET_RPM_ID(vesc_node_id: int) -> int: + """ + Return the 29-bit extended arbitration ID for a VESC SET_RPM command. + + Formula: arb_id = (VESC_PKT_SET_RPM << 8) | vesc_node_id + = (3 << 8) | vesc_node_id + """ + return (VESC_PKT_SET_RPM << 8) | vesc_node_id + + +# --------------------------------------------------------------------------- +# Frame builders — Orin → FC +# --------------------------------------------------------------------------- + +def build_heartbeat(seq: int = 0) -> bytes: + """Build a HEARTBEAT payload: uint32 sequence counter (big-endian, 4 bytes).""" + return struct.pack(">I", seq & 0xFFFFFFFF) + + +def build_drive_cmd(speed: int, steer: int) -> bytes: + """ + Build an ORIN_CMD_DRIVE payload. + + Parameters + ---------- + speed: int, −1000..+1000 (mapped directly to int16) + steer: int, −1000..+1000 + """ + return struct.pack(">hh", int(speed), int(steer)) + + +def build_mode_cmd(mode: int) -> bytes: + """Build an ORIN_CMD_MODE payload (1 byte).""" + return struct.pack(">B", mode & 0xFF) + + +def build_estop_cmd(action: int = 1) -> bytes: + """Build an ORIN_CMD_ESTOP payload. action=1 → ESTOP, 0 → CLEAR.""" + return struct.pack(">B", action & 0xFF) + + +# --------------------------------------------------------------------------- +# Frame builders — Mamba velocity commands (mamba_protocol.py encoding) +# --------------------------------------------------------------------------- + +def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: + """ + Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). + + Matches encode_velocity_cmd() in mamba_protocol.py. + """ + return struct.pack(">ff", float(left_mps), float(right_mps)) + + +# --------------------------------------------------------------------------- +# Frame builders — FC → Orin telemetry +# --------------------------------------------------------------------------- + +def build_fc_status( + pitch_x10: int = 0, + motor_cmd: int = 0, + vbat_mv: int = 24000, + balance_state: int = 1, + flags: int = 0, +) -> bytes: + """ + Build an FC_STATUS (0x400) payload. + + Layout (orin_can_fc_status_t, 8 bytes, big-endian): + int16 pitch_x10 + int16 motor_cmd + uint16 vbat_mv + uint8 balance_state + uint8 flags [bit0=estop_active, bit1=armed] + """ + return struct.pack( + ">hhHBB", + int(pitch_x10), + int(motor_cmd), + int(vbat_mv) & 0xFFFF, + int(balance_state) & 0xFF, + int(flags) & 0xFF, + ) + + +def build_fc_vesc( + left_rpm_x10: int = 0, + right_rpm_x10: int = 0, + left_current_x10: int = 0, + right_current_x10: int = 0, +) -> bytes: + """ + Build an FC_VESC (0x401) payload. + + Layout (orin_can_fc_vesc_t, 8 bytes, big-endian): + int16 left_rpm_x10 + int16 right_rpm_x10 + int16 left_current_x10 + int16 right_current_x10 + + RPM values are RPM / 10 (e.g. 3000 RPM → 300). + Current values are A × 10 (e.g. 5.5 A → 55). + """ + return struct.pack( + ">hhhh", + int(left_rpm_x10), + int(right_rpm_x10), + int(left_current_x10), + int(right_current_x10), + ) + + +def build_vesc_status( + rpm: int = 0, + current_x10: int = 0, + duty_x1000: int = 0, +) -> bytes: + """ + Build a VESC STATUS (packet type 9) payload. + + Layout (from vesc_can.h / VESC FW 6.x, big-endian): + int32 rpm + int16 current × 10 + int16 duty × 1000 + Total: 8 bytes. + """ + return struct.pack( + ">ihh", + int(rpm), + int(current_x10), + int(duty_x1000), + ) + + +# --------------------------------------------------------------------------- +# Frame parsers +# --------------------------------------------------------------------------- + +def parse_fc_status(data: bytes): + """ + Parse an FC_STATUS (0x400) payload. + + Returns + ------- + dict with keys: pitch_x10, motor_cmd, vbat_mv, balance_state, flags, + estop_active (bool), armed (bool) + """ + if len(data) < 8: + raise ValueError(f"FC_STATUS needs 8 bytes, got {len(data)}") + pitch_x10, motor_cmd, vbat_mv, balance_state, flags = struct.unpack( + ">hhHBB", data[:8] + ) + return { + "pitch_x10": pitch_x10, + "motor_cmd": motor_cmd, + "vbat_mv": vbat_mv, + "balance_state": balance_state, + "flags": flags, + "estop_active": bool(flags & 0x01), + "armed": bool(flags & 0x02), + } + + +def parse_fc_vesc(data: bytes): + """ + Parse an FC_VESC (0x401) payload. + + Returns + ------- + dict with keys: left_rpm_x10, right_rpm_x10, left_current_x10, + right_current_x10, left_rpm (float), right_rpm (float) + """ + if len(data) < 8: + raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}") + left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack( + ">hhhh", data[:8] + ) + return { + "left_rpm_x10": left_rpm_x10, + "right_rpm_x10": right_rpm_x10, + "left_current_x10": left_cur_x10, + "right_current_x10": right_cur_x10, + "left_rpm": left_rpm_x10 * 10.0, + "right_rpm": right_rpm_x10 * 10.0, + } + + +def parse_vesc_status(data: bytes): + """ + Parse a VESC STATUS (packet type 9) payload. + + Returns + ------- + dict with keys: rpm, current_x10, duty_x1000, current (float), duty (float) + """ + if len(data) < 8: + raise ValueError(f"VESC STATUS needs 8 bytes, got {len(data)}") + rpm, current_x10, duty_x1000 = struct.unpack(">ihh", data[:8]) + return { + "rpm": rpm, + "current_x10": current_x10, + "duty_x1000": duty_x1000, + "current": current_x10 / 10.0, + "duty": duty_x1000 / 1000.0, + } + + +def parse_velocity_cmd(data: bytes) -> Tuple[float, float]: + """ + Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). + + Returns + ------- + (left_mps, right_mps) + """ + if len(data) < 8: + raise ValueError(f"MAMBA_CMD_VELOCITY needs 8 bytes, got {len(data)}") + return struct.unpack(">ff", data[:8]) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg new file mode 100644 index 0000000..bb51679 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_can_e2e_test + +[install] +install_scripts=$base/lib/saltybot_can_e2e_test diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py new file mode 100644 index 0000000..eff1956 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py @@ -0,0 +1,23 @@ +from setuptools import setup + +package_name = "saltybot_can_e2e_test" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-jetson", + maintainer_email="sl-jetson@saltylab.local", + description="End-to-end CAN integration tests for Orin↔Mamba↔VESC full loop", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/conftest.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/conftest.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e45363ab1bd32f5b3dbf2a43819960afcfc88c26 GIT binary patch literal 3408 zcmcgu&2JmW6`x%$$<>myBPlm)3CuJhQNu>jBq%C7HYvc7gtbc`fR)8^cPOsC z+}+O3T48&r>=t%?l2Q~%dL6R2e z0y`gX-kUe`dtdXzQzZ?-_x4}@<~)UwB_{nZpAL@yBqOwgbi~kmNS9V6CdYlIu0-+@ zs%KelDOt(0e7vGEbyM9q-QQ@?Z_EwG{n7u-d8WV9px-QBmXg_vJu7XNHuK}o_7jk> zzmL#4G=6;p)?WPHLz|_`fR6Z<(AV5#DOn$GzlL)hI;H*%bh)pIO>It%KjVLShpGO~ zsU4@;G}tk-db&QtW@4JMI$NJ(bM<*P5BHqjs0`mG8J|abR?q!7#a+*zL6=h|w(zYa z)FFwp-%4U)Na7`3T1I6p+6ASW<$EngLRJa7__Liq;GipJxaGXhB1%Hs@+oF*f_M6yrT`UAc zOd}5mu-4xnM;7U0;%gA2fg0T9n3I3NIS z40n3eigp{YZbqR%&5W4eF_~ilYF!+*BZh5%+ruUpjACM1?O3>?-Po77>iT}rG_8AU z`!W@A##JF91Yk7-TZZCxn7>>7c}Qqj4N0@QvRaMdcFpGNEr)itO-ibF2@8F%O8xMX zu?>9GN|;_WepQ^jI^;jR*a0KO4g%`vB&d4@{!m2qzX0AZi>NU75$S0^*tvG7l^(7h zD*4AZAKZMj_U~h1jfcxGOaDI;MUOz`kXs2i>|=;K!jDBbgv5hjpk@a`0l)zV4t@NK zh(yGWLxU3QenkUBTalZ@3V`BxTbTNhXTQ#<6W}IkoA(_*qIlZ@HwQDCurB~-$MFI- zrz)Ifjfw(8i_j#dqNiZUr}~FF0ajFt9|^8eVC|l^{1ss76fll}Dx{Z%-~SN?iQmVS z2-T$k)#X5{X9Bsd1etm^Q0ln={QizGW&G)~o;it&oUSx-dbWY|Ttm|H4Ov$k8GWLm z=vpJI7w!nviB$2M#EVl=$LussBcML>E#Jk& zv%p$a8xRGfiC{6Ct}qwi+YdhBm?EqoRlVyF*M?PWI~I#Ag71-T_=bk#xWnluj6Wu1 zfh*Bx+$S!MU1Tu#Er)dtt7BhuNsGOPsk7B)W&GKr-{Yo8s{`Q^|4N0$JQfs#u@c}` zelV;rCd4vgWmG1?0*)A3$r*AR>_XX0j zxubcMRnvSN=0MTImD0+jRQ?{%8;0#$hQZZlLV`-BnP z+cK!{GoFJpfFiEk^&O8Fj1)3h=ecBWzBppi{)?p|o-ZOBBsFq*JX1fn7&d8z1FQ0U z`YeiT8x=V%HqAadh9OGKr@jR-9J%DD^i80Nnj03NJ<3Ru^tqx)%JU+Uq~D_7XrH2q zBNdeC!POX&Py}L Z86;mg5~HKp@34qd$uDFW(Ki5;{{XWMTR;E+ literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_drive_command.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_drive_command.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..aada60964e020905f13ac09c2047e14d861fc7c8 GIT binary patch literal 31776 zcmeHwdvF}bncpn13k(*IT|D_F2M#{vritc_BoI7+Ef`6HC+ zD_$w7IKQuZx@UH=6c%SuXUm$}{pRbQufOT(>7Mys-Lt)})~{fTz5Q>PkBUHJ+MaEZ6CB+bx*qzs-JoqP35=ZrWJ`9iOccGSZZ=I znvC_NuV_EH`WTI%-|)@%zs*lFM-;apB5A zzownkhr*|{vB{YBlW)JFosLe9Ml~Zo7RQ-P?Q(n~HI}fhNG2DI=wzHxPNv3Q(88yl z9?(oHYE79aWTsO|GY+1_>9K(bJ34BlqOq~4X@N)5iYAgK5CdnhG=2EAi5m4!OxP7S zrjpu=3F~5N$^yISgmy7%#9oXVaSd05YE31RiR8GZKlSuZ&Aeh-@yQ;)ZkIWzrLUl5 zQtO^13|GmR_7-or?#uo&q1s%->HgCv`Xd9UPolC?gHdpV?(T5%%Je#XySx=wsKl{wnpvyIk zP9-K{5z)R8w12La=Uo^>wmFuo7bNCbgg0@nAssc$ctI(iXO{|<z4!hTkgNQ0r3`w~pzVr1BSziDRWGml=h;x-J>RtqP@PVSRT9p%Fg#% z8WlyocO1x7 zN#;Een^4iw`SF`$@0h9fXvmp=a*Rvd7Nc^yccK#%RLgPu55xma7`YVk&Gu(7#5u>%!Ym8i0+}`O|Ke*8p5DnOpTn38FVUxZlXb~4KF~b zB3C{-Wf~2Xf!?67Lb=Lx^a`FkF?K4G9(K4Nnx^r3R=aM5ae%nsZ$1uiRrw#@10U42 z&UR+&_FjGbzSryP{73<~Tch~YR|dZ|`0Ck(hK||sY;DhixA%^>lIMM==ZC%TJeqAg zlC3?u=sm_Ra}50CCP%_fWaW*7Bl#l%s}iF^)qNNh6}jM9Jnc@BH1Jr; zz#}p?Cc86O(x^wbtGTzgimqZroKFekNv?Z#QB4HX@%< z&LB#cpZ8oE2z&PrD$1p29Qeb$F(k*A3xE@j07&=v6O{wP;Y{kl-_UL%? z>AqFf6+lL)U-Y(geijsDp zrKzCVkicr6#mAUmv~jO5udmm*N){hC!LVAR z==D3DQQD9>i772~H?)JyE0@ssvp9Cu`l&STy?-eAJXtCnBS~jp&kk*}*X+`4B*d&8 z+@4ZkKJiC2$Ce=M3mP|}keNdoLTmxG$NwSkZ+DO7%F|bNpqiPEga34;)_R04`Q!RLHhq2I<3$boaz3W=e_W%J z=5H6f>G`=T_Mx|bsN!+uxjiGnVpmaHX;AZ+7X_+H)KM$@Ek_LLv$2r zUJ!_@peU(zGhSdHA|{7f>Ht#pOrXy-kH;;N@-4dsBcoSHJtMK$I0lgOALkaMNYsFu z0~K>3V#L!1EN3vT#B((`93g9r1?yF=f-9l9wGk7BDj0EyG%`Ms8jVgcl`B^jF{2ma zlnrj&(h=cJKWUEV>iXGTfVu9JVWf;)qacaDl%9wtnIX?0AvotFh129}-Q&mz)fuEa z=W0kHVulaWcKRu+nySdpvD2N{<4)}630|Xp?!*BDGQ47(E?~9aU3wpH2VS~wM_y{D zNwPZAs-Rf$jjLDN$Sz|WE>V$(9h4~$iPWOQ5zJ>>B~4bxdV>EJ)Dv_uc1j{t zeLHWS7|4`F!0Fm`QcP{nPMd14_`5&^4gdt?ekBDF|o z<_Fx|fzNhG`mB2DM%S&WTjTGZ%XHc&k_R~gK3uDJYu)weiA?JrhK}?ZXx(rBy|nH| zrgb-cp>^YK*&8%UX&79kyd}&1x@+Bq8uq1`VVAxwS~^|j(#fT)n)Z%JeZ0q$_Pc1L z26gG3a5b|t{aD-~=+@&U>DJBR8tB$7=f0GuTi2{gYA;Q`cqbqgU1F zwJvS3xSlEXU1;IbXE0oxR<`^{0*|J7-)Rp{kj}F zPzmS|PQ|qaAhx!!Gi3IV%f|n*OUvj(E+-_+ri5O^)e2}PpbZ6?3Sm4$S4?kv-h5%g z&JlQ)z?TU;N8l?2$U$m+mB4ud?F6WuxRWBlgm~j?lrc#lN??@07=ajpIDlLD?sF%0 zxf2JtAkN#mug}0k5gG!i1Gc2TD=)Q&d1CSINO&|0p&lY^Xi~0C9}cm+98<&kw`7NSc3g*pCroS$hN{W@uT2&r&*h=fAf4sJ-*c3IoI3? z_X_A|E*XSIqazWt1wiAdqVt;0^iZl^Mmnj<*za=BJ|lbR~?UAB@(36hpAGc)fKI2LZ?S=ooqLR-3A_mPL`36JAn8vk)gZ~gU5&fSnF=T_jI zM~XGuO7Jd~W~G)k+DB~8vC^#6vK-#>mAbm(o5=CPl;ypNR`$X;?|grkT}RsK)t|Z- z2G-*Tcwov)@wuecu&UR%JvHen*r9L2uR=b?lcI&kuawjWHio^B{5GBYo=fsuS(40qWkPM6`&`R}Rv=a2Tm0GDGua$gy zE)OHUpPSCymT-el>P4il+^DwhlA*MTh9G&SHG*!SLY`v zP2)-?IdbLyi%>WktSHCf3)iNE;B>T;N@+*VpRn!u17X zuVEEquR(AYdwcBB`19jmi+>&d!E}JdV`$Sq_RFBD<#CS)8A@Dc@ls5Hx5f9#*dbWz zDGC#jnTYDs5;8Ed$KN@GSWZN6vLMKbR4Q$I7D%F}a2yJqge;z#Bv%BB$&z7~rii@k z3Hi@n1XGggw6Tlvu@``Yr>G!T6}XVdSg>jsJ^n-&UcWcJxf&MDXQU@1FTvbf%Om3pZf9`3Z*V_ZeQXF80L0`_s8OVfahfT=OYw{XOcNJOQ_k-}j9ljFwY*c%ic zMjFdLgWNpRd8!JO0pk*Kp{Z1${ZJohD=nm1KqsKvL^Y%&91ct;!Qlo1ZRV0`r$|$FV7tM3oSd|V!LLApY zm^MOf5NdQ3k(?j#qasY|6{3w2xD3E`4c<-i90+@J6|Bd^+}g+vYF`i+}# zukjL{;R0p5wUs_&8d)YqZxtvhen@rxaCXS+TQp_DVeW;ZLYj zTH5cd+jwK(&ZaH5cK*y)-r}F}-&K{W<|TE*oVwu$kIz20pze4-xN#}iIu~rc(Vhvm zW`jL5zB?PWjC%C?$xJ|7QjhXqRy{fcsoqXW#FaPiP%KhuNrR%9w!(rq3aa<&H~(O2 z_L)CtJr-c zRr09bT&)}pXAz8DJY4V%C`Zd27p)IHT(D5hq6c2Vh7hSXrqH$LDRf_i3rYvXBX*0G zsC>$DpQYnMSM{nQDrd8}*)EZu$4kSF$^ z5cs!6l*$YItNsN7|2kOU-`0Y_zlULLmo64@owCPlC{SvOIu0BcjVRBY6JEF{@YLYj zr)E--4h>ny(o8C{9O_p4(88ZqV-|RD;GZU1&DrBA7hr*ZL08~kJw5Ej2A9L~z`xSR zk5q8QJ%nOokDt82zg;2a(wD=&JVn_%;`DxLih`nCEfnQT|3ciYlEYp*!hR^q+wv6U zvWMbodRV=`@geknX@C;#S>iKYmP7AvTvhLHay@yAj{dNtn6GRcc#vZ5cD8AvM>~7; z{uUPex5m}`*XH&9b>U!vT3dhf3ny_kL3AjX&#A(hcX8OV5UVGz~7@c(yU! z6mFDv1M6qe*XSGdrtq4AySFynFiiSr!4_UC?hu>=PP_lGq&Dq%wgqh(3RgYb>}u6I z{g-#7u3NEHn?-vRv}(P+N#DH8)V1ybt-4Nd!SqBYOrWEn@sWh2h5j!bHN8rnyJ5Y) zMc2aX3-08GRgA+8q8?&zkHaQNat-)5;!mcwmUOf3hwJ;%sV7h=zbgiJN6(%v?I$yD zV4oL$8%tcExXYxKz}#UnktLGLqG4r_?fb_H{NXH;((!}mS(V@xwjJJ&p)TBp#6l*A zC`t9>5wfU}@g>T7lE7&KuMr?ClW_)MTBShRZqQ8^MA8bg6SGYjGelcyn~tVcW?mwr zl1ce_<|n5`NSX#0~Q9$ld zaH*%mWU@NSY+x|S3G){sZtd7x@~6YRL{qf5=bSXxQatC5)0~9L0)rLvt7CFyI*s{% zxjGRxJDN&Oq|%{{jx~$lNM^wzvr~av-3fJN)+gpLH!c!gg20;um~9HraD`c;m>rB6 zwv6AWJb0gMqm}J--(mb7Ww9V!ZYkSud|eXRe)zq4cCP&9rMd_c%Y0U$$Bf`K1Qx*@HwP5{uaKLOk1EfY3IpS~Nr8 z)t8pkt@G;E*?uIipPKD|3x!~tvwt&|L?z|RcY#7Gs+&bEb}>iTAD8r@djX?#*|RLhg-+D@g9W^rDbwZFf6`__J*|C@^Oa>Afag3CXMW zT-YUd`J9CZ9uSZyB}Qm0&KZhpAr+$L1#*x|0Tf}iKqjej$s`Wv;+7?;JBc$uCZQla z3TU{DR`M5ZCEV>*w^AKzrNHN}mGmmeYQgEqZ`5a0fVse2;yJQp`od9E3HptBdt_n@#XbIq){lEZGh zW>EM<)o2ZfGQBL%sUz2_krz^A%>=9gAzw7ke2E9H0l}R*)ce_1>~(}!MzPr_i2rlF z2#UUH#C4H^VXpz_{1>Jl*qx&Ak?nC=VkWR)ENRX{}t>!d59- zSEqGl9l7~?g*rOrP&bNoT!~&rAR_v3C0c3nEnEXauUcU>2Z!@N_-YQWSTE@+K#XY( zTg8N4=F3+xacW;gph6FBW^T!rJhx;yOn8pKv2w3yWri*1OhtReh$cFv=z+3#Y$zyS zyaz8U&zvMaL~yO9uBG*0*}AfhrS)K04t1;hta`Ak@3)CIcJ{c((x9yeyPN~cL+$tI z6MPFx(e&58r`A7&o+?{+l|5CKLr<;$rRk|Ma`Ch4srKxoale?JluRYNry`m~Ltd74T;)ip|BlmH2=&Kx7h z7+O?Q*=RYl_vzkYVYpP<#DZ*SGc)(7)KExpbz2^n(Y}JSn)~pIEu5x*M}>WC{hYdf zLEV@)JLrMa_kX#pk?%d&8c7qHcE5HdqaM7zn}Io+&JNor{T9@NGgo9vB4nqtDdyFK zu9VCMC$c#@L?X55a0K(~4oy})m}#@=WkHUB8P#&nYttF^5SbhS<|t+n6Q=05pdOl; zmMMu)y=;nk^^hwiv%!h1i4KuSEjk>*oMIepnyh*#(`Mr`lOs5XTe+(&s@(k*_3H=g zb$>>O34EJ+VgSHijj+yl_^txL&p;FIv!l_Hd{X!w zE^6s3qB=TfDypD3LWiii6cxDO?UXMvx%>R0+!oEoq-#%X%=z+TWuGZ(u!mV?htD zTUD=snt;V2ox4@ESB#K3if!VW#KqXg52t+b9=xnPQ_m`}1m36LgJtW=I+oUhWjWL> z@Hy$hq6w6+T!!G+zRv|8LZ6eZD;nI6xLaXdIClkoF8KNAbC52Iz0;+HAf6!U8rM5r zPQ(1Qf2Y?!gx)Dzca^|uP9c+SA|5UuxL0pjRj(-Ro!<5fzSEbL zM=xnWl;ld!JQ+id3C%0f%C^^6Atmb7ze4TRykZ;3zJ=+}E45cpJY5SG8*0$kV2S94 zg7}6;XQ3itrN&{O zyBNKkNKF|BwZsM3&0K4vvWd=^oQ!}s?dYBzSSQu&^5>iU5+>OKtsYG~hwn?!%w%FN zzb+9=(1&5Lj%la(3=B;hHp~zVe}9e+U!rRM$Dg2@4-`?&-KHyi93^7URc2oXFe7vY zpVg-)44PzNXau$b^Zs1gwxda==xt`gKgK`gP1%_3PL>L$|P? z?J=zSbtv0?3>en<%5)!_3mjWg?cZ&YDRU%3t9{v2^D5Z#)S`Nf*8ZXc3n)u#e+iu8 zf9(Ps0WGceljX?wZOsKe#{YUiF*+HO>z)k@7B=>hI zlfZR4P!C`m+1LAyd+sU(`MZE=aeg2N2eU>#_i*ue%(CF+OM>_$#SS0*~eG97Ws)9 zXg({^%Al8mg^c{nKJG7&`m0A8#hrC=ZW4LU9<#WlT}xcGE%lSMUx~=(>*w?@#t3h* zVq?r6Bekm<&!vqK=|5TJ7?I^rH_UKH@Ybh4M&#NMtE`(Whq~2$e#QvXhwG@glgU*jPwj}vwr zAm^Fj+6-Bg*sMPmu#!K;57Y0MqH`waG5Ck=bWkJsoqZhxQ}B4$36R3IrTCOSd&-_` zZ7IxW3{`V&h^a8FJ|!wWh@$@*RhHG59!AD~0{@=Ce;~m06{gpWQ$@a6Pb_3gB49Oh zNv1&LO37>xUE42HAQGuX*LhKg>y{*PBRE1=@p8u7fvb2qTfGU`te1hg>W(F^{af&M zWU4pGq(rFRHpx7(kSU3P71i4za;0Q8h^`%wDR2_0Mb~*zhgq*AawDK(!d}Q5Dl`5N zH;w_|M+pCO0N}KvPJ-?3kC2`P!a%4kR~d=KQe*h?q$<9>!d6?xCqenwT6*~GOMFPB z$B4gZU`9!J%}9wO}f z1Sk}R3DY_6WYl;eSB=%y!~=*81}3RKcJeG2z}XUMbnBRpdE-hbA!-n0E9 z1>pX{famZ>3c&r24eWcl1n(YJ%G6gjd~3s!XYHJ4ZDw8fPZi&evU`fJto)-IpJ(g6 MT3|mVj8*pk1OFjjLI3~& literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_estop.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_estop.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1d81f0f442927250eb0c1c27208b1235ecae1ec4 GIT binary patch literal 33418 zcmeHwe{dYvec$fk4p$$1@R2&k7R>YUZ~#v_ z+=06bQbg=A>$)*i(Xr|{4y7sy+&DEcW6#K$No%#v)K1%sr_Ce)0TReY9Wibaji)mm zQcw(6{iFSS-`jn=cefw_MpXPmJ9poG`+o1+?|a{$Z+AyUxnIID@$ElJRc)1|1YKB% z*9^=DWJ$U%MI>FiAVuU8vhHE$qHgbqXS3Ap(~CFrloGv!r6{_Bv*$!iSAsZ2#BFAArHJ#3xDbmg zLtH?_)v>s8#8rs6dKMQ%T&0L>h{z36~MJaTIk2uzZU9hqN`9M zJ2Vu}^oLRzJvpex^;9+!qRd*@@9)T7Nvhg@e<;)x>Ne_C(U|OS}2~-Q&*CX3KHpL zTn(wzX@C$44aL);eigYwS=7$@%JLsisY6%eYO+a7^(RB@E<;yR@zC*(*x7SO&pmrK zG?0!DqGc`ApVX5H9jzy_s)`W0>_43iMb4bv70Oa;Xrklj>CmOP+D}y?=JH4i-3|LA zBbm_ElzwS{X!x3b3B%MhL?JZC2CO-Eh?}nXPfy5(|5P^d0xCQFG%D=4Q)fE6jG%qZ#@whl6FG63mEqT=;6x zZYCZlGBp+%tCN`o)tj5oj1()uFlRN5^^Jhf6rW;0SWKh(>aQEmH9xCiA~kEti_J$*G;5f5*V6HeO#>#%H$R`$ zwQQzY&1$=1SCLOm@Jtu8dMpvo#FD#`F~UrSp1C+`1Y)sxCX>Y@PR3%!DjKe^GpYrB z(CPtxc3oPKihNBE2)wG@r_;n1cKTs8i=0O;DPb+cbU_1&zcPOqf$P%w$_&M_qmRSiV@Kob!;mRHH3hn#A3h+j4}qD-!s2t_e+JYOLLn(HlsZ8R`tf4{l9f)%zLAB zZqv@Y$`fh>QX7j@&>u(jryJ$tjS5wpi4W0G`eQL*`$(F?805<%@wAx|jK%QiG(DZl zBr{ngm&9WI*+eX+Hd4=O@Z|A0Qi)hx*VWX;5j;9%TIIlvL^7R@#U^BRGYV)ldPmq@ zsWsFm0%1z11^C&Qa3_D|eR9Er=fxf%dt~e;Sk448iNhN5`7uQ0OuVP;iI+d^bRUcH zvWW;9m#(BKh^bl3VnE>#Q^H~t76bZ*80FKYEY8p3K;P1pZDvPwe{#0f#eJ$)ii`lJ( z22?zEw3cNJY9qtL=@e*;z4nLwISUP8It}-H2;gB0O|y(4@Tl}KjiQSqno7?b5u0%0 z2*B3~jsbO)ZBxN;<;-cCjaoH;Ij1+%^xgbw4?8i;NQileTxL&|`KNG^JBuS&9y8|b zm83qJV(dVyg-TefssG1m`g&vNKPDV|4lz#cRRtvsA5E4d?rIyZRBDK=e&%fD6Jx#G zUGZ7V?r054Vb)gHVy14aAM?&A>))!`H0J$HKffpHr`1<6`ZS7*jef*KV?R;E$G%ui z>LY6A^u)slU)_TmJMibzk5Gx2*f2$O$K34EpWrP8ldyj1ZueP%;=rlsQt9Y0z<-EwNW_VkQ$CMW&%)89Vx z{hsNaAD^!6o>5NZrk$B-I`qB%>86h9+RnR5myM^7FmY6xmFf|Ihptv^!kk1dd&2m9 zCoo|M-aN<8c1Mtr9kgt)3G+SzAM%8G2zq^((Cw8Q&lf)q%VW=jhp+2wTP^GQNNt8w(d?M>2b)J?!D!b1uhu z==wTCx#V1aOz%8CUHkF7O1FCH)ceLEUjY#kTux)dpjcp0K-P7dg?p<&G^Rf&(Fp!zhb0y}5;2GZHMzBKMekY9TefRWP@Uk`AX;5XXuv-5YK z<>&9|Jf7bLEfv-w7XND87l|CbR*K_HEzrRQ5u&P(mn@vG5(~7sbX$w+hf5SV|1i3X zE!g?2SSXuGUkjxMZ0-%RjHyQm3zVnvbwzo42zl!@ID5u}5c;am0BDp+$PIp4>|;y= zVQeVGe>;_71p%zqWx?69jrdKLZTy=+)mt@-F52DGEx$Hh`^g#QQ-#yOT(a*fCkm$l zJs|4ld121V~^~n+Y@$=p@ih;0%DF46CGW(yys!DUO6hwUxjw0H**(5{ycKv9N?b zOaOb}@%fa63UJvZeQjRw6H9oXv4jhiMCa}^YPV29>jqyf?pI006$>7W?tBqKKcx`6 z8&-V+HH*Vq4CkpgIZ|7{CAHN`P?$hf0tE?FC3-Q`B#_8-g{eu3jp}n4ONPzeq}4mc zlzKTrKpCpW0}agoDd@*_sn2RtiV9*l?1_4MkthG4;h+P{hTKa;ChzDIT&%Ug3>WnY zE;d>uV!apS%jcrLz2BCk%LCSO*d-6{CCyA9ab@i24Ok)j4DD-Gv=}n!>hoQ@J#Ai| z5`p@tkC28Py@G4CVkBs7t=>Qeeh$-@put+ZO@9d%Mvv1+x>&;j}n$iv

W)mv?x-&(@|G%$V_mYth%i8RjlFaYkdt?mT0M!GVG0%F}z2P9K1i6 z_oUqMzJ}rbnmoJ@tO%EuhxheX->setoLU~<7nBEX1%UVEE9&d=@Lr6r=;QM6UX(-M z%71R~z9L#0kRridVy*lT@!!T+zxCXIdkHT;lJVq6zY5xWUIHM$Y@MIpMd)KHu?&V=^&EB%MLq9?Rv+wir$jT)8OdwTG} zrT;nh$3Oeq!$wj3qGrhH(XwYCuX3AfXtW0QP@VVIQJEpDqk#xhiZk16XhMG9bNJAN zJlaB)?{3umzitLXhgf?pjoLPfX>HUR{htdR=CMyTYCrjZIAFk(4>ro9&C4r&kX4Do zja7_MWt_B8Gnmw4kd1V6Xkr(y!F;0+aRm&DQLhs@XHM zw`aQMN{@`@*&`aJi znQox<l3#+CYw7aH|4j;v!OAr3IIBZLnya)G9-K4l4C@!m7+zJLT(%OI%G*9g5ec?Tq` zVvA$6&XBAWNL5`C1vI*Jw2>>dNrN3CQ-8>!#uwFWygv~KX@(OHygIlOht&Wn8Fu48 zp8=?c;^Dea&#>%7zg{Prnq{qy*!gU$W>}~?_ChuEqIBK2Svm>rQK_^E?q&^A zdr?Z}8m%G+IvPI6Y2e|o zsN+StVZ!R4;CAmzp4I-p+TZoYCvJ4TQujMs|JBx)Prd%^H+#kpPH$|v8)*5DJO0l+ zDM7xlyIme_AitqQe5i?~7|4#mijc~Lh+>YV!csrN1rTR7g+P)X%QjJGEx- z_cu+i=^iV4;Ng#Iz6fVLx6;-d@UB64Uh-7(h@VpEJxY3KbIa7`mis;g@0GPI(0PI4 zAF#yjEfZT=910-s+Lo!cJns#Sny9Hn;IV_->06k>-(A+5eHQ>k509?!ol5^#*X zJDVXCZcqc-Dt`C8VpNyG>In8FQn6&cW!616`*K(vXOGB{DDQwmI+%|eZkVy(164?+ zLwUCGI zcD20N(n=0D%s+U0JqL@Rfzg&M5z}5-ml;#Elr2|8AP6* zr?o`pOw$;G>PrB`Gwd6!;CzYIlTjs9cPK>tcz;z6N74%Qh2e!3qgCyv8U_dq0+9aW z5a~a9NP_{*1lNBckjJ>V@HF6M!sN&6{~T#X4sp%MJ}@|y`HT#)oc!(?Ba^1fhcYr; zt-urv#Pv)B^Y|L|t5n4m0#^wTd#7F_FiPMCfiD9%xtzVui#=Rx!Zi)8Ejd@aG*WLk zGz_F^;GEC~2PZ@>^H9~mH5hKpH0DO@QoK+>kHWF2W7fHq6%Uo2uRS{pRV~ zp0NPQ=8)4vlggIyqYO+1x6CT$Z${ZN84QU_5g-xQ%yL`V;F|3+2u=ofu=sfvzxVK)#S0YuQ+B!a@YL4B_v!k^2+PI5+wF(H`$=akFDqgv zYqqd_FvX)ONAz~rXtM)(xo=&y8)Ub@`^=i1tJrX}?^gYtjZL!~cTM59q5X|tn+zOg zJ-3VTd)RS!YKJ3*-9NWm9|0zuZY*`Ry#G4V)UOa2C$PAI{X8WTph>O~nPmp{cRgjW zsK2{j@wF~gE54`iO8~rC%1lZ%ahQAyck$0Lmw(C4TyE|s@M4AZ8LI^T_nLcK+^yv2 z!18{u%q|H|bij)4=`G=5Cxk6qwj6etrJeLHd52BtEMfplJ2B|&ux+}--U|}5I1*D* zXlY+U-w_EghPgb4VTS#>PgtOPUCFm~hx^bQ)6#CYe%k_LI_X1uANCWAfaCttc?=BG>DzqEXj|0W9c5X9LRaJP`sY`>f0j%^}qFIX;}=7`0L5eKM!fw2tkCW$$vdRAFKrL3P( zHr#`uqWq=uR}wdO&nTN0Ggc5u>yXFmtTRj&M7TO+qE8+2+pWi@T8|Nd?vTem&r^uX zBIV$*H;)sc>yU}QA!7TnsqM#zE_TSIK%p~-Z|^(y=DOU3_m-~XmHms@5GtSysPG8Q_VT4y!JwU4xs|m=L~<~>U@g;e9$D0 z9N+Rx0yfbT%VmK{z+M7f9m66*lM(UeEP=F0uY`Qm<&gnb@=-{t1R3GZHFj@Z$w*;& zkdtDl`Ijgc72^|8mdtCDlYEl^`p6GQ9YP~urIEXxt4RQ|Dog?-qxwgWXEQeU~?>)^nWLd30exm_M&2o(fG|SY_12_!^ z>K71UL>{`PGUfG1hTnXD&Vp*S z!}fLR#HvRkO81K;NVyNy`M2lm{M&`jzx|ztwyB0T(%84(D{CYDemiO5+ezo&{!X}U zVt~bw&cFSg4Q*2!cwVmE=ULy`(KfM<)nLYw&cEG`6*_;jQjr}AT<6d0A)SBwJ9TYS zb&h^Gr8~t{y89MA?e7p+T(J8CN+$3n{59H;VhVNv9|-J*3Lkb;0Pw~rUjlz|2$^md zqH<)qZ@I~IE-ug&0zcSxnjEo?s6`RkF!p0!KC)QnadUol&^*~qtaGG$+_v@=h<#;Z zOhlk??T6S$3Ltja0tUoB?qKaM_6aK3E2x`Q2d&%@H#-T5ebrX3URTOy-JwJ5vs-ef zWrAicBKFzkU4<8$23h5W*te3tabxScPq;%n`V+En!9FkIU@gQx+aRh4%2Iz z;)8L#rDyQ9SLyi4QhK@_1$sIX2AL<5!%I5EHYz*RbAwGC8~)dipIZMk^YrDexW5fl z8x1<_m1JX7e+RM5OP6W%)IX%lO91eZKJX9=qvc|a_h^}()Qx0QK36~H0$XDr)xDVZ&-P1JU#EP(OW^ki{64_qmc$%?W!j<+KW+XZFLVEF zIG5#iss{N!^4$ElsI?cE*2&GUiaeI_0)!c5?aI}IlS+hqnE{wDb7WSb-;5F=UuN@4 z1jxK-#@$vTjw=z3n#gftJEXR@yDQoB6CQ8|J)`$cwV0QrA`dW)Xp&I|RQLKgf!6hha=oNC56|<-uM;m2ruFI9@b&=!jmIUAHY^l&3DQGL_UG4I& zl4D_c&{)1vb|rny9~IZW+M_G_$a&W#`e>I!-^j4IRDN5ID_%11qH^3K{%-56(RbAB zDiydI;vVMj<{~Y^-%aqp%l3D(N5>sE7xQ;p$@AuNmEDr-^X?M{%0=WVySzJY7L>>H zCx5pk%Guj~AuTSISMZkLE0@YEa0QaxQh67Za~vf@7B1OmSARDb%ycXEEbi{cy|q4#v0z?q>Tgl#GXUJR&D=>gMzJpuV60^|cWq-;3s*bg z;MSskopLgEv!6l(1O^G*B=8zQ zdFPB;zSmca!%?h*SieHvuAs=jr#5#igT1@=3*#~dYqW3~>ymAsu`b!^G}a|^uQ58C zZev}{F5M-+y}NU2cPH7UyJXXItV^;zBxeV+%9IAM#Rp}shyo<>80w)#M{LT z(?~hc`R2ad1hWo1!E_?)!n=!D?ambDN9LBoUCCThxHp;AK3Srzd*R)ftEeA8_=e|B z!|vIJy;JzrANbzh$v`K+A7|gmJdLugol~tjsbshBk_*{0VuzTj{Z7?yz$Vx3?1v~P7&<|uKQJSLPyt(`F8a7U#c4r#lfhf+-oz7JhB8R(kIVDV zQXI^<3xNe8E{S>>nN=Ka<@ugkN}gMJsm-uC>JM4iX`i987b;n6^x#%b3d<6h>Z8n4 zD>%A`D|-a#G(EP2MV@bjxAZ;YJ=WZn+$hDHknut`?0e&eWx7vJJTD+Ly8aYx3M^3R z&Ft@r7qW4HJs6xyxSf2Kyi%B~~gD2&&YdnQW z7*aZWe#rc@PBJoxh+REXyLyPs*jagjw~zMxkojlX3FM!(L>;ep@zMo#SP5o7?8?WP zhxbWFNm03-fctXR#c!718t?C1#kO1PCdwzoo~b=KsYJ3* za$2Y!!2?>%$@-6wrv5tu|A9aYfmQ;$0Gw|zdxIhge2WqYr%hXYxv$~fO@1E;``S|9 zNxZ3yAUDZV_5}Xo5GMJ(h{`d^|K;K)c^KyL>ULehyU9u;#SQcd_Zrd?TCev%i?{oC z(SDO0`#jAxM)dz4lh8@@FR%|iT<~0gUSJP##W?VqGTH4lW%fQ_ypbGiC>_j^x`ci8GeS)LVn?qa7FGAjE6Vw>bzg`D6*!z+FSr zW>&Hf=RvN$uv@CMTd+b{u7$6fAaY!LVV85&>Dc8!+^~nv)3yesbWV- zLCUTEv9S;YCF0*{CrFX9XmK8yDsk+7vit999gT_?f;q?(?RYxZ2O`f8$F97hULmzx zsgh)sw9dmyRLN!KhD#&mmnWkB9_ezYHNJyiTNVb?M!mp*wdY@RU_gaqtlXc9JyyXb z^9mh7-V-TvlyrY0mDU|c^7o^ajod&D6y9|J~8^w{AW``BWZf^z4a*wvUAaE z=^yd|lx_HcLdzk(IgdN@HHQ7sq?#JI#(X8usMz>#zAs;+Z>fl{q(DW14}kC&>XY;y z_NRUTjmLz~D1~ca;!^BI>@q85@1kMO-z8Tr#=*c{G0O2?o_HpaWZyMN(tCCBC6;%% zJ@3t@@q(ceJvEffj_9YkGog<$hRpYVbc6WP34aj*d*>0p+LFX4VERXMU!2s=U!2s= zUebAvZ9`-Xq?EoUlboeJ0vr;AL&nV_^rc#Oi7+&1-l@;-m zDp3NPA;r5Fwo!ymwUwCLDb*iQ)D;56Sn}{ z|HNF?n)k|zSNq3G?pH{qHM7bSQ_2&spBTS7qcq>E+%Q{NH&t18t6{RTZn`o&=9}Aq zU57_*cCsCYM`jiJO)E#nMtIN(9dWnq9CxX-$cCbm^=3695&`6TYgOo*HRB`S8vVxT zbltA$RlCQE=Yln_WbOpn?!xw4)x;a(G#P9njXDM=fi>yVmtn;up>V@;aLx%ykG2i86v>2>_(8~;RawRl6?(dleqeEJm!1<_y z6>#qMa%Tl>ZGzL4-#xFbO~_@=l!$^iq!wxe$O)Gnwl>OL`{Jq%ur`-pVei{3A8ixs zEc=8O{~p&N>?&@tcWTj!>|CqE6SDeW&;|9s68JL$X914eYrFd2D3UY+r&TcBlKK|_ z69Kk@sGU-=lSnOb6IYT~&9>J5$v(R>Hg;94p&g;mY=q^=48D->Tst z7@k$k-?S3O62mX=IKnfOTjX*Ekn)fT{Vr4MJ0`T}S71V0YSv%635`XsEN{^ZJmRAF zOFKqv(F=~Y*oz+HdPCs8TMxxmd@Yf$dW@~IdF-%i&Le9@^LYRi{6DFT^?wZF8`sao>(cr$46?-`A>c5{2ts;BIPdXiRJTq zZb}@J-(wb_=P_Cpxj2Q57a!pi773`k1ZRv<3g(UyHeh`;N`<3TnLA36RgcJS)UwrS z?Jwo(RQ1sqEkh1nH*x~##lLqfB(ui$qwEbZsoaxU-m@&$3lRFPhQurQ~qx zXMJF~{u8RSc{cS2bom?s=IiBfsDQ+x6Lb?)r- zm$-zYakZsLgtoBB(j?~ce@;E0x=iVno0ezLhEoH$cQJ+DT4rDtqY_i;eX;md?XbZ- z`U`Az`D#3oe*rCOALlM#RdsV~*3GS1zqIOZ`2y|>cY<)@ZX8cAFcoZ^Rm|VC(l|-Z z@pO5|5uTykB9}98TWKUll2^HXT$@z3kHgCzU@EwMRxy9m%63*SUEXnoXOI!T@^(fM zMhexNx-H*ecf$khs{PeK{2Ti2sX_s=5JbQ zVj2UwyyFPZP)3o<8Mv)9QI#JUrw8Fux(6obB`*yy&v)I@CZe@wDd7)~R@5%cT0B@fPG=7YIOw3TMj}|Zo5My18 zf94=;zCJOV39HGgDqgH$l!_>$#EfGaDx+A_)sch_4Ln}Nr|HHf_5twxZ+nLm_|P|f zmp~=cwb9_737;|3vx#gv)}I{E_~#eA$j-k}T1+9*!Lm>Cs-+YnIZ`E`IyFFmH2hq( zM%%}kLd~epX^LBp!b9<)i}9G%9RI`~DJ9v5Q2FO3+vxHi#dl%MAviwEqpO^o&RQ`8T`+_GZdlsZz5c0~N z2NJ--8lQ}Bm;)?SJ}KiXb^r@~a!7vqfdsH{T;3?-B?tfuN8~Lsd2tY2=#}@$PtHpK HtZV-t-#w|a literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_fc_vesc_broadcast.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_fc_vesc_broadcast.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c4fdac55de2d05272b60ccd1c87903e215f06086 GIT binary patch literal 34943 zcmeHw3sfB0nO-&hplBMJw_XSeAt721K%fUgLT@zE$S_O^HS=KTZ9xlR1Km_N(g?p~ zd+cHC*^Dw9+wx{Mi_h-H$j5e=6MLi0vG*LGU$rjLW4v z?z$Y3bWu{Jgw!jkj^mC*7P~u-xyG_8rDN`dr;^8HC$d?LoRHZ)Cy~SM-h>zTtmC=I zdK!=1=&K6(kCd6($Og6(x#}6(@?1l_W}zl_pA$l_koKtw^k3_^#tC z5x45t>cr|wsZ^?xRQC!=^=!}LWlXG53)O6d$Sb6wf+0tuoZ;jk#4B*tvano)`9xR+ z3(G@Tz6h&iVSa=ah%iNUR7-)Pv44rG@FpVhMCeRk=v*Y;7dov)!~K2Xc%m+LLHXIm zf2JHg916CbJgkTaB|Orvu%MG&2fI$4R0c;9kpV577>tf6lvljY>pdJj7t!J^UPalO zh8uY{5gF-^^ebny@Nh&q6V;TV$e9GksXcx~*|9UAB(%YS0VH4iY?j}lXyPnSRZG}Z z93(iKP-=HJ2hyI_7acx5ID)4Qju?5d92UP5iXTyGKeeqvIW`_(IDKQ978yx|o<&%^ zFFZ6jGN25H6MbhRaV4fj6VbltkP?qY`n=wbsG>f0vQfdyXp!)ca`<3}ayG2>pATyh zgp7_2YLWgruR1oOoF7b_ZBb$u5@%6aTZhSo8e`Q~fB$yadd}N%#gX#1NBcg8%nyyl z1Kw1&{uZeMYK#!VL&uMVKGJryD^+L>QI9`ztSgnzf_RGoFXiX{wyu!c*-imGm#(&t zw6%u_tu0k}dTem0pLdIRBAghDr}B7^-dR$4v9K187=0&I$i1u|@dSY^O-7ea74}E^ zqWuvwuSnX^)VsZO_bDKUF*u)*5)KT|tlrHDXPFemFip6EQjuKrzI*#rg9cF#Bo_9^E?Ksjm z)+vXmR^#ZlMwhe`2tVb-qczGQkd^XaEMt76@RCjKiHQDh2n&_?qX`7{!|D<4C8YiBMm7Bot|kglK@UQF8yGlJbQ@;gOLjS~wC4rK;?jyUvQT1LcdC0Q~Zz zbWh55SKT4-wMW_QF2bcgqj!==uO|8K8F(&A-322Q%C25i*@#h{ARzFGl*~ufYht<# zpXvanJI&`2zARL>^A>rJb_B9ht`KS*RZMecNQ({A?3Bt6u|6jJg*@6afu)Lc6l<85 zqs1eUkyNf8Pad?TD;^m-qZQ$)to>?gv{JZJaws%7GKf}9t-VjZ@@Q74JCWj|v{16* z#hit;6%(#GdG$g?CEeF7RIQ_X#lpI3x>qi&M)-^Vg*D2A>$<#BTZyD;os;T=bCPgpx08thNG`ooEE%A-XRW7p2N3VNqyFRhhP=n8;+6tfP1^^{`wx;qlUmFnAcd-W8%T}`k+{syAGk%rv(_uZAz zLom1;z3*)ieMORD&K}_lW^LEqF;~!~I<~AvTK($vZV`P7soX(lkMK7?fx)_#25Wbx z+4GP?mNf_THBm0r37_bP?KQ}au}}0cmRq@=o9K@=xn&z=W4Y06&#s`n-J_f!lmikX-= z5N!mMm#9u;k2#^5EX4V1k%r2UvjODX)m&3kqbLd=Z^hs-b(%NE>-0HMzWg4h>VksUac0G9alhNmKvZZfoPq|H`5%xdGZLmnMDB>yQt%$ zDDvI!!f{cu3_z(<3@VOF5FJ_!m9$VXIMPDJKrlm9X_uZu3{!rm7-}X)XP)UcTs`@Q zE9fxzcMKl~QQ<4DlrN1G=!F(Ws4p@!6bfB&aPmUa z`vFc}{FDoU&G?T~SO4YnsF6Ea*>3q>5jc_C#E#r60*PS+=H4cB>0eO4W-rW=rjbgT zuh;+sH9$soTwD)g_@!o<5jl^KmZNDP%?C6E)(xOYfaF&K-i#x_hY3*wtN4 zM0c0&k6}sCY0b^R3aVME)4&qstcCOJN;r*#3EE{YdTEIE=^c-hUw-+K*}U}^kKW9$oIL#6Q*-%GTzq7~mHqsqpLulR+*IP? zqt{*Ad5t59xTt06y4wtpwwxO@mUAnl1Q-)QH!bE89u>}KzHf25Ob^U(MZtqsAKdz(SN!IUMI$?$H#q=)G6k8BSa#Z(Ar=& zmb?|+!KSS3dbw4Mz@7pFtCQM3XuwL4TSwJmRCkMA*_Q=hekIZvkZL3bz7zKqLAY20JppjY)-S?*Y_nm78B zpsyK&bo7i-!vnvYQN#9qpfcz|A5gl#Z|MX1miMx+=Uk&Ss((pbpMjgvCV7^W_H9y- z`KCTeEo^ie`Kd)|Ema)M%V??mhiIvMBUj_jTPl`|)*3=NO44dPTP+>^QP8)=!ehn! zJsXUs3;K;*Vg+hhtT0#*ENsT8j}--rMEm^Rvf8IAScLXjM>$wp$fK@!s8NbOmNwdK zM`_P`qx?qcjk5Nm$ET&Y?G=mDW7MutS6bTHHeE2=c;3VGCUw=4`4)(LS1*ZMXyDen z(?)x-x<)Mz7H9OVl85M7B}UH1UGG`hdmJ$z{{8qbz<*J!P<4YYRgVpz9lScFPg|YhcH&D#d=VwSGF}^g?sBN$jg;1 zleGW*x4%Z$AMJ-cI6E;o9Epx4I#SNqg@%-yu@~fS8s7|=v>`JTYf>eA2J2-0AWOO5 zE%5Gxz%HUClnu493p)aF%43H{)cY1mDagBx!1j0Ztji7M-E5_*RcUHy$jD+R&thjv zHqfccFuuNV;OERC-QJ&ahEKBxHB!#IQZ78B;a$I_Aln*|4Fxw|alGrnN*!XoD~|Cp zDr7?dt9};3%G?x)11GDs74bzBzYX!Ntt@Uk#ibm35w(}1b_L?zkLwPFqae51S_;Aw zv0*_jqzVMQpH%iTAKD@wC}!L?XTXRYb1sb50Vx++@LbA!fQ$1(VGZ0r_6&wXB$0(s z11tSV%7k6Cv&-)+HcQ<)-5-K zkD?&)N)Q)t)LzM7J-K%_zhS}&?z%{sQyOLqw@r9%7FAEVuCALaYMsblkaM~GWYRMy zH@sO;exsmjrl4x7I$2OPTTnOQUMN?R@?)2eB>l<_`7!>Rl^>fJ6F%Xvgk3W+u2X7( zgQUrIdNv{u4q)9ZtaxQ?^3*q;`ubC|mAs@2{?bb$^ZpIDB}dNQsbc*$iz{1~V;f;G<4cxW{-$@UiDrU$Gu#9<5EXhLtn#)Hg_sy3#Cf77g z?@Idi+@qY#1ixT;)$W<8wZyy$sn@3Y;tff8$CQVG`Qkdt_A+a86ePB>nZ% z)ieGbH)Q=cr-z9E;dn7FoYyEL?z;~B;tkx5v{N48MApJRUCo_~;jFALT=k^~`ivxH zMz(ooc zclr%rW z_r8l1HDC4HEG(8^yANDgFov&W>`~O@efT|ddsiX#1=2vUFnm0gm^xDfq3%`^BU@9E zr$$0htqY(-3zC!QuJ#1LyZM~_G42B#nMOO~8m;Ix5@Ts3r%A&B%cX@WhUkqpL|~Xe z6d+J&;au3h1(P2U=c2_4BnXTVI7i?-foBPz`6cb+1U^9^ot+5y`PRlKDe6-M(isTt z(-d}*z-I`2mH;sb+UE#7N8s}Wz5oy>A!r7{=A=5nC@q&;FF~shJcw2=CkJ|!INzds zD}S9%s^2QCoKv>V7B;>gLape88nl{zv$8@&S@#KtCG47saa~4gfn#+fW#vA^IXdsJ zNy=NnE)baUZ@nSwzgc-}(qF^9^A`6UAqp(312J5sGS&9`VM-Ml`5HHVlm4& zdUIk~z4B;d(76R^j4S5qZ656ky0;L8)34ER(8B~ILP*x;7FoXyy=Dw-si3)~Xg4-g-)L1ZYf+zvkP4z4 zvo;>o=L@=vKJ4>G+hKez>a!0yJ{P6`RrdLMS6bins(EU5(3>I1%I#Tx-xU0mF#;jT z()*^Ssksnc6^?HwxewFtJ9QFTe@uaLcnlg5&{Bl{MD1>6?_LEw%qO6T3E@b?v%8y` z>o{*h9g#C1)HxZK!Z;2R-!wBh&9-#$4MZp`czefSGZ>CEcBL8Y3h^fp$~;Qq76@>- zPzDZ*P==x)${|4vq&Zwh15SquNm0m!ipsDE4Y@TeJ2yf^UeNJiZ^m9~nT*gCALo1+ zgY!II!z!29rM*BQOxnYlIOu3ueQCm!?b?e1TNh>QUf_u86U=Meo=bMzjm&Iub2-^A56*zt`;)z zy`ywH&A|6sahq%Teq09*f>Aw25EPYIN3@&Sua1S2avP%Q+DFe*-n{UYaZ%%Gzr8Yx&6qQpCyCyeVd?FZGv0|E7 z!h7A4DIPElK|^N#2tmxRisL%+A;*ROEPcpvQ7vGL`a_P3)dN-;A97r*Cf?VIXc5Qs zA;$&oAFF};a$F=Kvgf#VHgk?^`}Rc~7na%0gSZZhB*rG~OY~?9+fhI3Z%E1qrymEn zTE)Qkf^^du_+C41)9o{U@B#X7PCl6QHwdqA5Xsh2uE_^2J`s$p<|-htH?f5Gx+POQ zVES>61?SZ&)2WHH84f&ipr^nU6XfPd?FZG zv0|E7!h7A4DIPH0CK6d4h+%ApMwlED|iEd_6R%*wsUe2zD8(uO5s?eyNV$Msx4QqCB=#o?asdn><(dR@miPhqDB0 zaK6A!l{if;4QC6^VD>JT+KYqEPQURa^B!~=Ppy^WI#-yUB`$<*8> zaZSa&bg6}GNlAk|<3byYof|ce#;|cgYP(o*ugZwEy$uF#k5PqE z(5bGq?ss+88aU?NXj|jXcFFwKR8ZwB5W8fjjLdaaKC=(m_x6gQ8>(WJX}e_B`m=8( zA=Oa}Ewy5gE9!cYDxX<4`&KG~3SHWpLY1%BqRLm2)^@pzcHCrBP0~_I`?iyf`Zewr zRlXjSqcpw7d%rUJdO#Zen7%*o{%T$6{zHqT^w(4C`Iefnul2Ibw?(b5u+;j>v|3*k z%+HVx`X8d!h05dNT4$r?Kd`;ds`1}2G~00Ua+$}hncf*reIFg9GgyY&8DnyFO*e&Q zhJ8?AY1q}_+n>5F+m-#6Ezp#6C^Evtj5twBXT;;Cq;Y&*+p{=uoxp)^agwW1Jda~% zXrhsvk#Wl)NRsw6foBNx5(p9a4Fa^WI-bw@QvEcRGA@&H+tzJ68phKV)(CvJyJW{M`~xw6g=+Z8fOqZ-g*z0xk-yGikTpjYp!i2Hz}c&^1a(eDkkw5{4g5v z7W@brZQ$aBV=RVq_1d>7_`y8>IHBBkOSd3%OZPjJz2gku%YB~Gex1M<2>c$!xp3Gw zk-4M$S&Cu$V_bmQ947*rvAwIGMd16p)IqGL5g@?iXZLYZ!8l8oI&y!Gj_YGBCl~4t z@o$3lNmt0ClWRV%d3DpbdT!0;S>KkMzO|F_v=6G*=(^0m}`nD3vhG})q z*UV9TQc(%c*5wy12+ghaOmnM#;^2b6;ENX)3Q8AzMK86#(0;kt>F~{Bl^}%*t)a zDm_yX$Q{V+Er0ndrzUq!t^0$PZ|wj2{@Jxnv;OT9t_6SbrH*+&4(2#=S|;QAZN|Te z^s-t=8?5Dx+TAm?yGa|YCF!qb;iL`L@|y)ywz%v}oPGGTOCanXJ;TeEx@@K>~Eu^ff@fYvOI z-cA2lYnGm3qwc^7rXA0)Tbmd((zxTKwOjS1@g<(#U8CoM*=+xILU4jmn8CVdi8X>OD*JVT;K6aw#(aW3u29*}-~?d@1KMag8GMAd2P+ePS#q}JS%O}p zPK>+Gy?G$>dLFfuu2tsg-SlBCQO#OwiE&!~@xzCR!_tr9D(AvOV>rsrxF(|JVa=j5 zrfcpe&PUUZ#mDPz{Uydf*CoRe+|VU!A!iQ(#_YUIt}uZ;1injv$q1ux1qznIU}%3x zY0}ldEPO_~nphy$j)~Cz4P|kaz~=}&M}Tdhex6*!P{ip07AApjax&vMA(OJ}6&4p9 zLTbIs@ng4D|INyENq;T(&Rg7bge|bF z4#Zdmh_bB!@!wKA|1p6h1Z>5I-=r7_N!SYt-vlEbc7*-mf^gwQsn;9Jf)SUjE!2JWE9$S4 zqJ=aB`DJHOz!YRf3xfWjgPkE`r(%a>7z8nr+S(m}2*k<6c{)=-rb0TiMH1oIuDdFw zrBuvRAr7jPP8QgS56!&rEE|C#olP}Pz2bR7TzFEP-GaE#@edFeu4dxGHEH6)hZVt9 zWr)&j$>(BmpF!YtML$!VVk?MQoSuv1AZRlQ_tK)xWr+*T+7)G7mblPNhqC3WK8vuz z_MOZ#!fM`TJ)S})RJ1FmVWSKA}9o*#{1~!*_Rl% zW;%?C(ueBj&NOAdrTckV(5J50;x=nlU707Tt1QC5Uq}DX)5pMHsD;%yyIqamT8aOd zcTfscjDJDc#1ld`ah4PYn-U7Qm&hzI>L*<3Kr7i&tn+rmxDvB5p=@j9)*j;u8HGfm z#s+0LzC{7Svh6CR+%II9Y#7y?70@o99Pq+hh2yZcLky{?PWNoS`) z`w?PX2Ej5q!2mMBU>56WJA*zZQI1I`v+PK!Nl1GH!SC%?oxK&k6~+(ll0mLdAd#n?3PR zJd#McC@PgR9L3q$=*VCnGaAAy`Ea8@#Mh_X@u5g07I3E=2^=~#Dqi~}#d+h2Xe<u01HD_9q0MAP@!^udzFWO~%R;$doYx zU<`DmKYCvKSCrx_038EYD?a~Mk6-=H{(=ATI5~f~pDo~UI``bb_!>j7F6u{&5k>m} zJOaA7|6693ZN;jIT-Zpwym`K4!$i)Tr7I@9H%lwN z8hSZ2TUs;Wz30lwsktoyyi+FSmEG`_U-Okup1A6o^R-Sm7UZ0lvR}x4G3Tc2d&&Dn z@1^Z?^2!Cd=%xG@@|i2=CC>|-(oICW??8>KS4n1}C@yn|x4^D>v;PCHMPPI&jzukIc zRdaGxGomJrCs#gwv#9ja;BVFae$(WI-`h7|v~A+>o5d?2jm`Ur<8sqXG3t|?lOzXb zcLYKH3MYq<0K@%8%ZZtm6A%HzeQEn734!5)6qw=;o%kW0WM)xm5p5@a7=%<85fJ7Q zX48q8rV||Xa+CEbmOP9IhG)Ld)&0qW6a3lM z=ji2VJ~7jr7Rw)YCr9!rf0J7IG_yVUTJgsA%`Rq6484?HMB$udUg<07vQ{~>ibX^3 zuHH=s#fv#iBTH7j8GibUNlA|wbQn69d}-s-pCaAoWS1}Kf}Aw3yV@cr#d#@{0(W|i z9L&2(!D&}-0L(XNzWF4diD|fgMJGpat_$a-9_D0MX06zk)vR5kK6U2Sd`DXz!b}I| z7Se1>o!H~QuhUr>3qCjL$OPRP@@9{@V6l`!Fy}_Q(3!iSSV|g821ru9U|Vt0N;|B2 z!3X7!*Bhi!w@Ro>QHRclFT|D85he0$xGynuL1|W)t=Brf5gf#GIGJM9yOq3xdUH)m zMwM@URBY6~0vxEvQLkGLffL)TFw?e<(y(XTk8p7_ZJZWyu4-)>1!5cAAux8$A@&-n zrezSI%tY^?81X9rkareUPC6&MrW&WT*`h|61_g!K^`%eXZ0Y4u24={@g#MeuvD8lp zpKuUO&@pgu)#4Mu$m!B9;RBN4!|7HdM`V6^RNz=0h*?^&_y$$&eIZofG$|9c4Ojy)Zrvkivav-gowq0dC`i4uDkyQAWaYPT|3DlnY4Q*yHjY5v9t`> zS;bXd&v>87t;;fY+2~^c#@Z#}dx(5I&v;2It;vado|=z)uA0;P8r^$gk-EuQ=MvBG zTAq=o<|gu?Iq6q@7*BrF@ZMo=1pg$rf1f4{&BN<3%J9d4>68I<;a%+WA)Kz5#r&Sd zwy~20m{ySoP6h9xGb)T$oH6FM0luSV&TSCAW?m3P6f+A1CEaZnO5w81En*fn(_(HB z8jfk_TJ3R#cr;^2Et|xi!R(9~PBzzumQvWy=XUx~GG;hBLmELX<)qaT_U}fY#Mx;J z<%3X2h-1zDIq0h8GF>%L?$JLJ_4&6M^w7ROOM`i!yk%*+YCZ@=M2}vE>A_6OYb!!B z&D$FDyHV0bXR2-6WpR2?H}ri=t=pzEPx3xY+rt2w*%R&a6#_W5U`bpdrphpCmXFVW z6xnEd33-DhXZt>7r|m_3Du)YvVnjj7bly!>#)w}rMug29T;FItoxbvu-U z%(R6ix^>5nI`5*dM8FDn6rUk5b~af){rCh1k>EY}_5gh{!eYq5%k8lXd>n$XJc1cQ zgAC{gD4=$Nz*Yi`Vn0K!mkERcI#Stsx&iobl^yu`$4kER$bTJwZa-hC_ajh?5Fn9) zMrhhu0)qsG2}B9R2xtITa=DE!s^j}^cQUJ58HO{ksmkd6Cn+H#`VHjz9D(NuFdpFZ zA`8ri^FbId!=5a2Gu}Ofr0kqLU z^Z7eIsdzmZrFyM+uBe^c4c|3exEZ=lFab3THJYTnX)22WDE!=z_1~<#2`V$(J8yB% z5w^gxIBoc6f;N95l)c_f~Wd<@M4Ip7;j)t1Ygf2f<3RSOOf$|8 z5?C>M3d*kinZ_`dNd`;U4q0jUQ41M#S|$toDa#~-OOwp|kwKZ36gGP_%b&9BY_XC_ z1&~29t)K-#3gt9X=v8xAZGAN8Nh6237IG-2W9)%=M{fr6$j;~JUmHYbV0q-QESL{+ zxT5<{EcHrzX8&)HLn7Quku9Ot17jph;fg-63^FT7+%ia~*cHkkhkms{Ee!fISgeAF zV6lXyQG?0AB9i(RkwNSM#-O}JGueq$X38H-b$Lo(0k_g6yUJfQ>djlZsbxZ7+b7d!iIwO-zk^ECCC>e zRg8vxmY)1a1TGR_vstdAxksN*qG+q9;fY8i^k95#jio7M)bxIv`t7~A4JkfuQ8@C>TikQNE)rN)2VzVL zw_~c9ED^dGm@nRnJCV7qvwpHnI9kC<97NdkuuMyYAafK$mI#Z5 zQ)GhR=@OAP!-2@fWG)-61On%~`*)Z;t!usB*YF1^AV{4)Y2c)s!}g?{m{hO^)X8Ec zol~Q{GcIOYAB{v86k&3>OOnh1j}vP)e6KOMjXV2(jw_91xUt5h^J>Se=hbxLDx_d` z>vpU$a~W~6pY??_* zXiZe7?LtyADA?cW@_5rXL8DgRpkq-*DkEB zT&PeipMfd8A+MQ{*Szw`WH!(x(X2?dX{+pHSll}nr z&Rg7bge|bFj%#u~mPE<2z`P%8mWC-20QdyYvtQapQUqQ`WT^wPIUV zb1P+|{sQ_I%o1fZ)1hp%IkXhMxiX{gethunA&lxhy8c#rIiEguit^6_MF>)Ga- z%<|S^{bck5w&f!ZFBpWJZ~16#`gYF+Z-x~!-GY_Qnd4h%{bPL*(k~Lm_4TjD8Q;=T z_!j}aIGx6iCK4_)z;E|SS^X0|bapX_g7RZ{Oaev2lkXWl1y@Rj?R9oOQTuC(I0Miz z2+ch%Tmo;t%tL~@<$BnC4dn2rnX~yh%K1D2=m9O7++{wwvvM)v6HTF9+VpjDeU<>R z>v0;k{9|**M}fG$gGDljFAj0Nd#ZqVYR7}5N%u{*h9+`S{f54SPbyt=c>o5vCuA7I zxI8dRwn`hPYOPWo2w|9+QeA>01TkdzLJuRUj_DJ@$m;Td@Bzv2k?4mkpQdGrz$xgs zn^{_XA{Y*ohfT%f<$5pMf&-#kjD|Koy2%fv2#y&75r?60oyJoOj@c*zX^i1>RR&Lu&n?lZ9DI_CTS7 z1`k}sPA(Ovom`^5WRw529n!{b)@~F?-}^V8w8Xw|#?)k{v)xZH_lq$#(WJQ)zL;z> zcN+2-KwG`1p$= zSQz4o;&u4C;uhqheTzUBK&EAR?VnN%u`BEYKit$pK_)#LPy5hM>$n#>G`i^p?MX^Y zGkJ!^gw_n(^Wb+fzr_OqCR96VvzmWDlQ>$w#TF-5f&g2yK9I}FJcvl5C2brakY`!j zG9HN4#otlO~ zBw`TertH?cOC(-TjM6Ju5n%JJ#Qac<^(^A$U9-MwvN%vZ=d0oN24tylt^S1=!_ENz zLd>t#@VxkVFgEh)=8FTQt9Z(h+}b)*Ob6WICaua-jyDc>%^dC`txCie2Y3`IRU+y{ z*N^FJy-iF<*N@MTX5%TQRSDF+T{C;Tblk1_19)E6T+(Cor?3L>qzll0@=SheUfG$f z+%-L(JftQMo=g^W@kjCeI3y)XJoaGM%)zu+*o-JyeYqZ+mGr{hj%3}wnc{tvrklNR zx8ser;7nVPUKp{(WETMrDhi_7gFk+fO0qbnGdR=9^hmqe8v}P=aAsdnN3PSKz%#RE zlOCf#hLwOPT_{o}%clJERn19d_tn+OBVEbEr;-Kwqj-KC(i`Kkhl4YR(_*<)Vh6@Z zhc->4=Vt)e?WUPaJ3!zdfkOoTiU8OXN&DXb_}n#pXB|fphQr#&Qn{9K!%dnHtE>GD zf&W9`2!S>NM**y+<^F;K3A{@YUjfinxpLf1ZSaC$=o;Zxx}jLnoO4-Bu2_38TebI4SC zk8-YvhldguaA2B!-7C@<2~oY*^@T_9spGznu`ltas_l`mc1}e5qC=tn$eB17F0$E` zHI9cKL8}jq7LR=1Cia@o$0U_WtxL=Hv^Lm35UCpu51$T)jI2{D4ynhFJkl0oi}QjgXC=*O}(5rw3F*G0^cO?LjtVXsps)}Erly3@8utF-lvhg5I-rd0id@#9Cuu~ z4%fZalEd+w^trsBO1VFk3Vtf({kgQ`URJIn>z*XQN+-M}^$vV69N=ER1K$z8w=&>p lxr6<*mU~Y)b~@_sNC5XVN5D} literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_heartbeat_timeout.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_heartbeat_timeout.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..dc3434d5173441abdf32837e4b0328383b747608 GIT binary patch literal 29111 zcmeHw3shXknda>mbTbWe&t9#kor~BC3ulwdNmd&$3a!JosJb+#$2hgYF>i&qN=P6#Wy)RWH81%8k@EBNeR2`c} zL4MO4my>ZdF*=aww=sQUI$9pjX0y>(Us4CYFXyi$``o^@B-=aC}R9U#^+|X!D4?9yi7$1gX zdMZB}1s$4(n$x3EjS^K~NewnVuA%!kY4N_MLtRZ;Y)HR66zki@zoF^I0)*rbpSuP zBHfofo?UkdJa_C4yBNWP>Gx7+pY)mu}X@t_U zM**LX2rP;dK57ml&ZuZ|AMFl%Qtl|#c3#w~ZUkxZ3n_0@9UB<|JEd}?`gx3E1L%5g zEk1Nktw3gV4S+_4)Ya9g8}LRBU`e1QQtPtTg%Np>@>K&|krqqJulg3()y{b<7E4Me z-0)Ovz$<@od&`9T(*adRGR9SSv&RF-s^jx;(Cmv(GhzUHMFsST&lQoF-6B#?j^*br z1)Y13nbTfTNbTimtap!XCgq_mrUUaWtK8GXqUf80aXpnY5Q`6wj&z4}RO$#RpHY=m ztvRPzt)jY7(duT3^{_5|!=cv0r%~$~Rjclx*orK*$}&7csVNDnv|>F~;O%mGp?veL z^3B)vKfiqA@+&2CP74HXH8Z>$kkoizu(KB#?h7zO^ z4R3@K>>*akczQ+i_YxWGL2f4Oali3yF?$wc4q|dVl_=>UN4C^w9S!+L8iHfa5m9z; zfQR=KaF_LFEE%%=+{H_;l#YK?GMVZqX8VGK7L7llW0a2%L=ywuVV}_qHOQFCF=h;v zs8y+0wU&b2B=Uf@#RTGYQGHt)+#7^did$#iQ&aS=;^Mf zx)uWKX9DYQhpKOfDyQ6k;`^F!Hne52a?_n$DSyLTk~_cPj$g_ze7frsUDpaf)%~fi z+sJq96UQcXu;xtp=J`PV-JldScrffy=_?Ehsi!dNS+D302WV&+i4D^*>yJh;#g7e< z8w2o#vDlCq6O2YN#A`ZDMkAw$&W%O~M*HE&$9&e+eY!)59C|eRm>MSZ)Lqow1laeN zV*bEDp6HLpbO?}rW0(Mtv(Prf^v8#WqEU3t^~lAz{vaFo)KbDiU=Kw!0{rCTsIR-u z9FO--NZNnQ!N&VHX`p|zz~$NSrr+V&ct6MM3Ezt+k{ zE$yn*g}`j?drsC>ncVMy-0$Rae-hJBkJ*usRy+|$m2Ir<6`e(KDK4H@yv_X7`5uTX zOSkS7otk&1Rbdp}wl~PhR@@K9qm@ z@h6g45@-#6%6RlJ6Xj^p!xu6Aqr=0r1nRqtC|xHhj|sMHWg=tcK>1F@hWlbN=JQ7X znK+9XLO9kZF^$FdsfmHXxDXS^QYQvqXk7|av8;=??_~?jO?d^k*9~;sRG)7NogJ}W}4PV71@En9AUEt}Z zIQB%Gye%+ei@~4GRU!?|Rq8Gzzu`_5#MHiouChfS2F{eP4~wjrs>Uv-+ygO8)q5$O zi%q+!93(~}^%TO=1ROSSq;{Fo?qP~YQw7PySP6wr9fN@gT=-^f+lDdZr5Lj)csz(%kRavdeW=6`A=V?csLfm&5*x{cvq zBNk>O(tW4PSARzWFov8nh#15_dro(Uy(Hg_45-wf)ItKe1d0fd+@q2}q@r~sh~!KN zBn(XMu9FQJaP5O@M|}?xvytY#aA2ehxUErn$Pdmup_u zX6p{m<{z1JcPzSdjksqUUugPgTV|{G&*mSPbGKXJO>;HPH;>HLw9V%4op(RPJ}x^0 zH=yJ+22Q3kFxc3mxbz&bajxP98+%fPr@_5KqInd$hw-Djl>rz4)$PD#3Ojte*)j=?SxaeCbxV>SsEdYes=94t(0 z&vZ09TgoYnzwcp9Zy1s+RA=lM^7 zPK^BSRndy|Mp{`}X^l8|8G=U4jcNI^(rD!aZEnS5tNrZh#b~7DESpxexRSeUS~24& z-euEz__sFYG>G|7l9IWKPw_``n;dDO-d^h7?6F~{j-K&AMD=pa{Ed}+-b0S06aOy! zyB~6>@m`XD*kjcm=4?j~Nh29gFAW{+;TV%XTc7ZWQ0H04g+Gk=TVF(O0rvxxdPy)`^VB%jA5ok}6&d!+aU_&&_|eoF@57p;nyRyG$@bJ{uOf~QtC z7c9vS-YUXeP`oNyS2h=j+7&dejPFH0&{q5_!}le;#@|K0w>VYwy5D$CEqw<#Rn+b( zIaTBXr`1JUM@v~Axkz7_3}!WA|XzBvM^G}g9raZhU{S* z^*MgbOBFn?vLazGU8+omBB5-OWwOvFiHd%f^edGY-XF;!jY6^{Qo>td z@PB8FVA)qsn<6DxJ#9Vn$7!o4c~yL}veAna&}n^?Q@X;@Ye|0ajr>lHUdk#~GfLI6 zqhb!{swPj`s9360D>aeQj5SQzD#l7N@A4A(Scw&TA^!dNFG=Poc~EjSj8FOxvEBdk z!jy*D1!xU+z47oE>{7TQI$e1T)wCaaX}bzpmZ5{@8el^WJpdJVzdm$X9@9vH{a{sL zqd(pd{r5x(`8YMA!K5H!7>fxMETOiH&zc&IS6pyI~huZAG~)mazlS zDAfJ}(eI(Kd(`27fN#DdIg9<@-P>{V)YXp3sxQ=irtXs`zV!GD$ENnpZrDB_*#4JW ze|nF!n;&i7Xc* zd^;7J1re_SAf&7ZjI6uE+7Et)2iwn%eaPLkvfTSvr6M!F^B84;)2MPdGQKlQYoHYx zX=5hsE82jqG!@MFSoJK`xBsAFHy4em*tEso(oj^zhBulBCS>tc0MRgXNep44eMr+iT@b}mjBQU!!L^%A$RjX&;H^SI}hUGT3w&`!gYgGiEWm-3Q2%7tp)-BW>bZ;(b{ScX_&s({n>vKF>M+Z|jP1;! z)kfbAd%?~2+`H5dWt6lhjgswYzMUFrkL&8>Y>%ZH|oc6X}b2ISNhZnr9#^0Q`b>gD%2?tSZrnu#8wfRIa5KW?ztwh{6Z(#)TYi^Nd z*4sM0(M&G_4F`}3BHM^a03x!@_?z>#5s{`(IEXAW?3TC9<`cm{L}XcsxNqLV2;@Yh znP%48HoY;kAR`vYUN5U&C~KW5YyI}d*|N@wyt__gR=(%L8&kAz?^TV>)b}K3$OyPk zuD2-ajrz8k`nEeBIA70eyH9UK`u5&sk&1MD> zXgGjvv9xNc>T}U)@2<({LTPxWGz{m}vFXzAY;X&l*9RGx3GQ0(ntyZNUDLrW!YdqC z$At41;V^vj3^U718B4=PupJ59*9S%349ARjm$CJ37gGZmN7?@PPcXM#kt{JaW1kRd zG9OEjJ4u;%~O!_qiIXs__r{FfAih}{#~MJ5ximS_bt=by-V_gNAj@W7g!aoE8|~L zyMo4*@vq1S+GwA9nQw~x8t|`;=fqC?Z#>WCzXLocYIl`9C-Q;k^8f$fxen*?FPe%b zA2f_b;|J4~ZfwygGmT;_nS|C+j^UWZprME}R+Q=bxJLDT7+~3AUyYKBmaJ-wz#zce z1;)~tsU4uKQxgd6R^LzYY-KH0ttzRU)ZZuYaRN^PghOlvuRcQoG)o)he3D#O34Dsc zrwKewV3NRR2z-`6mX&-s&sw@0q<)T4eu)5`T~WVG;41_)0)jk^)Ni&Wud&Rn%C^iU zeWN%8mugS{LKaA$=$*y9M3ij=dFNj^NC;}n!F%{=Nzio;(#UVM+)(91^2xM1f?10etucw+|o3rSIqOlX$HDH-ua( ze>Kl#y|d-Vqz)5Y%jmTyYd))fl)PorTE@QV#uPcdwVW0kjyfJ@y|LsGwaUK}OP&Ch zI~~|(%GTb36M1@=7USrDB=?uz-olXSAKRN+3~lxQqP@j9A==y9OXp75V;O}feNevK zFpaeTlJ^y%e6f1V7M|C7!%%8{BoE4$4QHOWDPO!w@Ez#a^}JmEJ)Blk`C?I;wSV?5 z$&Y>=R6%>1TvH_`KuU-go!P08Dk_$LII&kdrjP8QQmGQR7 z2ihv$Dc)Ar*xEIFpAkln874bLWHK2k%IF8htLO*9@Qt^k)en+%;D$_g3X>rvfCY8c z7_B0gSnaT@a<+<#avJ0THE}V{jdpO44Vm!t#pPIke>|zfSRytUON_ANqTEzxuW7{d zy+8P@+&@Gsb7uP>lb=opnkdPOyG0V`E?Y-P*}8qjXGg!BF5Rb{9X**zDj3;#?_d5BRy#x`w$>&F%HIQv+W- zIq81Zw^+Az(mfxlG1gpCb>!!{;y)rRk!FU+TT-cy`yd!mpM+SN6>QsYm9ExBk=K zpK4SMAFbW&7&rTm(Ra9Q5LqR0c2%E1QbV1^Og+ZQ`$2&2w0fn*C9BtW-zZESMcHAO zt-fi-RI^sP`WJ{I22w8)xCAg>$1a32&ZbihnEeyO4KuM56>*kwWYOlSHzY4JWnkxvr1Lcr4J{Q-GD4gjO0hklOH5i|6HnG@~~Gp(7zWbtI!ERmJb zTEn2n1G?k7`dfsS_5s+@XWN-*Rw=X&HV#WaLoU{$OmX-uc|S$qfr>-+{Y=3~(zdO- zKLBsI#HK=I-IQ!+)F}dABk+d=zDnR6fd?uWg>F%Oj?#UdzzYQa2!Ls8Z2CrFYGWuK zx9aIueZ1+i4Y{d9|KRTT1)}IHP}ksic);yz!0~Wj!&?R1Y-Q0~Vq9G_=UsmX#w?`^ z-ZeAcHP3DO(vx%B557`5yZw>bwViX`E|$=Y`pAnDcVr<8$6eZ839|JLZ~? zzM_5S@N9F>?AlXvUgh=jnl$5p?^1W4EqeHl4?g@B1-`bSab`o~ z9T%L7fyVpvzE9zIS!8|VjT{#CW9BVuoGCNXl4%1^`dV${*WSZ0n4$MrViAf`Z78ka zpb^aCe#Ek@Yn)kU(7^ntB7D!!wdH8ZwB=}lnS^1>(eg%h^GtO!Sz@%jp4Uuv8!cpA zf%`xJ-q_ZBBf`SSuA=3Qb1sIcWQk&5JqFl;#t zJY>t!@`l_zBO7!uKWsT*s*>G|=~qly{b0ML#qzb&-s(y1b9?Z*s!i_scib3U)KiEkYuE!a+sAM#xIUee+bd5iv*Th4b2gNS@)C@m4dL zAWh3k!z0tOzJhu1iqt56FurHxylm0U=kVt8@Dc0?%t+cw%-m0anRm;x2kk+b!cc4{; zMrD<)D)OOK1K2Pwcn4?`+WJ+}Ch~!{f($OovvEn*wY3tAxC6L2HsTLRI|U}Cuz@!< z-bzuHqy6PPfvtq>e38||>1&oz%BbTg?xT665(DPb6`C28(vY$m2N;@*o{LNk69oTY9#X z2s_@+NoaI10km795;nt^>=|8(M3FZzhUxEM<9w)9yKATf`oe|y4WMeHK!D*sxcAIhDfVi&7Y*sDRx*}0FOU`ehOf8Bb z=!*1uap{Ejc5%hE!q2x(99|4mECecNaQl;SW1ed^w8^|JPyR98mUq`Bm8^%XTU0)I z;R_WLhZbRUUQ#<#QhR-1x}*ns4H^01Qy2>m}BsUyDanC0etz{yJ z`XXxkK^xcKM4b9p1Ud*DCGZGrjoB|T@IB;hghU?Jl{9%yCq zKp8KoC+m!%R4{p9O`4QPveYti3$HIW6;x%Ew2_x-y`jY~;XeLNV(vAx_!gxyPieig z<;NV%B)3)3YDlD3Sy^edYFE&R&I9Sn%9oW!D<5d{L$+Hky;wQGNOZZhmh_r_jgZ%t zGrQa&cA%7kG+85Y;iM+huRF*eX=cBs(7=BC!PskJ*P}uLS7W%xlwVm+VlDlO0`8*UxxE(m z(aXlqFf{rH4H^8K?YR7Q4Ju~v9DC;52R~TyjYs}={A2Cvw-IcJ<(6$}%1`pTL5gK! zI=-nJvNNt09veJwJ7H`ZtMXpLv{VXPQASG_kZ|O```=7{r-ioztYYm z^Hd;qaZp^hKECdd`8C#=W$U~CC#J-4SL2Rz<3aqa1e4v(pO~<;-eKq4&VBpUTPQZ{ zG-P>FZSON=d8`MFFgodMhS90qGUL|b9YpLt0{aQA2&P^2@L7_}tj5felmsKgwSSY?}^j`)+B| z9gpNI!X{*?ANecRU2A*hBQLtHcl~L>tL3{V0Qp-<^X+?z(+BR&7-GHKx{DlOtz&wr{TCH^S~Y4L%j zwP&Wa$4EN)A$uvWSvYtE>GF}qir}Rh`3!Omx(tkq1>x%rH}zMyJoNJ6>F~i zqjY;oCMElOX7;DW8pZwe&3sgvhDC$)9e3@C3U}J7B7k6Y?ZCYsGmcdPINszqdV$sv zmNwNo$o0SrYu#Bdti@sra-bV!u;&5mjeK@tZNWcYSj+2)+4+gtIz!H|YO}yf&s~sz zL_WwFE4i@N(yc9jVXctBGwilhQ5)GfQj)^}JW7pKQ;JS589di8`M*ZfULem${ zJdBdcHMLhZX<^M${J70#P(Lq^d*rJ9a*HJ?;06|z7A8L+b;1x;+RvW+*^@4bE9h2u zV~44}qv<`*_yvfwK(&mrE`6fLFG|!?p5Gzx9|_zh@Sv?_jW$=rl$-siidi#J{B}hP zZp1)+ruM98rd^ux2PjSJLnYz3N^$MP;oD`kQ~k4LTP7Z%;}w^l*@w|I|Da=P2Y=5L zZ^EF7AZ&*MzSh`2)7XyTAMVM0Mhpf_ifL>AW*J6VI5DzPz~=Uu&FvVG;eNK+NNT{> zcDKKjNRPnevK)@dOAo-oBTPCh^O&F5&_2_U7K@1xMcwmAC3VZI*S<@P@izn>tX})C6itBCaCY_DIu9o1 zy&lg&sMiP@#Zk&|SUrqa%FAzSVJha-ny7KVOQUhj3%9W}s_{!|LXW3>A}Ez>h8aB1 zq66`BnxPh_v00^s zmvL|`mt3?Ps#4!jX%%nWolmo_N*ZakfIyG{jhm_Jw4#jPXlNX!pK^%u;tcx|(ta@$ z6!VAPY`@ZV;;~fWl3(sh71)3GF7R6|Qea7#s z98iA?{MF*kQ|PWNxzr(*HZppOCi9&lXl&A1|3fN4G}w#-3})# zIULSA1yb3XX{l(jbo2GPS4+1~ONFpeC@Q^Duvl0*RrhLP{gwRt1+pW2R|5FaQ3rm1 j4~kDwr1&r>6A-+jmU0Ynvsf&_HGu^< zvmiy-Dl@k0OrCj1%9cyiovO^#>8{8elT+?|cTSa4WyeXK?(^I#b;f&V+kvGBrK5ncAMZOkGcXroLxIW(C9b9dGDanOWJhDzmC* zb!PP@twC$lBK}ocq`ce9>y%l;(khUq%e1vDEr7J3Ok2m&Dv=hFY3o^971F9@+6I;u zMp}(b+xUv!e?T;b`t3iY!Xgx@eI*>J>+-%*6R8KRjjZVMwv~yJA`PpwZW}ez81Xb| zEh{I|Xw+aPX=P%gV?)W9bupDmoK2;N+r}>$Kfe5i(Th|gW5&}~Dw7&Z8>w_AIc&yR zn6g=I!QkPs3rW-38#IjVMrvpzY5e$~eZ?3uQx}qZ`Hztp8y$_Ohb&_-iD1%*Clbl= zOmc{2VVEor${QQsYota;lS8R^CTWb0WyVZ{QW<9N3rCI`XUsTC7`%kr7vdumNy`|G zqt3>-3FO4s2p*QUGVyfAVtHOlpG%KjOrt2g2+NhSjMQ*?%uEg$Thn7Got~698&3}> zTY|y9F(dNAi=76ZYbN6(#^FPK#@V%$VA#eTbIf z>pk6)DO;(HVJ=0rE>Fa>N*mE5Ucj0WPsSH1%OE{d9`Rv(mgju2UTZjkL8{Szy2|mN z%ARWl*zdWlMYYr3-CxwSA*8O?KH)iwGGFw33Ns(j6Db4qwwIwa-%n`X=E~#dU@SG1 z^9{u_@mzJviX~soB-2C5A*ABv%P_`sei|xP&NnzVHqzqFm7f_iG0bvRu~=dxZdpUA zL?#xqsJzXWb3^SfS(pLsR&ucY(D8OFK9adK5+B@tCS{J&xM}}%GGmRU+s!enGjsbSijNl&3N)@Fd8UDoEcGXM5R z1m3b9($AeI*w1%Xu*5lFX$c=e`=S=be|?Tf&1LOWOzFTkq@Z%#FC) zQf4;dM-|`K;yKn*LB*y)NlU9b_+K)=*Ulw9pND<)3{CFEH;# zAF)nj-NY)+&=SCqmuTj>3rNbFQakJ^g$cwa6-_5SQbv=CrW2-_kEMWpA*Gz9l(Q5t zFQinwqO&xerGa^k)JRqlh=6@!N>)ZHF^NLC8sZ5r(lG3e6R${49t-xOOGZI0IWZmO z>BKh@=fF%j@l{;JGQbRsVKgD`3Et2a%$w#kyy$)B41mR^c{?#OuUQAQT-o4+Wm0P+ zfkp;zem+Zi`>!Xo4TW!zH;;A#h4&22Bd>qcBLZ(d&wg0XB;?o!s$EfiZYv@MGe_b% z=L^jd(*QDPjW0$uCX$x(GeV7uHz8*30AS?Bo&|j5t#PS1pA4+h)yRa&uwvD4aE*IG+^7ZE1sq+3)o*{L3X9M{l?+ikvumJMgrXin8ilwe;+v@6 zvW~l7Mh(vsql$XfJFwI0x4G>jjSHv6f%9_}1<*3mw^AzfCb`8+fVppV0 zlqsKOkF!l$gt6So^TCr!dd!S5QqX1?5jKb{NhfP z_uYNrn@6Tw4^6LnZbm=+upGJHK(-CHTJBEHY~J%ARCE3K)#KOuuJ*m%J!5pvbbsu7 zW#4;oy89E;>moCu7xV7F6Q1cf^zG*9j*m^R>zN51|6k$yVk$xNhF-8VAq#y?q81mj zIIYk}l4sB&<5n(UriRb5pt&0*LYVTO85xUbN~9bokVi>16+}4zetYLb`uY8r*v}ti zSR#K7sVbD*oky+P*Ag;`TboZ3ATh%v=`mLkiw%t>Y^l`TPw9s#U56?>o-{L; z*rQF7lg(`eS^=EGXd5LGVA2K^z=Y99Uaw!D*CF&gB*|x92ajfqdVUM$9Gh_u>$LLf zc`q7ot_-2SqY$e%D=vYOU&MjYN1?fcpjnr-0r5C+UBR2iy-{yJa0)NQl?5j8YS#%a zWdwmaUN#_Qgs9|#zPro{V*x(-1|%d4s6 z`XVKQ4E_?A`I!d7vut7=_Y$5o}&QRPG3d=bB+^Jx1dDPI!TFL2v^ zt;8$sYrG;-5z(U+?Vh|;T}e!il8LAkT#Z^(7bS~dm@@+VJmbE79y1v&JFT@k@|}OH ze|iybHi^_|P?R(7SL40SGhXSysT4Tp|0Eh>WB1QRTVw1F7qs?5W48)pxBk>X z-q;N;Ydn-Tc1fvcm#DRW#gB2L%3K@W|L;e1sYIa)Fo?Ka-^7T=gN=_%KHvHO(JU`D ziRu#?g=Ecav@q1 z=@{Q+b^iA)t+&sH-_teVW-`Y66vgbBa4hE*e&9l1wH={Ra!<%GO2Ci8$cu#M3#AGsxy0K5jfFxq~9Yvz}TOAU~89;^y z4aTk7ubs~72WK0$PBm;r;MzpCVe53b1;KAT|L%VL+ z1}Whd8I^%+6Eb*@3gof-`oSBgd5|(TY*pc+LV$bsC1)Wbh8yqhR~fgf%1CS;z=OGF zBQ0j=HIiMK)ch4pz2!;GkVrj3p&c)ac+Ve>`kvR$za+Tu@Sieyfu=OO+-z`=b!(!2 z$P2Znx*hTY$&XUf?w6{xD5*%+E`M%E{t=L|tt;QRR2-|Kj{yPK5OL_{XS}`ehwU8>2EG>K1~Iv&1_&beyUs zaizRlpyO0VAeBWc3r1#W86#6ld!lC?I?f$i4%=}&ZP4mAv--9c`VlZ`Y%pn96XUSR zg0T;^{WTwC51&0|_92)HNJE(!L40y6vmF>%rZ%{=0A~clHk`q&K|So}HNgW8xoamE z)OPkU0mba6vV*YG*%FNTA`;A#1jw3$#eHwef^1etau&oOBV{p}3}Tx3DZ+XMprx7% zMu#bZxTyJ?1mXk+2_y&%0YC_{#S~tpQES*t(#nA-h1VejLCo_?ViH8Pq`%y zvSyp?Vq_u$zB-Oo=qeEdp~B$vHk{#;gXx`jFbOof|fb^C8dZ@Th1 z8Iu8$RBViUdXpn2lTp%*y)p(Q5xcKSMB%say6R~%j621gojQzh_6+#v@;JMq2x49? zWDo1cL+HYNZ@)Bt^E84>$_T&DWCY3K`&}t`WF?akRu#zzl3u&>Q&Br+592a|dXlRJ zP4W5y`m6G}^H^#5vt@*(@E4R3BIS_^7j7^4w6eJMOFpgI8+Ic%Zm&bHC2lXdxXLjg z?M9-n!R?jRNb(hC(Oj6@tM_&9!PSi0uQ`>^?E?eL=|Rrz)stL%aG=PlBYTotG*y1r zo-EFf9);%v>_W?_YiXlRwyvxr=k`m~QORJaTP3*tQuuPTL1tMBcVTW{6{&XN_N9+Z zH*SxS$t;?82GG0w8fX%sDBL!kR?hxraYy}%Q7e8gLc1}481bC%jrv;UyE0p-Xk^V{ z3k^iV=l?z$-i=)2HPIS@0x1@&F0g;g5*DCLYoj%gV%MGedY%+pv#haF+PK~#-dVh< zsH-i-s+ziw>!SP^J2e>Bwacn&Y2&)8sE(zLYncyqtNpOXb)?SGtFCPomA1lmf1Ux@1h}ITJymbSm&w)D8GU68!uRRBt6_TuxHREB7-@cE=uNd)crD>t@+wdWb-)hl2 za*mZYeq}z4--dtE<5!0bef{LKht6b@rU5f=(j+6gZ9#`kytT-~0q)ymS%)qr^|iKP zJ_6o<_9Ro~xEe!BT%H9TikbNtBsvr`uE*qSWW(DJum77LKfU(ZTv>e3%6Ue}%)Tez zv<^c#Gp%>zJf`^?$}~SmKOA{lJm0s7@myHk;RH`u(Dz!%T)v;h_8_*R&=~HA z=9*~&5VRuCAqZ|Z1JXgk-54O=)8P=Lo+QoUqP9%Tk_!{!eWVD))n}Do8V(lE4^arq zIS_`HQ&+BLsAsv>Rkg0HBNx+DKG!;`e5jirju}hgEA_czN!$e;f*d)eiNe`(*g0s= zzeu1}-N5ca7dIlMEUUV2c4V@H3%C(g+DdnF;0cjuKs2EiEsHedDcxHIj`|ff6~9dB z{#OG;m)I4RMuc%+B*Yxyz@76P;r5p_?pLyLzp7x|S1oHSarIiYlj~R-Ai~Nn@lK*a zZ1q~@cO6s3`7yStFz&0DRoBwSy=+}s$I`~V%!j&F|DzfA;bFB{|7b=mmaczQBbIHwTRrg{NL9Xn8dt4p!!_IU%av)l zfqLYULEietWYQdyns=M|DvIHnHThhzB~7jiMkq?^^Tc)lY=xHl5tw_Bfql;Gfp{K& zoZ8b@uxbiVFt#4rH~i@AUtIamKl{rACMl;(vz)9vfz2UU*rmJ}B| z(7w6W&9s;}(=~bbyuXC=}kg(-+sfU9*t!z#3Up@eW?Ex>|K3 zakC3vV0mh|(>rXzMb?wF$O^y~Sx?UD^f#lQq(xRcCIh5c+G+Rnla827MoF}+s*ptN zzA6!g(;};lGp(P@K2i7}eiwxaow@UOt7o_Fo7%c>dh`D2@H3<{(<*VtmoH@XBR72v zTzzd;hmnB(X7nRhF36Y+P>pSjd-@SaOeUisQj8Unh}~BuqVStOi8HMqxm9`l(Cw~l z+u`g+JEOeI11MG2_9)dBfNG27ar&Flk5X;zm<&+;?6iCOQAbQBqadoELK3n2szek{ zwY71k^`o~!w?A?FaJKzOc4OfaIT{{Tywpt^9<;d7+*52Q{)4xExNj0E$5YrPB*cZZ z%VO;j?r}KXAX-)g`}ZUt+gmA?v+!6b?Yiod$_-n*i{#6NTAtL} z#M?71F(hIlKln50cuGok&8OZpkC(gSDov^^ZxN>4;qNYR9%=9ns?) zfc+Sfg_o#|!c!g>ZF08~`PIFuUG{I%J`N@v&^F@8nWqz3j6C)4n3HV@>UD~Y}BSn#?G6yUtjsw%CA22&e6MjZ?{f2@48>V>$~gzo*m!t`L3Ow$ySFOm5X1wa8rl> zI4tE%Ac2iOeaVc{3!Vb#6OK|)8#Tu+UdD2Ll=cpRIDtU|%$JIcTg-V%F7^Dv)9`QqDv-W-t;8nW$MSy{v>S zd0e|!PjaQr$~q-!GS}`^K6iR4^=F$FFNH6yT7_=P3%DSy5$&P4N54DOUCaduZBTd> zyWGG9Nm{s+v&#|=$Lf9Ed$5+dAk`JQAT6f{Id@f0a_zwZL%g9N^s*;8cUAdad$Kq` zdX&2$EvK%fjW%g@lyxj^w8?y^8@V7Y@lFoTzx1X#X{Rc3ICghIDt%-w>Vg!ZMeF*B zr_pct+c`m^PT}au+2Q&GfJ7na4b(@wz-!(arm}(f0m@ zvPOT?dLymQ+A$e$q}@{(_jTl!IL<&;Z~V1bu@xS_=GV}u3oRYKO#|fL5x7HuWL@{A z!#C*;fzRVVKb?opqWZvF2H3h0r0g$!cO47ywI=gnmd4>yxskG z7G68V9{&nDNfA5z%QFG&^bc~mJhzI*S1*3e>nn>cUVMg}kJhEhhr<3n6g;Qs z$K-VQsjR;9_Amoe;hnR({WqiU%!Z$mQ5hhLVWZsBcRFG+8AZK4DPuqqvHOl9QT**; ziR296&PVhkUyO2P+#k^oPAxcu2=S{GPB{H=tQPZAoNei(lhY4xA^l+bX9TaXv~=OAQ8}Ng`mQz=5vr^A;7rkZc$~tnJmCEN@M>XG2H(IS)3SU}C zp_D9zt5&ND4gz!JbhY+jyQdf4B)Z$^L3*H5mK+JTiblq7q|(JNP+7uMecf%e(taoU z3oBzG;TL$+^YM%Jl2ZtZOn9=Dba8POr7<4Ago8VWUmqGG0I( znL?ARu<@1HG3jUJP1eYwaAMMM=BzR4HyWQYI!sy;;cg=irKh^^3|FQzvF9vKWiC@` z($@lx)=iW!Okfv*ZxL8fC8Aj^y(#!I&`KrNFn^dBuYRs=X~D*cPgKd81%PVQu<@PX z-REak?7DK~y_HYg8k%0Y?aIgAt6zEJ;#c=)L#J>`4uPrq=Go9G{x=gkbz{Gb$-o<3 zGI%d^$`O;vDCx#U83U4t-B%@|aGa4N9yJ|0b=!Y?=yo*Qb@-1Df3M=ZC%%0k`-y0F zi(M6o$OAy1^J@*eImH#0A-tzX_G?CQPe92piQ;;Xp_j3wswl2pAO#&(6j!FYntA@B zBx+m3%sjPuW}Xj++7_*DNlM072-NI#rDVmcV;>^5l`SjlxT>HvUF)ch-7JyX%5MtL z`qQrohr=O{II8IJbGXOT<^c!qaqlC^N!!cwd@P*2(Vc4rqaJXlN(Ms?9#xgcqpDp@ zY^C!kH}8$GFtHu=ejL<FSvJ!mdcv)6j4?1d_cN5nSPeNuGOjeaA+O}N{3manY;GYoHX0utjK>6KTftXT zw&px7leKv#%yhsJ!h9JI;if+LDE5`+qi7%O2VYC>%!q*P%!s{_Y(FCTFX9}&A{NSC zn7!PxSg37FWQ~-fRxt^jBQOe(w;Qm7n+2RZ09zRU6fMJghaKjpzEKln-}1qSR}L-8 zQcEdeEKpy%$h-|U{=c92K1+YV0EADm}5{+7WX^~b0B z<3BAU)NK8e{MH=5^{M#Or{eSM*5C1ze}D+-M*0B z`N`~WoX&nSmW};pw#II6=i}_Q?~70M<=y44KUXH?5qrt3(sj4pyEKBoLEsMv{2_rq zBJdOeICmBAySqz?1n%I!wFSW5cjxu*`~YWXKWzh_|7Fj7$mbvQJk$s>;fQ5SAdkF+ zv^0l}VfpK5-(6}QErU#RE0A=~P!cCW zxr~PMXYAG2w?sQ(^%bgV2}Y2$(EvMRC6TxKy1cW4N;51-5meN%#J-gI(3f!3arB6L z-%G&;t}~$zVN6P~Zkg{P^Wi;0?fGuE&bM+OXY%xxNOj&g3rB+m>#UUy7hW~)-3=A; z^k_j9_=zcI3e;tN@!UWEk~#-}JmfgZO1l1xSK-{s{}Zidz5u@vEriiHRN`n1V>GTm z^~1c;_+gy*L-tB`i8mK@E%C&kqWtL98f>Gg`w&Ll<4{=YmO}-hBC3(&KFef}N9vcv zRbzL>lDP7emjb1!#*x!~hBSztjI7L?A*-S_1v8{}88f6-lq!DhGll3xb291CspkKe zIk#R{3Xn-$+qD(_#_-V)*_91X7ba?1U}8lzW!P8dCWHZXiZ&wzmu8vCY$p< z&_u;KPS_OXnUko3?GJaBf>8pe0CFMvgjotlc9RFeA}~wl6t3}N2+Aesq5U@d7WNhIoe(>T<|n@0BTE}!;H#0@_SV9eTI zG1eZDG?ch=%7XZROwYT?)kN1b%pa^;^XA3ZFJ1}d`6kvkyt(i7eK#}H_03oG2kUU= zOV3TLL*wX}9{xA2_gr~Z#$>>ec28m4r`!?;S+kq%Vq_u$z^_ynv+o$T= zp}+;Yvi0rL^&KpToeA|F)8S4Qc{gtVOoewrcMEhuyAE{ysIhaZu@lNQGKM=@I`l-O zw|3q+2E{ti1>FwOwVhLIJE0T?x~^Fai-8}tblzFxOogVpSl%1^-SXP0{JFA3yaz+Z zXQ5D}Be}yh*{*y6_PcTQK=%6leOZ+sggV&^Dh&Rrn>wd9IqtA#Dqg>=z>sBX+_#`L zE>Dd+oe<1|_NEmb-Ol$!_)tE(+yr_kSvuPj5y;yUv0S%EE|{w)g~T(}uXCkbia^{3 zt@265&OM*4WG#g+m9hdQtDGDYU5;gyC%ve3V)yk`*JD{}C)!Eo@fw_Dp9?hh>?z{E zHm~J`l^G8dY`W?=qYHGk=!`CvMrU-{F*>6QbhQu}DL{Ix z6H6WA@(-9;@Jxk{5&URkX-?m45(Ww`kOsHSMrvYJPHyQ-(g%hw&`0DUTc1gzqQ#dh=ie#9=a31{$_1nGk%=Vxv=Co4`WWyWpMlIh$tY+3 zlks75_KjOE#@Z$Aavdj2XZ&+|U_SuS)B5YL{PS1d-aA{h<@;4z-V3d}`P@us(|nm$ z{p4czrmD8g`+`BH(IK{}Z}H;Jf&5(SWxt>XrVk57zqpjUT{_-Z&rf`;CVh z0Jo%5hRHgb`idV(?Aue&}t>x5}i*o8&u60!{E9=NbIhD_~jw&DOM$TJH z;Y(V=+zB54dn{z}TB=?$AKs&)z;WEicBS|VY(f5ku;^{aTi1v*^$Tt&ZT(^-z!rpE zDQ1(sBjrecoGc(2Gc^g*PU37ban8t`jpJi>BO{zaCdSfMCZ5h%IJOs?u9EcSyUf|7 z!M@(?bns%U#zbbgS%HbBpFuQNl{fz;ogce{$;15bz>y2wB_82pFL1*w0`IL7maCy6povQ4Z_1S+jzK(3AA)_*I?O7ST=j(99WHO4n*)3y060!Sk zMI!N5uM^u5lgS96K8$3xY&MUh?m1n2M~|JKkXsYu9IcHX+JNumwVBC_CX8>nfK19& z*lEm?lqtC)@J5CI6y38$YBk@{eJ+QRoDLHwoM& zK+GiP8;zUia+Qu=XPwSC9p^dg2u-k`@juL1lmf%%m_kN1%VK%wmv+CE?Xqz9j*2ACi4Gul}WVzqM}GvwF(2I=g231Aq06RaxJf g2mWy0@52h;4)2`i^OijddOe+Ul?Z)6A@*Yb4;2C(9{>OV literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py new file mode 100644 index 0000000..db8c471 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +""" +conftest.py — pytest fixtures for the saltybot_can_e2e_test suite. + +No ROS2 node infrastructure is started; all tests run purely in Python. +""" + +import sys +import os + +# Ensure the package root is on sys.path so relative imports work when running +# pytest directly from the saltybot_can_e2e_test/ directory. +_pkg_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +if _pkg_root not in sys.path: + sys.path.insert(0, _pkg_root) + +# Also add the saltybot_can_bridge package so we can import mamba_protocol. +_bridge_pkg = os.path.join( + os.path.dirname(_pkg_root), "saltybot_can_bridge" +) +if _bridge_pkg not in sys.path: + sys.path.insert(0, _bridge_pkg) + +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + VESC_CAN_ID_LEFT, + VESC_CAN_ID_RIGHT, +) + + +# --------------------------------------------------------------------------- +# Core fixtures +# --------------------------------------------------------------------------- + + +@pytest.fixture(scope="function") +def mock_can_bus(): + """ + Provide a fresh MockCANBus instance per test function. + The bus is automatically shut down after each test. + """ + bus = MockCANBus(loopback=False) + yield bus + bus.shutdown() + + +@pytest.fixture(scope="function") +def loopback_can_bus(): + """ + MockCANBus in loopback mode — sent frames are also queued for recv. + Useful for testing round-trip behaviour without a second node. + """ + bus = MockCANBus(loopback=True) + yield bus + bus.shutdown() + + +@pytest.fixture(scope="function") +def bridge_components(): + """ + Return the mamba_protocol encode/decode callables and a fresh mock bus. + + Yields a dict with keys: + bus — MockCANBus instance + encode_vel — encode_velocity_cmd(left, right) → bytes + encode_mode — encode_mode_cmd(mode) → bytes + encode_estop — encode_estop_cmd(stop) → bytes + decode_vesc — decode_vesc_state(data) → VescStateTelemetry + """ + from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, + decode_vesc_state, + decode_battery_telem, + decode_imu_telem, + ) + + bus = MockCANBus(loopback=False) + yield { + "bus": bus, + "encode_vel": encode_velocity_cmd, + "encode_mode": encode_mode_cmd, + "encode_estop": encode_estop_cmd, + "decode_vesc": decode_vesc_state, + "decode_battery": decode_battery_telem, + "decode_imu": decode_imu_telem, + "left_vesc_id": VESC_CAN_ID_LEFT, + "right_vesc_id": VESC_CAN_ID_RIGHT, + } + bus.shutdown() diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py new file mode 100644 index 0000000..dfbee87 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +""" +test_drive_command.py — Integration tests for the drive command path. + +Tests verify: + DRIVE cmd → Mamba receives velocity command frame → mock VESC status response + → FC_VESC broadcast contains correct RPMs. + +All tests run without real hardware or a running ROS2 system. +Run with: python -m pytest test/test_drive_command.py -v +""" + +import struct +import pytest + +from saltybot_can_e2e_test.protocol_defs import ( + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + FC_VESC, + MODE_DRIVE, + MODE_IDLE, + VESC_CAN_ID_LEFT, + VESC_CAN_ID_RIGHT, + VESC_STATUS_ID, + build_velocity_cmd, + build_fc_vesc, + build_vesc_status, + parse_velocity_cmd, + parse_fc_vesc, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, +) + + +# --------------------------------------------------------------------------- +# Helper +# --------------------------------------------------------------------------- + +def _send_drive(bus, left_mps: float, right_mps: float) -> None: + """Simulate the bridge encoding and sending a velocity command.""" + from saltybot_can_e2e_test.can_mock import MockCANBus + + payload = encode_velocity_cmd(left_mps, right_mps) + # Create a minimal message object compatible with our mock + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + bus.send(_Msg(MAMBA_CMD_VELOCITY, payload)) + bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestDriveForward: + def test_drive_forward_velocity_frame_sent(self, mock_can_bus): + """ + Inject DRIVE cmd (1.0 m/s, 1.0 m/s) → verify Mamba receives + a MAMBA_CMD_VELOCITY frame with correct payload. + """ + _send_drive(mock_can_bus, 1.0, 1.0) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1, "Expected exactly one velocity command frame" + + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left - 1.0) < 1e-4, f"Left speed {left} != 1.0" + assert abs(right - 1.0) < 1e-4, f"Right speed {right} != 1.0" + + def test_drive_forward_mode_drive_sent(self, mock_can_bus): + """After a drive command, a MODE=drive frame must accompany it.""" + _send_drive(mock_can_bus, 1.0, 1.0) + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert len(mode_frames) >= 1, "Expected at least one MODE frame" + assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) + + def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus): + """ + Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct. + (In the real loop Mamba computes RPM from m/s and broadcasts FC_VESC.) + This test checks the FC_VESC frame format and parser. + """ + # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics) + fc_payload = build_fc_vesc( + left_rpm_x10=300, right_rpm_x10=300, + left_current_x10=50, right_current_x10=50, + ) + mock_can_bus.inject(FC_VESC, fc_payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None, "FC_VESC frame not received" + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_rpm_x10"] == 300 + assert parsed["right_rpm_x10"] == 300 + assert abs(parsed["left_rpm"] - 3000.0) < 0.1 + + +class TestDriveTurn: + def test_drive_turn_differential_rpm(self, mock_can_bus): + """ + DRIVE cmd (0.5, −0.5) → verify differential RPM in velocity command. + """ + _send_drive(mock_can_bus, 0.5, -0.5) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left - 0.5) < 1e-4, f"Left speed {left} != 0.5" + assert abs(right - (-0.5)) < 1e-4, f"Right speed {right} != -0.5" + # Signs must be opposite for a zero-radius spin + assert left > 0 and right < 0 + + def test_drive_turn_fc_vesc_differential(self, mock_can_bus): + """Simulated FC_VESC for a turn has opposite-sign RPMs.""" + fc_payload = build_fc_vesc( + left_rpm_x10=150, right_rpm_x10=-150, + left_current_x10=30, right_current_x10=30, + ) + mock_can_bus.inject(FC_VESC, fc_payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_rpm_x10"] > 0 + assert parsed["right_rpm_x10"] < 0 + + +class TestDriveZero: + def test_drive_zero_stops_motors(self, mock_can_bus): + """ + After a non-zero drive cmd, sending zero velocity must result in + RPM=0 being commanded to both VESCs. + """ + _send_drive(mock_can_bus, 1.0, 1.0) + mock_can_bus.reset() # clear prior frames + + _send_drive(mock_can_bus, 0.0, 0.0) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left) < 1e-5, "Left motor not stopped" + assert abs(right) < 1e-5, "Right motor not stopped" + + +class TestDriveCmdTimeout: + def test_drive_cmd_timeout_sends_zero(self, mock_can_bus): + """ + Simulate the watchdog behaviour: if no DRIVE cmd arrives for >500 ms, + zero velocity is sent. We test the encoding directly (without timers). + """ + # The watchdog in CanBridgeNode calls encode_velocity_cmd(0.0, 0.0) and + # sends it on MAMBA_CMD_VELOCITY. Replicate that here. + zero_payload = encode_velocity_cmd(0.0, 0.0) + + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, zero_payload)) + mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE))) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left) < 1e-5 + assert abs(right) < 1e-5 + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert len(mode_frames) == 1 + assert bytes(mode_frames[0].data) == bytes([MODE_IDLE]) + + +@pytest.mark.parametrize("left_mps,right_mps", [ + (0.5, 0.5), + (1.0, 0.0), + (0.0, -1.0), + (-0.5, -0.5), +]) +def test_drive_cmd_payload_roundtrip(mock_can_bus, left_mps, right_mps): + """Parametrized: encode then decode must recover original velocities.""" + payload = encode_velocity_cmd(left_mps, right_mps) + l, r = parse_velocity_cmd(payload) + assert abs(l - left_mps) < 1e-4 + assert abs(r - right_mps) < 1e-4 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py new file mode 100644 index 0000000..b77581e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py @@ -0,0 +1,264 @@ +#!/usr/bin/env python3 +""" +test_estop.py — E-stop command integration tests. + +Covers: + - ESTOP command halts motors immediately + - ESTOP persists: DRIVE commands ignored while ESTOP is active + - ESTOP clear restores normal drive operation + - Firmware-side estop via FC_STATUS flags is detected correctly + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_estop.py -v +""" + +import struct +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + MAMBA_CMD_ESTOP, + ORIN_CMD_ESTOP, + FC_STATUS, + MODE_IDLE, + MODE_DRIVE, + MODE_ESTOP, + build_estop_cmd, + build_mode_cmd, + build_velocity_cmd, + build_fc_status, + parse_velocity_cmd, + parse_fc_status, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, +) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class _Msg: + """Minimal CAN message stand-in.""" + def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = is_extended_id + + +class EstopStateMachine: + """ + Lightweight state machine that mirrors the bridge estop logic. + + Tracks whether ESTOP is active and gates velocity commands accordingly. + Sends frames to the supplied MockCANBus. + """ + + def __init__(self, bus: MockCANBus): + self._bus = bus + self._estop_active = False + self._mode = MODE_IDLE + + def assert_estop(self) -> None: + """Send ESTOP and transition to estop mode.""" + self._estop_active = True + self._mode = MODE_ESTOP + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) + self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + + def clear_estop(self) -> None: + """Clear ESTOP and return to IDLE mode.""" + self._estop_active = False + self._mode = MODE_IDLE + self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False))) + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE))) + + def send_drive(self, left_mps: float, right_mps: float) -> None: + """Send velocity command only if ESTOP is not active.""" + if self._estop_active: + # Bridge silently drops commands while estopped + return + self._mode = MODE_DRIVE + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + + @property + def estop_active(self) -> bool: + return self._estop_active + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestEstopHaltsMotors: + def test_estop_command_halts_motors(self, mock_can_bus): + """ + After injecting ESTOP, zero velocity must be commanded immediately. + """ + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1, "No velocity frame after ESTOP" + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP" + assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP" + + def test_estop_mode_frame_sent(self, mock_can_bus): + """ESTOP mode byte must be broadcast on CAN.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any( + bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames + ), "MODE=ESTOP not found in sent frames" + + def test_estop_flag_byte_is_0x01(self, mock_can_bus): + """MAMBA_CMD_ESTOP payload must be 0x01 when asserting e-stop.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert len(estop_frames) >= 1 + assert bytes(estop_frames[-1].data) == b"\x01", \ + f"ESTOP payload {estop_frames[-1].data!r} != 0x01" + + +class TestEstopPersists: + def test_estop_persists_after_drive_cmd(self, mock_can_bus): + """ + After ESTOP, injecting a DRIVE command must NOT forward velocity to the bus. + """ + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + mock_can_bus.reset() # start fresh after initial ESTOP frames + + sm.send_drive(1.0, 1.0) # should be suppressed + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 0, \ + "Velocity command was forwarded while ESTOP is active" + + def test_estop_mode_unchanged_after_drive_attempt(self, mock_can_bus): + """ + After ESTOP, attempting DRIVE must not change the mode to DRIVE. + """ + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + mock_can_bus.reset() + + sm.send_drive(0.5, 0.5) + + # No mode frames should have been emitted (drive was suppressed) + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert all( + bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames + ), "MODE=DRIVE was set despite active ESTOP" + + +class TestEstopClear: + def test_estop_clear_restores_drive(self, mock_can_bus): + """After ESTOP_CLEAR, drive commands must be accepted again.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + sm.clear_estop() + mock_can_bus.reset() + + sm.send_drive(0.8, 0.8) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear" + l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(l - 0.8) < 1e-4 + assert abs(r - 0.8) < 1e-4 + + def test_estop_clear_flag_byte_is_0x00(self, mock_can_bus): + """MAMBA_CMD_ESTOP payload must be 0x00 when clearing e-stop.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + sm.clear_estop() + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert len(estop_frames) >= 2 + # Last ESTOP frame should be the clear + assert bytes(estop_frames[-1].data) == b"\x00", \ + f"ESTOP clear payload {estop_frames[-1].data!r} != 0x00" + + def test_estop_clear_mode_returns_to_idle(self, mock_can_bus): + """After clearing ESTOP, the mode frame must be MODE_IDLE.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + sm.clear_estop() + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + last_mode = bytes(mode_frames[-1].data) + assert last_mode == bytes([MODE_IDLE]), \ + f"Mode after ESTOP clear is {last_mode!r}, expected MODE_IDLE" + + +class TestFirmwareSideEstop: + def test_fc_status_estop_flag_detected(self, mock_can_bus): + """ + Simulate firmware sending estop via FC_STATUS flags (bit0=estop_active). + Verify the Orin bridge side correctly parses the flag. + """ + # Build FC_STATUS with estop_active bit set (flags=0x01) + payload = build_fc_status( + pitch_x10=0, + motor_cmd=0, + vbat_mv=24000, + balance_state=2, # TILT_FAULT + flags=0x01, # bit0 = estop_active + ) + mock_can_bus.inject(FC_STATUS, payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None, "FC_STATUS frame not received" + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["estop_active"] is True, \ + "estop_active flag not set in FC_STATUS" + assert parsed["balance_state"] == 2 + + def test_fc_status_no_estop_flag(self, mock_can_bus): + """FC_STATUS with flags=0x00 must NOT set estop_active.""" + payload = build_fc_status(flags=0x00) + mock_can_bus.inject(FC_STATUS, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["estop_active"] is False + + def test_fc_status_armed_flag_detected(self, mock_can_bus): + """FC_STATUS flags bit1=armed must parse correctly.""" + payload = build_fc_status(flags=0x02) # bit1 = armed + mock_can_bus.inject(FC_STATUS, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["armed"] is True + assert parsed["estop_active"] is False + + def test_fc_status_roundtrip(self, mock_can_bus): + """build_fc_status → inject → recv → parse_fc_status must be identity.""" + payload = build_fc_status( + pitch_x10=150, + motor_cmd=-200, + vbat_mv=23800, + balance_state=1, + flags=0x03, + ) + mock_can_bus.inject(FC_STATUS, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["pitch_x10"] == 150 + assert parsed["motor_cmd"] == -200 + assert parsed["vbat_mv"] == 23800 + assert parsed["balance_state"] == 1 + assert parsed["estop_active"] is True + assert parsed["armed"] is True diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py new file mode 100644 index 0000000..a7e9f9a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py @@ -0,0 +1,315 @@ +#!/usr/bin/env python3 +""" +test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests. + +Covers: + - VESC STATUS extended frame for left VESC (ID 56) triggers FC_VESC broadcast + - Both left (56) and right (68) VESC STATUS combined in FC_VESC + - FC_VESC broadcast rate (~10 Hz) + - current_x10 scaling matches protocol spec + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_fc_vesc_broadcast.py -v +""" + +import struct +import time +import threading +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + FC_VESC, + VESC_CAN_ID_LEFT, + VESC_CAN_ID_RIGHT, + VESC_STATUS_ID, + VESC_SET_RPM_ID, + VESC_TELEM_STATE, + build_vesc_status, + build_fc_vesc, + parse_fc_vesc, + parse_vesc_status, +) +from saltybot_can_bridge.mamba_protocol import ( + VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE, + decode_vesc_state, +) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class VescStatusAggregator: + """ + Simulates the firmware logic that: + 1. Receives VESC STATUS extended frames from left/right VESCs + 2. Builds an FC_VESC broadcast payload + 3. Injects the FC_VESC frame onto the mock bus + + This represents the Mamba → Orin telemetry path. + """ + + def __init__(self, bus: MockCANBus): + self._bus = bus + self._left_rpm_x10 = 0 + self._right_rpm_x10 = 0 + self._left_current_x10 = 0 + self._right_current_x10 = 0 + self._left_seen = False + self._right_seen = False + + def process_vesc_status(self, arb_id: int, data: bytes) -> None: + """ + Process an incoming VESC STATUS frame (extended 29-bit ID). + Updates internal state; broadcasts FC_VESC when at least one side is known. + """ + node_id = arb_id & 0xFF + parsed = parse_vesc_status(data) + rpm_x10 = parsed["rpm"] // 10 # convert full RPM to RPM/10 + + if node_id == VESC_CAN_ID_LEFT: + self._left_rpm_x10 = rpm_x10 + self._left_current_x10 = parsed["current_x10"] + self._left_seen = True + elif node_id == VESC_CAN_ID_RIGHT: + self._right_rpm_x10 = rpm_x10 + self._right_current_x10 = parsed["current_x10"] + self._right_seen = True + + # Broadcast FC_VESC whenever we receive any update + self._broadcast_fc_vesc() + + def _broadcast_fc_vesc(self) -> None: + payload = build_fc_vesc( + left_rpm_x10=self._left_rpm_x10, + right_rpm_x10=self._right_rpm_x10, + left_current_x10=self._left_current_x10, + right_current_x10=self._right_current_x10, + ) + self._bus.inject(FC_VESC, payload) + + +def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int, + current_x10: int = 50, duty_x1000: int = 250) -> None: + """Inject a VESC STATUS extended frame for the given node ID.""" + arb_id = VESC_STATUS_ID(vesc_id) + payload = build_vesc_status(rpm=rpm, current_x10=current_x10, duty_x1000=duty_x1000) + bus.inject(arb_id, payload, is_extended_id=True) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestVescStatusToFcVesc: + def test_left_vesc_status_triggers_broadcast(self, mock_can_bus): + """ + Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains + the correct left RPM (rpm / 10). + """ + agg = VescStatusAggregator(mock_can_bus) + + # Left VESC: 3000 RPM → rpm_x10 = 300 + arb_id = VESC_STATUS_ID(VESC_CAN_ID_LEFT) + payload = build_vesc_status(rpm=3000, current_x10=55) + agg.process_vesc_status(arb_id, payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None, "No FC_VESC broadcast after left VESC STATUS" + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_rpm_x10"] == 300, \ + f"left_rpm_x10 {parsed['left_rpm_x10']} != 300" + assert abs(parsed["left_rpm"] - 3000.0) < 1.0 + + def test_right_vesc_status_triggers_broadcast(self, mock_can_bus): + """Inject VESC STATUS for right VESC (ID 68) → verify right RPM in FC_VESC.""" + agg = VescStatusAggregator(mock_can_bus) + + arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT) + payload = build_vesc_status(rpm=2000, current_x10=40) + agg.process_vesc_status(arb_id, payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["right_rpm_x10"] == 200 + + def test_left_vesc_id_matches_constant(self): + """VESC_STATUS_ID(56) must equal (9 << 8) | 56 = 0x938.""" + assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == (9 << 8) | 56 + assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == 0x938 + + def test_right_vesc_id_matches_constant(self): + """VESC_STATUS_ID(68) must equal (9 << 8) | 68 = 0x944.""" + assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == (9 << 8) | 68 + assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == 0x944 + + +class TestBothVescStatusCombined: + def test_both_vesc_status_combined_in_fc_vesc(self, mock_can_bus): + """ + Inject both left (56) and right (68) VESC STATUS frames. + Final FC_VESC must contain both RPMs. + """ + agg = VescStatusAggregator(mock_can_bus) + + # Left: 3000 RPM + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_LEFT), + build_vesc_status(rpm=3000, current_x10=50), + ) + # Right: -1500 RPM (reverse) + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_RIGHT), + build_vesc_status(rpm=-1500, current_x10=30), + ) + + # Drain two FC_VESC frames (one per update), check the latest + frames = [] + while True: + f = mock_can_bus.recv(timeout=0.05) + if f is None: + break + frames.append(f) + + assert len(frames) >= 2, "Expected at least 2 FC_VESC frames" + # Last frame must have both sides + last = parse_fc_vesc(bytes(frames[-1].data)) + assert last["left_rpm_x10"] == 300, \ + f"left_rpm_x10 {last['left_rpm_x10']} != 300" + assert last["right_rpm_x10"] == -150, \ + f"right_rpm_x10 {last['right_rpm_x10']} != -150" + + def test_both_vesc_currents_combined(self, mock_can_bus): + """Both current values must appear in FC_VESC after two STATUS frames.""" + agg = VescStatusAggregator(mock_can_bus) + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_LEFT), + build_vesc_status(rpm=1000, current_x10=55), + ) + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_RIGHT), + build_vesc_status(rpm=1000, current_x10=42), + ) + frames = [] + while True: + f = mock_can_bus.recv(timeout=0.05) + if f is None: + break + frames.append(f) + last = parse_fc_vesc(bytes(frames[-1].data)) + assert last["left_current_x10"] == 55 + assert last["right_current_x10"] == 42 + + +class TestVescBroadcastRate: + def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus): + """ + Simulate FC_VESC broadcasts at ~10 Hz and verify the rate. + We inject 12 frames over ~120 ms, then verify count and average interval. + """ + _FC_VESC_HZ = 10 + _interval = 1.0 / _FC_VESC_HZ + + timestamps = [] + stop_event = threading.Event() + + def broadcaster(): + while not stop_event.is_set(): + t = time.monotonic() + mock_can_bus.inject( + FC_VESC, + build_fc_vesc(100, -100, 30, 30), + timestamp=t, + ) + timestamps.append(t) + time.sleep(_interval) + + t = threading.Thread(target=broadcaster, daemon=True) + t.start() + time.sleep(0.15) # collect ~1.5 broadcasts + stop_event.set() + t.join(timeout=0.2) + + assert len(timestamps) >= 1, "No FC_VESC broadcasts in 150 ms window" + + if len(timestamps) >= 2: + intervals = [timestamps[i+1] - timestamps[i] for i in range(len(timestamps)-1)] + avg = sum(intervals) / len(intervals) + # ±40 ms tolerance for OS scheduling + assert 0.06 <= avg <= 0.14, \ + f"FC_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms" + + def test_fc_vesc_frame_is_8_bytes(self): + """FC_VESC payload must always be exactly 8 bytes.""" + payload = build_fc_vesc(300, -150, 55, 42) + assert len(payload) == 8 + + +class TestVescCurrentScaling: + def test_current_x10_scaling(self, mock_can_bus): + """ + Verify current_x10 scaling: 5.5 A → current_x10=55. + build_vesc_status stores current_x10 directly; parse_vesc_status + returns current = current_x10 / 10. + """ + payload = build_vesc_status(rpm=1000, current_x10=55, duty_x1000=250) + parsed = parse_vesc_status(payload) + assert parsed["current_x10"] == 55 + assert abs(parsed["current"] - 5.5) < 0.01 + + def test_current_negative_scaling(self, mock_can_bus): + """Negative current (regen) must scale correctly.""" + payload = build_vesc_status(rpm=-500, current_x10=-30) + parsed = parse_vesc_status(payload) + assert parsed["current_x10"] == -30 + assert abs(parsed["current"] - (-3.0)) < 0.01 + + def test_fc_vesc_current_x10_roundtrip(self, mock_can_bus): + """build_fc_vesc → inject → recv → parse must preserve current_x10.""" + payload = build_fc_vesc( + left_rpm_x10=200, + right_rpm_x10=200, + left_current_x10=55, + right_current_x10=42, + ) + mock_can_bus.inject(FC_VESC, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_current_x10"] == 55 + assert parsed["right_current_x10"] == 42 + + @pytest.mark.parametrize("vesc_id", [VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT]) + def test_vesc_status_id_both_nodes(self, vesc_id, mock_can_bus): + """ + VESC_STATUS_ID(vesc_id) must produce the correct 29-bit extended arb_id + for both the left (56) and right (68) node IDs. + """ + expected = (9 << 8) | vesc_id + assert VESC_STATUS_ID(vesc_id) == expected + + @pytest.mark.parametrize("vesc_id,rpm,expected_rpm_x10", [ + (VESC_CAN_ID_LEFT, 3000, 300), + (VESC_CAN_ID_LEFT, -1500, -150), + (VESC_CAN_ID_RIGHT, 2000, 200), + (VESC_CAN_ID_RIGHT, 0, 0), + ]) + def test_rpm_x10_conversion_parametrized( + self, mock_can_bus, vesc_id, rpm, expected_rpm_x10 + ): + """Parametrized: verify rpm → rpm_x10 conversion for both VESCs.""" + agg = VescStatusAggregator(mock_can_bus) + agg.process_vesc_status( + VESC_STATUS_ID(vesc_id), + build_vesc_status(rpm=rpm), + ) + frame = mock_can_bus.recv(timeout=0.05) + assert frame is not None + parsed = parse_fc_vesc(bytes(frame.data)) + if vesc_id == VESC_CAN_ID_LEFT: + assert parsed["left_rpm_x10"] == expected_rpm_x10, \ + f"left_rpm_x10={parsed['left_rpm_x10']} expected {expected_rpm_x10}" + else: + assert parsed["right_rpm_x10"] == expected_rpm_x10, \ + f"right_rpm_x10={parsed['right_rpm_x10']} expected {expected_rpm_x10}" diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py new file mode 100644 index 0000000..adf7db2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +""" +test_heartbeat_timeout.py — Tests for heartbeat loss and recovery. + +Covers: + - Heartbeat loss triggers e-stop escalation (timeout logic) + - Heartbeat recovery restores previous mode + - Heartbeat interval is sent at ~100 ms + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_heartbeat_timeout.py -v +""" + +import time +import struct +import threading +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + ORIN_CMD_HEARTBEAT, + ORIN_CMD_ESTOP, + ORIN_CMD_MODE, + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + MAMBA_CMD_ESTOP, + MODE_IDLE, + MODE_DRIVE, + MODE_ESTOP, + build_heartbeat, + build_estop_cmd, + build_mode_cmd, + build_velocity_cmd, + parse_velocity_cmd, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, +) + +# Heartbeat timeout from orin_can.h: 500 ms +ORIN_HB_TIMEOUT_MS = 500 +ORIN_HB_TIMEOUT_S = ORIN_HB_TIMEOUT_MS / 1000.0 + +# Expected heartbeat interval +HB_INTERVAL_MS = 100 +HB_INTERVAL_S = HB_INTERVAL_MS / 1000.0 + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class HeartbeatSimulator: + """ + Simulate periodic heartbeat injection into a MockCANBus. + + Call start() to begin sending heartbeats every interval_s. + Call stop() to cease — after ORIN_HB_TIMEOUT_S the firmware would declare + Orin offline. + """ + + def __init__(self, bus: MockCANBus, interval_s: float = HB_INTERVAL_S): + self._bus = bus + self._interval_s = interval_s + self._seq = 0 + self._running = False + self._thread: threading.Thread | None = None + + def start(self): + self._running = True + self._thread = threading.Thread(target=self._run, daemon=True) + self._thread.start() + + def stop(self): + self._running = False + + def _run(self): + while self._running: + self._bus.inject( + ORIN_CMD_HEARTBEAT, + build_heartbeat(self._seq), + is_extended_id=False, + ) + self._seq += 1 + time.sleep(self._interval_s) + + +def _simulate_estop_on_timeout(bus: MockCANBus) -> None: + """ + Simulate the firmware-side logic: when heartbeat timeout expires, + the FC sends an e-stop command by setting estop mode on the Mamba bus. + We model this as the bridge sending zero velocity + ESTOP mode. + """ + + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) + bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestHeartbeatLoss: + def test_heartbeat_loss_triggers_estop(self, mock_can_bus): + """ + After heartbeat ceases, the bridge must command zero velocity and + set ESTOP mode. We simulate this directly using _simulate_estop_on_timeout. + """ + # First confirm the bus is clean + assert len(mock_can_bus.get_sent_frames()) == 0 + + # Simulate bridge detecting timeout and escalating + _simulate_estop_on_timeout(mock_can_bus) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1, "Zero velocity not sent after timeout" + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l) < 1e-5, "Left not zero on timeout" + assert abs(r) < 1e-5, "Right not zero on timeout" + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any( + bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames + ), "ESTOP mode not asserted on heartbeat timeout" + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert len(estop_frames) >= 1, "ESTOP command not sent" + assert bytes(estop_frames[0].data) == b"\x01" + + def test_heartbeat_loss_zero_velocity(self, mock_can_bus): + """Zero velocity frame must appear among sent frames after timeout.""" + _simulate_estop_on_timeout(mock_can_bus) + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1 + for f in vel_frames: + l, r = parse_velocity_cmd(bytes(f.data)) + assert abs(l) < 1e-5 + assert abs(r) < 1e-5 + + +class TestHeartbeatRecovery: + def test_heartbeat_recovery_restores_drive_mode(self, mock_can_bus): + """ + After heartbeat loss + recovery, drive commands must be accepted again. + We simulate: ESTOP → clear estop → send drive → verify velocity frame. + """ + + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + # Phase 1: timeout → estop + _simulate_estop_on_timeout(mock_can_bus) + mock_can_bus.reset() + + # Phase 2: recovery — clear estop, restore drive mode + mock_can_bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False))) + mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5))) + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert any(bytes(f.data) == b"\x00" for f in estop_frames), \ + "ESTOP clear not sent on recovery" + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any( + bytes(f.data) == bytes([MODE_DRIVE]) for f in mode_frames + ), "DRIVE mode not restored after recovery" + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1 + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l - 0.5) < 1e-4 + + def test_heartbeat_sequence_increments(self, mock_can_bus): + """Heartbeat payloads must have incrementing sequence numbers.""" + payloads = [] + for seq in range(5): + mock_can_bus.inject( + ORIN_CMD_HEARTBEAT, + build_heartbeat(seq), + is_extended_id=False, + ) + + for i in range(5): + frame = mock_can_bus.recv(timeout=0.05) + assert frame is not None + (seq_val,) = struct.unpack(">I", bytes(frame.data)) + assert seq_val == i, f"Expected seq {i}, got {seq_val}" + + +class TestHeartbeatInterval: + def test_heartbeat_interval_approx_100ms(self, mock_can_bus): + """ + Start HeartbeatSimulator, collect timestamps over ~300 ms, and verify + the average interval is within 20% of 100 ms. + """ + sim = HeartbeatSimulator(mock_can_bus, interval_s=0.1) + sim.start() + time.sleep(0.35) + sim.stop() + + timestamps = [] + while True: + frame = mock_can_bus.recv(timeout=0.01) + if frame is None: + break + if frame.arbitration_id == ORIN_CMD_HEARTBEAT: + timestamps.append(frame.timestamp) + + assert len(timestamps) >= 2, "Not enough heartbeat frames captured" + + intervals = [ + timestamps[i + 1] - timestamps[i] + for i in range(len(timestamps) - 1) + ] + avg_interval = sum(intervals) / len(intervals) + # Allow ±30 ms tolerance (OS scheduling jitter in CI) + assert 0.07 <= avg_interval <= 0.13, \ + f"Average heartbeat interval {avg_interval*1000:.1f} ms not ~100 ms" + + def test_heartbeat_payload_is_4_bytes(self, mock_can_bus): + """Heartbeat payload must be exactly 4 bytes (uint32 sequence).""" + for seq in (0, 1, 0xFFFFFFFF): + payload = build_heartbeat(seq) + assert len(payload) == 4, \ + f"Heartbeat payload length {len(payload)} != 4" diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py new file mode 100644 index 0000000..d87ab29 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py @@ -0,0 +1,236 @@ +#!/usr/bin/env python3 +""" +test_mode_switching.py — Mode transition integration tests. + +Covers: + - idle → drive: drive commands become accepted + - drive → estop: immediate motor stop + - MODE frame byte values match protocol constants + - Unknown mode byte is ignored (no state change) + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_mode_switching.py -v +""" + +import struct +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + MAMBA_CMD_ESTOP, + MODE_IDLE, + MODE_DRIVE, + MODE_ESTOP, + build_mode_cmd, + build_velocity_cmd, + parse_velocity_cmd, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, +) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class _Msg: + def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = is_extended_id + + +class ModeStateMachine: + """ + Minimal state machine tracking mode transitions and gating commands. + """ + + def __init__(self, bus: MockCANBus): + self._bus = bus + self._mode = MODE_IDLE + + def set_mode(self, mode: int) -> bool: + """ + Transition to mode. Returns True if accepted, False if invalid. + Invalid mode values (not 0, 1, 2) are ignored. + """ + if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + return False # silently ignore + + prev_mode = self._mode + self._mode = mode + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(mode))) + + # Side-effects of entering ESTOP from DRIVE + if mode == MODE_ESTOP and prev_mode == MODE_DRIVE: + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + + return True + + def send_drive(self, left_mps: float, right_mps: float) -> bool: + """ + Send a velocity command. Returns True if forwarded, False if blocked. + """ + if self._mode != MODE_DRIVE: + return False + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) + return True + + @property + def mode(self) -> int: + return self._mode + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestIdleToDrive: + def test_idle_to_drive_mode_frame(self, mock_can_bus): + """Transitioning to DRIVE must emit a MODE=drive frame.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert len(mode_frames) == 1 + assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) + + def test_idle_blocks_drive_commands(self, mock_can_bus): + """In IDLE mode, drive commands must be suppressed.""" + sm = ModeStateMachine(mock_can_bus) + # Attempt drive without entering DRIVE mode + forwarded = sm.send_drive(1.0, 1.0) + assert forwarded is False, "Drive cmd should be blocked in IDLE mode" + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 0 + + def test_drive_mode_allows_commands(self, mock_can_bus): + """After entering DRIVE mode, velocity commands must be forwarded.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + mock_can_bus.reset() + + forwarded = sm.send_drive(0.5, 0.5) + assert forwarded is True + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(l - 0.5) < 1e-4 + assert abs(r - 0.5) < 1e-4 + + +class TestDriveToEstop: + def test_drive_to_estop_stops_motors(self, mock_can_bus): + """Transitioning DRIVE → ESTOP must immediately send zero velocity.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + sm.send_drive(1.0, 1.0) + mock_can_bus.reset() + + sm.set_mode(MODE_ESTOP) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1, "No velocity frame on DRIVE→ESTOP transition" + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP" + assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP" + + def test_drive_to_estop_mode_frame(self, mock_can_bus): + """DRIVE → ESTOP must broadcast MODE=estop.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + sm.set_mode(MODE_ESTOP) + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any(bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames) + + def test_estop_blocks_subsequent_drive(self, mock_can_bus): + """After DRIVE → ESTOP, drive commands must be blocked.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + sm.set_mode(MODE_ESTOP) + mock_can_bus.reset() + + forwarded = sm.send_drive(1.0, 1.0) + assert forwarded is False + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 0 + + +class TestModeCommandEncoding: + def test_mode_idle_byte(self, mock_can_bus): + """MODE_IDLE must encode as 0x00.""" + assert encode_mode_cmd(MODE_IDLE) == b"\x00" + + def test_mode_drive_byte(self, mock_can_bus): + """MODE_DRIVE must encode as 0x01.""" + assert encode_mode_cmd(MODE_DRIVE) == b"\x01" + + def test_mode_estop_byte(self, mock_can_bus): + """MODE_ESTOP must encode as 0x02.""" + assert encode_mode_cmd(MODE_ESTOP) == b"\x02" + + def test_mode_frame_length(self, mock_can_bus): + """Mode command payload must be exactly 1 byte.""" + for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + payload = encode_mode_cmd(mode) + assert len(payload) == 1, f"Mode {mode} payload length {len(payload)} != 1" + + def test_protocol_defs_build_mode_cmd_matches(self): + """build_mode_cmd in protocol_defs must produce identical bytes.""" + for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + assert build_mode_cmd(mode) == encode_mode_cmd(mode), \ + f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})" + + +class TestInvalidMode: + def test_invalid_mode_byte_ignored(self, mock_can_bus): + """Unknown mode byte (e.g. 0xFF) must be rejected — no state change.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + initial_mode = sm.mode + mock_can_bus.reset() + + accepted = sm.set_mode(0xFF) + assert accepted is False, "Invalid mode 0xFF should be rejected" + assert sm.mode == initial_mode, "Mode changed despite invalid value" + assert len(mock_can_bus.get_sent_frames()) == 0, \ + "Frames sent for invalid mode command" + + def test_invalid_mode_99_ignored(self, mock_can_bus): + """Mode 99 must be rejected.""" + sm = ModeStateMachine(mock_can_bus) + accepted = sm.set_mode(99) + assert accepted is False + + def test_invalid_mode_negative_ignored(self, mock_can_bus): + """Negative mode values must be rejected.""" + sm = ModeStateMachine(mock_can_bus) + accepted = sm.set_mode(-1) + assert accepted is False + + def test_mamba_protocol_invalid_mode_raises(self): + """mamba_protocol.encode_mode_cmd must raise on invalid mode.""" + with pytest.raises(ValueError): + encode_mode_cmd(99) + with pytest.raises(ValueError): + encode_mode_cmd(-1) + + +@pytest.mark.parametrize("mode,expected_byte", [ + (MODE_IDLE, b"\x00"), + (MODE_DRIVE, b"\x01"), + (MODE_ESTOP, b"\x02"), +]) +def test_mode_encoding_parametrized(mode, expected_byte): + """Parametrized check that all mode constants encode to the right byte.""" + assert encode_mode_cmd(mode) == expected_byte -- 2.47.2