From 9d2b19104f40baf9a3a6ae5305e674a00252ff7d Mon Sep 17 00:00:00 2001 From: sl-uwb Date: Tue, 17 Mar 2026 11:36:37 -0400 Subject: [PATCH] feat: UWB geofence speed limiting (Issue #657) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add saltybot_uwb_geofence ROS2 package — Jetson-side node that subscribes to /saltybot/pose/authoritative (UWB+IMU fused PoseWithCovarianceStamped), enforces configurable polygon speed-limit zones (YAML), and publishes speed-limited /cmd_vel_limited with smooth ramp transitions. Emergency boundary: if robot exits outer polygon, cmd_vel is zeroed and /saltybot/geofence_violation (Bool) is latched True, triggering the existing e-stop cascade. Publishes /saltybot/geofence/status (JSON). Pure-geometry helpers (zone_checker.py) have no ROS2 dependency; 35 unit tests pass (pytest). ESP32 UWB firmware untouched. Co-Authored-By: Claude Sonnet 4.6 --- .../config/uwb_geofence_zones.yaml | 56 +++ .../launch/uwb_geofence.launch.py | 47 ++ .../src/saltybot_uwb_geofence/package.xml | 27 ++ .../resource/saltybot_uwb_geofence | 0 .../saltybot_uwb_geofence/__init__.py | 0 .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 205 bytes .../uwb_geofence_node.cpython-314.pyc | Bin 0 -> 16940 bytes .../__pycache__/zone_checker.cpython-314.pyc | Bin 0 -> 5456 bytes .../uwb_geofence_node.py | 296 ++++++++++++ .../saltybot_uwb_geofence/zone_checker.py | 104 ++++ .../src/saltybot_uwb_geofence/setup.cfg | 5 + .../src/saltybot_uwb_geofence/setup.py | 27 ++ .../saltybot_uwb_geofence/test/__init__.py | 0 .../test/__pycache__/__init__.cpython-314.pyc | Bin 0 -> 188 bytes ..._zone_checker.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 43625 bytes .../test/test_zone_checker.py | 177 +++++++ .../config/vesc_health_params.yaml | 27 ++ .../launch/vesc_health.launch.py | 50 ++ .../src/saltybot_vesc_health/package.xml | 33 ++ .../resource/saltybot_vesc_health | 0 .../saltybot_vesc_health/__init__.py | 0 .../__pycache__/__init__.cpython-314.pyc | Bin 0 -> 203 bytes .../health_monitor_node.cpython-314.pyc | Bin 0 -> 24291 bytes .../__pycache__/recovery_fsm.cpython-314.pyc | Bin 0 -> 12953 bytes .../health_monitor_node.py | 450 ++++++++++++++++++ .../saltybot_vesc_health/recovery_fsm.py | 223 +++++++++ .../src/saltybot_vesc_health/setup.cfg | 4 + .../ros2_ws/src/saltybot_vesc_health/setup.py | 27 ++ .../src/saltybot_vesc_health/test/__init__.py | 0 .../test/__pycache__/__init__.cpython-314.pyc | Bin 0 -> 187 bytes ...t_vesc_health.cpython-314-pytest-9.0.2.pyc | Bin 0 -> 72161 bytes .../test/test_vesc_health.py | 355 ++++++++++++++ 32 files changed, 1908 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/config/uwb_geofence_zones.yaml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/launch/uwb_geofence.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/resource/saltybot_uwb_geofence create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/uwb_geofence_node.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/zone_checker.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/uwb_geofence_node.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/zone_checker.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/test/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/test_zone_checker.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_uwb_geofence/test/test_zone_checker.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/config/vesc_health_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/launch/vesc_health.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/resource/saltybot_vesc_health create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/health_monitor_node.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/recovery_fsm.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/health_monitor_node.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/recovery_fsm.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/test/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/__init__.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/test_vesc_health.cpython-314-pytest-9.0.2.pyc create mode 100644 jetson/ros2_ws/src/saltybot_vesc_health/test/test_vesc_health.py diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/config/uwb_geofence_zones.yaml b/jetson/ros2_ws/src/saltybot_uwb_geofence/config/uwb_geofence_zones.yaml new file mode 100644 index 0000000..d4dc4ab --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/config/uwb_geofence_zones.yaml @@ -0,0 +1,56 @@ +# UWB Geofence Zones Configuration +# All coordinates are in the UWB local frame (metres, X-forward, Y-left) +# +# emergency_boundary: outer hard limit — robot must stay inside. +# If it exits, cmd_vel_limited is zeroed and /saltybot/geofence_violation +# is latched True (triggers e-stop cascade). +# +# zones: list of named speed-limit regions. +# vertices: flat [x1, y1, x2, y2, ...] polygon, at least 3 points. +# max_speed: m/s (linear) +# max_angular: rad/s (yaw) +# The most restrictive zone the robot is currently inside wins. +# Transitions between limits are smoothed over `transition_time` seconds. + +# ── Emergency boundary (6 m × 10 m space, centred at origin) ───────────────── +emergency_boundary: + vertices: + [-6.0, -5.0, + 6.0, -5.0, + 6.0, 5.0, + -6.0, 5.0] + +# ── Named speed-limit zones ─────────────────────────────────────────────────── +zones: + # Caution perimeter — 1 m inside the emergency boundary + - name: caution_perimeter + description: "Slow near room walls (1 m buffer inside emergency boundary)" + max_speed: 0.4 + max_angular: 0.8 + vertices: + [-5.0, -4.0, + 5.0, -4.0, + 5.0, 4.0, + -5.0, 4.0] + + # Dock approach — slow zone in front of the docking station (positive Y end) + - name: dock_approach + description: "Very slow approach corridor to docking station" + max_speed: 0.15 + max_angular: 0.3 + vertices: + [-0.75, 2.5, + 0.75, 2.5, + 0.75, 4.5, + -0.75, 4.5] + + # Static obstacle cluster A (example — update to match your layout) + - name: obstacle_cluster_a + description: "Caution zone around obstacle cluster near (+2, -1)" + max_speed: 0.25 + max_angular: 0.5 + vertices: + [1.0, -2.0, + 3.0, -2.0, + 3.0, 0.0, + 1.0, 0.0] diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/launch/uwb_geofence.launch.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/launch/uwb_geofence.launch.py new file mode 100644 index 0000000..adc7365 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/launch/uwb_geofence.launch.py @@ -0,0 +1,47 @@ +"""Launch file for saltybot_uwb_geofence node.""" + +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + pkg_share = str( + Path(__file__).parent.parent / "config" / "uwb_geofence_zones.yaml" + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + "zones_file", + default_value=pkg_share, + description="Path to YAML zones config file", + ), + DeclareLaunchArgument( + "transition_time", + default_value="0.5", + description="Speed ramp time in seconds", + ), + DeclareLaunchArgument( + "frequency", + default_value="20.0", + description="Node update frequency in Hz", + ), + Node( + package="saltybot_uwb_geofence", + executable="uwb_geofence_node", + name="uwb_geofence", + output="screen", + parameters=[ + { + "zones_file": LaunchConfiguration("zones_file"), + "transition_time": LaunchConfiguration("transition_time"), + "frequency": LaunchConfiguration("frequency"), + } + ], + ), + ] + ) diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/package.xml b/jetson/ros2_ws/src/saltybot_uwb_geofence/package.xml new file mode 100644 index 0000000..7f5114f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/package.xml @@ -0,0 +1,27 @@ + + + + saltybot_uwb_geofence + 0.1.0 + + UWB-position-based geofence speed limiter for SaltyBot (Issue #657). + Subscribes to /saltybot/pose/authoritative (UWB+IMU fused), enforces + configurable polygon speed zones, smooth transitions, and emergency + e-stop on boundary exit. + + SaltyLab UWB + MIT + + ament_python + rclpy + geometry_msgs + std_msgs + python3-yaml + + pytest + geometry_msgs + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/resource/saltybot_uwb_geofence b/jetson/ros2_ws/src/saltybot_uwb_geofence/resource/saltybot_uwb_geofence new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__init__.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f024b2e244dc48c2b0a52e880176eca301051935 GIT binary patch literal 205 zcmdPq_I|p@<2{{|u76m8BnAoLW?@ zU!0nx@93#toS0KmnUk2Lo0eIWTb@{ys-KlwQkTZlX-=wL5i8JYkmHI$j8DvrjEqIhKo$Tn Cr8ofq literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/uwb_geofence_node.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/uwb_geofence_node.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bb1ffda62867facb83d240268ae1cc1876576eb8 GIT binary patch literal 16940 zcmb_@Yit`=dgu(FBZ?HM_gfmhMN6V=`7Or}*|IHJvaO62%5mf{6giTZP$V-P+7`1x zwQcUMypLj&tH@@d;5jb-H1Hq$DC(c^MR|;Jf_`kEDQbb@ zD3N-V;^<*|NGIwVF|8Zci+U0pL<7Y7VdIcVG?6mHuz83PnV|x)V8|j`h6=^PA**Q3 z*0YH=Qm;rXg4j50A99EeQf3-19x4$_NZJhPQn8eznc=dba6@Uml%UhcbC%UL1ER!IdNVjWimX*)^RbETXE-iq-pNQ(`Z z8@ZAL7b(gQ^r)jQH#AXuvUF&Kakh}3v&{Sd0P#x`$Jx_-c$yFSc=kes=l$$xU@jo? z0y`ZR*bCmEIDb4WvYo@xXpCoD_U+y8bXhDHVpCC{5SZeltQhXMuxxJ>A*aG(ZzLS$ zd%ZDnHY@~0uNb((v+N8Xp5sMf-ZK}SiS~{G#6&=xJrTa*6#`yp`GV-3i|~HLV#urBqnYc z71Sg&>W#vfVo`!ND*ELCT@ZypXeQefU^{c+{5e+SgFG2@UOb*Ffp8G06ArPgisyJZ ztPLFS0NR-uUWl@MPgD#?Sf4lQ^ZI#<*uEfA(*s4i1o5B51bukrvpKr z?TjOYhomE3!3!PX1*eN;PkMc`EEck}Ucg`pK~Xmw2?ytApet8+K@9i+HK47Xl;C!XXnCEk%fvd z7#~>;;c3LdfMPRS&%b*Ek`g>@C>J<~ue*s$`tksOh5AS8MMAy0NmNo8~J;)fo1 z#IQP%KIK@(7)~L4=*Mvgy>K{RrbHSJXx*zWQP0t$fzwq|ZfZBp>EVE5;P5kY1~~4F zU{V9cdFk z!=1m-$A%!u@*y}6eK?h{F?!&TBTJ!Y${U3xAnyipeBj=R2B3nnwB*(8^1;1B;SV1j z{9xGh<2pzzz~Elh9e4-MYsjmkUZiJfj^=c|bPIJy|1StDnFL;p2_c-a5Pye)xiUd_ z=DcDQQ8r7_-j||qQ1nLmsosHM`7jQ8r+TIX!rWCjCwgD!#b`LxD}EVjZOI8nTd9wO=k4LK8L_Ak+=`QGHw1N!wDRqK2S{{*bsa1|2 zLkUGFgb92K;0M$wc-(EGs2Au3YLaAQaKY%NUW5CbqxNL!*$5OI%ii!R7$i7)(yY8s zB1ZB|5-tIgO8qgbT#_4p(L7GqU8vRL^to>e{yjK{@NC;p1800LjEOTp7lz>qo(p5) z3ZDyO=8fGnDEyJ^Fxv7vkliSocJ*$31w@oeP zS+$^5b=7QB-}0RL>MGf$zV%u4Bl>M>6+N?-!38@bq^@rAW>aPYFxD$+Eq1GO2E3vJ zYO6Cu-qd;IilMAnsmGPTTZ!@pn9G#376U}7@&*ui8fNP0&+tfRvlyl{;(mCz1YJP!x!?^@~X;R7n=Sd~4#gKbkds9-7drC=b z@rHdrtmP{yg@|J#N{$wD6_aO`H^5${q_tRC(N>D9bnCb(8^u)vq`JB^NPwv6Gb`}0 zUQ6H_ua&I1x0PhkSYgQoEZKBmCSI8f;UNI7dGB0MvTtJJX5_{m9+e8F1^&$#vZL9& zJ;$VC;J&@Fpy*LK;%wRaQK>YyEXx^ZOMiP*vY?a%98o0Ti`S`lZXVf5vMB(` zfSI6}4~dd(h8J@{jaR(E7%w?IT0?}zmP$OiG$uTlRJI{6OLxg6&#_eM*(y{>g`Qy8 z>(4?k9wJ3}u0g-#)Vx##%q+~7!25WRX8n@Q6Z8Ty$F*eu-N?;Tx>2GL z8c+}!Ln`$NyjSEsQCXrUOuB$~1#}>cY&7t7c=bRx;Zw_?dKLsA?w+qw* z#&+GDWU7`L)|k$96IEbKGYv_mA!Dt2V70$zc<$P1DEbQE~TumJTdAEEbEL~H_^CrwPlUjmtm|jvZc3H2G^Kf>U*NGZ}r3)b08;U z&#G^Y=~pwBzLw}2Sz|`EjNeucW}uLZqcyD)MsiN)3rUx+Mbm+Qnmf*+GEMuV~YhDXHVLBDCs|AzznHl6N~&QJ3u>V^wGGP!e}V&N$bhSp zO!ZRh2c7SCE)V|T%=gZ$F})8N>ka#Ldy1*fF0-Q6+ZR^KKB~P_n`+yeXx+C?(fi9X zW%WzGn@5*>S6kD4r;~lB6T61)(d&BJHbVbK4~tDeMrwnEhg=67Z}ejp65C05oSrlE zfHC0qDL4k z>(?<_F_1U8ds!g_ASnvjIOv}3@o%7`AY3VeDW$Q0z$oj+@PyvZE^u9+KIa10w@{j(=GG}ChiDHS)5Y{2RgoFdYhqkJQLTw`t9T;@ zAT<0bj8Y?Y(U4#8=%p0aCSr6O^THTi8jQ(pQmEhu*0zqSr6yID*sY2ozinf8t5f(YeWTVo~HqZ{}6|2B#u`)MG@44dVVk!nq%^C(JwFQ%Lyr9>p;BBO! z*0biQO~IqZT*)|SoN{RlYrAy2@hL3n^k#Gcb*&+{W+~BlJW)6w&SbpEiM!Mn^nxFwHLlNvJ zptT0px#kcn#6m>TeHE-8px2Io3YP$%lS;N?k{Csu8gJWLnyaG+#y9{i#9n|YI~UHU zFjve2|G4lTt7?@&x9xiaHJPB6^-cwWqcxJ=#O)W}2t*=^MxO1E3}_yZ=%aCcM?Wi> zVHEK0mkRwnT7L+yDgooCia%0ejvXM*u(CS4BW{E0D$wD$i3PhsHybZd2jeW0g#i<2 z@1tT=K%r7HfaVfZp5X`&Ara3CqTXqqugE2q1#7P6N9y3cIl?p1Q&r3`QVuT7*$dk3k^O5lJ^aE9rgHGm?&j!(kw|Axft}fMbC{xyWyL|c0AH=>FTY2SfYr6l%WdDn)4ldPn0YGo>T`B&k{7(7G z+jsrxBNNFZ6DjA#RLjemvW8{jdZDSf($D&GlA#?>7Bq+fUm*Hr>;w`bQH7&)&O`cyT;2_EMtLz2=zsdq>sp){Ri{-=nyZ zKdGraU1|LP?T*u3)L(U%47KZiR#|wuv*2f4g+nd+pY3ZNYS7=s|p! z7zl~ANTRO^2LMK*=Y@k13;ht>(aERIn~;v;(AwMOgX9>b^9apZr2rv1f-F#(s@e~j z_n8lD@7vN_N$Ry1nfRhRD6kra|i0Eq6=+odPG66 z6a$&d*tAkc-kz~;P$9TLO*ldj%MV9^#f%0ssBj1}gsTv2py&W(+nYdqiP)QZOu+)KxtOaoxveN@E%z-ud_tgFqgo@|bFL zkl`?#RQX~~$LaTHrDcU%mwV)*B4|KRxnHd|$dY-btcVHnQH0<0fCM)U5}bJh70sL> zSGrK>r;K=p%;pFf4nRUwXob{*PW1|uhw0cTvBJic~sw;C6bwuQW`g!(t{!u_~o<1o3j#!}F zrQp@#eEY$z8av4@de(UWI|t5#T?)SEod;)Wt~gT26-*+3P98V?s$N+GIk`G5*QMhu zZUa}CQ<`@cwHlmt18g3uEfuP8Db*;xMq5Q^sX(=SoSK1dz_b3B&IZDT$s7Zc zU*^nEs1)$71@lhU4NtS12xxzmiqV*Yo`)WEEko0ca1!&)*=%BR$&oWr3Ke*S&_j2H zK1^9X=nn>lKoE8P00m3`?&dxLtNjQeCiw-^Zf=fkh=bL{tcCK#hQ8C%qGheF~l6q+%^Y zfi!2mCMAbLc}3A6nPl5j6uYNN$(p1*gpHSPk(AN?MA)J~g_CW8dT1|A+neD(VQ*gc zr`r!C+YkJr{TPVBP;%q#>u)dlKbU)eZgCfgsbEhiY=QqoVatOC*UFo#^$n)Lv*41g_di&kQk=u6i2CBP1WmY03W`h5we=3!aYt&#Ue76%{L8<**2|4RS; z_I+vl{x$pljH52?Xh}L+mIqfV{&aNBu`5$rxm0++v^CMOf30*sn2uIP?sptWl(k}k z?S5%{!g=V^(nDY)TA91=eBl>m?T>6!ISaCH9hxUrsR9zixJs@wFtaEs3_h_sagZ>gQGW_rLg9 zuWPXoYt9oB)wYYkv?Z-=iH?K!#J|1v^K17HO#nr0wxN-|B$9c#>vbqfWPVef$dguUa5p0YJ97cYzHj-$zr zqifb<4~xs!%~V14x|On3{&D%zo6AlAyY@B@8izN(|3;#!FJrA)8qHWyivX35Bs-3L zOsATTXRN3^C`&t!C!NPX9!RyE%2*q28x!sO?$Rl?KV$9uur0Ck*vCz&9fKKbCZ0RHKlrvt~DK7V+M#3sW!>frkR!`({g)ogJ(!b^or{5> z0Y)er=To7h(TCH4M~5CfJ1{o%<*3~`V3b4I+Q3Ru`OHQN_U#ytF;(K*1R~ zQy%u+JgIs)ZD2RF+>Pu8m|M644%E-^v%=2?zap4OGx5{H<9XAQw^tgQ2!fCNtw-G8a%a$v+LHTn#P6SgE0wT$AH{5 ze}E}uqJ?)bz^h021_s}R;GEM+ZdIcB6?~X|9Rs{kg>PYi4h+Ko#9$BuoKoQi1~(ya z8aCZ`!V=_04G^@0f>u_jEW(@Ay!F13z3jE?oouC_0dV&6X;0hR6ZUpE=Tzk=9-Enx zo)zz^er00y`0AT?OIK$Shc71_uRhW1Kr?BgI=U7I*8oa!I#A5+8x9cv?elBQo`=?o zZB95i&l;CY;ZR%N4d>Xh9?q|o7Cg7gzUaC2DKoJ3ctVi@cJ5cD#IV4L5yjPvXAf=c zLd{Lr@*WatjYBzka8jW0g2$+A-|nEvXL+79$_TV0CPxAX)sOHNlsok@k3`mt1v6-} zG>Gjyt69U9B{gkX2%FhCGAxwmK^ug#sNG<<+rHoNDYJu&&FLWAn#{c6=_Ip!r!Ymh zNTQ2Ec3$q-Ag0K&*0@dN1?AeJdOSe7vrvPO?wGuXP{0r{aA5(C>bKvZJ~NCzHK14bI`QiM{G1Ma4?e$0 zgWmUZ3+VDbx1dhz^D!OXJWo4eaXiKqLdZvuZO~gYfRrrGPDniG?$p77uIn|Z_orme zN(lnqp^{1FY{6+s;_OX$a z$qO7htUd-v!MpJ>_5n1MG*PI){s2K3u*=Vcu3Je$3~m>=C*f&9`1e2oBg3nB0UlR# z_Q{2JLmej_KYkWIL}7Pi$G-!<%R-rg-64};2Rk*7avb>@X9tpFM>i{fUV`7RL}%e+ z78iWwp@+{k1hAIkK3aiDK$WQQXAnfOD{z>~>)<<>jRBz-iuZ6>nv2%Vl@IJCH~0Uq zBg^|z_8kdkM<#D(VadWd?PO4h@G{c%3mLN)53Zc} z=**onAC2A_O_d&87|obV*UXK}qpN$?T0x~y4aWOLW2Um^R{Y)gt*^cNwPpT(W!Iwq zhUL2DJJ$U9-?cu};PZg0S)~=X`rqxp^}@R^ES&h}nXjLDXEak*d28g|ky~fqJquYQ zUmtnrY@VzKWtB^{OV-=Im9C$>^`p0b^7fD3PPLy*m7P-S6c#O<*?I|lg}f===UsTP z=Feack*_DKO{ ztq1oHqwO=s3ho_7+wayj^_2#DpQ-xyXbUv>{fYsW`h+bUaOpqk(hcmQKIx$`?b2g< zSI6-xJu>syQ((-AfWiv5p&~N&(=qs{6PSDf7ndx|u40iH{eXjksjN?wIFCMA0?Z#J z;PETqy&xdFA>jN99t{2u1d@eN+l7=83NWJ>KJn1Lymz5<2cCBF2L@K9Qg&JWG@g0K+!-JaW$B8nU%q(FC$$>`ZL|8@;WVgvSBWIq(ydVoRM})tD z0@!0z6sHX+P1BDI<+R~*7e%-H6IJ+kRN*hFs$WsN?^C5Y5wW=buKb#s%+o`3ZCQ_A3Rc%p(Dpg9)z2mWW z1C&<1(wuYd+;iXOJLk;25Da(`lppCoPk$Uh=nGP@ip><52R{IzfOw>%LBunMnM173 zZX_~$*rwZn+YZ|gIdsP%r|#T{&NvBJXStb-&iVu%S!{;U3sph#q z7e3%lchNL79OC0H{b$Y$W6^?)XWDl2+R7p-i zQ(Dqq;*z;cCeP*MwBDghnl27yB+%_s6r4!QqAqb-Uel%Q9y(@585T2K5{p?W%8_X$ zEz03^YDDiC5#S;yRj)z&p{#sho4acdRnvsSX*c5q4 zvBLvb!93tVD4-Y`WOrXiC;{)*qc@llhG%$|xAFEaav+YcSWq+U$&9E^_{f-NlNm+S z&lyfE={YPLt`kZopHk!>BUptK4kRn;8kuafA$OhD;FP*FX{c-8;VwU(8I%~(vYIZb7Mns>sX)i_8a$P_~Y@VQ;_0u4EBfW;nAK0 zQb3Q8m)XxOQd&O0K>4kdS$_Z;h@KJ=NO8dfC*pVzg{>6OfOUFsUfY-CK{o27x|9r} z-D7ZyYso1NESWk2^L|oKKk8wvQAVDqG3eag@PyIW|6(+UwWh(Rta|&>>O$x=lVdwk zj_n6%T?oaPYCs9hAQQEQ0$$y=jXlmDU5}6lJqNv2rWNQ$Dy31_Hc{?VX-vYL4))J&2FoR?VVhxt zu7-DIwT1(Wa!Lxbn3$5mq%{JhEgbffYNIM*O^TBPid);jP0&IO_7VsM^bbFB`-oh zyGLi5Ps|42m~p=`AMKvr^vTIPO&_1R%iN3h&ieX3kM>SoC~dkXloI6&lRK`TpY?Bs zcc1(EX6$|c{JpPFBgXy9POi7!UgJO9@ER_z z+;C9;VK8~arJ9}td!PxEaSi*BqGT|MAz=plfSI0z9vTHSB?Qo#IsGHlz%(opQg$p5 zHq}Pi^j6C5BxoBBLXY8PR6@lL57ZqGz67BL-^<8UZ86)Rjp7hA*8`SY;Zf}|mahp$ zV{oxJ7Gp!G{}4j7HZF+7rwW${rG8--89~TJX?TPwp$Mz1Ytd!gLd$n+TF5ypm6)R% zld4DX?A`-@0?uwdirKd=X+@A~KQoS{qeo1P#>frWCVt3^`Ho>Q<+PmQR3$Cz9cj5^ z=@K{+kO=IkSJ${xI42RrwT}a8!vsQ_j?=?B#ILlh0n!N26q#aeNXf|w5yGO9^hTD( zi*$0_Qe;>GDn;86>6(x*V5_}PAj9wJ%(>Q4x52UBMs{sIpHl^ws2-K;TEF11*N!Ua?gVnH6siH#hS-o zJb2Y#*jI76i`|!Vf2eD!1R4tcl|WO8zjpRgf1&q}4b4}aMfScwc%{G0e&oF8+gMq* zVRF~hg`1;uEuGVwZ*QGz*>i`VYk9SJv{DzkSGTdUq4h>f$yupye6M#gfErsCg2>-c zIA$6o2_#lvA?E+Tz>1}?a#2{BN3{khzye`Y5@pjV z%I>z&w#VT_4xPg3GJxU9aDp_0gfwt}wE@x!gsnAU(|W7gszTlhH0=Y+aEaM1T>#+M z2&oRCtvXmyK&FJY+He-LR|oIM^sm%HQ24MKkoZ&KCpB>WMRlzPp#GSjH}~B}_r07j zoFMo8jj{;|&gyxDHPyB4Bz{0q1IG#j%PxVp4UjyC7T#XlKjy7oSM6vqUv*pcnq&Ux z^5_UzvszpY3UckZ)qRC;KYd&^q}B~QYyWlCJB~52P<4HI4FO@6*iwqR<)i;X=u|+p z9bk?EmPkqxvysGgB7ZC`r?a^%mw;SDB*}RaD_P5-LGB^Sc}_~Cz#S#IWw*t(o2fQ9 zBrjyI+LEKxl@3ixWhGf(8Ez~Mxy5is%&OqZApdv^+z+JRonFJ1mXoRazjj>u=)((p z|6<)w$`lq}4ZEn1!xXWw5r}(`G>;gLk-VzthAk_O14+x_`foZx2fU3Z@CN9E8%akN zR9P`^utUIyfG2<_@`e{y8;6u6u3a#ggu&=&OC!G&8l)B%4 zv&dBH*IsWSt`mf^IN3JUJ-KeKzRi5oczsQI^W@fIZ)Me*>p~e%j?JywQuJ1QtICdh zz76xCrVqW}_m;aScTRQR+&LSHT-o$9N~#eFbjd1!LW^|Mo(Z}r?9z4=0kE%w%oIECq$pz8C#|6-+S)zt%w z%_z8fp#}Nt3$Oiuewu{@$n?`rGyQQ5=P(8vqBn0ibrGi|-LS(qpGxzeXS(X+a0Sb* z+Q~F7A|T~e3xwTrP&R#2rw6hB2*%5La zktioaMh+ zb8QO_s4qCt=B*1ZBDsm=A(9uZ-?Zoh$vn#Hz(+ofs4ad%XB_})mJ>_C&Z-j*;c?jx zRk!diwZly`^L6v4Rl;{HWZAo`0~pDXt3#dY6`E`6zGLhe27g+eP=wI{Ss5)tuVwod zSN+0Ig_v-0n-i{(otQ8)X0F}3#VCAP()-LWOA}EDHs)1AI&F#2Ziy3NFO4QGxi None: + super().__init__("uwb_geofence") + + # ── Parameters ──────────────────────────────────────────────────────── + self.declare_parameter( + "zones_file", + str(Path(__file__).parent.parent.parent / "config" / "uwb_geofence_zones.yaml"), + ) + self.declare_parameter("transition_time", 0.5) # seconds + self.declare_parameter("frequency", 20.0) # Hz + self.declare_parameter("default_max_speed", 1.0) # m/s (outside all named zones) + self.declare_parameter("default_max_angular", 1.5) # rad/s + self.declare_parameter("pose_topic", "/saltybot/pose/authoritative") + self.declare_parameter("cmd_vel_topic", "/cmd_vel") + + zones_file = self.get_parameter("zones_file").value + self._transition_time: float = self.get_parameter("transition_time").value + frequency: float = self.get_parameter("frequency").value + self._default_max_speed: float = self.get_parameter("default_max_speed").value + self._default_max_angular: float = self.get_parameter("default_max_angular").value + pose_topic: str = self.get_parameter("pose_topic").value + cmd_vel_topic: str = self.get_parameter("cmd_vel_topic").value + + # ── Load zones config ───────────────────────────────────────────────── + self._zones: List[Dict[str, Any]] = [] + self._emergency_boundary: List[Tuple[float, float]] = [] + self._load_zones(zones_file) + + # ── Runtime state ───────────────────────────────────────────────────── + self._robot_x: float = 0.0 + self._robot_y: float = 0.0 + self._pose_received: bool = False + self._last_cmd_vel: Optional[Twist] = None + + # Current smoothed speed limits (ramp toward target) + self._current_max_speed: float = self._default_max_speed + self._current_max_angular: float = self._default_max_angular + + # E-stop latch (requires operator reset) + self._violation_latched: bool = False + + # ── Subscriptions ───────────────────────────────────────────────────── + self.create_subscription( + PoseWithCovarianceStamped, + pose_topic, + self._on_pose, + 10, + ) + self.create_subscription( + Twist, + cmd_vel_topic, + self._on_cmd_vel, + 10, + ) + + # ── Publications ────────────────────────────────────────────────────── + self._pub_limited = self.create_publisher(Twist, "/cmd_vel_limited", 10) + self._pub_status = self.create_publisher(String, "/saltybot/geofence/status", 10) + self._pub_violation = self.create_publisher(Bool, "/saltybot/geofence_violation", 1) + + # ── Timer ───────────────────────────────────────────────────────────── + self._dt: float = 1.0 / frequency + self.create_timer(self._dt, self._tick) + + self.get_logger().info( + f"uwb_geofence started: {len(self._zones)} zone(s), " + f"freq={frequency:.0f}Hz, transition={self._transition_time}s" + ) + + # ── Config loading ──────────────────────────────────────────────────────── + + def _load_zones(self, path: str) -> None: + """Parse YAML zones file and populate self._zones and self._emergency_boundary.""" + p = Path(path) + if not p.exists(): + self.get_logger().warn(f"zones_file not found: {path}; running with empty zones") + return + + with open(p) as fh: + cfg = yaml.safe_load(fh) + + # Emergency boundary (mandatory) + eb = cfg.get("emergency_boundary", {}) + flat = eb.get("vertices", []) + if flat: + try: + self._emergency_boundary = parse_flat_vertices(flat) + except ValueError as exc: + self.get_logger().error(f"emergency_boundary vertices invalid: {exc}") + else: + self.get_logger().warn("No emergency_boundary defined — boundary checking disabled") + + # Named speed zones + for z in cfg.get("zones", []): + try: + verts = parse_flat_vertices(z["vertices"]) + except (KeyError, ValueError) as exc: + self.get_logger().error(f"Skipping zone '{z.get('name', '?')}': {exc}") + continue + self._zones.append( + { + "name": z.get("name", "unnamed"), + "description": z.get("description", ""), + "max_speed": float(z.get("max_speed", self._default_max_speed)), + "max_angular": float(z.get("max_angular", self._default_max_angular)), + "vertices": verts, + } + ) + self.get_logger().info( + f" zone '{z['name']}': max_speed={z.get('max_speed')} m/s, " + f"{len(verts)} vertices" + ) + + # ── Callbacks ───────────────────────────────────────────────────────────── + + def _on_pose(self, msg: PoseWithCovarianceStamped) -> None: + self._robot_x = msg.pose.pose.position.x + self._robot_y = msg.pose.pose.position.y + self._pose_received = True + + def _on_cmd_vel(self, msg: Twist) -> None: + self._last_cmd_vel = msg + + # ── Main loop ──────────────────────────────────────────────────────────── + + def _tick(self) -> None: + """Called at `frequency` Hz. Compute limits, ramp, publish.""" + if not self._pose_received or self._last_cmd_vel is None: + return + + x, y = self._robot_x, self._robot_y + + # ── Emergency boundary check ────────────────────────────────────────── + if self._emergency_boundary: + inside_boundary = point_in_polygon(x, y, self._emergency_boundary) + if not inside_boundary: + self._trigger_violation(x, y) + return + + # Clear latch once back inside boundary (manual operator resume) + if self._violation_latched: + # Publish safe=False until operator clears via /saltybot/geofence_violation + # (this node keeps publishing zero until reset) + self._publish_zero_cmd() + self._pub_violation.publish(Bool(data=True)) + return + + # ── Zone classification ─────────────────────────────────────────────── + active_zones: List[str] = [] + target_max_speed = self._default_max_speed + target_max_angular = self._default_max_angular + + for zone in self._zones: + if point_in_polygon(x, y, zone["vertices"]): + active_zones.append(zone["name"]) + if zone["max_speed"] < target_max_speed: + target_max_speed = zone["max_speed"] + if zone["max_angular"] < target_max_angular: + target_max_angular = zone["max_angular"] + + # ── Smooth ramp toward target limits ────────────────────────────────── + ramp_rate_speed = (self._default_max_speed / max(self._transition_time, 0.01)) + ramp_rate_angular = (self._default_max_angular / max(self._transition_time, 0.01)) + + self._current_max_speed = ramp_toward( + self._current_max_speed, target_max_speed, ramp_rate_speed * self._dt + ) + self._current_max_angular = ramp_toward( + self._current_max_angular, target_max_angular, ramp_rate_angular * self._dt + ) + + # ── Apply limits to cmd_vel ─────────────────────────────────────────── + cmd = self._last_cmd_vel + lx, ly, lz, ax, ay, wz = apply_speed_limit( + cmd.linear.x, cmd.linear.y, cmd.angular.z, + self._current_max_speed, self._current_max_angular, + linear_z=cmd.linear.z, angular_x=cmd.angular.x, angular_y=cmd.angular.y, + ) + limited = Twist() + limited.linear.x = lx + limited.linear.y = ly + limited.linear.z = lz + limited.angular.x = ax + limited.angular.y = ay + limited.angular.z = wz + self._pub_limited.publish(limited) + + # ── Status ─────────────────────────────────────────────────────────── + dist_to_boundary = ( + min_dist_to_boundary(x, y, self._emergency_boundary) + if self._emergency_boundary + else -1.0 + ) + status = { + "robot_xy": [round(x, 3), round(y, 3)], + "active_zones": active_zones, + "current_max_speed": round(self._current_max_speed, 3), + "current_max_angular": round(self._current_max_angular, 3), + "target_max_speed": round(target_max_speed, 3), + "dist_to_emergency_boundary": round(dist_to_boundary, 3), + "violation": False, + } + self._pub_status.publish(String(data=json.dumps(status))) + self._pub_violation.publish(Bool(data=False)) + + # ── Helpers ─────────────────────────────────────────────────────────────── + + def _trigger_violation(self, x: float, y: float) -> None: + """Robot has exited emergency boundary. Zero all motion, latch violation.""" + if not self._violation_latched: + self.get_logger().error( + f"GEOFENCE VIOLATION: robot ({x:.3f}, {y:.3f}) is outside emergency boundary!" + ) + self._violation_latched = True + self._publish_zero_cmd() + self._pub_violation.publish(Bool(data=True)) + status = { + "robot_xy": [round(x, 3), round(y, 3)], + "active_zones": [], + "current_max_speed": 0.0, + "current_max_angular": 0.0, + "target_max_speed": 0.0, + "dist_to_emergency_boundary": 0.0, + "violation": True, + } + self._pub_status.publish(String(data=json.dumps(status))) + + def _publish_zero_cmd(self) -> None: + self._pub_limited.publish(Twist()) + + + +def main(args=None) -> None: + rclpy.init(args=args) + node = UwbGeofenceNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/zone_checker.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/zone_checker.py new file mode 100644 index 0000000..7c4d3ef --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/zone_checker.py @@ -0,0 +1,104 @@ +"""zone_checker.py — Pure-geometry helpers for UWB geofence zones. + +No ROS2 dependencies; fully unit-testable. + +Coordinate system: UWB local frame, metres, right-hand X-Y plane. +""" + +from __future__ import annotations + +import math +from typing import List, Tuple + +Vertex = Tuple[float, float] +Polygon = List[Vertex] + + +def parse_flat_vertices(flat: List[float]) -> Polygon: + """Convert flat [x1, y1, x2, y2, ...] list to [(x1,y1), (x2,y2), ...]. + + Raises ValueError if fewer than 3 vertices are provided or the list + length is odd. + """ + if len(flat) % 2 != 0: + raise ValueError(f"Flat vertex list must have even length, got {len(flat)}") + if len(flat) < 6: + raise ValueError(f"Need at least 3 vertices (6 values), got {len(flat)}") + return [(flat[i], flat[i + 1]) for i in range(0, len(flat), 2)] + + +def point_in_polygon(x: float, y: float, polygon: Polygon) -> bool: + """Ray-casting point-in-polygon test. + + Returns True if (x, y) is strictly inside or on the boundary of + *polygon*. Polygon vertices must be ordered (CW or CCW). + """ + n = len(polygon) + if n < 3: + return False + inside = False + x1, y1 = polygon[-1] + for x2, y2 in polygon: + # Check if the ray from (x,y) rightward crosses edge (x1,y1)-(x2,y2) + if (y1 > y) != (y2 > y): + x_intersect = (x2 - x1) * (y - y1) / (y2 - y1) + x1 + if x <= x_intersect: + inside = not inside + x1, y1 = x2, y2 + return inside + + +def min_dist_to_boundary(x: float, y: float, polygon: Polygon) -> float: + """Minimum distance from (x, y) to any edge of *polygon* (metres). + + Uses point-to-segment distance with clamped projection. + """ + if len(polygon) < 2: + return float("inf") + min_d = float("inf") + n = len(polygon) + for i in range(n): + ax, ay = polygon[i] + bx, by = polygon[(i + 1) % n] + dx, dy = bx - ax, by - ay + seg_len_sq = dx * dx + dy * dy + if seg_len_sq < 1e-12: + d = math.hypot(x - ax, y - ay) + else: + t = max(0.0, min(1.0, ((x - ax) * dx + (y - ay) * dy) / seg_len_sq)) + px, py = ax + t * dx, ay + t * dy + d = math.hypot(x - px, y - py) + if d < min_d: + min_d = d + return min_d + + +# ── Velocity helpers (no ROS2 deps — also used by tests) ───────────────────── + +def ramp_toward(current: float, target: float, step: float) -> float: + """Move *current* toward *target* by at most *step*, never overshoot.""" + if current < target: + return min(current + step, target) + if current > target: + return max(current - step, target) + return current + + +def apply_speed_limit(linear_x: float, linear_y: float, angular_z: float, + max_speed: float, max_angular: float, + linear_z: float = 0.0, + angular_x: float = 0.0, + angular_y: float = 0.0 + ) -> tuple: + """Return (linear_x, linear_y, linear_z, angular_x, angular_y, angular_z) + with linear speed and angular_z clamped to limits. + """ + speed = math.hypot(linear_x, linear_y) + if speed > max_speed and speed > 1e-6: + scale = max_speed / speed + lx = linear_x * scale + ly = linear_y * scale + else: + lx, ly = linear_x, linear_y + wz = max(-max_angular, min(max_angular, angular_z)) + return lx, ly, linear_z, angular_x, angular_y, wz diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/setup.cfg b/jetson/ros2_ws/src/saltybot_uwb_geofence/setup.cfg new file mode 100644 index 0000000..522a50f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_uwb_geofence + +[install] +install_scripts=$base/lib/saltybot_uwb_geofence diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/setup.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/setup.py new file mode 100644 index 0000000..1ef5870 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup, find_packages + +setup( + name="saltybot_uwb_geofence", + version="0.1.0", + packages=find_packages(), + data_files=[ + ( + "share/ament_index/resource_index/packages", + ["resource/saltybot_uwb_geofence"], + ), + ("share/saltybot_uwb_geofence", ["package.xml"]), + ("share/saltybot_uwb_geofence/config", ["config/uwb_geofence_zones.yaml"]), + ("share/saltybot_uwb_geofence/launch", ["launch/uwb_geofence.launch.py"]), + ], + install_requires=["setuptools", "pyyaml"], + zip_safe=True, + author="SaltyLab UWB", + author_email="sl-uwb@saltylab.local", + description="UWB geofence speed limiter for SaltyBot (Issue #657)", + license="MIT", + entry_points={ + "console_scripts": [ + "uwb_geofence_node=saltybot_uwb_geofence.uwb_geofence_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__init__.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..aecbbf0d0fbc3908fd65fc6cbe81754e0d079de6 GIT binary patch literal 188 zcmdPq_I|p@<2{{|u766{#OuoLW?@ zU!0nx@93#toS0KmnUk2Lo0eIWTb@{ys-KlwQk>|XEI)>gK>celye zkScP%s;m2b?{&Y|J?hbnKz0JF*65q}UcdMHP4|1h_kG{{-q%mm)rJ)ulg1yURcGnGaqJEIyZzWs-*4uy^I zp=zW0P>oTuR#~pBQ?$SeMGJQMdEG{ByQ0-S?=$M~9l>{m z7uWO2*y(iKh@DO9Mka!^r!Mh%*e+2vcsAFxIW^# z-#+_(_aG3*9e+_Ve7NiWmm)^sAtj4@zd|vBnjbVo_tB`(f}oXJ$WWt-QKePjw^f?U zPvAElqegukZHih=O4!xYRVwixP!#p{ze4IorB|H8W2k)I*JZ_XKE&1MeM9S_{+>gM zGSs<=O5+&#o}#Q)dacK(D97y1!cQ}bhg}s7bo#XLsm4x!mz6{FY5v{{JKHhU4$1Fz zN|zPciSL0&3a-RDtjtZJbxKsU*=}L~ih^=A?lOhtK7n>=0mm=$&hNMHO^1K8_!Wwh z3u-|v6b*LxT&bbnh<#N@&GmMg{KhW`*)`kWXM9@4E}y=QJ=W@X?aXE%H?qYHQCG`T zxbuU$cs`F<%cGCwMz*)H#O+7hRGpqOBUah>_VT1!N@s-SpwN}z6~D~i=9j0DF7hK? zmhgSx-?Qh6??NM8c1v5nWzQbgg3h*lTa`{d$PDkxjnDv##dWhGmWZd*F+G{n6WKwG znWR~R)3LtcRN6>o@@7RkiyV1#Wh{RtJDl!kSz`m~Y+pPb%ftthW>qX7Kb@o+%)0&T zC#SNRgSwv8&88TJ@E}HD@_a5G&%_O!X{#`U`DFUEUPW!Gri<|#wappX$|JmZmou^> zPY-xw3ms-($4T7dd`CXn*RlU_M?Ri5M$+-VCr_vJ!E^XA9iK@W`D~^`&*rzq&LN+k z;F0@*#4^66vCRZN15jk{IwLa78q0wOx69Grsl*}>9ii=Fn0CL^N1r-;VR3m|b z$yWgf1z-|(f=MrO@k0*Rod2c3(6(rR!OO=sS?3S>tYI%;<(Olz*|h**s-r;wOwFkc z1pp&p<(Ma2xBzAy0}6R1&w($8uRT2Nw)Q_jtO47G}qc?0(E?g+XeQmfR|Xs7@u<V2TcoOwpT(tRjMrhWTAzO<}?`9lgN?Y}=i{=EAaWC$R0zSE3*XwiX@TUomSL zt{8eMo*76d%?1wYSB*B=-Sy%TA?wEeXUlj@qwFEH}z7jCL+_-964 zGkQV9MBw$j2u`VM95IoMiY{#zF=Vo1v$951e)$EF#~HvGy%p_rffn5X23o(%enBrN zKaL5W#!eyrLUuw8Y*@FIXO}PL8}<(eVuE8?tMpRM>;Uf+knpz_3^Dz6Ss+O94N8mh zgtE!XKY&_SD=Dzlk0@)|ALZB;ctlD0+Ja^af26f++ zpjpAET(d%tr}E%L!_jzpILT&7*4wW5SO?0}1ig_z{D;`X&-VAnFg6B^Gcl{^=6QO- zoZQq`^N)j|JJdFP`Nc22_*+knM{m}4&DM6kQ?vGWBQrHy?gW&&?MT9BsVsrmGO)p2=f)Y9;3#E7CoW2Xh`2<$+;g`+EPAAVnWPt3D zx1Yk6d7gsdRf6y#Pu;@#BzkYwJ~dnW6rRLXOV>=zc6t=MkcQ7x?NcAoG&*dm zF~)7hVrDoN8_f0(r;`ZR#A4t}(!_@$7Kz2c_~earDwE7)V=;X_itAg5>?X1g#9SVW z(-JGK+(%q~o>$;*zd5m6EO?W2ssNXq5&j z)Cg-TXth=aTBAYcG-|bK&^isWrV-Iu+&Rd9IlmOoPBl9C5DQ3h0nImbAR7D?W$2_0CE%Z4DBw!G`KQ`w z2>esssV)cqL}JiFc#?GmSMuhc=-#u#o~7-!mdQWa#TC0ec$5IRr=We}SP7nHVf>6% z`61i`YuZr6Z&=_xm{T720Nhi_dmwl(t*T_+pnZQkDqMV8g%$?4T~WZNRW5)}tF&vj zzrm-~fKT&rUM&{@zji{a!AOZ%6WZVIJ)u)i)ai#4m$r>amrgy*tUh`4@QK**7f$Zi z4pMTr83d2D<%5Vr8s54Mj2tCzI|`8Tr}B_j4-hZbZL?_z6tHP1vY838U3LI6H8F0f zJWJjRe(p+`HA%-HE1Pq>qH@k{4QsrVMmwEM+YV=Br!&&S#bqw1Zr$pNb>`_VSqz=U z(KWIuC6UY+dJ=l5e5yZbJ~|hFxIpR3)`q(Be+qI@d8=i^r26#OagdqF+OfdZC#EAi z5V_XMN#yC9s`Z&spPr2D5K$38IT4;>)$r&n>J9K9ijEx@nZ)UOsTgMPK;zC%$t@{IE%aHa7eZP)dx1TZ?}W<{;!E zAgo#jYD$C+-vh!{Yc-3Gu(fj#wiXCmZz1f@_v%M6(myU?R(~FG{TLB~Sq^a;g^m+B zLF6RJQ9Vk*e@5h!L_P&l6o<0I1|d*PPYs+gisO(g+h$aF+kr!S35WJNacJ+gN09;& z*?Uv9J~QfGh7U4@pJF&1immvD&ApeW&xt0Ybn^2maA3@5~OQ1IHLB2z&C zEu063{NUn<(;I4N1E;6?asdYxZSdgsp#L8_5e$5FB4p<9PJ-H|h9; zB6!50=rMvB#&VIKf$?=)-RvH;Z!Ec!qZ5>JA0uuL>O8KLL%_l9JxS*Ty9X|h^LRP2@>xg=1Zzn=@sc|J=Gof053!MGlK=+vbXOOfWbAAo>qTLsK!yap8>hG60Hzg`(fzoIxkhK6UD;QcXwrpI0mXM7ZQZj~3@5clG z$hfw6U-s-g(xZ(-EZa6TOdlZ93gWHavY8&Jw|qnE!T30to~9+HXK5Hq6&awc905!W z&HIVz;X^r9bSS$S9{n>ksy<634N?}1BRCw!>j;fj5fk1EJ(g`d)o5`rq;}%W%h(Bvg9$7$2LqXpxQ|~j2eYKz$5SuD z!7OR_A5iT+aR)eB_kD;AOg)pMPRI3_g+-Q4gKP1PE599?$nsLCz9B{JAL|F<0@QQU zksd^@9pogk|E6kvX4L(Yksc8h0VY6Md8Sx3{M0P!4e}t0j`fQI;`G($L>da(F_aV6 zLBVSWMW%uPS~w5)y>P!|`O(UxOju|VT(swcF=`;R_*MVHT=T7BTJmNFSGH+YES^iAhyCjStXNHMh2w!+Ub)Ho!l^UgMLP3c1Ym)y zk4;B95t%SJi9ktXeP+~MlaWpl6#)idR-P$V4L>!j?joHzok3C3najEm#_6k%i8K_n zV>ltMgMt%=$W#zO3(EtrB@Iw<09#tSD^GO1|1?0%>Xg>)ildm$P-%Vu)4xE-Rb<_+ z0FnCQ*yF;@1j;}pNAA6Jjb9^DTp#4%NR|dPZdDPEWNAP@$OGD1kzY2SRJ6#0+E&vT z*<2Ze+L8MibW$gqv76Y8-A*=R_rx$#Kq9+us@7*l-OZ+dnZi#oHp5DsV%6~US#>wD z8FU6kiOq18#A#wIWI;QI6XH53IDwrnu7Vr+Ll8jU8xS zwl39fyF1I)r5d((b@PlTw&Fw9EuM=#w)Jumdls?SLxFkLda%qx&#P>m#W`dyzSff* z%o!tMn|o0cV^_ArRO5F+EED4$#6Rp9Uk}2Vi1GE)ktY$ESk4I<7F(Ygb;o4nNf8wR z#za_ordT!n)U3LLmzz*2u#z(O#m6RvTC-Y-+=5|LuVeg&Vd&t;jr3n(gW z7Z;BgovP>LJ%S75|2>EW07nP_j*LGwabfcDXAoj2fZ%lG0Akl6OAv`1xv5&88TH6y zD zV6L!3+x3*ZF-Mr#S#~V)I^OvuLlZ7}e~L3S-p0H4#?X*C8n)EaHZ6zYZiA(^Sjl(A z7gtD{K|Pxt^juQ85^VF^)m*m%%dW%O$kzyleiOtFzrw>`r*sNC_8<^s+4d`!>C_q` z%ubi=Bf06yOB9L``R5>BJPltr=KG~$)Z*gn#*R$Ks5Wl9Vnz;(2HC!;Zo;%~&d;-Ih=;DOTZLQCY+C3TR5K$3eKxXBcV%6|dvuZa18J$5<0y0?_!ZKc*sxX!CO6pw+0uy5oj!AX#e8mr%)N}h?I#+ zvm;U_B4fCP+=Gt}-XKMlge(bf)g|*5hc__Si!QI#&4IT%$ZCxYg9b3XajfPTH;>f{ z10K&smp4(q0Il@jLYfx_x%U7t5GuI`fO65^tT`yuLnzeaM4_Gu>;WJ`HY?U=M(ttf zAXE4$hC){26sv}(&#FCyLUaa2358rGahh;P7PMnHA+Cdh6W9abDk!38Awwbk8T_cD zZOilnbOS#|gsFHwPN6uFB#|K^w6kA7PvlpLTqN>EBBMlJC-M~{UnTMlB4b3JC31)e z6H9#vS-uB|OVx>N!hfFjyK&zGL0@R^tqNagpO5H%MxEhj;F!P{+D#`J*+Zc{l-5n5 zPcXj)J>;@rC%G)x$$u$bD1m=*P)q2zO+VY^vf$GTaamx5;ew!AQ_-EPxO2Eqq%t3a zC%O~aXKX)o_NC}@Bq=sV&S8J9K>yQFisGB$z=mOq}T-~ zX-uEigL+-N*E$(1RRYy$Z=|P2O-s4Ey|e6a-MCJ<rO0wQ^1Ny>+yBnVlxT=h4c!nOLcjd-sjmjsH;H-taWr3~L7BeR-W8yy@%L z>AC5ooFh*IJKdYkXoJ{iaEe?E_;y1_w~O>|FtO%)&Jb|yIkP=MwLB@D<&kfJ-7K~f zo&~CP8drRq+wW0`eq0%jp={pw+msz$hi^~QH&ckQy=2Ah+JCl%S#o<#K+BngZ#f0X zo{m{A>7F3@ehzlHJP3%)#B*CKgg>rn8EYC#z<#@?d(`*VidB>9`b+sYup4dt>-kIF zuYC@I%doQqxeqkE)Vj@8Pp{yKJRDpkle;aQS-&?MOFw4ucEpedlDh=V&X51I?zvde)) zUdav^;3aawPx=KvK?Hpa7CN9yBYoN)Hd3SZ?8mgwcAESy;z3ho9yDq0 ztLD2c=;7(#BBB!cHW4*=08W*G5rG4|xE7=35DC?cKID0_uh~{okzc==P z+!49i1Lls1?Y89bA(4Yk&V-c?3?U9dA188!$Y($}n&?+4{`*A!HHg=w+r)>x)g z^6+KK2lKHqhAZ18O~_zvG%A>~?|=4~9w=jvGoW%we11EdT*%=aT6CeI{%8gB41JD( zmJay3`8<>1C0i;wI?q8Ao}nYrN@xq}Pkr3s>t@NbAuW^3JLk>NOI>qi$-EL6NH0ws zH9Nd5mAb66%&k8MZrK)3p~N9fLDZtd?gwQWEJEbsheH!XdTM!H?|UG6pjKTnukb(J zp}I5}Dj9)9)dC+nRSWQ;Q)Rc>{)P`7(o;J;Cel(|t`b5{R3m+L1M@j+22z>RT)tt- zgy(S&xJxg_pgc}dk{8U->2x-3Fn?amZ%&t6vjNowOWtvm`M8E^qn|{gyD>kqUJkGo zW=UKvLLo~pLLL>lUIfl~pbf#sD%R{jrgoMoWuXj{vY2eeR{b_pX{q$Eyz*F%6RlY1 zTpvB#T{@EPJWcSzM-O+>!?eirV!g}QDT5mFe-8p%ht^Hln>pS@qz_~!vSIANc;9qn zD6C|2fk!uz;xxL#k~BxAQoUgM}Ya<_=WK_zBpv2rz5)%xqcGl4~MUx z{7+CKKX>hOHzL?LZ+&Le=O$^7t4!fx2ANjk6swP?&#KQ6WYQTFCCIdcX%U=TAxixr zEX8Dfb_^%3152$su0fM+X3M-ajtr854YSxiV zrQzx)*u}jy8}ES8E7ZixcP+qB#Y>|Ruv_d}qMT2o5wuGb(bgB!Xxtx+G1q7;q^(?| zA@7PO6Tc`c3#8GI*YRfB<&?GnH|Glx2099tD7`fr<*!(HZ5$}3(Wpe<<5-@ekcT4D z44jRp4Cl;;-Q$`s zBU&fPe#2Tle5bDY@`lU#u>;d}kBtW2YF<65wq9;}V<*0_dbsnomk}UemmoJHusN|l zGivMT%OWNMv|ecCn4$_qOaxHVrA`q;CObAu*X1W1l_N;LT;zqFVbnF39TwWyl9(0n zFsty!tRR`$716@VQBzD&LS_|iJ7QXkSO^aM;)HqVV1Wz^Y3oOaS)>w?cfS;7l~zhf zWlL$Lgiv72!7Oj%xqHJbZ{xWJ4p$7g2(v7G>_2*|VHvQHw9V`+ii4LvO?hTOyfCKD zp!W?j80pWR%b1T8MHxE%1*+;Mkya2FYeB8XU{i08ft{I`wP4Gei}rfVg_`j5?kx0N z-kP_fQ?M*oV(PXQ_e6?nLeD!&6g6d;#F$R{4=dKk76pbVj|#3o02(Zsj$f7 z3?S3eIJn=(0r#VR#d_5%V-?F72l_iSBK{g=jyVc{JDjU`GGt{tJ?HJjlYm4;>dR6-~9Lisnbu$KNLMH$?u9 z2r2#N(aucLX(AW!pWgr?UTya%z0{78RTNr9w&JVER(us3N32u=1;hbXg&nuaos&!F za>>%UR7Vx?`FMqRTBTQrr|Z)`yiA;4BOYR}5wFnj+I754oL+RNvKQS|u@~KiwJHN; z=~Z{o$JCfj#8K@h1<-NQ{~TfJA5!_)Y9Hd^s5!UEo91rcMJ1}dT6w{T zRV?3e->2~cOhyBY1{naKrce-e>g9 z5Z(U$zTer-zrS~of4|+l=Dh%&c)O$PeHP$I=M=UCFG)*OPG0g< z@y2VF(~*6XUN2jyMK(Mv_4aqrGd z_pk(gziZlrbXkUkD_;9ohNeyE!wRHU*!p4nN4Q`qy?Oyl>DBhrv%js^(S)#kUSTbv zU*@k?TI#!eFk&N%U`Fh_#y5*-bqgA26$RtdTZ1CXp>1_#Xo(-xzDq9$ zY0&>8o*lCR8Q_AzfA>_jnEv$nc@)KLs`(DpbQJ{J;-67?A==`$8g7np2L*Al5z`mr z6BznJDa|p#u(Y^n;Wig0cM^3jWt>T6_?lB}IFmR79w6Coc6iNt25Ex43%r3#DhSAT9mIsz`UH%JXyRkW&Lt`7H-x{V!4Q;DBjL*q&|dZ_L$U z%+XNyfFm@~+lb&1V<)li1OM(l*xa*5Z0_09 z!i>+by{F{~whfQo_lxa91bSw0Fm9YNLuW>ES)*KAG?ZnwXas)x{~_`Lk^e_T;$JUC znfkDuLLDG^Qc*gfk8}3i6ED4L9d9#8o=-sN%5wxXpDc=dUfI7+m-tN(px*N4uYKw( zpCYB=_=f4_?W18ry?5%DeJ%Vi!z`{RyoExC(C)ZQImu)#vJ?k!U^1=DCJwgZ)e(ox{TgydKHBEap1<+RXUAIog0e z{xH?ym>ch}Uf%E_^m5@1%D$u4JTC92cP|&^(90XNMy=^XXq#BNTVUHnIkc@wTjuB+ z-oHiQsJ6Uh-dej)x+G5TdLtt5LnZ62xAS&Xx_FmHZH2Zn+E~E5H1(E0TAS>e?Qifd zG+JqAA?X>&x`+4}S}~{ehFrZCf#kMY-+>YIE+0kp+|dkM%7i}fr%09D4VS~{A#sj^ zMxf`C`M_`d9aK`LWCSmrc z^kl-on_6NyJ(*AHXJHe0?o84WQX!n~EXt&nD*D^hhPSzx>SWliHC_7D^zuEPOxwe2 zCe@9XJKunL>_!N#cc+{)S`ifi5>~A&Q&feBi2$m)gxvr# zlO3C-EAtc1+7TXuft0+o2&2GTP0Ps@MEhm9DQX|n^Z+=0TDc`k#&F(_|uIPBTvL3|p?nui z7R2BniNQ?y?%Z2fuJWDlt&2J)_a&1tF%cF_ph7L-&DhJG7??nbxh|Do?$0Z2GUjA= zOVukT<^tvWhqsT-awW-x`m>JxYh1FYk#NPwR=YfBzn|dzJW9;((CMp0eohrx`@@(d z&Pqy2yx4rjJ14AO9q>?2nU(wJs%5g~19J#jH0 ztsKjJ32CL_PeNL0_$v-+w4S)wTXTuGcS9Oqlc_|{ep*`2%&$)E)XbX`K@R$x9{MV`ex))XRk6^@#{^^0C> z3XsS3S8)IJd%XXfT=(C(aw7CPb~{Nsb{F@*7!H@ZpQ0RCOW4~u?$R&Q{Xct;_kX~3 z|DB$17VG|td4#~44oh!W>i&yzjJ5yey#E!@K2`H&G`d|}S;Uk|q*1m!;xWAuo1|zt zhMCT{RdG#DxvHDOMKPEKTrmA{nD@m;9JT>$MNAshd#;GBWQvyyNj)1&4ED$JMtme6 zqbPriU4d~TH?Q}|{rmVS`E6XivhLAuulk);;|C_1CKA)@p1Pr~!Gf6ovj45+D<{>J zmyW+Wa#LN&Khx^UQC-ADz>zj3bIej7kaIcx2({IE zTFL&5GW}m7eqx0xiPR8@5Lr&-Svrg9aPDsAf1pGnm++q_7-jadcl>02d5c+R(thUF zTVb}TIT-4=RpUn#wzI8w)L^LjJq3g}oz+DPZEXRzvmI|32+`H)S0MZZzF0!rLuO?x z)}Kwlc2w;fPNfaF{nu%w#%zw|MyMmUbHyPvfbDwnoSrh0W|c@XE3GuH;$&)1vWqp> z=i_N(q>na)oa>7XB(tZJnMAVvLNhsX;=UM6yc$Tx}n zkjP&WnIyt~+cOkuC9)30RCf<%`-juXr}alrD^;9NfV>y*`Fw9Hhu>9{FDk!Y{Z~ru zonX-Ce_sK)(-`sXcwYgz^_UXbd$HQA?zpJlsp# 500 ms gap + frame_timeout_s: 0.5 + + # Timeout before e-stop escalation (s, measured from last good frame) + estop_timeout_s: 2.0 + + # Health publish + watchdog timer rate + health_rate_hz: 10 + + # How often to poll CAN interface for bus-off state (via ip link show) + bus_off_check_rate_hz: 1 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/launch/vesc_health.launch.py b/jetson/ros2_ws/src/saltybot_vesc_health/launch/vesc_health.launch.py new file mode 100644 index 0000000..d193705 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/launch/vesc_health.launch.py @@ -0,0 +1,50 @@ +"""Launch file for VESC CAN health monitor node.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_dir = get_package_share_directory("saltybot_vesc_health") + config_file = os.path.join(pkg_dir, "config", "vesc_health_params.yaml") + + return LaunchDescription([ + DeclareLaunchArgument( + "config_file", + default_value=config_file, + description="Path to vesc_health_params.yaml", + ), + DeclareLaunchArgument( + "can_interface", + default_value="can0", + description="SocketCAN interface name", + ), + DeclareLaunchArgument( + "frame_timeout_s", + default_value="0.5", + description="Frame-drop detection threshold (s)", + ), + DeclareLaunchArgument( + "estop_timeout_s", + default_value="2.0", + description="E-stop escalation threshold (s from last good frame)", + ), + Node( + package="saltybot_vesc_health", + executable="vesc_health_node", + name="vesc_health_monitor", + output="screen", + parameters=[ + LaunchConfiguration("config_file"), + { + "can_interface": LaunchConfiguration("can_interface"), + "frame_timeout_s": LaunchConfiguration("frame_timeout_s"), + "estop_timeout_s": LaunchConfiguration("estop_timeout_s"), + }, + ], + ), + ]) diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/package.xml b/jetson/ros2_ws/src/saltybot_vesc_health/package.xml new file mode 100644 index 0000000..c9d6992 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/package.xml @@ -0,0 +1,33 @@ + + + + saltybot_vesc_health + 0.1.0 + + VESC CAN health monitor for SaltyBot dual FSESC 6.7 Pro (FW 6.6). + Monitors CAN IDs 61 (left) and 79 (right) for frame drops, bus-off errors, + and timeouts (>500 ms). Drives a recovery sequence (GET_VALUES alive frames), + escalates to e-stop if unresponsive >2 s. Publishes /vesc/health (JSON), + /diagnostics, /estop, and /cmd_vel (zero on e-stop). Issue #651. + + sl-jetson + MIT + + rclpy + std_msgs + geometry_msgs + diagnostic_msgs + + python3-can + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/resource/saltybot_vesc_health b/jetson/ros2_ws/src/saltybot_vesc_health/resource/saltybot_vesc_health new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__init__.py b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1f3d5a04c1001fb466b625bf3b1e8cb60928ddca GIT binary patch literal 203 zcmdPq>P{wCAAftgHh(Vb_lhJP_LlF~@{~08CD?>lDIJKx) zzc@8X-_cXQI5DTBGAA)fH!ZU$w>+^ZRX;1Wq&PoMzbL=hD83x1t|%FzC@H@rzAUvk zIX)v5s3!xvV0?ULUS>&ryk0@&Ee@O9{FKt1RJ$Tppv54k6@wU`m>C%vi)!yw9CN0ntrQ*8Xx~=Fbtp6;IR@7=RG*JbAr?!^BpRNKXIe@8c3Fe?fE!9kwm zE^;Cl=Y}|u@8P=*aYGHI4L!!VkzGx36T6z@W_Gp2Ew~zca=NW?YYkVSK-=Orofpqz>71VY?t*wh4JTUTg=#A$@lxEC4e_GA zr@Xr&UeR3{uk5ahSJiMuTrDR$N;uJZDzDY3^gmubB*bfmOmRVUp(MA28}r0##XQlC z6dzxAs!sImeu?8oFcX#Bsdcqnt2zUCs-cek1nyLkg&VOF1afWh!c)cZyejV4b0Kb@ zv5E`vDug8kQHD}cEA{42{IiMUd|myAg~RQ=!r5S8EPhrPkAy?RqDOxQ?`^>oIBo!f*!ctqH>$IIFol>*~I zVMK~VV_WQ}Ct{6}(NQ5NNf9Zwg#pGxoae&4n$Pj{!v3MjZgho-7buShVT!;x1VJR4kM#3?) zDx3~ph@e~Bg&0QEKXE!XEQL-7N6kei z$75$=&HZsH6h7k>{`bYVgy;HC^a}CdSa3WTN2f=n$hbgt{pmcK*Hn{I=*-zvoBy&V z?R^uc$3n5QtB)l0RP{|ibr32{Y+`&ofRSw6wpHk!^imTep}?7NBo@bH(LAJYmFehA z6}L-L-~y`>WN!t~EIehL>iY}68VtwjO)IlKd^Qk1!zOKbe8m51a7=6HOfbSGT6yxo z`A{sbyobr46v4!gMTSH13&Q!c!PL?S49Bqu?DoC@Eyp+(Y{nbM@P-3ne<+On(Et`( zD#YRvr8l5>>jpvSj|`s+#%ZCaU{RMAh@UkulB$RVKLgZPjZ=dzZ=<4MoCeNLf{W346$Yc2emKVr^_n zJA%5Bh0VhGKz#V@NaTz_HBcXD!w`7(o)y;hsiOB(9Ga`?!64yrhov*2{V6Nm5DA zWUF|he>+?Fdld|yLAp|_WwQ{Nh({VV;?Xn=W`Uz01p|JVVE#cR5*IOzLx$bo2B}3} zCHDeBiBJG=4rH1)IMXjK?D3W+@We?mrJWX;=XQ6$Lf}bx^j2ex|x*RI=akbO-U#}K~)CP$fv%vj{L8SDcSmmPduI5nr3U%mRqHR|Kt zDPfiRrZwuPp1DeW^BVP2qgtiDGC<>P$@(YXtJJO!+oRL#M1C6D9!2+?JQ zv_OG6wA)!|q82>b-7~b1r_~W%Xdz#_L!UQj>GU-%%Ankv&eu|do3xaei=CztL$pg6 zjD=O|HEHFddqurcEq|4I>F0S?)GJ$~o;DB=%oX)2GV1No(&=l)`0~-Cz3CDyh1S$J z%JAmX+bBRA`?Z?sYgQXX0rB-~p6eT4n(b+F3DfRJ)%X|jJYlUd|0yk%5cRH;Un*4Y}fM(sr0+Bt#XbAF7RF%&q$wrTHP=?C+NwnKhhjQqB z;0csu(;?+hnU!VWt;hvxb1_A8!2}jL*B=Qh!(_4zdWCysuu6tgtwqxg%u1 zSr)BrOID|P|3`C%MeDxw{cBnGug@75t@~5u^EF%7%xGTO#C*-pIeyW)_p|cqWcijv z`IZ^~LivGY`Qb$Q;VIJ_j-^!osfF@=$@2C@c{}o5>HMLE^8Ly3jzoC}@^jPqE820V z^5+{5E|j+=%exZgT~jvw?3{b$Ro4UW7EBK$>vtvUcg^qaUa0R$)}Kh!pIE5vTPS~F z%J2lpiG}*UWW6s@?^~!nxlsPnl;xqFE2>^9EWLL8>hYzbvTH+Ehn9+KmI^B#+RR0E z^w##!$(7gLtp2m=g)%So9`9lUCEWLFHr{;g&t6-oX`V7IS}T#?x|=FssxRkW%3ZXU zBCAqJR%}gFZ2f8R?do}PV4>ng0xn&us8RE4Za2;MzqnB0qqfu8#kWf}@HgD)?4sM{ zsce_JDujepc(-`oyKinJdGLkA!58KSe2dnT?2T(>`>9M!92Au@wPsnuT6Vo|+VB?} zXH3bB`x6`YFIo?9c=;rM(R*z?pR<9fPK-+?blpE-Bibl2_8DnNp$%nYw}-Oi3krd~sg}KccmwPvb>{ zXxybrm*5QvOR}&=A5)yF1`j#ar2hmbx80Dz7ohh7H<;pWp?YtoIf?QE&ps>W0pdX{ z+fx!;4OazuIm)*|`vCPmONIE9sy=6exWwp%_}NIf5kv(FkT|qv!0phvk1*A9hUPD= z!5az10;d7dE8AjBWr@08gPcPe)o^f3&H?_8Ux)@Lt4NK48zB-6hS4G^{x$W} z;mE|;2zx34Fnt<9XM(9(M`7dS4)4A!YXZn9vk`>C6VTPa8VV@db|I69dF>!W=!U=D z*WPoiz2jJy)Qv&QCg@PYvRxTGsYPX5N|h%YJ0@Zh-UBD?qo{`}*t)(k9AtFSYmlC! z9Aoes!!cSb4T7>wh+mc~bcm8hwb}NW1=L0E*Y2wCy|(1^Tt0E>#DcRb>0Fm^u3PdP zduTMca_?I>SHZHuX3M=-x-MC|B~iL1S-Le*x^=0v@}}d4W9F;NMy_nz1I|#EJJtC{ z&obciREye3T=Kg3 z$3~+o_n$aKzg-4i$^9ZmoBzk{l^rhbj>~b_V!TsYcbGT+oHrxA0^tzPf!)kM5ZEyF z^|7hDVlD;0L(2j1ME)0 zNa}6|B*&?}vpt|!MOwN(Rolc$)##+BFSCxCo0c_14n604vEuXmSn(rcK|f=M$X+0A z1^6vxfk;D0D+{DSSs;gzAqzSSWCfqaMn;(UB9~${uE*Xx{?75c*6nF5tMyzR)0dlZ z=@i;sb8g?Ij;H4q_?evIBP6;Uwtm@*R`_oVFo{1jA`$g^ zhQ<6I4X~Cy^)cr(W5N+l` ze4j~!NMALY1btLW!`7!GAE$LZQPY62&y{XLOJ!+&o?^AJeGk@Iqn3ZV#84J1-2sEIb4T&J_6vN0T6 zs#gJhky|zxZ3Xu{h1cwF+ON0V+ zogF;IdmM*twiT3$a6DGZ+7_EH#~S|?noF_9x#s5k9HQw$c1fce0I`tTv|#Ni7Y&(0@yGtsC`myzPemC&)BJ3n21V^Ca06QKFJe3B1HeLMlbfGLAYg5WE)RWgXsM*)+J&nZuDy2kwdvu7 zl1)?Hz!tfcV15N?4{p46x2E~hiK!!Ca5d~fxWmW^TLD5kpUQ-_@_PTGwU)6}wFztO zblsx00i}4H@(3WV3zse=-Rtl_?*>C&c+GXymGsmnJoWDee|Tom)AZPkhh7?bT!hQz zp)Vd6vg_24!X*Ew^6+l+ou+oK(_#MkZbzrp_6vujv+`dR_54qob4@C@ns?mi5KWud z2qwudQFP3NisF7Pn%BJWrY9I1pFxObY+VbI#{+;B!EJ; z$S+RjHze{K7V>CXU%QJ_M7k+a-ZT?iC~vu%1LdVcxEmAYjWfdw<=a8HZL}3L zbgSoQ;`7$(yVlmn4rQan$fWYKcCMque5b_GvEldJ5?-Uia})lSO{XJ~u{Fh+BtTR9 z!(%SH&mkhxjLWL~!}TD}_Q%@%H1WG66<{6FE6EB?W=IQ=7sMRRo(dw)>+O^wZufc3 zV??ypaclA6Xhp|LrIDVf!DrBR*%|OTm2fPr<9vo8ZVO3vYS3HxqOFP3h<4N{+I>b3 z_o8XS(8Ns`j$qoq-r1)^oHdNmYj_@HlgRvpA!^!#=hPup!|p=ZK`V4!X^t9w(>Q9c zV{(VcoMVImX(C8|m+`Z~EN?6LsfEI%TVf8MO#W_?4Wa0yxsfr~rm;}?9IOjxBj*RC zI`je#Q@O)|D12^$e)xk%C*okSgWrhDIjXVfh-@Am3!I5f=5)N+-+1E45vY#rP$fmB z$Z#+glZ_G_K1nGg+ZERx;?<>d$dJY;*8<;7G%S6cZp;%=&_(HGq*ZE34nHw?m`dYR z<4pqMN3$vAw<%kXQm#ZIY(Y;07Cg8q*H(6~sOH+xn?vBNubiK@%vfekA2{B3%zO4P z)zr`&|iPZ=zuD zymRkAe=O`(bWFUhOfD!T*1YVC7#YvMwYOs<_g^<4`fy`4;yaeojylVoY7?dF%oJ~Q zbnG(T*~ueb!J`vBVLbYJHrUm*N!b!Pu_2RAc&^63s(8trVhH&QJ2~}zQy`zKXAsZ- z5v|1UXN)bb|UCn;@?dj0tlXV)V zKzjpO?df2r747Lz-4*S9GpjuvjR0#8N?JBv9qoXImGtxgqJi1zjm&0m+Me<|V&X?h z*2LjfG^2NTe-tpm5{$YxXYX+mUWyMei(MqIWr&z3bwiN1H3&Ze_Y{ zEk*r1W1{{6z6J2Q8kW=?)@b$8*A>0Aeu-Y%vU(X-Xa#XsUVE=>1V*1UGbj5=v#>EV za$tfU@7)V40iytl6EZUq9Ne*Wt1up8b_%k)lQ|&6ULjpY#R)PhqxK44{e$q*XSGyk zVz}CqO*%Lto0)NZqE4wq)(iD{guBO8n}}+`0Jv2%W_UetoeYim;}Ji6G=pOjDb8ej ze^+m(zx`N`uM38G>Y|_ZPj(H6Jx7mriT*yV%QY8PbdNFb`SQRks1==`Py{s!J44h!_{sDLE-e>^gkH*CoEp z%o>^~NBELqNNA;K0J23@6fseb^hAm%$7D!7HBq(-ilNMtjDuQgZ27pc?TRVssZDrl zryCYM-rFxuneP=>L0EGayR#~FCOuSn zskq{L*Y#b~^)r^~Z$O{DwVO;HMcw>EqcJZZ1TwEk5%#7-v*<&c`kc*)ip`68n=@P6 zknn7HqO}9m*nub7Iy3WX(sMvCH_`&Vu=(h9zg= zl~E+Gyol(nfom_n`SSO_x@@O&vJvrbruVW?v<64(yReql@MGHzKs@ELxn*mFZ=jWj0N&M&pcm{ zR)W6%0ZMcAIZRDow+fQgkrmd|KY)K`{xWJ0(r69OaB00tUo}*%t8*}s|E!K@Op51+ z{cRhK8dk@rx2_`vh^lAIfCLA()U zPHsT+@yzBkm{)oPgvcQWdBj9!GP#UJo-~5P_1c-Uid^a+51b47$<>m1 ztj7FmuchtO3HV!vMU@^6zp#g(1)2u$|=ZRqvdQ zfAISIuYY7&sD@t*)8$|8xzuyH_fqfMdy^H7A6GOk)zmK)RYI}xFvnDx3zrs`8*0Z4 z_m{eavu-JW&s@V?>Aa`oH{?(A`9llmDI=@lNj}^EWxl@iw(oZLM~CP8`ahbOuX{1+ z^nL8~!SRKl6{Y^4wr}Y$ad%9PjyB^RcYViRaby7x=muuGO?=qOx=F!V5qc#5tgHB&X(@O@?oHnQqrZgij znM8|5$%c$tO3Zo5(rgl~tUVnK2igPUt=k@S@>(k^+RH(EwwJ7E+16~1+R;y<=UG8- zj||poK&Z1$OQ)}*Q^avmjY|6LqMcNV*|Uhc>(h0#l-`M_LDFrokUsm<2c2GafX7ej zM4IT(5kl6sZ2iA4W0F2Lq}sn+y^L{`O>{(pu~9g1LC2A0TW~B8je-2d{(*XV@jE|1 zBs&%6lk^R*24ypI-;%zAQrQLd3P_}))QB-YH1wO_{qA?(lC+6i`ddIs|BIp~iWaD1 zj`|qsW4g0ywl(QBf?&++Rw^({k%=&!)Ch$o@D`l3mvUh_fCNm23+W(5X%;ud0DB$G zC?_%OZD$8Tl<_$-oX=z@D^ROmD^`q7Akj59nt;$(}|1j|5ul(pM=v-slV(p>3*7nD3 ziYfH*Zd5?43U~2?PDi)J{NFZIc9)qyvN*bnZ6B36y1gsyqtC=$NPj~Mmyq$(ocVpk zMUX9!JZS694fiRUF)!0)7KgZTd6I)5S&wlL{{uaO;}Y6Xg2vr5x~@tDS{Yrna=kO% zsWKfYL@VrZ)>iHXrTnlk2deAx)QRG_;O$Jy@4fL0`6uboc zDeO0Yt~yF}rOyv*V~biqzs&I6hkZwnDx$=^-R!fZ&*6%e)gZSU0GHxO07YJLSs{bZ z$pM58eH#?CDteTR*)>qBlFOj@>$Mu`D|(WW#>5=l6>4S2Z>;&;O&Vz?!jFe2zu$!_#iP4{ndldA0&Fn0-j7{GAn3YO}cgyjXJfmuzQJ^ zW`(}?tPI@xQs?(;$SU=375fl&VaJoOeO*uHWm-7tqgGG%+3Dhs&8u8Xfx=1<1A+6b zBz!k31h{$9rhR@&ka@4{d^P=b4gX|yS0fp5)bEBoVP~4mQ_sMpKTI|0KEgg)rTQJ< zXp6CF`{)E&9EF(jr6J9(B&b}G;l%Td$HAEcPWdSzBkOk+@DWp1wnWvo zg{qch)!sza-i50DQ|8OgOU^}W`DYrt^%?w~%dVDPtG-%2-7xR*lK(T^*Iccc-Z1ak zv}`t(=BmDoaGkEGyY$@D;d@2(3q>2C0I6%9HP4)%4c#{Wwf#f;+&4ekHeY*u>c|_< zQ;G2d`}_8pZ_d~5pE|PWtp2RH?Ap1j=celxiW{bme&)_6U&gn=nr%x|Zd-J3yH~p{ zS-UGyyX%VOE$32g`xVO%a+mT7sW)#&lGQsB)jJmRc4R@dW85@5Ehe{o}(tW4g#ela}p=<=cAl&Hp{$ z(NnjQ!=hyZzJ-6V5ed?b=zYOQJ^f)If39W|j5k(ASNv1dQC9I;N-?c5>AIdlsq@EJ zV=?8~k2$+}&wYxfhnMLxi^HF|+*a<$&jfc0b7sO*w2)%X2pwO9XbCG9eaeFrwZ=gW zO4rJk7S5YZt0_NLVWu*Bpgu$I!!o`M#p@vTlm#Gmo%$L%8Tx4VTX2ahBGRi53CAB3 zkFsz|?Stbv_)D{2B3{$ei2y;UM8LufB3TxY-q3E6Iw3Bi%S=dA#KYgIJ=H(doVC&cD)nt?g>tg1c^dc+u@$TG#Mi^E=J6>u2ld*X_Py zddvMdN0CV&mV8W-3GJ1iwA(u@<~tTgN6E^)La&VN6@m3{^}Rx;{4hIu_7sNm3sL`R zGRjrj6q<};Hdox|5KUt@;4)js?w*uUNczCf_TUxPJDlgkH9Ih#4p6Y8K>wkC<}U+= zhd(9}M;6ljGe&1flTY}j5=W;f6KH(uQBk&@Rvch^2Zg%d*UE4rVPhv15$vpvylbbf zo?3LQ`?R2pZg94K(NXtlK{>tIo!|R6^VV`@9xz|e{!Owkcf#dudKbjcle}m&*W9P* zI&cv#>4&47h15%kF&4^z}hQGlW&6dk4L+Z4qp`b&!5qv(eeeUl>Mq9np*(mNEf z*($=fI>5MvhW&v!e3?#9;47+ls7w8l$8d0L%6qiZ6OED zM?2we@~99d`$y$uZhf?g%ps3Dd8jlV`FPOIM*!4 zg&&D%W(Pto4AtRWJa|@|i&0O4;=3;aiCA*n=cCQZy!b?PEcjJ8qizvSU%(uldaAaD zM1vn;wqm4Jrrl@afe8`K7?A3yZF zE%a8@uq8tu6EzH`Ac5~!zzt<=#-`NQ2cpK4buEPd`N;zv6Cs@R4`f<9;hHVQ8&6M+ zjs~T)YajFMBX{pJ&<2N@4YUacRe_!1?`uE&Jgl*O;)%mu{r&#_?i1pGzvIOtN4ntR z#^w=jaEDJEKkiQ(ZGizi7?PAn(MK22$pIL#9_iEe|99en2Lo`}zctWbS-cf=y zPl)3$Ab@b*w`vt&j>p)P|gxo&*a^l_I( zYXz0$emD27we<6cHqKcEQ?uFisnrQ1vf1@pRW-a4_7iLA7qL1#@Q20i0{5ZdXm2up zxY^LYjr*{Xr*xB%(%Uw4lo|=sQ%`}lUqKD;ddW%S0oEU#U|?IlxJ=T(g15l(|FMLu$cEO z`)rm(Tqt|ZQUSUy6;bpH%Ae mu25FV^t$#o?n9%c!^3^p=IC%4?>KpiJ+_WY1dNcOe@mFFKJD=oZ@<|Md6G+&Fun-amNwy@U6UcUN30`B5gTb+LGZRp9 zyFII2b%S&&(4r%p_PT7)u-XHZB`93@eg zD2^VZM-7sp5zB@#<0vCBqbA8j>c%niXo*zPNL5fx6vtFioN2}~Q_{yMD@j(;Zie=f zE$y}$hV)pV$J%F5M)xTXO0uiHHt4l)>veFB7Rpk1t4mkv-1#_ztt8 zR~ho<_kYTs9vy*I06QNL z0?{ZRWf2cTB*gc#A}_I*7ui@m#IrZ9@G(}p!V_MK#3Wt_2ZD%jqhViig9; zSn?_$)Z85Xc!FnSoVTt-qC6W2NQVgNKiABM1$xu zTxyhz94#@NK{9bh$;>fQ31^ZloEgMNiR_%_#o#Cr8-$DH5=DkA%9ap6D+EG(NH!5_ zl1pIke%Sj%YQk%kEq;FtxWVt2ZGQh8Y$b|ihu{D0g+Ns8ar*t?h#*Q)kUFtAbeE{? zE?YDXmQ7^aa*2v3xx(*9X%z|j0}{~aN)~kr%{nk-ywMIMywK z#9NF8E_cD3=1_KZU*#n+9_tq3;vxSH7$*dkL6_r_|2pi}uk5gRqsF8DusG+NUldB< zl_YC~f>;UJA5mFnbz&sztVxV!ot24UsD3aaU>6x!94S8N)*+>RC!|PLu0*nN7C??w zHV*>f#AC7zg`A%VIk{A2T)%=8JE23hVgV`RM*%2Xu>>R@7i0$(wQZ4eobcL(QUtdQ zGbd&)$Yk5p@Wg5V;MuY1VYze*Rm%2LXNLz>5|u4y&yD!s8ssKqE0%^i?i?pOu*eOc zIyXJcy)9JWqKGr)T&l2Cqh&~-A`2NDWmQY!&1_YBVmw>bn)sb;RTCU)RdZr2Th;c# zS!FWL+j6KTXn;g-oCccL9cr&+Yz0M)v_1p0j;{e~H-j=Zab{8jB~5A;QnLaVV4n?Z zGu9?*#x{v)EJ&Q4bG)u9XUV~p!i+L)2It(;TT0e*?a*6BdiU(m>mf8{Rm&n9kMTs6vjI@FApr!LNMxi&QrnD+!Dy5yZ(!rOd62qb zPQ3drYI7ghC2Q$(E~&W zC<;FSsseUxlZ_zbWwQd#ZxkwdIsh^`8V^XaHD|D>UDQdrMAgHyKe&(SmjiMs;bP!N zNm%rYXF$4_g!ph^Au7pC1Z*lILSz$)Kwevil1oAOOA5e#NtDYJvFgXYD|OPZEG>ya zR0XO!pwI?2BD1hiNKn(%CBu;qC@KVPb=2?DS7?sr44jc;x(zMVL(_jEq02@%P~iZU z%mC?v5c_XOG$KmxDDr5Aa5g9*b2u){1tbyQ)hr;BqXJNs7pjA59U6W5o^co})qa@@JT>46ozkL6p=8^9NPFgOQZBmC}u?@WKT3mMcb@F`K3)^&{2xdBe2fBBKqO zG;5{&Q`c+Bw`wI_FS#yg?aH&)coYhuIXp<+NR)a7X_|^@=%6m5&?RXSg>FIe(euVF zFn@ih3Mx(i+aRUHJL?Y(^}q>V7Uh4N7#9q6ULQ^-%q7(5tGY=E6QKY0Ga z6uX5#pcbD4(Eyefn*Kh}GAN9MLzodgT8<@zNWd2hdoUwfp$$s1IWUix4xt$;UPfj_ zJ{lJKv4@!#`0H|s%E2OHw7s2W6^gTHtSo!<)TXr(O8XFIB&etLY}!)$)akypbaQE$ z$=aQF&t*KFDNpC8@r>_O%6ICCrzPV#kn$W@H`1Q2=M?Se`gKi5rlu!V)3bc~dv88z zJCJERl4?7WY(2URCV=}RXQsLKVNY+(#uk23QN7plr2UCp) z)2>5VckS{-*12cds*p7vvPKfPotNfN;RuXY`A;|wWgYVgC$Mh-vUjjUV=t4b{TT(> z3crqzRR^hUXD?(RfTMKdS5QDnF$vGYbTP+flL&V~I&uRT;x)n$yN&~f$j!W%W6Q~6 z#Gdah!h$}KS-SbfM$m+S59SECh<(qar7yCoX|L!rpVzD-TtcpdQd&n?Lt#O zBfEVin)*GUDWG%^vU!@yQ3WikCOu7YG}LnRc`-*9lSPEHt~V!|wKXXUDU1j~!_uMM zXh^k&L`4Xyq~sHNVI{AnkdlbtBeF3k{fN{P#$fmsVzO$)bWN@(MoZTSR)%5P3rQ*4 z)Vh2+?Pz#{B(*Q)+4pH>#(ON~J(hI8k#xNA%?YQgxJiwWx{#0{TQRNU<3T5>?LtW3 z0<^xOa6&=?6$FHoBcEa-#|I+gg@l?TDcFpfj(olKi;yBmlQlqB)}eKigA$-ALYkMT zBX?m(Ae(S>u@Rj*DCZ61a@1} zQ`g&&Z`De;>lb3D2rL9&~uu`wlc{yoRM?le`d#hay3CVICZBJlmmMBE8_w2$1hgVT> zjZ1-RyejX@3*|kL@@O}3aXe4`$d8-3% zmx{Ao1gi}VV#WSo@Kz|h^x6Vjh}8X4usIXdx|wpg3at**3LPm=N7C(0I=o;TYXf|# z8eiJ(BWfc*VBG?1Wb6$od&7g4w7o6c+?IAUf;E$|*Qf0D59qYLDO*{Ssq>}ke2*)A zH%FdgtvglM{kXDwdE}|R{9g12kAyeI% zs_y(WobjDV`A#IO29mCU0&sA-*qvC~-U>!Jn)kR|U#h+@?d_?-YP9dai?DS+y(*sZ-ASm>cq0M@01W01+r<#;^$!pi}MwM4vy;oX}y z9e#${%4zb*TawUEk_w4+1Ed;(dg(!ql04Q&i6P%#& z>?Y`ykn+sGABK>G3<`hi#ziQ+n)Ra7Cpo)7Sse3Ta_euQgtwvMWpu}!zixjEy7VXQ zP@GeEtG>=}b=By5KEY|tkmXN1o@wY!HT3@2pE-Osb@*(uemvzIPg=(F8`ln=fq8_E zS+YQJItK5P0liNEH=*Tz7)!iQgsNYNjlYE9BI-lLhQTxq*uZ?{@H%-uxS-S(;E)ou zq++FX;}W#&gca4wf@385QJbrK>h&XBM{!k>I9l_iz}5^)2d5N&Co4u`L7qjAo$5d7 zh{VEdB+M$#6}U+#;YA<%8^d0MqAJiEx9xr)aL_8d^|D^o3FJ^fMIzR^4raOrQe6X| zH>JBy-8N<_J5rS$k6j%}%Y|Q6{9@rB>!3hR2M|DX6VX0IK_RLA>_h4qV>MaUE2&b~A3|iIHZiumu*TG4X+$aQZAgsYKD@>>JTp@b zEvo~W14F3;L%*m<*PlxaFAuCS=f2p>uDTx`PVYUcj6C{9UDImIqv3R2e`4bH@EX&< ztzpvwKFrNJ@U(1}!PR)v1=pJOFin}uo*O{WZWy7ojwMhQO4|gTYn-6JGUrK{AcgEI z2ph^JmBQ$azlA1U{NqPj{NvJbi1is}%t{R8(m9BFV2x>q@k;CuY9*v*fq;`4YF1Js zpBj+ZfQ@rNoS>AH?OYj@ouusGTu|Oa%B5U6lp$6Daf&jGS759H%FY=VMl3K|p^HcC z!N>(hEnq}BS1Y->y}%cBvQ>#bo)PDk2F3$1hy;K=btA5i0Kf+a;1CC{foK9o`)6kb zeilr#LZ6M$0UMM{qI_6_fPrj*5cd@<3Hza4ktl>TyReG%xy4id(n+TK!9X7 z86{WdHn_zF{u6kAP7NDP4bLdZRwVMs3sM+Fk_mFayLBTCg#>j`6R>DM>t&iU8e5U; z3KFyy(H(s%J#BHB=-0OuECbb7TtD}A4pxYjXzOxd$aoEBCcsld`1mSx?q&&MHFg0U13O;!4{*L9G?AA}VX)RPMKf#Q_lig z%%!+d5`&%m4E~~rYzfV}Hnc_BlC?+4GY=&UAW4PNx`85oC*@b1xOzcXD~5(an1|WI zw=pZkgax8=_!W{Um*pe50>BBEi@ zvsytOkKQ1S3dkxa!2|-X!m)TTzV^X|S5WUf_r*u)%hP+jQ z_h%ImGUK0xm?G*?(Bebz5XjcimXeKd>}u3Ry(N-?dc`Lckrne= zS>v+t-&*%Ai?`mp`QFN@H7A?2bgy!dZO0S?a|xzcvdGSuLW}Rk(G0}G&*7hQ7%~{m zfDEG<@W5yWJQO(ef>4FSx@$B;JMh_4XxE_v}-);uDW;h&e4o}U&_62)wt&FT$MgsdbpJ7KAq}5{rS{d_t<0iSkf`}O@||p zD1Hfl9Z`T6nNh+K@W5~cJa!?9ACV0My5L!uG@=Oozaa`G3VQ|^JCF7W09!$OVbFk> z(6W0wvagXdI|&m3*Wlnee(U)1@oZhw7F@Td-0csKh zNH&88gy(i*P<2Y_#t+EM;>7UVlLD|(j<2&^XA;|im`&(dTq@U@rR;m`i>^Gxc9Ie7JX3m*3q32n z$dsOB3wRyjxt%FQ#wg2WF@*MO^(u>ff2Mr=Ci1p#;~M$}E&dl~NH0%O(R?+D#kWLpH&y(X(j* zOKsBunt9U!5_!E$^?wZJ{U3wi{{Xtk69;+wgtwIJ6hAc%FTnR`@NJoZc$e+DPosHJ z04k>hBzKwN;p=DtO*PplN`m5iBIXgYGg*_^{^YRL&xqboK4Ly0{0R&}<`Gd?z=uFI z{mf8KGn*Y0-SQi%<6o(Y=Vc6Cb33@w_nd;l^D;BtzC3li^*IHF^;*hc|H>fJ^fbK= f{#JN^cgJ8~SDPfPd}ZjNJ?j)?r0>75N?`dP5>&Ie literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/health_monitor_node.py b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/health_monitor_node.py new file mode 100644 index 0000000..b12af23 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/health_monitor_node.py @@ -0,0 +1,450 @@ +#!/usr/bin/env python3 +"""VESC CAN health monitor node for SaltyBot. + +Monitors both VESC motor controllers (CAN IDs 61 and 79) for frame drops, +bus-off errors, and timeouts. Drives a recovery sequence on fault and +escalates to an e-stop if a VESC stays unresponsive beyond 2 s. + +Subscribed topics +----------------- + /vesc/left/state (std_msgs/String) — JSON telemetry from vesc_telemetry node + /vesc/right/state (std_msgs/String) — JSON telemetry from vesc_telemetry node + +Published topics +---------------- + /vesc/health (std_msgs/String) — JSON health summary (10 Hz) + /diagnostics (diagnostic_msgs/DiagnosticArray) + /estop (std_msgs/String) — JSON e-stop event on state change + /cmd_vel (geometry_msgs/Twist) — zero velocity when e-stop active + +Parameters +---------- + can_interface str 'can0' SocketCAN interface + left_can_id int 61 + right_can_id int 79 + sender_can_id int 127 This node's CAN ID for alive frames + frame_timeout_s float 0.5 Timeout before recovery starts + estop_timeout_s float 2.0 Timeout before e-stop escalation + health_rate_hz int 10 Publish + watchdog rate + bus_off_check_rate_hz int 1 How often to poll CAN interface state +""" + +from __future__ import annotations + +import json +import subprocess +import threading +import time +from typing import Optional + +try: + import rclpy + from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue + from geometry_msgs.msg import Twist + from rclpy.node import Node + from std_msgs.msg import String + _ROS_AVAILABLE = True +except ImportError: # pragma: no cover — missing outside ROS2 env + _ROS_AVAILABLE = False + Node = object # type: ignore[assignment,misc] + +try: + import can as python_can + _CAN_AVAILABLE = True +except ImportError: + _CAN_AVAILABLE = False + +from .recovery_fsm import Action, HealthFsm, VescHealthState + + +class VescHealthMonitor(Node): + """ROS2 node: VESC CAN health watchdog + auto-recovery.""" + + def __init__(self) -> None: + super().__init__("vesc_health_monitor") + + # ---------------------------------------------------------------- + # Parameters + # ---------------------------------------------------------------- + self.declare_parameter("can_interface", "can0") + self.declare_parameter("left_can_id", 61) + self.declare_parameter("right_can_id", 79) + self.declare_parameter("sender_can_id", 127) + self.declare_parameter("frame_timeout_s", 0.5) + self.declare_parameter("estop_timeout_s", 2.0) + self.declare_parameter("health_rate_hz", 10) + self.declare_parameter("bus_off_check_rate_hz", 1) + + self._iface = self.get_parameter("can_interface").value + self._left_id = self.get_parameter("left_can_id").value + self._right_id = self.get_parameter("right_can_id").value + self._sender_id = self.get_parameter("sender_can_id").value + timeout_s = self.get_parameter("frame_timeout_s").value + estop_s = self.get_parameter("estop_timeout_s").value + hz = max(1, int(self.get_parameter("health_rate_hz").value)) + bus_hz = max(1, int(self.get_parameter("bus_off_check_rate_hz").value)) + + # ---------------------------------------------------------------- + # State machine + # ---------------------------------------------------------------- + self._fsm = HealthFsm( + left_id=self._left_id, + right_id=self._right_id, + timeout_s=timeout_s, + estop_s=estop_s, + ) + self._fsm_lock = threading.Lock() + + # ---------------------------------------------------------------- + # Fault event log (ring buffer, newest last, max 200 entries) + # ---------------------------------------------------------------- + self._fault_log: list[dict] = [] + self._fault_log_lock = threading.Lock() + + # ---------------------------------------------------------------- + # SocketCAN (for sending alive / GET_VALUES recovery frames) + # ---------------------------------------------------------------- + self._bus: Optional["python_can.BusABC"] = None + self._init_can() + + # ---------------------------------------------------------------- + # Publishers + # ---------------------------------------------------------------- + self._pub_health = self.create_publisher(String, "/vesc/health", 10) + self._pub_diag = self.create_publisher(DiagnosticArray, "/diagnostics", 10) + self._pub_estop = self.create_publisher(String, "/estop", 10) + self._pub_cmd = self.create_publisher(Twist, "/cmd_vel", 10) + + # ---------------------------------------------------------------- + # Subscribers + # ---------------------------------------------------------------- + self.create_subscription(String, "/vesc/left/state", self._on_left_state, 10) + self.create_subscription(String, "/vesc/right/state", self._on_right_state, 10) + + # ---------------------------------------------------------------- + # Timers + # ---------------------------------------------------------------- + self.create_timer(1.0 / hz, self._on_health_tick) + self.create_timer(1.0 / bus_hz, self._check_bus_off) + + self.get_logger().info( + f"vesc_health_monitor: iface={self._iface}, " + f"left={self._left_id}, right={self._right_id}, " + f"timeout={timeout_s}s, estop={estop_s}s, {hz} Hz" + ) + + # ---------------------------------------------------------------- + # CAN initialisation + # ---------------------------------------------------------------- + + def _init_can(self) -> None: + if not _CAN_AVAILABLE: + self.get_logger().warn( + "python-can not installed — alive frame recovery disabled" + ) + return + try: + self._bus = python_can.interface.Bus( + channel=self._iface, bustype="socketcan" + ) + self.get_logger().info(f"CAN bus opened for recovery: {self._iface}") + except Exception as exc: + self.get_logger().warn( + f"Could not open CAN bus for recovery frames ({exc}); " + "health monitoring continues via topic subscription" + ) + + # ---------------------------------------------------------------- + # State subscribers + # ---------------------------------------------------------------- + + def _on_left_state(self, msg: String) -> None: + self._handle_state_msg(msg, self._left_id) + + def _on_right_state(self, msg: String) -> None: + self._handle_state_msg(msg, self._right_id) + + def _handle_state_msg(self, msg: String, can_id: int) -> None: + try: + d = json.loads(msg.data) + except json.JSONDecodeError: + return + + # The telemetry node sets "alive": true when the VESC is responding. + # We track our own timeout independently for tighter control. + now = time.monotonic() + if d.get("alive", False): + with self._fsm_lock: + actions = self._fsm.on_frame(can_id, now) + self._dispatch_actions(actions, can_id) + + # ---------------------------------------------------------------- + # Health timer (main watchdog loop) + # ---------------------------------------------------------------- + + def _on_health_tick(self) -> None: + now = time.monotonic() + with self._fsm_lock: + per_vesc = self._fsm.tick(now) + + for can_id, actions in per_vesc.items(): + self._dispatch_actions(actions, can_id) + + self._publish_health(now) + self._publish_diagnostics(now) + + # If any VESC is in e-stop, keep asserting zero velocity + with self._fsm_lock: + if self._fsm.any_estop: + self._pub_cmd.publish(Twist()) # zero Twist = stop + + # ---------------------------------------------------------------- + # Bus-off check + # ---------------------------------------------------------------- + + def _check_bus_off(self) -> None: + bus_off = self._is_can_bus_off() + now = time.monotonic() + with self._fsm_lock: + if bus_off: + actions = self._fsm.on_bus_off(now) + else: + actions = self._fsm.on_bus_ok(now) + self._dispatch_actions(actions, can_id=None) + + def _is_can_bus_off(self) -> bool: + """Return True if the CAN interface is in bus-off state.""" + try: + result = subprocess.run( + ["ip", "-json", "link", "show", self._iface], + capture_output=True, text=True, timeout=2.0 + ) + if result.returncode != 0: + return False + data = json.loads(result.stdout) + if not data: + return False + flags = data[0].get("flags", []) + # Linux SocketCAN sets "BUS-OFF" in the flags list + return "BUS-OFF" in [f.upper() for f in flags] + except Exception: + return False + + # ---------------------------------------------------------------- + # Action dispatcher + # ---------------------------------------------------------------- + + def _dispatch_actions(self, actions: list[Action], can_id: int | None) -> None: + label = self._id_to_label(can_id) if can_id is not None else "bus" + for action in actions: + if action == Action.SEND_ALIVE: + self._send_alive(can_id) + elif action == Action.TRIGGER_ESTOP: + self._on_estop_trigger(can_id, label) + elif action == Action.CLEAR_ESTOP: + self._on_estop_clear(can_id, label) + elif action == Action.LOG_WARN: + self.get_logger().warn( + f"VESC {label} (id={can_id}): " + "no CAN frames for >500 ms — sending alive frame" + ) + self._log_fault_event("timeout", can_id, label) + elif action == Action.LOG_ERROR: + self.get_logger().error( + f"VESC {label} (id={can_id}): " + "unresponsive >2 s — e-stop triggered" + ) + self._log_fault_event("estop", can_id, label) + elif action == Action.LOG_RECOVERY: + self.get_logger().info( + f"VESC {label} (id={can_id}): CAN frames resumed — recovered" + ) + self._log_fault_event("recovery", can_id, label) + + def _on_estop_trigger(self, can_id: int | None, label: str) -> None: + self._pub_cmd.publish(Twist()) # immediate zero velocity + payload = json.dumps({ + "event": "estop_triggered", + "can_id": can_id, + "label": label, + "stamp": time.time(), + }) + self._pub_estop.publish(String(data=payload)) + + def _on_estop_clear(self, can_id: int | None, label: str) -> None: + payload = json.dumps({ + "event": "estop_cleared", + "can_id": can_id, + "label": label, + "stamp": time.time(), + }) + self._pub_estop.publish(String(data=payload)) + + # ---------------------------------------------------------------- + # Alive / recovery frame + # ---------------------------------------------------------------- + + def _send_alive(self, can_id: int | None) -> None: + """Send GET_VALUES request to prompt telemetry refresh.""" + if self._bus is None or can_id is None: + return + try: + arb_id, payload = _make_get_values_request(self._sender_id, can_id) + self._bus.send(python_can.Message( + arbitration_id=arb_id, + data=payload, + is_extended_id=True, + )) + self.get_logger().debug( + f"Sent GET_VALUES alive frame to VESC id={can_id}" + ) + except Exception as exc: + self.get_logger().warn(f"Failed to send alive frame (id={can_id}): {exc}") + + # ---------------------------------------------------------------- + # Publishers + # ---------------------------------------------------------------- + + def _publish_health(self, now: float) -> None: + with self._fsm_lock: + left_state = self._fsm.left.state.value + right_state = self._fsm.right.state.value + left_elapsed = self._fsm.left.elapsed(now) + right_elapsed = self._fsm.right.elapsed(now) + left_estop = self._fsm.left.estop_active + right_estop = self._fsm.right.estop_active + bus_off = self._fsm._bus_off + any_estop = self._fsm.any_estop + + health = { + "stamp": time.time(), + "left": { + "can_id": self._left_id, + "state": left_state, + "elapsed_s": round(min(left_elapsed, 9999.0), 3), + "estop_active": left_estop, + }, + "right": { + "can_id": self._right_id, + "state": right_state, + "elapsed_s": round(min(right_elapsed, 9999.0), 3), + "estop_active": right_estop, + }, + "bus_off": bus_off, + "estop_active": any_estop, + } + + with self._fault_log_lock: + health["recent_faults"] = list(self._fault_log[-10:]) + + self._pub_health.publish(String(data=json.dumps(health))) + + def _publish_diagnostics(self, now: float) -> None: + array = DiagnosticArray() + array.header.stamp = self.get_clock().now().to_msg() + + with self._fsm_lock: + monitors = [ + (self._fsm.left, "left"), + (self._fsm.right, "right"), + ] + + for mon, label in monitors: + status = DiagnosticStatus() + status.name = f"VESC/health/{label} (CAN ID {mon.can_id})" + status.hardware_id = f"vesc_health_{mon.can_id}" + + elapsed = mon.elapsed(now) + + if mon.state == VescHealthState.ESTOP: + status.level = DiagnosticStatus.ERROR + status.message = "E-STOP: VESC unresponsive >2 s" + elif mon.state == VescHealthState.BUS_OFF: + status.level = DiagnosticStatus.ERROR + status.message = "CAN bus-off error" + elif mon.state == VescHealthState.DEGRADED: + status.level = DiagnosticStatus.WARN + status.message = f"Frame timeout ({elapsed:.2f} s) — recovery active" + else: + status.level = DiagnosticStatus.OK + status.message = "OK" + + status.values = [ + KeyValue(key="state", value=mon.state.value), + KeyValue(key="elapsed_s", value=f"{min(elapsed, 9999.0):.3f}"), + KeyValue(key="estop_active", value=str(mon.estop_active)), + ] + array.status.append(status) + + self._pub_diag.publish(array) + + # ---------------------------------------------------------------- + # Fault event log + # ---------------------------------------------------------------- + + def _log_fault_event(self, event: str, can_id: int | None, label: str) -> None: + entry = { + "event": event, + "can_id": can_id, + "label": label, + "stamp": time.time(), + } + with self._fault_log_lock: + self._fault_log.append(entry) + if len(self._fault_log) > 200: + self._fault_log = self._fault_log[-200:] + + # ---------------------------------------------------------------- + # Helpers + # ---------------------------------------------------------------- + + def _id_to_label(self, can_id: int | None) -> str: + if can_id == self._left_id: + return "left" + if can_id == self._right_id: + return "right" + return str(can_id) + + # ---------------------------------------------------------------- + # Cleanup + # ---------------------------------------------------------------- + + def destroy_node(self) -> None: + if self._bus is not None: + self._bus.shutdown() + super().destroy_node() + + +# --------------------------------------------------------------------------- +# Inline GET_VALUES request builder (avoids cross-package import at runtime) +# --------------------------------------------------------------------------- + +_CAN_PACKET_PROCESS_SHORT_BUFFER = 32 +_COMM_GET_VALUES = 4 + + +def _make_get_values_request(sender_id: int, target_id: int) -> tuple[int, bytes]: + """Build a GET_VALUES short-buffer CAN frame for the given VESC.""" + arb_id = (_CAN_PACKET_PROCESS_SHORT_BUFFER << 8) | (target_id & 0xFF) + payload = bytes([sender_id & 0xFF, 0x00, _COMM_GET_VALUES]) + return arb_id, payload + + +# --------------------------------------------------------------------------- +# Entry point +# --------------------------------------------------------------------------- + +def main(args=None): + rclpy.init(args=args) + node = VescHealthMonitor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/recovery_fsm.py b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/recovery_fsm.py new file mode 100644 index 0000000..71c3dbd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/recovery_fsm.py @@ -0,0 +1,223 @@ +"""VESC CAN health monitor — pure state machine. + +No ROS2, python-can, or hardware dependencies. Safe to import in unit tests. + +State transitions per VESC +-------------------------- + + HEALTHY ──(elapsed > timeout_s = 0.5 s)──► DEGRADED (send alive, log warn) + │ + ◄──(frame received)─────────────┘ (log recovery) + │ + (elapsed > estop_s = 2.0 s)──► ESTOP (trigger e-stop) + │ + ◄──(frame received)──────────────────────────┘ (clear e-stop) + + BUS_OFF is a parallel override: set by node when the CAN interface reports + bus-off; cleared when the interface recovers. While active it always + produces TRIGGER_ESTOP. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from enum import Enum + + +# --------------------------------------------------------------------------- +# Enumerations +# --------------------------------------------------------------------------- + +class VescHealthState(str, Enum): + HEALTHY = "healthy" + DEGRADED = "degraded" # >timeout_s, alive frames being sent + ESTOP = "estop" # >estop_s unresponsive, e-stop asserted + BUS_OFF = "bus_off" # CAN bus-off error + + +class Action(str, Enum): + SEND_ALIVE = "send_alive" # send GET_VALUES request + TRIGGER_ESTOP = "trigger_estop" # publish zero cmd_vel / /estop + CLEAR_ESTOP = "clear_estop" # stop asserting e-stop + LOG_WARN = "log_warn" # WARN-level log + LOG_ERROR = "log_error" # ERROR-level log + LOG_RECOVERY = "log_recovery" # INFO-level recovery log + + +# --------------------------------------------------------------------------- +# Per-VESC state machine +# --------------------------------------------------------------------------- + +@dataclass +class VescMonitor: + """Monitors one VESC and drives the recovery state machine. + + All timestamps are ``time.monotonic()`` seconds. + + Usage:: + + mon = VescMonitor(can_id=61) + actions = mon.tick(now) # call at ~10 Hz + actions = mon.on_frame(now) # call when a CAN frame arrives + """ + + can_id: int + timeout_s: float = 0.5 # frame-drop detection threshold + estop_s: float = 2.0 # escalation to e-stop threshold + alive_retry_s: float = 0.2 # interval between alive retries in DEGRADED + + # Mutable runtime state (not init args) + state: VescHealthState = field(default=VescHealthState.HEALTHY, init=False) + last_frame_ts: float = field(default=0.0, init=False) # 0 = never + _first_tick_ts: float = field(default=0.0, init=False) # set on first tick + _last_alive_ts: float = field(default=0.0, init=False) + + # ------------------------------------------------------------------ + + def on_frame(self, now: float) -> list[Action]: + """Call whenever a STATUS frame arrives from this VESC.""" + self.last_frame_ts = now + + prev = self.state + if prev in (VescHealthState.DEGRADED, VescHealthState.ESTOP, + VescHealthState.BUS_OFF): + self.state = VescHealthState.HEALTHY + actions: list[Action] = [Action.LOG_RECOVERY] + if prev in (VescHealthState.ESTOP, VescHealthState.BUS_OFF): + actions.append(Action.CLEAR_ESTOP) + return actions + return [] + + def on_bus_off(self, now: float) -> list[Action]: + """Call when the CAN interface enters bus-off state.""" + if self.state != VescHealthState.BUS_OFF: + self.state = VescHealthState.BUS_OFF + return [Action.LOG_ERROR, Action.TRIGGER_ESTOP] + return [Action.TRIGGER_ESTOP] + + def on_bus_ok(self, now: float) -> list[Action]: + """Call when the CAN interface recovers from bus-off.""" + if self.state == VescHealthState.BUS_OFF: + self.state = VescHealthState.HEALTHY + self.last_frame_ts = 0.0 # wait for first fresh frame + return [Action.LOG_RECOVERY, Action.CLEAR_ESTOP] + return [] + + def tick(self, now: float) -> list[Action]: + """Periodic update (~10 Hz). Returns list of actions to take.""" + # BUS_OFF and ESTOP keep asserting until a frame or bus recovery + if self.state == VescHealthState.BUS_OFF: + return [Action.TRIGGER_ESTOP] + if self.state == VescHealthState.ESTOP: + return [Action.TRIGGER_ESTOP] + + # Track elapsed since startup for the "never received" case so we + # don't immediately e-stop on node startup before any frames arrive. + if self._first_tick_ts == 0.0: + self._first_tick_ts = now + + if self.last_frame_ts > 0: + elapsed = now - self.last_frame_ts + else: + elapsed = now - self._first_tick_ts + + if elapsed >= self.estop_s: + # Escalate to e-stop + if self.state != VescHealthState.ESTOP: + self.state = VescHealthState.ESTOP + return [Action.TRIGGER_ESTOP, Action.LOG_ERROR] + return [Action.TRIGGER_ESTOP] + + if elapsed >= self.timeout_s: + if self.state == VescHealthState.HEALTHY: + # First timeout — transition to DEGRADED and send alive + self.state = VescHealthState.DEGRADED + self._last_alive_ts = now + return [Action.SEND_ALIVE, Action.LOG_WARN] + elif self.state == VescHealthState.DEGRADED: + # Retry alive at interval + if now - self._last_alive_ts >= self.alive_retry_s: + self._last_alive_ts = now + return [Action.SEND_ALIVE] + return [] + + # elapsed < timeout_s — connection is healthy + if self.state == VescHealthState.DEGRADED: + # Frame arrived (on_frame handles ESTOP/BUS_OFF → HEALTHY) + # If somehow we reach here still DEGRADED, clear it + self.state = VescHealthState.HEALTHY + return [Action.LOG_RECOVERY] + + return [] + + # ------------------------------------------------------------------ + # Introspection helpers + # ------------------------------------------------------------------ + + @property + def is_healthy(self) -> bool: + return self.state == VescHealthState.HEALTHY + + @property + def estop_active(self) -> bool: + return self.state in (VescHealthState.ESTOP, VescHealthState.BUS_OFF) + + def elapsed(self, now: float) -> float: + """Seconds since last frame (inf if never received).""" + return (now - self.last_frame_ts) if self.last_frame_ts > 0 else float("inf") + + +# --------------------------------------------------------------------------- +# Dual-VESC wrapper +# --------------------------------------------------------------------------- + +@dataclass +class HealthFsm: + """Manages two VescMonitor instances and aggregates bus-off state.""" + + left_id: int = 61 + right_id: int = 79 + timeout_s: float = 0.5 + estop_s: float = 2.0 + + def __post_init__(self) -> None: + self.left = VescMonitor(self.left_id, self.timeout_s, self.estop_s) + self.right = VescMonitor(self.right_id, self.timeout_s, self.estop_s) + self._bus_off = False + + def monitors(self) -> tuple[VescMonitor, VescMonitor]: + return self.left, self.right + + def on_frame(self, can_id: int, now: float) -> list[Action]: + mon = self._get(can_id) + return mon.on_frame(now) if mon else [] + + def on_bus_off(self, now: float) -> list[Action]: + if not self._bus_off: + self._bus_off = True + return self.left.on_bus_off(now) + self.right.on_bus_off(now) + return [Action.TRIGGER_ESTOP] + + def on_bus_ok(self, now: float) -> list[Action]: + if self._bus_off: + self._bus_off = False + return self.left.on_bus_ok(now) + self.right.on_bus_ok(now) + return [] + + def tick(self, now: float) -> dict[int, list[Action]]: + """Returns {can_id: [actions]} for each VESC.""" + return { + self.left_id: self.left.tick(now), + self.right_id: self.right.tick(now), + } + + @property + def any_estop(self) -> bool: + return self.left.estop_active or self.right.estop_active + + def _get(self, can_id: int) -> VescMonitor | None: + if can_id == self.left_id: + return self.left + if can_id == self.right_id: + return self.right + return None diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/setup.cfg b/jetson/ros2_ws/src/saltybot_vesc_health/setup.cfg new file mode 100644 index 0000000..6dadf59 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_vesc_health +[install] +install_scripts=$base/lib/saltybot_vesc_health diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/setup.py b/jetson/ros2_ws/src/saltybot_vesc_health/setup.py new file mode 100644 index 0000000..6e97504 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/setup.py @@ -0,0 +1,27 @@ +from setuptools import setup + +package_name = "saltybot_vesc_health" + +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"]), + (f"share/{package_name}/launch", ["launch/vesc_health.launch.py"]), + (f"share/{package_name}/config", ["config/vesc_health_params.yaml"]), + ], + install_requires=["setuptools", "python-can"], + zip_safe=True, + maintainer="sl-jetson", + maintainer_email="sl-jetson@saltylab.local", + description="VESC CAN health monitor with watchdog and auto-recovery", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "vesc_health_node = saltybot_vesc_health.health_monitor_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/test/__init__.py b/jetson/ros2_ws/src/saltybot_vesc_health/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3c45a8b7c6474e7cefb6e2de8e0ee3a25d402304 GIT binary patch literal 187 zcmdPq_I|p@<2{{|u766`>zmoLW?@ zU!0nx@93#toS0KmnUk2Lo0eIWTb@{ys-KlwQk& z9G{U2)RUoKl3HA%A0MBYmst`YuUAlci^C>2KczG$)vkyYXc5RM#UREfW=2NFB4!{9 E0NL*_ga7~l literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/test_vesc_health.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/test_vesc_health.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000000000000000000000000000000000000..de400d2d960bf556cffabf07625acba0fb28b713 GIT binary patch literal 72161 zcmeHw3v^t^dEVW{F81QNBq)lMM2ZWNq6iWM2oMxO@gY$nL`s$@YDq1Xgyt3m79b)J zSlnGuB4t@jYR6zKM^YliR4T)IC7kGVyEY{$w|^WY0}~qK-r_TY2wy(+O(qB zl#Y8&)9;_T^SF0sUlbp*!+U_)xpU`nXYS1Zo_{trHq=_U{y6*1p+D-htUkICSICWA z{#3xS&RTIRYaO%Vfky+`Ap5P_9~uv?vi66w)vNfENH&5e!AI@=HQAc|wb|PJb=kW8 z_1XGWR=g_P5DzW0;^Af1@S<$vCmXf|;?=m1(EV^A8(nHOTX7pHHC;j8a&{3*t3_I! zOk2#->XFtU)0$XXBhsQWZ3#rTuXm(1hO8%go&M)~ofPtq zjSo4g{Nld-nG1Wj8&C4nKJ?hI)F3dnr51{fN_hGPMbBni@Kl>Q}Ek z;EbMB#d}BkW2`tU^U!!^)9AoJj5_BG^`~O=Sag=<*fWgUkd#H|heom~=Tz@7BCJPs1U>{8YcM@2%6squlc`bsoR>C?K?EVZU64F*!PX$f{;(?=h!moBrn2W*h zQkh&e#$enTxfpOlxNQmKsxfI!_GU9w^Xl9`X@5GCax(3i)baK`kG5wpg-#Fm9^W)D zP0 zGU?Q#)s^_qEJ1MAx?%+bt(PcxYX|$ejASqP2_vD-vO&4nc4JMcEs(VP?T< zdT=KK8tYIXdWnM9$3LK-->PGG>V=VwU2@140(WtJRLj|9FdopgWMxI=u>!v(~q3mrmDiI3J&?-7t9{VrL(IZ~3aZ(9-wn8qYq)dyaha z61}+$Bxv=b{3oq!02nWLY*V)Cc54)Hc$t+A#e)dL@hXJX@lZCBu(EbMjE_~FTS9jz1O|5ybT~>ghFu34k{5UU7s$3Q5dspfKeA>^u*+>D9>6@LwaTu8jUuLgn=SFa z+mSff>}!cWrSEFC6oeAUO}uOWUB2?P&U}}t{1bT3c$L;eW4S~?v=odKOW;f|J?U!6)V| zi?LliXSc|Sh@11imQ36Jo;{Br-v1-DFSgY4cMP*~{M{n%(_HO?AfK?vlf6!EaT2u5 zaMDSooxahNX&}s8Jsu~Ij}HxJhek5F>fur3$mEtMGbcvJhx=L9<~I?Xwcd{| z|NIM6_Kt;UY&sf4WG%TO!5j@IAheQJ~i2chLBpqVMfeyC0C?g$61P;<9_FW2Ui`B8p zDwQ)HB*ZH&Q4SbDuG@nWri)yF{96)1g2f&TfHwvPkZ}K?gz2grFhG=HfJH|-H4IP% zSSjKDK_fgjU;x#U5fZB&h+g@ipDIuI-gf~5$Pp0b0sTVpu+~FkxrBs%3|3asF903S z2pap9kXS?Zhp~7i-%FN5KO*f_9vEB*GNWdGX|*pGY3jEyWOlmLI36V8(06_0nY8%U zOrjz6(MUg(GYPPa7&G6Mt~z?RNIDo3G4(r^NksCfRsZ;yVO0}@RZEU+^T4XQieQz$ zCG(n=RAYr}T9(6_uB#9>)mPLHPIW7oY3zsk5&~w*{&1>Wl{do9{PO5meY{44KE`s1 zh-j}pu#Bg2=3g?p@63-cWX^@7Y}N@Mbnbxmxww&UzYz778{{u#p0yR-8=~ z5Z&jrBgom}rJZ$@#>jhHwA51LkSv6c=uV1tP|%4$r%^ow>o%v0@@z$rAyJJ>tj1gs z0H;Ekga7bYf$GRg4~E}#OfJOD9Z(wz#qhYC`%W!FLPoSDWP0+HLeY;w(9L-Mk?)(kAcKGi!G;84*S=e8fq66AhWo`1GFWd=n*uU&jXO)iSYp6La6fqG%4v~ z^uIxFGqxI3Yl!EuECH|5RdFjFIvNq_iI8|Qb|?{Mm~lWrX~=QD$wzxv7Ee9A{k1Qyr*fzcGAkjg z>gW*FP|U4o8{}G)bvw&?(G^!k+7@GT}8{Zq%jUbaT0pD|p(hKf2E& zQtI^Fb(wJXT}*Q1zI4)ycXRCp5wy9${dnuke}jNw#TSwN1S6R_Et| z?%FPd5xjHHh48;K?S?%h09Dj)C&(%$NnX~-<@ zD6_w2q`h;Go>|=C!G$n?fH#_Km3e#-ygx%zOlxwWr6y-5nLsg_%+)5L3h5sohQf{1 zZ(~qylBf}P(PR>uy-ao(dfSmv=;O9wz?@G|kVKGMnoJVSIMkQy&1RjUN!L8(LylS<Ffn#te!g*WN? zrYnYE9PmM;djt!h8l0z)=n5XBn1b=rOeEp##$zUY5W;{aA_}1N62t>Y3CI>6qtpTt ziSk4QQZsh3(II%wDU7`oD^nn#!~hO`=!uT$KxCE) zNx(b7xyCghKl0fB_wHOVWv@8D`rL~P_6q(tYpD`Z@8%7ekwzS}$}H~i0By-9dIXK; z^V1R!leI@7JPhrc%TY7#x;Q2-pfmdjn0OJBG%xnpMG(Hb_Ap)Yxe^9)?NJFB=)uIK z1uK=mFeZit!8OOk_D#XWix?(u@?hdBkF;9K5wL3A%c!YQqiMuoN}5sOSJN?Z4LD`h zju~SqOicK#lG%6jFmZjnp=7@b=?7}!jnGJ6YfOyU=wf0Un0Sdp{NlwRH!ItL_+X-O z({H~0zy6aSCp)8fSQ#RA(o|%O0*8VOf{YPAtrz%7+0cUXO8)pb%u8Ths>!Rn3t^{H zB?&`j?ohbtLf>3tr;eS%Z{I)Lv~D8uj~hF=Y_#nhr!Pj#sMAkDih}1Tc!7ce3I-`S zfk0<^pQS_!9;OFG<+)66DBN|W9sz7F!(6h=vYGi^B5FMR?$aiI*I;+Jl;($Pd3|_v zZ)(u#?N9ZadEcLkTqB%onYmwZUlOLFKkfycu;iUgmWX2yCqhi}{j`9mlKYYn+bsEo zN)oid>!3axi zB6_6hgbyE;HVOx&U4p1tvz*dr9@YrBSk)gR4G>?%W}esHB?x}`+J7 zm`x_h5^P$Xt=deAsTm|<(sJr*XHt{La_ZJhvNU<7nh`$~Gl|)$a5~gEg9+qQLot&S zldR_eH(Oysqg_<)9tw!+a2}vw4+Re)fHj9tR3%A4R!B3WL2Z+(;+aVWecZG{#&cLCt)t2#G7~FQkm*<7RUJ%Y<9D zA<76PUtUBOKbW}o0!mOi(W~b%&9_E-Uxy7%L6x)Jw%Ce z8jV5r5S8?c(-^9LW4{X07^Qyd;wv=}+<@ zz>CqAniO`I_9ur_w}7Y@h&rFaSUI0XF!+tTKl|UG`o@1P3sh?4 z^f{{FrxE0Xq!n4V7m#g0<^UPAtZ+Gy>ns9>bRe#k>7!;oGU&N^_Qfz|=2kryLsug~*yz(T=tZ*haR5TNBoJ_XC$1mB?>e|;_ zCQJO%^^?i+nY6^idEc5zH(Dlp17{NByl>;2H>Vn`2R)p31jOEGI+^)M!dh_N`-^bi z<(cGhZ&)ufA{91%TZ>e9IR5CqeLeA{Tg1EZu%k#p+QZiqk3Sjr>^v+YSI&i58x={G za}A#N%hkwMowN7`Ci!tQqq$v2Vz5vL^~}FZ_5^uq8XfM>Z6(6nN3(Gmayi17 zpcp0F|BWqkjSm!K`}1VW9b|vkF} zLuRCr#huD5?(hI@$tQXQRWjM~n(62o+Vt?8Z_~q6bd7Io0d7eMp>{opJM2x!&yB=M zm@ul`+MP^B1REmQd#sv{u9~tpOm3TwZdkD0zd3uuRCJZRl@V&$%`#(e5O?#Y+(?|< zrsvk~WHKUXs)%Uh7a{w-$?|0*1%LH&X;A*f<=G*lJiAIsb4@!H#`e12M)EbCN~$U; zY8RH`hwW43)*26K)*2)wu0-z3=`+=mQSPgHa1G_zMEk3Djg-8ya$vjWD(TmalV{iX z=(Fm0Ej)qcQ~Tx1vrQHl^`Oss)ORQ_ld4EZF?RJc>EVAAGpV7X6;j?zl02>CTsZGs zYKF^l;8^_h*>#*r@p@=R7BTwF`8muP=XC_%`O`n4%X;uTxWQ#VX!P!2yL$jSoL$Jo zeLCbsVkZ~&{XCwun6(>kQlT=n8!fh`-I%0;zeEAklXp^#6ysdQVIwBCEpW$Mce=z4 z(V5StQfZJk+$|XPD6|!!Y7F_GL*<$Na>yCKR%6NSU+jLZ`>ls2m%P1ycF8>x_Ir6L z8d1?c`Fx(914KmydJcbX=s6~zmo-2uqTR`4MDXNe@gBwXEdw7@qM|INHRjM&=vx|? z(z?;dI^S|BhS7Al6eK7Aqg zMD~xThll#m#pLklU=p6xM;+{s9D>_`p%E@m;Z_tzDGK&fmM(vm-r$*X=#SW%U$KA5 z{z}t@zF%KH8|$31S8B3??x{81)6wn)+x?rfyP2>+rSL~29&poU>~3*4Z^})HYgBn; z*KcJth~UwC%kTP<{igj+(`4V5m(MQWFwvmBT-W4_K9@!8-Tc0V=xy9I82gDkazjL7 z4^M6+PGX0s%B|hWWJH9cBH;nTsA&CXG&}w|1;0W;VMXg}^q7JX{OgL=>hQ)77e%0G zZK@7${lG%N6|I!ltY{@b@7cui1(TxHynxPnLE`d>^svz|3ul%T3$*JxoI zzz90wyEMNK1$LVP3DSb|UD)}eki9`BQe@{#3E2Vm$&oB2U&|^IqO`1ci@XP8BBp+e>{n3ZAT`~p9He-K zw5-?ui^(2{{>d50_2TMHjU!*cf2D3eYb>W3RKa8A(MtMN-infhy6g{^y_D#WVhjwj zmqJ?B@@G>atJJMbH>T}ZaK$eF!?mTNF;u=Z%ip*#M&)SZ0+d)?C3I2oo`;`I^u*zz zUDILZsiSiCDEWAR$0MK9iE_y+?)9>sNI21AK~>aBCan<-!c9`D*U2RN-Tjxj`wHPT zu8S&E?q@I_nZr8a#5e8w#WZj+(fV5JTh8ReZ$CfVv>uyyg;&Lon->T;mS9BF>gnj} zDSOl8;B<7;g6;mz*_)=K@NA({c$~;2H*Ln=B<|)-xsf1V%snc+VbXX5ig4fDH}w`kLpUuZl)ovEXm+PMzy!Z8<0?ZB=H8sBpUP3I(cL|3J>D$ z-<;jbmytS(lN*zgRYiHbosKzRMR`q$&<*Szt}oPD=)k0AF+TsJ4k@RVFy zVDT?$JpUC1g#{MBO^+#<#J?`Eh=ezh7JMaX!B_IOD4SVeu@_G~TJYaC2`soPM}(1h z7@-}nMpzS%WNQ;vwk}S-N^5ep^civJ{gVR&W_|c)0JIA2TKMRv89Wdj+Q+|2Cw7Lpm+AYtZTU~#SC-JGi!#K#HBT=-Cq1qO5POAy?% zh2RgU4kp)Sptnq!LjiAQUYILAD`JbpFir*>LdZ8G3UIx>5V$T`^k3t(Grx(z6)|uK z4)HvwD2_x=-RH)H^Y4XSdkm`mj{|g<# zASHcf;Wk?!vA&{y-8kicxh#A6 zv&n=z`4J5AJ6vPsKc7in$8oo2(v4RB-@uuqvFy(81Dkv#uU`DXFGi#N4%J0Ee~ z2(?^|V6~rBoVG)ltTHZ}Y44dH|18RyxNNS%XWa9%Vq1dCOXuz}Qf>Zhe@HdY`+001 zg|_LnP45IJ4}IC5ZEE=e;J3L^?akARL%;s<-3HNs56-CAV%s1*{B{T>eAn3OW=qkh zf|5;F;UG@a0bg4ftt8b;rtOnV*G=0T>pjk71M#57e@P*ydL5&#TeT#yu3n-bb*aip z<&C)Poe1ET?U^ z&^^T^`n%rg?Tv06%Bd)!LnX4~Ei3SBV>TZKz{5NC0m5tXyw8D2OJVi0G zFLc&Wuol4qankaWWXgpjnrmNhBe7lVc!UDSEK8_g`{H^kfPr~Z^dV$F~lYxV!?ED-Gc4@&DrbN z83`(dKO(P)ZrTj0aPQ`+Hu-=@koHzwrXjPq!vk40nwfk^X3`_bmCxVH<7cjeB=V~G zd(Dy<*klC`Gj6aYAi`8*um>0;J8CM9LLxmcM!Ucn+Z?Br|jVss{o` zhSle-Y0Z9?}i@{8v8GXR4B`0#LUEklxXSdE+esB!&dzpvb;ZS!7@N zBBMfNA3twEiH&03Z$ozyWmdjtb8bzoFUiM-&j#d}uRJ0HO^1`=g z*X36Hc+8&v+GO_ZasE|Dv#e*hH=XgFSicl?F@e-Ts7(82qtaMcX7i`=!So z(lJsxtcezgUG`n+>Ud!66iP@;Gxphp%}%X`#7q^s5|y?n(h?Ya&Nu8;P)ZH##3wQ|(==F^~c{(T@D4oNX>O26^Q%2H5po zvR_gihm({_w4qi9_F$LFYm0ug*LbMGTAUXYP1O3%aIZVMOw0(aW;1nACu$bI@Qbzx z0zg*?m<}Nx#y{BLbTy1}o{ukrQO**E*9W(-Yc|h<(Jl`aFAdJz>w?Ps5BD;qVo_r5Np10?u9A){>dr?Tv&j}#0%%UXQOKo z`%3k1G+b;zGlH!f^@)fNZQ4hPlxCM!vzZx}2wAK)pv?viZ4#>Ni8rjV zoCB!PNl@?%0t0vHB$1wDOk@v7TVCKgk~#&KT-dgn8akEg|8G=NF9oECcG(`Y+bNcn zDlfv~F)Kv7uC_+aSGgW#5kQyx@>mmOg;}DUf|YP2RVuG!KYZGg z(9x)P7p)E>OjDRd^b@lmGd>-_tY71PRO`$qT#}Al+!6T(dWn;7h!iE?)`LQ81DS}k zUC6EcsT4NmF#R)Wa+Ld2IyRY7a`k#%Z5R1^%HB%BCJNdpXh+Zz=EN4GvpOl2;5y$x zP7;4c6me>38KZ_yqj4YQTv;=q#&zhy z(tdrU5wEKiB-(SS4Y^LudrMR~7b};_uU22sgORF^D!UC;$8BmPL=0jT$!Ke6g;v?ckdLL| zoC#+}wqywU(n8BW&xsxFN$wfkrMAI`9$fUu;Dayk-=WgXO9))<<3eA#UUZ^O3sHXP z;;MN%Y{2t3Yp)`n!cCh|ck}itb~J$}Hxl!Sd$|OEw#X8^O5I~Q3x}gTg&aOk3x`mg zn)GIrbG;T$7z-zig;UKn75|&2wb$0^fXC|S4=6*KE5P|bG#$MY{dR^>G~*$6lCv$H zJ$`(Wm+${lje`h0?6s~#xM36d-69h?9=IlMpo>my4_(Kx1Nc1;we)eC(y#+@BQ9OW@xvL+pfg=Q`s}LEgF_%!5L8Nf#i*)rnyfy8ii?+o>vx@^T5kmN zhMX2AUM8c~z=b}u$KWNmFDzLzy#(j^u9>4#eb>y>A-VzDEJARp*kl{V&Zz+)iDwZL@PHYuJugAzYLV#Y5o7 z!pxseHS?zviAR`k9h>>qsXsYY+GjNWD)D_lK0Wc1MLxCT&+5=#fX}Pxq z94gcjbZGH9LkKdIu=*??Fj_*nJ3Y;|C6m1)U^3{CreqyW4x*dBchmQSjz= z_EQ8cKaV0hcX@2ru_rm7xQzffu5>_A%P`z=Q~5I7AR*iz$+;FIO5pj2}X zQmooZWyXiIx);6&kPbas6A5BEAb)Xvr;>?%okPo>+obj&o2IkcM7J9#V60v%#grJu z^~gt1--#m7RZp9pEM+DVpnzw-)mh*UZm#O2vg6K3CYgqy^HeI+Cr%UOZd4_H;fxg37bjdyP%6jGP;pZM=J;>b=FceR26~%imgkKJ)ha*~M!ns^5z)p0e+H zHF54!3-(?7Z`QtR!r{?BQ&IVNp0dlk8PAF;BDJF2V}a5Vvf#NCFyejP>)0;sZM>o(LUFj@P((sXlUaQ}{E=q-5|)`Q z>YzC%GyX*$3_vO)B4gQg@Jx8 zqMU}UNM%$>zqr^;)o<*VQl82FfDpk33RTY7A5{+hDA$a?SqzI-`)ZMMH6pW`JjOjdN0cwX0lf@~eyT7z3ya zOZH1zyR~~a>ea*>;*E(KpR%#`X!-NGR=kP$1=mFOII!3z`yA(UadMQigzwHKQlPA( ziWu4^qT9E7kMCRgb$W0Qf{TGO{S|3%+K3^60*8raJ=^#E#yc>%Y1`kk=kde)aabMR zpOGPq0WS}5=lDU5KDlnUEB4mxKu zQ+D?&pFa0#{JgVjp}Bjyxf{_bM7(DdPkdTGN0`S+)~=2y7I!#}c;b2f0B0NLmBV8+ zq?HqQbc%7aI>lIKK?INTsVSmT7&WyVVQFd#IK`z@P)=UpO#8+r2QcMN-)_P`w({Q9m!yaUUBPlr(HuF&}9J{NQ8s&@22;N;{0U!2Im+U*I)JnWxetDvB;Bcr! zIi*sEu}-PH%B-c@qeP65HLby{rF=f$WY$8b3FN#a|*on8*Wh%{uS@=Jj16C_q?hUKab$+mdflz>`{9*9<@Z2c1Qp4n8|HlcUq zG~c`|3=<@*IRL|J6T!F_HRoz%IgK!6OP`6bywqWfYpB4gW^4Tl>hS4p`6g>wEj;p$ zr?@kS(UBAvS}{9{+*ZR{E?sO5E$J+p^)6{YA^09Ny%52_-t=-kt#;-rV#UwExx)(m zZ`NKx>())fi4Wp#9#Ma0q!n3sUoC6lVF6*8`Y(kG&C>hQMn47>FJD@=Xv=J-+~q#j z&7gZ+NDoJngQWnuZ0I zCm3Fd*-*}ca_#HXf-0yEUdM2qe2L^h?{LOlSI0-QCz7r~U9PL>swz;bg%(vBgW+9N zauL1S_r`Map}OSuFYbD6*OVQbBZn)o`Dkpx_WaG-F;Ww|X*23>9?@O$fsEuYC@G?S z6*#RdrlRPA5->F##L|=GqMDcxX~`v`QX$fk*vrHu7?~0-(vrlo*$xf@4!$qI&y+NY z*3JG+3=37YWE7qiT&Sv?86W8jm6(!J`6d1WzATkjiL^+}T1lUq8B?O_H}+Y5HSygg zkm5{s z&BvX`z@CsbNscc+f?&0^3Ml@t^$3`=vBe}Z;Fn2GNE)Q)t+U}()+ex)p~kw~id$=} ztyM#Ti-7~Mp;3(05Fi00V^UsY{T@xS*X>WjYIHIaRjOLN&CQ z^Pqn~pGj#h*3m7^3Fi+fV2o!OT8bKc0cR6}i?y2c$)~EIX~y@fNCkyM>Wd7Kc1laZ zl@zNOd0luK!=3MKPxB1)w*GQg`L=&f%W}49_msW+``^{AGN6nsJIP?a;9s;F|n%f$7R%CB=hrSf{Y zo=W=M%(xy^zp>BCT3+C6A9bk88~dJL-sO5ql&hHQA=-gm+UOd17sz4D;d)|jW!le8 zn%DVUPbJ@&b3JOTjo(A5^dwRRpoBYYj(UUsTkE*o5xJ44ZQ{`g}C`^IIj&{ z&l)koOL9Gx%)0U*i!>X%O30E|-sO5qeSxcju6bNf`QN1!*Yo@M21XSwhVkZbJ?m~2{I-kMJe~ZA!8|Zrz!6)P&g>j2lAkOA`@CumRvrRjv?486b?3|5KbUwOs!S?*k z(sP+MqwePIohZhi%psmv5|4`T3jBPGQaXFcd0n$8OlNr&@@RDHd<;26WN0OoiP!0T z49SNTz>`w*WY!8P1+XX&Jo&?SJ_g}qKNdYmX-pu*tf73eLyl9XkVhCOF#ylyJ>AOghA@l%#YhL!7b@4Dn5r(@4oNZ}4WYUg*qfhib~V8(!znxEYaZmb0MUDB z9wOX$-#MHJ9(z*%nrRyt?itrKSUSrcJnb82^B2V?1ANiwkqj`gQwppYdJ_j)5`hgcSBygX^@ZlJ8Prz`c{|IfX#?q|@yzqFh;eMtT6YjiG>V zPp%GNJIk{}CsU*2*(BdxF3fC9>VU%8_n_g-Wwhx<2YE(oV)NzCt(d50yA)#6WN6cN zq4Ud+;b-!(>1f-6?f%W$ZR`LdmBJsrd`f1SvD?Jmyp|h>4o^NNGwBf&crUv2)#G0{ zGi9$jA7{a|=H-X{`<&taef~^EvAO*`&Pek3_Tjg9!U$j2{oW#@BM4@kNc&Npr4-yr z!QB)zQ_xNUfduZ3oY>MLZXyEom;#y>nLQYC<|g7w5WWpQAO{B@)`!9?$Q}81a!0;_ z+>x&&*AdMh)`r6meArkW-o^ZsU$MjC7`Z5q@eWX(eW*@exM$a8nrtRd668e!-Xq{e z0^TFYhXg=mHUggzw(Iz9IkDzXz|+9KR5sB&Jf6zLQ)39TLr+j+q5*1!Fe0$7efa~V zp0$p_2ic?6e)y3i%|Nqt?4@J7#HNYoz$G|qj{U>fLx~Xa=Hg0+;Q}C)sAgO7o)eg5 z@ZU0s84|fnHd4TWuwenjtUZ#@ASPiJsar7V6O3CaR7=tTP_*sc9xmR{-H4~kN!V*H z2g;c&cT45hWb!5R%1!+~oQDq?k0fm0hP9fbi^ZF1HA`wiJY&5z$(;bng)?dARNTgf zwYu+8p^Jgsa_*R7FqKW7ViN}5F4>G>RCTVp*Evr9FW3%)W)yLDQ$RGe+(B?R(>Fu! z99ze>0+8le92|#jW*2L7>of4-cN)5HZmi28UK{^Oo*W$+%8oj)#_vx#4`F(h4X#~H zEi3ad3g!YyZC_azy(oj-k5P)L zgpOGrdMSKtgJJb^dP#Aoay_F>_hRQ|((14Gu6}Lvjikdx88){i4Fou$S}^BTE6htT z*~G6KRv1}{vP-Mk3>Qo*j686X8Sp8LQcxeIv8NM3AAI~8gOBR{i;Ymjk+=>YwRbc3 zT8$7i3z}BK2rFoX(6t@^?%DS0t)%3iWnqCX_3*(`RnGigQd$C7YWsFJ)UvHp`Hu9< z{~{&|mi%rVVQ8_@c@Rqyf^7m6xkbr`pM2tp{R=Rok#`5j)}o1w)HS4 za@fLm_F#?<{R2KB7Zehuz?b(ZJ_Fk^d7veb3+%?KUQEm>Q8K$3C4)un>_v7V@RO8= zLkpDieH~QEI0eKV<_r9CT({$r3fOg+dOk~wu%A2FF%jz?;zBEN6pd!mXvIWXEC*$= z@_hIlY`RwRzgc_b1Q<8>PDZq}8I@z63d=lt1etV(tH#K1?W6H{oC5MSR)m|{O^+!c zhJ+~tU8j3h;d?%;jfC5-G*yM~CVzW(^HwQu&Q5|%9U4g{&jdG}YPmZXNhbS8`;y6= zeSCaqI6E|wVIVbkN0J|M(#E9{L!%>YPU<;lD4W8zv`i-DWRaAMxM`fwd>;dvYc^7- zaQr?B2k@uKZyOgUfL?Nyvoz_sl$mfVKO323F3z(Qe42vSDEL(hew%{dqaa7Y-%>#A z9~%iapt%rrkUBarGz?#QjJ@R}KrRq+_Rf=|{o})_-Oi7p9R94#?<4r2Di8=<3f2Zf zSC(6Wz-O#~+HfTl4g@b-2tK&os%|(N{JZL4p!I`VE4CU=`2x`^w?_l*mn;NVTI&K` zmn;NV);0xNFIfn#bS@8Uy<{P{vb#Reamhk(W!<8{%1ai4D{c1#HeIq1TzMuCp=uCZ zSD(EJU4pa0*p)|uO@X^9@$L^cSfTofGjmmUyceqd+$VnW6R$2m-!-); PHXmxfRAq(kWS#u~BgY&{ literal 0 HcmV?d00001 diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/test/test_vesc_health.py b/jetson/ros2_ws/src/saltybot_vesc_health/test/test_vesc_health.py new file mode 100644 index 0000000..0d268a0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_vesc_health/test/test_vesc_health.py @@ -0,0 +1,355 @@ +"""Unit tests for VESC CAN health monitor — recovery FSM. + +No ROS2, python-can, or hardware required. Tests cover: + - VescMonitor state machine transitions + - Timeout → DEGRADED → ESTOP escalation + - Frame-received recovery from DEGRADED and ESTOP + - Bus-off override and recovery + - Alive retry interval in DEGRADED + - HealthFsm dual-VESC wrapper + - Edge cases (never seen, exact boundary, bus_ok when not bus_off) +""" + +import pytest + +from saltybot_vesc_health.recovery_fsm import ( + Action, + HealthFsm, + VescHealthState, + VescMonitor, +) + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +T0 = 1_000.0 # arbitrary monotonic baseline (seconds) + + +def fresh_mon(**kwargs) -> VescMonitor: + """Return a VescMonitor that has never seen a frame.""" + return VescMonitor(can_id=61, **kwargs) + + +def alive_mon(last_ts: float = T0, **kwargs) -> VescMonitor: + """Return a VescMonitor whose last frame was at last_ts.""" + mon = VescMonitor(can_id=61, **kwargs) + mon.last_frame_ts = last_ts + return mon + + +# --------------------------------------------------------------------------- +# VescMonitor — initial state +# --------------------------------------------------------------------------- + +class TestVescMonitorInit: + def test_starts_healthy(self): + assert fresh_mon().state == VescHealthState.HEALTHY + + def test_never_received_is_unhealthy_after_timeout(self): + mon = fresh_mon(timeout_s=0.5) + # First tick records startup time; second tick 600ms later sees timeout + mon.tick(T0) # startup: elapsed=0, still healthy + actions = mon.tick(T0 + 0.6) # 600 ms > timeout → DEGRADED + assert Action.SEND_ALIVE in actions + assert Action.LOG_WARN in actions + assert mon.state == VescHealthState.DEGRADED + + def test_elapsed_never_received(self): + mon = fresh_mon() + assert mon.elapsed(T0) == float("inf") + + +# --------------------------------------------------------------------------- +# VescMonitor — healthy zone +# --------------------------------------------------------------------------- + +class TestHealthyZone: + def test_no_actions_when_fresh_frames_arriving(self): + mon = alive_mon(last_ts=T0) + actions = mon.tick(T0 + 0.1) # 100 ms — well within 500 ms + assert actions == [] + assert mon.state == VescHealthState.HEALTHY + + def test_no_transition_at_just_under_timeout(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5) + actions = mon.tick(T0 + 0.499) + assert actions == [] + assert mon.state == VescHealthState.HEALTHY + + +# --------------------------------------------------------------------------- +# VescMonitor — timeout → DEGRADED +# --------------------------------------------------------------------------- + +class TestTimeoutToDegraded: + def test_transitions_at_timeout(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5) + actions = mon.tick(T0 + 0.5) + assert mon.state == VescHealthState.DEGRADED + assert Action.SEND_ALIVE in actions + assert Action.LOG_WARN in actions + + def test_send_alive_not_repeated_immediately(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, alive_retry_s=0.2) + mon.tick(T0 + 0.5) # → DEGRADED, sends alive + actions = mon.tick(T0 + 0.55) # only 50 ms later → no retry yet + assert Action.SEND_ALIVE not in actions + + def test_alive_retry_at_interval(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, alive_retry_s=0.2) + mon.tick(T0 + 0.5) # → DEGRADED, first alive + actions = mon.tick(T0 + 0.71) # 210 ms later → retry + assert Action.SEND_ALIVE in actions + + +# --------------------------------------------------------------------------- +# VescMonitor — DEGRADED → ESTOP +# --------------------------------------------------------------------------- + +class TestDegradedToEstop: + def test_escalates_at_estop_threshold(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0) + mon.tick(T0 + 0.5) # → DEGRADED + actions = mon.tick(T0 + 2.0) + assert mon.state == VescHealthState.ESTOP + assert Action.TRIGGER_ESTOP in actions + assert Action.LOG_ERROR in actions + + def test_estop_keeps_asserting(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0) + mon.tick(T0 + 0.5) + mon.tick(T0 + 2.0) # → ESTOP + actions1 = mon.tick(T0 + 2.1) + actions2 = mon.tick(T0 + 3.0) + assert Action.TRIGGER_ESTOP in actions1 + assert Action.TRIGGER_ESTOP in actions2 + + def test_no_duplicate_log_error_while_in_estop(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0) + mon.tick(T0 + 0.5) + actions_at_escalation = mon.tick(T0 + 2.0) # LOG_ERROR here + assert Action.LOG_ERROR in actions_at_escalation + later_actions = mon.tick(T0 + 2.1) + assert Action.LOG_ERROR not in later_actions # not repeated + + +# --------------------------------------------------------------------------- +# VescMonitor — recovery from DEGRADED +# --------------------------------------------------------------------------- + +class TestRecoveryFromDegraded: + def test_on_frame_clears_degraded(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5) + mon.tick(T0 + 0.5) # → DEGRADED + actions = mon.on_frame(T0 + 0.9) + assert mon.state == VescHealthState.HEALTHY + assert Action.LOG_RECOVERY in actions + + def test_on_frame_does_not_trigger_clear_estop_from_degraded(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5) + mon.tick(T0 + 0.5) # → DEGRADED (not ESTOP) + actions = mon.on_frame(T0 + 0.9) + assert Action.CLEAR_ESTOP not in actions + + +# --------------------------------------------------------------------------- +# VescMonitor — recovery from ESTOP +# --------------------------------------------------------------------------- + +class TestRecoveryFromEstop: + def test_on_frame_clears_estop(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0) + mon.tick(T0 + 0.5) + mon.tick(T0 + 2.0) # → ESTOP + actions = mon.on_frame(T0 + 2.5) + assert mon.state == VescHealthState.HEALTHY + assert Action.LOG_RECOVERY in actions + assert Action.CLEAR_ESTOP in actions + + def test_tick_after_recovery_is_clean(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0) + mon.tick(T0 + 0.5) + mon.tick(T0 + 2.0) + mon.on_frame(T0 + 2.5) + actions = mon.tick(T0 + 2.6) + assert actions == [] + + +# --------------------------------------------------------------------------- +# VescMonitor — bus-off +# --------------------------------------------------------------------------- + +class TestBusOff: + def test_bus_off_triggers_estop(self): + mon = alive_mon(last_ts=T0) + actions = mon.on_bus_off(T0 + 1.0) + assert mon.state == VescHealthState.BUS_OFF + assert Action.TRIGGER_ESTOP in actions + assert Action.LOG_ERROR in actions + + def test_bus_off_repeated_call_still_asserts_estop(self): + mon = alive_mon(last_ts=T0) + mon.on_bus_off(T0) + actions = mon.on_bus_off(T0 + 0.5) + assert Action.TRIGGER_ESTOP in actions + + def test_bus_ok_clears_bus_off(self): + mon = alive_mon(last_ts=T0) + mon.on_bus_off(T0) + actions = mon.on_bus_ok(T0 + 1.0) + assert mon.state == VescHealthState.HEALTHY + assert Action.CLEAR_ESTOP in actions + assert Action.LOG_RECOVERY in actions + + def test_bus_ok_when_not_bus_off_is_noop(self): + mon = alive_mon(last_ts=T0) + assert mon.on_bus_ok(T0) == [] + + def test_bus_off_tick_keeps_asserting_estop(self): + mon = alive_mon(last_ts=T0) + mon.on_bus_off(T0) + actions = mon.tick(T0 + 1.0) + assert Action.TRIGGER_ESTOP in actions + + def test_on_frame_clears_bus_off(self): + mon = alive_mon(last_ts=T0) + mon.on_bus_off(T0) + actions = mon.on_frame(T0 + 0.5) + assert mon.state == VescHealthState.HEALTHY + assert Action.CLEAR_ESTOP in actions + + +# --------------------------------------------------------------------------- +# VescMonitor — elapsed helper +# --------------------------------------------------------------------------- + +class TestElapsed: + def test_elapsed_after_frame(self): + mon = alive_mon(last_ts=T0) + assert mon.elapsed(T0 + 0.3) == pytest.approx(0.3) + + def test_elapsed_never_received(self): + mon = fresh_mon() + assert mon.elapsed(T0) == float("inf") + + +# --------------------------------------------------------------------------- +# VescMonitor — estop_active property +# --------------------------------------------------------------------------- + +class TestEstopActive: + def test_healthy_not_active(self): + assert not alive_mon().estop_active + + def test_estop_state_is_active(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5, estop_s=2.0) + mon.tick(T0 + 0.5) + mon.tick(T0 + 2.0) + assert mon.estop_active + + def test_bus_off_is_active(self): + mon = alive_mon(last_ts=T0) + mon.on_bus_off(T0) + assert mon.estop_active + + def test_degraded_not_active(self): + mon = alive_mon(last_ts=T0, timeout_s=0.5) + mon.tick(T0 + 0.5) + assert not mon.estop_active + + +# --------------------------------------------------------------------------- +# HealthFsm — dual-VESC wrapper +# --------------------------------------------------------------------------- + +class TestHealthFsm: + def _make_fsm(self) -> HealthFsm: + fsm = HealthFsm(left_id=61, right_id=79, timeout_s=0.5, estop_s=2.0) + fsm.left.last_frame_ts = T0 + fsm.right.last_frame_ts = T0 + return fsm + + def test_tick_returns_per_vesc_actions(self): + fsm = self._make_fsm() + result = fsm.tick(T0 + 0.1) + assert 61 in result + assert 79 in result + + def test_on_frame_updates_left(self): + fsm = self._make_fsm() + fsm.left.last_frame_ts = T0 - 1.0 # stale + fsm.left.state = VescHealthState.DEGRADED + actions = fsm.on_frame(61, T0 + 0.1) + assert Action.LOG_RECOVERY in actions + assert fsm.left.state == VescHealthState.HEALTHY + + def test_on_frame_unknown_id_is_noop(self): + fsm = self._make_fsm() + assert fsm.on_frame(99, T0) == [] + + def test_any_estop_true_when_one_vesc_estop(self): + fsm = self._make_fsm() + fsm.left.last_frame_ts = T0 - 3.0 + fsm.tick(T0) # should escalate left to ESTOP + assert fsm.any_estop + + def test_any_estop_false_when_both_healthy(self): + fsm = self._make_fsm() + fsm.tick(T0 + 0.1) + assert not fsm.any_estop + + def test_bus_off_propagates_to_both(self): + fsm = self._make_fsm() + actions = fsm.on_bus_off(T0) + assert fsm.left.state == VescHealthState.BUS_OFF + assert fsm.right.state == VescHealthState.BUS_OFF + assert any(a == Action.TRIGGER_ESTOP for a in actions) + + def test_bus_ok_clears_both(self): + fsm = self._make_fsm() + fsm.on_bus_off(T0) + actions = fsm.on_bus_ok(T0 + 1.0) + assert fsm.left.state == VescHealthState.HEALTHY + assert fsm.right.state == VescHealthState.HEALTHY + assert any(a == Action.CLEAR_ESTOP for a in actions) + + def test_bus_ok_when_not_bus_off_is_noop(self): + fsm = self._make_fsm() + assert fsm.on_bus_ok(T0) == [] + + def test_left_timeout_right_healthy(self): + fsm = self._make_fsm() + fsm.left.last_frame_ts = T0 - 1.0 # stale + result = fsm.tick(T0) + assert Action.SEND_ALIVE in result[61] + assert result[79] == [] + + +# --------------------------------------------------------------------------- +# _make_get_values_request (inline copy in node module) +# --------------------------------------------------------------------------- + +class TestMakeGetValuesRequest: + def test_arb_id_encodes_target(self): + from saltybot_vesc_health.health_monitor_node import _make_get_values_request + arb_id, _ = _make_get_values_request(127, 61) + assert (arb_id & 0xFF) == 61 + + def test_payload_has_sender_and_comm_id(self): + from saltybot_vesc_health.health_monitor_node import ( + _make_get_values_request, + _COMM_GET_VALUES, + ) + _, payload = _make_get_values_request(42, 61) + assert payload[0] == 42 + assert payload[2] == _COMM_GET_VALUES + + def test_arb_id_is_extended(self): + from saltybot_vesc_health.health_monitor_node import _make_get_values_request + arb_id, _ = _make_get_values_request(127, 61) + assert arb_id > 0x7FF + + +if __name__ == "__main__": + pytest.main([__file__, "-v"])