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 0000000..f024b2e
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/__init__.cpython-314.pyc differ
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 0000000..bb1ffda
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/uwb_geofence_node.cpython-314.pyc differ
diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/zone_checker.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/zone_checker.cpython-314.pyc
new file mode 100644
index 0000000..b1f45c3
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/__pycache__/zone_checker.cpython-314.pyc differ
diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/uwb_geofence_node.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/uwb_geofence_node.py
new file mode 100644
index 0000000..ec7b203
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/saltybot_uwb_geofence/uwb_geofence_node.py
@@ -0,0 +1,296 @@
+#!/usr/bin/env python3
+"""UWB Geofence Speed Limiter for SaltyBot (Issue #657).
+
+Subscribes to:
+ /saltybot/pose/authoritative geometry_msgs/PoseWithCovarianceStamped
+ /cmd_vel geometry_msgs/Twist
+
+Publishes:
+ /cmd_vel_limited geometry_msgs/Twist -- speed-limited output
+ /saltybot/geofence/status std_msgs/String -- JSON telemetry
+ /saltybot/geofence_violation std_msgs/Bool -- triggers e-stop cascade
+
+Zones are defined in a YAML file (zones_file parameter). Each zone has a
+name, polygon vertices in UWB local frame (metres), max_speed (m/s) and
+max_angular (rad/s). The most restrictive of all containing zones wins.
+
+An outer emergency_boundary polygon is mandatory. If the robot exits it,
+all motion is zeroed and /saltybot/geofence_violation is latched True.
+
+Speed transitions are smoothed over `transition_time` seconds using a
+linear ramp so that passing through a zone boundary never causes a
+velocity spike.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from pathlib import Path
+from typing import Any, Dict, List, Optional, Tuple
+
+import yaml
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from std_msgs.msg import Bool, String
+
+from saltybot_uwb_geofence.zone_checker import (
+ apply_speed_limit,
+ min_dist_to_boundary,
+ parse_flat_vertices,
+ point_in_polygon,
+ ramp_toward,
+)
+
+
+class UwbGeofenceNode(Node):
+ """ROS2 node enforcing UWB-position-based speed limits inside geofence zones."""
+
+ def __init__(self) -> 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 0000000..aecbbf0
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/__init__.cpython-314.pyc differ
diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/test_zone_checker.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/test_zone_checker.cpython-314-pytest-9.0.2.pyc
new file mode 100644
index 0000000..488816c
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/__pycache__/test_zone_checker.cpython-314-pytest-9.0.2.pyc differ
diff --git a/jetson/ros2_ws/src/saltybot_uwb_geofence/test/test_zone_checker.py b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/test_zone_checker.py
new file mode 100644
index 0000000..bbd57a8
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_uwb_geofence/test/test_zone_checker.py
@@ -0,0 +1,177 @@
+"""Unit tests for zone_checker.py — pure geometry, no ROS2 required."""
+
+import math
+import pytest
+
+from saltybot_uwb_geofence.zone_checker import (
+ parse_flat_vertices,
+ point_in_polygon,
+ min_dist_to_boundary,
+)
+
+
+# ── parse_flat_vertices ───────────────────────────────────────────────────────
+
+class TestParseFlatVertices:
+ def test_square(self):
+ verts = parse_flat_vertices([0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0])
+ assert verts == [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
+
+ def test_triangle(self):
+ verts = parse_flat_vertices([0.0, 0.0, 2.0, 0.0, 1.0, 2.0])
+ assert len(verts) == 3
+
+ def test_odd_length_raises(self):
+ with pytest.raises(ValueError, match="even"):
+ parse_flat_vertices([1.0, 2.0, 3.0])
+
+ def test_too_few_points_raises(self):
+ with pytest.raises(ValueError, match="3 vertices"):
+ parse_flat_vertices([0.0, 0.0, 1.0, 1.0])
+
+
+# ── point_in_polygon ──────────────────────────────────────────────────────────
+
+UNIT_SQUARE = [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0)]
+TRIANGLE = [(0.0, 0.0), (4.0, 0.0), (2.0, 4.0)]
+
+
+class TestPointInPolygon:
+ # --- Unit square ---
+ def test_centre_inside(self):
+ assert point_in_polygon(0.5, 0.5, UNIT_SQUARE) is True
+
+ def test_outside_right(self):
+ assert point_in_polygon(1.5, 0.5, UNIT_SQUARE) is False
+
+ def test_outside_left(self):
+ assert point_in_polygon(-0.1, 0.5, UNIT_SQUARE) is False
+
+ def test_outside_above(self):
+ assert point_in_polygon(0.5, 1.5, UNIT_SQUARE) is False
+
+ def test_outside_below(self):
+ assert point_in_polygon(0.5, -0.1, UNIT_SQUARE) is False
+
+ def test_far_outside(self):
+ assert point_in_polygon(10.0, 10.0, UNIT_SQUARE) is False
+
+ # --- Triangle ---
+ def test_triangle_apex_region(self):
+ assert point_in_polygon(2.0, 2.0, TRIANGLE) is True
+
+ def test_triangle_base_centre(self):
+ assert point_in_polygon(2.0, 0.5, TRIANGLE) is True
+
+ def test_triangle_outside_top(self):
+ assert point_in_polygon(2.0, 5.0, TRIANGLE) is False
+
+ def test_triangle_outside_right(self):
+ assert point_in_polygon(4.5, 0.5, TRIANGLE) is False
+
+ # --- Degenerate ---
+ def test_empty_polygon(self):
+ assert point_in_polygon(0.5, 0.5, []) is False
+
+ def test_two_vertex_polygon(self):
+ assert point_in_polygon(0.5, 0.5, [(0.0, 0.0), (1.0, 1.0)]) is False
+
+ # --- 10 × 8 room (matches emergency boundary in default config) ---
+ def test_inside_room(self):
+ room = [(-6.0, -5.0), (6.0, -5.0), (6.0, 5.0), (-6.0, 5.0)]
+ assert point_in_polygon(0.0, 0.0, room) is True
+
+ def test_outside_room(self):
+ room = [(-6.0, -5.0), (6.0, -5.0), (6.0, 5.0), (-6.0, 5.0)]
+ assert point_in_polygon(7.0, 0.0, room) is False
+
+
+# ── min_dist_to_boundary ──────────────────────────────────────────────────────
+
+class TestMinDistToBoundary:
+ def test_centre_of_square(self):
+ # Centre of unit square: distance to each edge = 0.5
+ d = min_dist_to_boundary(0.5, 0.5, UNIT_SQUARE)
+ assert abs(d - 0.5) < 1e-9
+
+ def test_on_edge(self):
+ d = min_dist_to_boundary(0.5, 0.0, UNIT_SQUARE)
+ assert d < 1e-9
+
+ def test_outside_corner(self):
+ # Point at (1.5, 0.5): closest edge is the right edge at x=1
+ d = min_dist_to_boundary(1.5, 0.5, UNIT_SQUARE)
+ assert abs(d - 0.5) < 1e-9
+
+ def test_outside_above(self):
+ d = min_dist_to_boundary(0.5, 1.3, UNIT_SQUARE)
+ assert abs(d - 0.3) < 1e-9
+
+ def test_empty_polygon(self):
+ assert min_dist_to_boundary(0.0, 0.0, []) == float("inf")
+
+ def test_single_vertex(self):
+ assert min_dist_to_boundary(1.0, 1.0, [(0.0, 0.0)]) == float("inf")
+
+
+# ── ramp_toward & apply_speed_limit ──────────────────────────────────────────
+
+from saltybot_uwb_geofence.zone_checker import ramp_toward, apply_speed_limit
+
+
+class TestRampToward:
+ def test_ramp_up(self):
+ val = ramp_toward(0.0, 1.0, 0.1)
+ assert abs(val - 0.1) < 1e-9
+
+ def test_ramp_down(self):
+ val = ramp_toward(1.0, 0.0, 0.1)
+ assert abs(val - 0.9) < 1e-9
+
+ def test_no_overshoot_up(self):
+ val = ramp_toward(0.95, 1.0, 0.1)
+ assert val == 1.0
+
+ def test_no_overshoot_down(self):
+ val = ramp_toward(0.05, 0.0, 0.1)
+ assert val == 0.0
+
+ def test_already_at_target(self):
+ val = ramp_toward(0.5, 0.5, 0.1)
+ assert val == 0.5
+
+
+class TestApplySpeedLimit:
+ def _call(self, vx: float, vy: float = 0.0, wz: float = 0.0,
+ max_speed: float = 1.0, max_ang: float = 1.5):
+ return apply_speed_limit(vx, vy, wz, max_speed, max_ang)
+
+ def test_within_limit_unchanged(self):
+ lx, ly, *_, wz = self._call(0.3, 0.0, 0.2)
+ assert abs(lx - 0.3) < 1e-9
+ assert abs(wz - 0.2) < 1e-9
+
+ def test_linear_exceeds_limit(self):
+ lx, ly, *_ = self._call(2.0, 0.0, 0.0)
+ speed = math.hypot(lx, ly)
+ assert speed <= 1.0 + 1e-9
+
+ def test_direction_preserved_when_limited(self):
+ lx, ly, *_ = self._call(3.0, 4.0, 0.0, max_speed=1.0)
+ speed = math.hypot(lx, ly)
+ assert abs(speed - 1.0) < 1e-9
+ assert abs(lx / ly - 3.0 / 4.0) < 1e-6
+
+ def test_angular_clamp_positive(self):
+ *_, wz = self._call(0.0, 0.0, 2.0, max_ang=1.0)
+ assert abs(wz - 1.0) < 1e-9
+
+ def test_angular_clamp_negative(self):
+ *_, wz = self._call(0.0, 0.0, -2.0, max_ang=1.0)
+ assert abs(wz + 1.0) < 1e-9
+
+ def test_zero_cmd_stays_zero(self):
+ lx, ly, lz, ax, ay, wz = self._call(0.0, 0.0, 0.0)
+ assert lx == 0.0
+ assert ly == 0.0
+ assert wz == 0.0
diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/config/vesc_health_params.yaml b/jetson/ros2_ws/src/saltybot_vesc_health/config/vesc_health_params.yaml
new file mode 100644
index 0000000..c42307f
--- /dev/null
+++ b/jetson/ros2_ws/src/saltybot_vesc_health/config/vesc_health_params.yaml
@@ -0,0 +1,27 @@
+# VESC CAN Health Monitor — SaltyBot dual FSESC 6.7 Pro
+# Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/651
+
+vesc_health_monitor:
+ ros__parameters:
+ # SocketCAN interface (must match can-bringup.service / Issue #643)
+ can_interface: "can0"
+
+ # VESC CAN IDs (must match vesc_driver / vesc_telemetry nodes)
+ left_can_id: 61 # 0x3D — left drive motor
+ right_can_id: 79 # 0x4F — right drive motor
+
+ # This node's CAN ID used as sender_id in GET_VALUES recovery requests
+ sender_can_id: 127
+
+ # Timeout before recovery sequence begins (s)
+ # Frames expected at 20 Hz from vesc_telemetry; alert if > 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 0000000..1f3d5a0
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/__init__.cpython-314.pyc differ
diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/health_monitor_node.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/health_monitor_node.cpython-314.pyc
new file mode 100644
index 0000000..8204136
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/health_monitor_node.cpython-314.pyc differ
diff --git a/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/recovery_fsm.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/recovery_fsm.cpython-314.pyc
new file mode 100644
index 0000000..cbf0fd7
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_vesc_health/saltybot_vesc_health/__pycache__/recovery_fsm.cpython-314.pyc differ
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 0000000..3c45a8b
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/__init__.cpython-314.pyc differ
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 0000000..de400d2
Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_vesc_health/test/__pycache__/test_vesc_health.cpython-314-pytest-9.0.2.pyc differ
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"])