diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml b/jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml new file mode 100644 index 0000000..a542bf8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/package.xml @@ -0,0 +1,33 @@ + + + + saltybot_can_e2e_test + 0.1.0 + + End-to-end CAN integration test suite for the SaltyBot Orin↔Mamba↔VESC full loop. + + Tests verify the complete CAN pipeline: drive commands, heartbeat timeout, + e-stop escalation, mode switching, and FC_VESC status broadcasts. + + No real hardware or a running ROS2 system is required. + Run with: python -m pytest test/ -v + + Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/695 + + sl-jetson + MIT + + + saltybot_can_bridge + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/resource/saltybot_can_e2e_test b/jetson/ros2_ws/src/saltybot_can_e2e_test/resource/saltybot_can_e2e_test new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py new file mode 100644 index 0000000..2d216a8 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__init__.py @@ -0,0 +1 @@ +# saltybot_can_e2e_test — End-to-end CAN integration test helpers diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/__init__.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/__init__.cpython-314.pyc new file mode 100644 index 0000000..5dd04a3 Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/__init__.cpython-314.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/can_mock.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/can_mock.cpython-314.pyc new file mode 100644 index 0000000..b1be9c8 Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/can_mock.cpython-314.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/protocol_defs.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/protocol_defs.cpython-314.pyc new file mode 100644 index 0000000..d794d8a Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/__pycache__/protocol_defs.cpython-314.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/can_mock.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/can_mock.py new file mode 100644 index 0000000..40a8bf3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/can_mock.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +""" +can_mock.py — Mock CAN bus for unit/integration tests. + +Implements the same minimal interface as python-can's Bus class so test code +can inject frames and capture outbound traffic without real hardware or a +running SocketCAN interface. + +Interface +--------- + MockCANBus.send(msg, timeout=None) — capture frame; if loopback, also enqueue for recv + MockCANBus.recv(timeout=None) — return next injected frame (or None on timeout) + MockCANBus.inject(arb_id, data, — queue a frame as if received from the bus + is_extended_id=False) + MockCANBus.get_sent_frames() — return copy of all sent frames list + MockCANBus.reset() — clear all state + MockCANBus.shutdown() — mark as shut down +""" + +import queue +import threading +import time +from typing import List, Optional + +try: + import can + _Message = can.Message +except ImportError: + # Lightweight stand-in when python-can is not installed + class _Message: # type: ignore[no-redef] + def __init__( + self, + arbitration_id: int = 0, + data: bytes = b"", + is_extended_id: bool = False, + timestamp: Optional[float] = None, + ) -> None: + self.arbitration_id = arbitration_id + self.data = bytearray(data) + self.is_extended_id = is_extended_id + self.timestamp = timestamp if timestamp is not None else time.monotonic() + + +class MockCANBus: + """ + Thread-safe mock CAN bus. + + Parameters + ---------- + loopback: bool + When True, every frame passed to send() is also placed in the recv + queue, simulating a loopback interface. + filters: list[dict] or None + Optional list of {"can_id": int, "can_mask": int} dicts. Only frames + matching at least one filter are returned by recv(). If None, all + frames are returned. + """ + + def __init__(self, loopback: bool = False, filters=None) -> None: + self._loopback = loopback + self._filters = filters + self._recv_q: queue.Queue = queue.Queue() + self._sent: List[_Message] = [] + self._sent_lock = threading.Lock() + self._shutdown = False + + # ------------------------------------------------------------------ + # python-can Bus interface (subset used by CanBridgeNode) + # ------------------------------------------------------------------ + + def send(self, msg, timeout: Optional[float] = None) -> None: + """Record an outbound frame. If loopback is enabled, also enqueue it.""" + if self._shutdown: + raise RuntimeError("MockCANBus is shut down") + # Normalise to _Message so callers can pass any object with the right attrs + with self._sent_lock: + self._sent.append(msg) + if self._loopback: + self._recv_q.put(msg) + + def recv(self, timeout: Optional[float] = None) -> Optional[_Message]: + """ + Return the next injected frame. Blocks up to timeout seconds. + Returns None if nothing arrives within the timeout. + """ + if self._shutdown: + return None + try: + return self._recv_q.get(block=True, timeout=timeout) + except queue.Empty: + return None + + def shutdown(self) -> None: + """Mark the bus as shut down; subsequent recv() returns None.""" + self._shutdown = True + + # ------------------------------------------------------------------ + # Test helpers + # ------------------------------------------------------------------ + + def inject( + self, + arbitration_id: int, + data: bytes, + is_extended_id: bool = False, + timestamp: Optional[float] = None, + ) -> None: + """ + Inject a frame into the receive queue as if it arrived from the bus. + + Parameters + ---------- + arbitration_id: CAN arbitration ID + data: frame payload bytes + is_extended_id: True for 29-bit extended frames (VESC style) + timestamp: optional monotonic timestamp; defaults to time.monotonic() + """ + msg = _Message( + arbitration_id=arbitration_id, + data=data, + is_extended_id=is_extended_id, + timestamp=timestamp if timestamp is not None else time.monotonic(), + ) + self._recv_q.put(msg) + + def get_sent_frames(self) -> List[_Message]: + """Return a snapshot of all frames sent through this bus.""" + with self._sent_lock: + return list(self._sent) + + def get_sent_frames_by_id(self, arbitration_id: int) -> List[_Message]: + """Return only sent frames whose arbitration_id matches.""" + with self._sent_lock: + return [f for f in self._sent if f.arbitration_id == arbitration_id] + + def reset(self) -> None: + """Clear all sent frames and drain the receive queue.""" + with self._sent_lock: + self._sent.clear() + while not self._recv_q.empty(): + try: + self._recv_q.get_nowait() + except queue.Empty: + break + self._shutdown = False + + def pending_recv(self) -> int: + """Return the number of frames waiting in the receive queue.""" + return self._recv_q.qsize() + + # ------------------------------------------------------------------ + # Context manager support + # ------------------------------------------------------------------ + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + self.shutdown() + return False diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py new file mode 100644 index 0000000..41bdd6c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/saltybot_can_e2e_test/protocol_defs.py @@ -0,0 +1,314 @@ +#!/usr/bin/env python3 +""" +protocol_defs.py — CAN message ID constants and frame builders/parsers for the +Orin↔Mamba↔VESC integration test suite. + +All IDs and payload formats are derived from: + include/orin_can.h — Orin↔FC (Mamba) protocol + include/vesc_can.h — VESC CAN protocol + saltybot_can_bridge/mamba_protocol.py — existing bridge constants + +CAN IDs used in tests +--------------------- +Orin → FC (Mamba) commands (standard 11-bit, matching orin_can.h): + ORIN_CMD_HEARTBEAT 0x300 + ORIN_CMD_DRIVE 0x301 int16 speed (−1000..+1000), int16 steer (−1000..+1000) + ORIN_CMD_MODE 0x302 uint8 mode byte + ORIN_CMD_ESTOP 0x303 uint8 action (1=ESTOP, 0=CLEAR) + +FC (Mamba) → Orin telemetry (standard 11-bit, matching orin_can.h): + FC_STATUS 0x400 8 bytes (see orin_can_fc_status_t) + FC_VESC 0x401 8 bytes (see orin_can_fc_vesc_t) + FC_IMU 0x402 8 bytes + FC_BARO 0x403 8 bytes + +Mamba ↔ VESC internal commands (matching mamba_protocol.py): + MAMBA_CMD_VELOCITY 0x100 8 bytes left_mps (f32) | right_mps (f32) big-endian + MAMBA_CMD_MODE 0x101 1 byte mode (0=idle,1=drive,2=estop) + MAMBA_CMD_ESTOP 0x102 1 byte 0x01=stop + +VESC STATUS (extended 29-bit, matching vesc_can.h): + arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id = (9 << 8) | node_id + Payload: int32 RPM (BE), int16 current×10 (BE), int16 duty×1000 (BE) +""" + +import struct +from typing import Tuple + +# --------------------------------------------------------------------------- +# Orin → FC (Mamba) command IDs (from orin_can.h) +# --------------------------------------------------------------------------- + +ORIN_CMD_HEARTBEAT: int = 0x300 +ORIN_CMD_DRIVE: int = 0x301 +ORIN_CMD_MODE: int = 0x302 +ORIN_CMD_ESTOP: int = 0x303 + +# --------------------------------------------------------------------------- +# FC (Mamba) → Orin telemetry IDs (from orin_can.h) +# --------------------------------------------------------------------------- + +FC_STATUS: int = 0x400 +FC_VESC: int = 0x401 +FC_IMU: int = 0x402 +FC_BARO: int = 0x403 + +# --------------------------------------------------------------------------- +# Mamba → VESC internal command IDs (from mamba_protocol.py) +# --------------------------------------------------------------------------- + +MAMBA_CMD_VELOCITY: int = 0x100 +MAMBA_CMD_MODE: int = 0x101 +MAMBA_CMD_ESTOP: int = 0x102 + +MAMBA_TELEM_IMU: int = 0x200 +MAMBA_TELEM_BATTERY: int = 0x201 +VESC_TELEM_STATE: int = 0x300 + +# --------------------------------------------------------------------------- +# Mode constants +# --------------------------------------------------------------------------- + +MODE_IDLE: int = 0 +MODE_DRIVE: int = 1 +MODE_ESTOP: int = 2 + +# --------------------------------------------------------------------------- +# VESC protocol constants (from vesc_can.h) +# --------------------------------------------------------------------------- + +VESC_PKT_STATUS: int = 9 # STATUS packet type (upper byte of arb_id) +VESC_PKT_SET_RPM: int = 3 # SET_RPM packet type + +VESC_CAN_ID_LEFT: int = 56 +VESC_CAN_ID_RIGHT: int = 68 + + +def VESC_STATUS_ID(vesc_node_id: int) -> int: + """ + Return the 29-bit extended arbitration ID for a VESC STATUS frame. + + Formula (from vesc_can.h): arb_id = (VESC_PKT_STATUS << 8) | vesc_node_id + = (9 << 8) | vesc_node_id + """ + return (VESC_PKT_STATUS << 8) | vesc_node_id + + +def VESC_SET_RPM_ID(vesc_node_id: int) -> int: + """ + Return the 29-bit extended arbitration ID for a VESC SET_RPM command. + + Formula: arb_id = (VESC_PKT_SET_RPM << 8) | vesc_node_id + = (3 << 8) | vesc_node_id + """ + return (VESC_PKT_SET_RPM << 8) | vesc_node_id + + +# --------------------------------------------------------------------------- +# Frame builders — Orin → FC +# --------------------------------------------------------------------------- + +def build_heartbeat(seq: int = 0) -> bytes: + """Build a HEARTBEAT payload: uint32 sequence counter (big-endian, 4 bytes).""" + return struct.pack(">I", seq & 0xFFFFFFFF) + + +def build_drive_cmd(speed: int, steer: int) -> bytes: + """ + Build an ORIN_CMD_DRIVE payload. + + Parameters + ---------- + speed: int, −1000..+1000 (mapped directly to int16) + steer: int, −1000..+1000 + """ + return struct.pack(">hh", int(speed), int(steer)) + + +def build_mode_cmd(mode: int) -> bytes: + """Build an ORIN_CMD_MODE payload (1 byte).""" + return struct.pack(">B", mode & 0xFF) + + +def build_estop_cmd(action: int = 1) -> bytes: + """Build an ORIN_CMD_ESTOP payload. action=1 → ESTOP, 0 → CLEAR.""" + return struct.pack(">B", action & 0xFF) + + +# --------------------------------------------------------------------------- +# Frame builders — Mamba velocity commands (mamba_protocol.py encoding) +# --------------------------------------------------------------------------- + +def build_velocity_cmd(left_mps: float, right_mps: float) -> bytes: + """ + Build a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). + + Matches encode_velocity_cmd() in mamba_protocol.py. + """ + return struct.pack(">ff", float(left_mps), float(right_mps)) + + +# --------------------------------------------------------------------------- +# Frame builders — FC → Orin telemetry +# --------------------------------------------------------------------------- + +def build_fc_status( + pitch_x10: int = 0, + motor_cmd: int = 0, + vbat_mv: int = 24000, + balance_state: int = 1, + flags: int = 0, +) -> bytes: + """ + Build an FC_STATUS (0x400) payload. + + Layout (orin_can_fc_status_t, 8 bytes, big-endian): + int16 pitch_x10 + int16 motor_cmd + uint16 vbat_mv + uint8 balance_state + uint8 flags [bit0=estop_active, bit1=armed] + """ + return struct.pack( + ">hhHBB", + int(pitch_x10), + int(motor_cmd), + int(vbat_mv) & 0xFFFF, + int(balance_state) & 0xFF, + int(flags) & 0xFF, + ) + + +def build_fc_vesc( + left_rpm_x10: int = 0, + right_rpm_x10: int = 0, + left_current_x10: int = 0, + right_current_x10: int = 0, +) -> bytes: + """ + Build an FC_VESC (0x401) payload. + + Layout (orin_can_fc_vesc_t, 8 bytes, big-endian): + int16 left_rpm_x10 + int16 right_rpm_x10 + int16 left_current_x10 + int16 right_current_x10 + + RPM values are RPM / 10 (e.g. 3000 RPM → 300). + Current values are A × 10 (e.g. 5.5 A → 55). + """ + return struct.pack( + ">hhhh", + int(left_rpm_x10), + int(right_rpm_x10), + int(left_current_x10), + int(right_current_x10), + ) + + +def build_vesc_status( + rpm: int = 0, + current_x10: int = 0, + duty_x1000: int = 0, +) -> bytes: + """ + Build a VESC STATUS (packet type 9) payload. + + Layout (from vesc_can.h / VESC FW 6.x, big-endian): + int32 rpm + int16 current × 10 + int16 duty × 1000 + Total: 8 bytes. + """ + return struct.pack( + ">ihh", + int(rpm), + int(current_x10), + int(duty_x1000), + ) + + +# --------------------------------------------------------------------------- +# Frame parsers +# --------------------------------------------------------------------------- + +def parse_fc_status(data: bytes): + """ + Parse an FC_STATUS (0x400) payload. + + Returns + ------- + dict with keys: pitch_x10, motor_cmd, vbat_mv, balance_state, flags, + estop_active (bool), armed (bool) + """ + if len(data) < 8: + raise ValueError(f"FC_STATUS needs 8 bytes, got {len(data)}") + pitch_x10, motor_cmd, vbat_mv, balance_state, flags = struct.unpack( + ">hhHBB", data[:8] + ) + return { + "pitch_x10": pitch_x10, + "motor_cmd": motor_cmd, + "vbat_mv": vbat_mv, + "balance_state": balance_state, + "flags": flags, + "estop_active": bool(flags & 0x01), + "armed": bool(flags & 0x02), + } + + +def parse_fc_vesc(data: bytes): + """ + Parse an FC_VESC (0x401) payload. + + Returns + ------- + dict with keys: left_rpm_x10, right_rpm_x10, left_current_x10, + right_current_x10, left_rpm (float), right_rpm (float) + """ + if len(data) < 8: + raise ValueError(f"FC_VESC needs 8 bytes, got {len(data)}") + left_rpm_x10, right_rpm_x10, left_cur_x10, right_cur_x10 = struct.unpack( + ">hhhh", data[:8] + ) + return { + "left_rpm_x10": left_rpm_x10, + "right_rpm_x10": right_rpm_x10, + "left_current_x10": left_cur_x10, + "right_current_x10": right_cur_x10, + "left_rpm": left_rpm_x10 * 10.0, + "right_rpm": right_rpm_x10 * 10.0, + } + + +def parse_vesc_status(data: bytes): + """ + Parse a VESC STATUS (packet type 9) payload. + + Returns + ------- + dict with keys: rpm, current_x10, duty_x1000, current (float), duty (float) + """ + if len(data) < 8: + raise ValueError(f"VESC STATUS needs 8 bytes, got {len(data)}") + rpm, current_x10, duty_x1000 = struct.unpack(">ihh", data[:8]) + return { + "rpm": rpm, + "current_x10": current_x10, + "duty_x1000": duty_x1000, + "current": current_x10 / 10.0, + "duty": duty_x1000 / 1000.0, + } + + +def parse_velocity_cmd(data: bytes) -> Tuple[float, float]: + """ + Parse a MAMBA_CMD_VELOCITY payload (8 bytes, 2 × float32 big-endian). + + Returns + ------- + (left_mps, right_mps) + """ + if len(data) < 8: + raise ValueError(f"MAMBA_CMD_VELOCITY needs 8 bytes, got {len(data)}") + return struct.unpack(">ff", data[:8]) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg new file mode 100644 index 0000000..bb51679 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_can_e2e_test + +[install] +install_scripts=$base/lib/saltybot_can_e2e_test diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py new file mode 100644 index 0000000..eff1956 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/setup.py @@ -0,0 +1,23 @@ +from setuptools import setup + +package_name = "saltybot_can_e2e_test" + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-jetson", + maintainer_email="sl-jetson@saltylab.local", + description="End-to-end CAN integration tests for Orin↔Mamba↔VESC full loop", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/conftest.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/conftest.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000..e45363a Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/conftest.cpython-314-pytest-9.0.2.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_drive_command.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_drive_command.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000..aada609 Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_drive_command.cpython-314-pytest-9.0.2.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_estop.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_estop.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000..1d81f0f Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_estop.cpython-314-pytest-9.0.2.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_fc_vesc_broadcast.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_fc_vesc_broadcast.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000..c4fdac5 Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_fc_vesc_broadcast.cpython-314-pytest-9.0.2.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_heartbeat_timeout.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_heartbeat_timeout.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000..dc3434d Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_heartbeat_timeout.cpython-314-pytest-9.0.2.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_mode_switching.cpython-314-pytest-9.0.2.pyc b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_mode_switching.cpython-314-pytest-9.0.2.pyc new file mode 100644 index 0000000..125c022 Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/__pycache__/test_mode_switching.cpython-314-pytest-9.0.2.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py new file mode 100644 index 0000000..db8c471 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/conftest.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +""" +conftest.py — pytest fixtures for the saltybot_can_e2e_test suite. + +No ROS2 node infrastructure is started; all tests run purely in Python. +""" + +import sys +import os + +# Ensure the package root is on sys.path so relative imports work when running +# pytest directly from the saltybot_can_e2e_test/ directory. +_pkg_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +if _pkg_root not in sys.path: + sys.path.insert(0, _pkg_root) + +# Also add the saltybot_can_bridge package so we can import mamba_protocol. +_bridge_pkg = os.path.join( + os.path.dirname(_pkg_root), "saltybot_can_bridge" +) +if _bridge_pkg not in sys.path: + sys.path.insert(0, _bridge_pkg) + +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + VESC_CAN_ID_LEFT, + VESC_CAN_ID_RIGHT, +) + + +# --------------------------------------------------------------------------- +# Core fixtures +# --------------------------------------------------------------------------- + + +@pytest.fixture(scope="function") +def mock_can_bus(): + """ + Provide a fresh MockCANBus instance per test function. + The bus is automatically shut down after each test. + """ + bus = MockCANBus(loopback=False) + yield bus + bus.shutdown() + + +@pytest.fixture(scope="function") +def loopback_can_bus(): + """ + MockCANBus in loopback mode — sent frames are also queued for recv. + Useful for testing round-trip behaviour without a second node. + """ + bus = MockCANBus(loopback=True) + yield bus + bus.shutdown() + + +@pytest.fixture(scope="function") +def bridge_components(): + """ + Return the mamba_protocol encode/decode callables and a fresh mock bus. + + Yields a dict with keys: + bus — MockCANBus instance + encode_vel — encode_velocity_cmd(left, right) → bytes + encode_mode — encode_mode_cmd(mode) → bytes + encode_estop — encode_estop_cmd(stop) → bytes + decode_vesc — decode_vesc_state(data) → VescStateTelemetry + """ + from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, + decode_vesc_state, + decode_battery_telem, + decode_imu_telem, + ) + + bus = MockCANBus(loopback=False) + yield { + "bus": bus, + "encode_vel": encode_velocity_cmd, + "encode_mode": encode_mode_cmd, + "encode_estop": encode_estop_cmd, + "decode_vesc": decode_vesc_state, + "decode_battery": decode_battery_telem, + "decode_imu": decode_imu_telem, + "left_vesc_id": VESC_CAN_ID_LEFT, + "right_vesc_id": VESC_CAN_ID_RIGHT, + } + bus.shutdown() diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py new file mode 100644 index 0000000..dfbee87 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_drive_command.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +""" +test_drive_command.py — Integration tests for the drive command path. + +Tests verify: + DRIVE cmd → Mamba receives velocity command frame → mock VESC status response + → FC_VESC broadcast contains correct RPMs. + +All tests run without real hardware or a running ROS2 system. +Run with: python -m pytest test/test_drive_command.py -v +""" + +import struct +import pytest + +from saltybot_can_e2e_test.protocol_defs import ( + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + FC_VESC, + MODE_DRIVE, + MODE_IDLE, + VESC_CAN_ID_LEFT, + VESC_CAN_ID_RIGHT, + VESC_STATUS_ID, + build_velocity_cmd, + build_fc_vesc, + build_vesc_status, + parse_velocity_cmd, + parse_fc_vesc, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, +) + + +# --------------------------------------------------------------------------- +# Helper +# --------------------------------------------------------------------------- + +def _send_drive(bus, left_mps: float, right_mps: float) -> None: + """Simulate the bridge encoding and sending a velocity command.""" + from saltybot_can_e2e_test.can_mock import MockCANBus + + payload = encode_velocity_cmd(left_mps, right_mps) + # Create a minimal message object compatible with our mock + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + bus.send(_Msg(MAMBA_CMD_VELOCITY, payload)) + bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestDriveForward: + def test_drive_forward_velocity_frame_sent(self, mock_can_bus): + """ + Inject DRIVE cmd (1.0 m/s, 1.0 m/s) → verify Mamba receives + a MAMBA_CMD_VELOCITY frame with correct payload. + """ + _send_drive(mock_can_bus, 1.0, 1.0) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1, "Expected exactly one velocity command frame" + + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left - 1.0) < 1e-4, f"Left speed {left} != 1.0" + assert abs(right - 1.0) < 1e-4, f"Right speed {right} != 1.0" + + def test_drive_forward_mode_drive_sent(self, mock_can_bus): + """After a drive command, a MODE=drive frame must accompany it.""" + _send_drive(mock_can_bus, 1.0, 1.0) + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert len(mode_frames) >= 1, "Expected at least one MODE frame" + assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) + + def test_drive_forward_fc_vesc_broadcast(self, mock_can_bus): + """ + Simulate FC_VESC broadcast arriving after drive cmd; verify parse is correct. + (In the real loop Mamba computes RPM from m/s and broadcasts FC_VESC.) + This test checks the FC_VESC frame format and parser. + """ + # Simulate: 1.0 m/s → ~300 RPM × 10 = 3000 (representative, not physics) + fc_payload = build_fc_vesc( + left_rpm_x10=300, right_rpm_x10=300, + left_current_x10=50, right_current_x10=50, + ) + mock_can_bus.inject(FC_VESC, fc_payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None, "FC_VESC frame not received" + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_rpm_x10"] == 300 + assert parsed["right_rpm_x10"] == 300 + assert abs(parsed["left_rpm"] - 3000.0) < 0.1 + + +class TestDriveTurn: + def test_drive_turn_differential_rpm(self, mock_can_bus): + """ + DRIVE cmd (0.5, −0.5) → verify differential RPM in velocity command. + """ + _send_drive(mock_can_bus, 0.5, -0.5) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left - 0.5) < 1e-4, f"Left speed {left} != 0.5" + assert abs(right - (-0.5)) < 1e-4, f"Right speed {right} != -0.5" + # Signs must be opposite for a zero-radius spin + assert left > 0 and right < 0 + + def test_drive_turn_fc_vesc_differential(self, mock_can_bus): + """Simulated FC_VESC for a turn has opposite-sign RPMs.""" + fc_payload = build_fc_vesc( + left_rpm_x10=150, right_rpm_x10=-150, + left_current_x10=30, right_current_x10=30, + ) + mock_can_bus.inject(FC_VESC, fc_payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_rpm_x10"] > 0 + assert parsed["right_rpm_x10"] < 0 + + +class TestDriveZero: + def test_drive_zero_stops_motors(self, mock_can_bus): + """ + After a non-zero drive cmd, sending zero velocity must result in + RPM=0 being commanded to both VESCs. + """ + _send_drive(mock_can_bus, 1.0, 1.0) + mock_can_bus.reset() # clear prior frames + + _send_drive(mock_can_bus, 0.0, 0.0) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left) < 1e-5, "Left motor not stopped" + assert abs(right) < 1e-5, "Right motor not stopped" + + +class TestDriveCmdTimeout: + def test_drive_cmd_timeout_sends_zero(self, mock_can_bus): + """ + Simulate the watchdog behaviour: if no DRIVE cmd arrives for >500 ms, + zero velocity is sent. We test the encoding directly (without timers). + """ + # The watchdog in CanBridgeNode calls encode_velocity_cmd(0.0, 0.0) and + # sends it on MAMBA_CMD_VELOCITY. Replicate that here. + zero_payload = encode_velocity_cmd(0.0, 0.0) + + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, zero_payload)) + mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE))) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + left, right = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(left) < 1e-5 + assert abs(right) < 1e-5 + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert len(mode_frames) == 1 + assert bytes(mode_frames[0].data) == bytes([MODE_IDLE]) + + +@pytest.mark.parametrize("left_mps,right_mps", [ + (0.5, 0.5), + (1.0, 0.0), + (0.0, -1.0), + (-0.5, -0.5), +]) +def test_drive_cmd_payload_roundtrip(mock_can_bus, left_mps, right_mps): + """Parametrized: encode then decode must recover original velocities.""" + payload = encode_velocity_cmd(left_mps, right_mps) + l, r = parse_velocity_cmd(payload) + assert abs(l - left_mps) < 1e-4 + assert abs(r - right_mps) < 1e-4 diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py new file mode 100644 index 0000000..b77581e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_estop.py @@ -0,0 +1,264 @@ +#!/usr/bin/env python3 +""" +test_estop.py — E-stop command integration tests. + +Covers: + - ESTOP command halts motors immediately + - ESTOP persists: DRIVE commands ignored while ESTOP is active + - ESTOP clear restores normal drive operation + - Firmware-side estop via FC_STATUS flags is detected correctly + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_estop.py -v +""" + +import struct +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + MAMBA_CMD_ESTOP, + ORIN_CMD_ESTOP, + FC_STATUS, + MODE_IDLE, + MODE_DRIVE, + MODE_ESTOP, + build_estop_cmd, + build_mode_cmd, + build_velocity_cmd, + build_fc_status, + parse_velocity_cmd, + parse_fc_status, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, +) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class _Msg: + """Minimal CAN message stand-in.""" + def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = is_extended_id + + +class EstopStateMachine: + """ + Lightweight state machine that mirrors the bridge estop logic. + + Tracks whether ESTOP is active and gates velocity commands accordingly. + Sends frames to the supplied MockCANBus. + """ + + def __init__(self, bus: MockCANBus): + self._bus = bus + self._estop_active = False + self._mode = MODE_IDLE + + def assert_estop(self) -> None: + """Send ESTOP and transition to estop mode.""" + self._estop_active = True + self._mode = MODE_ESTOP + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) + self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + + def clear_estop(self) -> None: + """Clear ESTOP and return to IDLE mode.""" + self._estop_active = False + self._mode = MODE_IDLE + self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False))) + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_IDLE))) + + def send_drive(self, left_mps: float, right_mps: float) -> None: + """Send velocity command only if ESTOP is not active.""" + if self._estop_active: + # Bridge silently drops commands while estopped + return + self._mode = MODE_DRIVE + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + + @property + def estop_active(self) -> bool: + return self._estop_active + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestEstopHaltsMotors: + def test_estop_command_halts_motors(self, mock_can_bus): + """ + After injecting ESTOP, zero velocity must be commanded immediately. + """ + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1, "No velocity frame after ESTOP" + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP" + assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP" + + def test_estop_mode_frame_sent(self, mock_can_bus): + """ESTOP mode byte must be broadcast on CAN.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any( + bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames + ), "MODE=ESTOP not found in sent frames" + + def test_estop_flag_byte_is_0x01(self, mock_can_bus): + """MAMBA_CMD_ESTOP payload must be 0x01 when asserting e-stop.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert len(estop_frames) >= 1 + assert bytes(estop_frames[-1].data) == b"\x01", \ + f"ESTOP payload {estop_frames[-1].data!r} != 0x01" + + +class TestEstopPersists: + def test_estop_persists_after_drive_cmd(self, mock_can_bus): + """ + After ESTOP, injecting a DRIVE command must NOT forward velocity to the bus. + """ + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + mock_can_bus.reset() # start fresh after initial ESTOP frames + + sm.send_drive(1.0, 1.0) # should be suppressed + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 0, \ + "Velocity command was forwarded while ESTOP is active" + + def test_estop_mode_unchanged_after_drive_attempt(self, mock_can_bus): + """ + After ESTOP, attempting DRIVE must not change the mode to DRIVE. + """ + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + mock_can_bus.reset() + + sm.send_drive(0.5, 0.5) + + # No mode frames should have been emitted (drive was suppressed) + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert all( + bytes(f.data) != bytes([MODE_DRIVE]) for f in mode_frames + ), "MODE=DRIVE was set despite active ESTOP" + + +class TestEstopClear: + def test_estop_clear_restores_drive(self, mock_can_bus): + """After ESTOP_CLEAR, drive commands must be accepted again.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + sm.clear_estop() + mock_can_bus.reset() + + sm.send_drive(0.8, 0.8) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1, "Velocity command not sent after ESTOP clear" + l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(l - 0.8) < 1e-4 + assert abs(r - 0.8) < 1e-4 + + def test_estop_clear_flag_byte_is_0x00(self, mock_can_bus): + """MAMBA_CMD_ESTOP payload must be 0x00 when clearing e-stop.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + sm.clear_estop() + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert len(estop_frames) >= 2 + # Last ESTOP frame should be the clear + assert bytes(estop_frames[-1].data) == b"\x00", \ + f"ESTOP clear payload {estop_frames[-1].data!r} != 0x00" + + def test_estop_clear_mode_returns_to_idle(self, mock_can_bus): + """After clearing ESTOP, the mode frame must be MODE_IDLE.""" + sm = EstopStateMachine(mock_can_bus) + sm.assert_estop() + sm.clear_estop() + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + last_mode = bytes(mode_frames[-1].data) + assert last_mode == bytes([MODE_IDLE]), \ + f"Mode after ESTOP clear is {last_mode!r}, expected MODE_IDLE" + + +class TestFirmwareSideEstop: + def test_fc_status_estop_flag_detected(self, mock_can_bus): + """ + Simulate firmware sending estop via FC_STATUS flags (bit0=estop_active). + Verify the Orin bridge side correctly parses the flag. + """ + # Build FC_STATUS with estop_active bit set (flags=0x01) + payload = build_fc_status( + pitch_x10=0, + motor_cmd=0, + vbat_mv=24000, + balance_state=2, # TILT_FAULT + flags=0x01, # bit0 = estop_active + ) + mock_can_bus.inject(FC_STATUS, payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None, "FC_STATUS frame not received" + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["estop_active"] is True, \ + "estop_active flag not set in FC_STATUS" + assert parsed["balance_state"] == 2 + + def test_fc_status_no_estop_flag(self, mock_can_bus): + """FC_STATUS with flags=0x00 must NOT set estop_active.""" + payload = build_fc_status(flags=0x00) + mock_can_bus.inject(FC_STATUS, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["estop_active"] is False + + def test_fc_status_armed_flag_detected(self, mock_can_bus): + """FC_STATUS flags bit1=armed must parse correctly.""" + payload = build_fc_status(flags=0x02) # bit1 = armed + mock_can_bus.inject(FC_STATUS, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["armed"] is True + assert parsed["estop_active"] is False + + def test_fc_status_roundtrip(self, mock_can_bus): + """build_fc_status → inject → recv → parse_fc_status must be identity.""" + payload = build_fc_status( + pitch_x10=150, + motor_cmd=-200, + vbat_mv=23800, + balance_state=1, + flags=0x03, + ) + mock_can_bus.inject(FC_STATUS, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_status(bytes(frame.data)) + assert parsed["pitch_x10"] == 150 + assert parsed["motor_cmd"] == -200 + assert parsed["vbat_mv"] == 23800 + assert parsed["balance_state"] == 1 + assert parsed["estop_active"] is True + assert parsed["armed"] is True diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py new file mode 100644 index 0000000..a7e9f9a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_fc_vesc_broadcast.py @@ -0,0 +1,315 @@ +#!/usr/bin/env python3 +""" +test_fc_vesc_broadcast.py — FC_VESC broadcast and VESC STATUS integration tests. + +Covers: + - VESC STATUS extended frame for left VESC (ID 56) triggers FC_VESC broadcast + - Both left (56) and right (68) VESC STATUS combined in FC_VESC + - FC_VESC broadcast rate (~10 Hz) + - current_x10 scaling matches protocol spec + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_fc_vesc_broadcast.py -v +""" + +import struct +import time +import threading +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + FC_VESC, + VESC_CAN_ID_LEFT, + VESC_CAN_ID_RIGHT, + VESC_STATUS_ID, + VESC_SET_RPM_ID, + VESC_TELEM_STATE, + build_vesc_status, + build_fc_vesc, + parse_fc_vesc, + parse_vesc_status, +) +from saltybot_can_bridge.mamba_protocol import ( + VESC_TELEM_STATE as BRIDGE_VESC_TELEM_STATE, + decode_vesc_state, +) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class VescStatusAggregator: + """ + Simulates the firmware logic that: + 1. Receives VESC STATUS extended frames from left/right VESCs + 2. Builds an FC_VESC broadcast payload + 3. Injects the FC_VESC frame onto the mock bus + + This represents the Mamba → Orin telemetry path. + """ + + def __init__(self, bus: MockCANBus): + self._bus = bus + self._left_rpm_x10 = 0 + self._right_rpm_x10 = 0 + self._left_current_x10 = 0 + self._right_current_x10 = 0 + self._left_seen = False + self._right_seen = False + + def process_vesc_status(self, arb_id: int, data: bytes) -> None: + """ + Process an incoming VESC STATUS frame (extended 29-bit ID). + Updates internal state; broadcasts FC_VESC when at least one side is known. + """ + node_id = arb_id & 0xFF + parsed = parse_vesc_status(data) + rpm_x10 = parsed["rpm"] // 10 # convert full RPM to RPM/10 + + if node_id == VESC_CAN_ID_LEFT: + self._left_rpm_x10 = rpm_x10 + self._left_current_x10 = parsed["current_x10"] + self._left_seen = True + elif node_id == VESC_CAN_ID_RIGHT: + self._right_rpm_x10 = rpm_x10 + self._right_current_x10 = parsed["current_x10"] + self._right_seen = True + + # Broadcast FC_VESC whenever we receive any update + self._broadcast_fc_vesc() + + def _broadcast_fc_vesc(self) -> None: + payload = build_fc_vesc( + left_rpm_x10=self._left_rpm_x10, + right_rpm_x10=self._right_rpm_x10, + left_current_x10=self._left_current_x10, + right_current_x10=self._right_current_x10, + ) + self._bus.inject(FC_VESC, payload) + + +def _inject_vesc_status(bus: MockCANBus, vesc_id: int, rpm: int, + current_x10: int = 50, duty_x1000: int = 250) -> None: + """Inject a VESC STATUS extended frame for the given node ID.""" + arb_id = VESC_STATUS_ID(vesc_id) + payload = build_vesc_status(rpm=rpm, current_x10=current_x10, duty_x1000=duty_x1000) + bus.inject(arb_id, payload, is_extended_id=True) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestVescStatusToFcVesc: + def test_left_vesc_status_triggers_broadcast(self, mock_can_bus): + """ + Inject VESC STATUS for left VESC (ID 56) → verify FC_VESC contains + the correct left RPM (rpm / 10). + """ + agg = VescStatusAggregator(mock_can_bus) + + # Left VESC: 3000 RPM → rpm_x10 = 300 + arb_id = VESC_STATUS_ID(VESC_CAN_ID_LEFT) + payload = build_vesc_status(rpm=3000, current_x10=55) + agg.process_vesc_status(arb_id, payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None, "No FC_VESC broadcast after left VESC STATUS" + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_rpm_x10"] == 300, \ + f"left_rpm_x10 {parsed['left_rpm_x10']} != 300" + assert abs(parsed["left_rpm"] - 3000.0) < 1.0 + + def test_right_vesc_status_triggers_broadcast(self, mock_can_bus): + """Inject VESC STATUS for right VESC (ID 68) → verify right RPM in FC_VESC.""" + agg = VescStatusAggregator(mock_can_bus) + + arb_id = VESC_STATUS_ID(VESC_CAN_ID_RIGHT) + payload = build_vesc_status(rpm=2000, current_x10=40) + agg.process_vesc_status(arb_id, payload) + + frame = mock_can_bus.recv(timeout=0.1) + assert frame is not None + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["right_rpm_x10"] == 200 + + def test_left_vesc_id_matches_constant(self): + """VESC_STATUS_ID(56) must equal (9 << 8) | 56 = 0x938.""" + assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == (9 << 8) | 56 + assert VESC_STATUS_ID(VESC_CAN_ID_LEFT) == 0x938 + + def test_right_vesc_id_matches_constant(self): + """VESC_STATUS_ID(68) must equal (9 << 8) | 68 = 0x944.""" + assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == (9 << 8) | 68 + assert VESC_STATUS_ID(VESC_CAN_ID_RIGHT) == 0x944 + + +class TestBothVescStatusCombined: + def test_both_vesc_status_combined_in_fc_vesc(self, mock_can_bus): + """ + Inject both left (56) and right (68) VESC STATUS frames. + Final FC_VESC must contain both RPMs. + """ + agg = VescStatusAggregator(mock_can_bus) + + # Left: 3000 RPM + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_LEFT), + build_vesc_status(rpm=3000, current_x10=50), + ) + # Right: -1500 RPM (reverse) + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_RIGHT), + build_vesc_status(rpm=-1500, current_x10=30), + ) + + # Drain two FC_VESC frames (one per update), check the latest + frames = [] + while True: + f = mock_can_bus.recv(timeout=0.05) + if f is None: + break + frames.append(f) + + assert len(frames) >= 2, "Expected at least 2 FC_VESC frames" + # Last frame must have both sides + last = parse_fc_vesc(bytes(frames[-1].data)) + assert last["left_rpm_x10"] == 300, \ + f"left_rpm_x10 {last['left_rpm_x10']} != 300" + assert last["right_rpm_x10"] == -150, \ + f"right_rpm_x10 {last['right_rpm_x10']} != -150" + + def test_both_vesc_currents_combined(self, mock_can_bus): + """Both current values must appear in FC_VESC after two STATUS frames.""" + agg = VescStatusAggregator(mock_can_bus) + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_LEFT), + build_vesc_status(rpm=1000, current_x10=55), + ) + agg.process_vesc_status( + VESC_STATUS_ID(VESC_CAN_ID_RIGHT), + build_vesc_status(rpm=1000, current_x10=42), + ) + frames = [] + while True: + f = mock_can_bus.recv(timeout=0.05) + if f is None: + break + frames.append(f) + last = parse_fc_vesc(bytes(frames[-1].data)) + assert last["left_current_x10"] == 55 + assert last["right_current_x10"] == 42 + + +class TestVescBroadcastRate: + def test_fc_vesc_broadcast_at_10hz(self, mock_can_bus): + """ + Simulate FC_VESC broadcasts at ~10 Hz and verify the rate. + We inject 12 frames over ~120 ms, then verify count and average interval. + """ + _FC_VESC_HZ = 10 + _interval = 1.0 / _FC_VESC_HZ + + timestamps = [] + stop_event = threading.Event() + + def broadcaster(): + while not stop_event.is_set(): + t = time.monotonic() + mock_can_bus.inject( + FC_VESC, + build_fc_vesc(100, -100, 30, 30), + timestamp=t, + ) + timestamps.append(t) + time.sleep(_interval) + + t = threading.Thread(target=broadcaster, daemon=True) + t.start() + time.sleep(0.15) # collect ~1.5 broadcasts + stop_event.set() + t.join(timeout=0.2) + + assert len(timestamps) >= 1, "No FC_VESC broadcasts in 150 ms window" + + if len(timestamps) >= 2: + intervals = [timestamps[i+1] - timestamps[i] for i in range(len(timestamps)-1)] + avg = sum(intervals) / len(intervals) + # ±40 ms tolerance for OS scheduling + assert 0.06 <= avg <= 0.14, \ + f"FC_VESC broadcast interval {avg*1000:.1f} ms not ~100 ms" + + def test_fc_vesc_frame_is_8_bytes(self): + """FC_VESC payload must always be exactly 8 bytes.""" + payload = build_fc_vesc(300, -150, 55, 42) + assert len(payload) == 8 + + +class TestVescCurrentScaling: + def test_current_x10_scaling(self, mock_can_bus): + """ + Verify current_x10 scaling: 5.5 A → current_x10=55. + build_vesc_status stores current_x10 directly; parse_vesc_status + returns current = current_x10 / 10. + """ + payload = build_vesc_status(rpm=1000, current_x10=55, duty_x1000=250) + parsed = parse_vesc_status(payload) + assert parsed["current_x10"] == 55 + assert abs(parsed["current"] - 5.5) < 0.01 + + def test_current_negative_scaling(self, mock_can_bus): + """Negative current (regen) must scale correctly.""" + payload = build_vesc_status(rpm=-500, current_x10=-30) + parsed = parse_vesc_status(payload) + assert parsed["current_x10"] == -30 + assert abs(parsed["current"] - (-3.0)) < 0.01 + + def test_fc_vesc_current_x10_roundtrip(self, mock_can_bus): + """build_fc_vesc → inject → recv → parse must preserve current_x10.""" + payload = build_fc_vesc( + left_rpm_x10=200, + right_rpm_x10=200, + left_current_x10=55, + right_current_x10=42, + ) + mock_can_bus.inject(FC_VESC, payload) + frame = mock_can_bus.recv(timeout=0.1) + parsed = parse_fc_vesc(bytes(frame.data)) + assert parsed["left_current_x10"] == 55 + assert parsed["right_current_x10"] == 42 + + @pytest.mark.parametrize("vesc_id", [VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT]) + def test_vesc_status_id_both_nodes(self, vesc_id, mock_can_bus): + """ + VESC_STATUS_ID(vesc_id) must produce the correct 29-bit extended arb_id + for both the left (56) and right (68) node IDs. + """ + expected = (9 << 8) | vesc_id + assert VESC_STATUS_ID(vesc_id) == expected + + @pytest.mark.parametrize("vesc_id,rpm,expected_rpm_x10", [ + (VESC_CAN_ID_LEFT, 3000, 300), + (VESC_CAN_ID_LEFT, -1500, -150), + (VESC_CAN_ID_RIGHT, 2000, 200), + (VESC_CAN_ID_RIGHT, 0, 0), + ]) + def test_rpm_x10_conversion_parametrized( + self, mock_can_bus, vesc_id, rpm, expected_rpm_x10 + ): + """Parametrized: verify rpm → rpm_x10 conversion for both VESCs.""" + agg = VescStatusAggregator(mock_can_bus) + agg.process_vesc_status( + VESC_STATUS_ID(vesc_id), + build_vesc_status(rpm=rpm), + ) + frame = mock_can_bus.recv(timeout=0.05) + assert frame is not None + parsed = parse_fc_vesc(bytes(frame.data)) + if vesc_id == VESC_CAN_ID_LEFT: + assert parsed["left_rpm_x10"] == expected_rpm_x10, \ + f"left_rpm_x10={parsed['left_rpm_x10']} expected {expected_rpm_x10}" + else: + assert parsed["right_rpm_x10"] == expected_rpm_x10, \ + f"right_rpm_x10={parsed['right_rpm_x10']} expected {expected_rpm_x10}" diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py new file mode 100644 index 0000000..adf7db2 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_heartbeat_timeout.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +""" +test_heartbeat_timeout.py — Tests for heartbeat loss and recovery. + +Covers: + - Heartbeat loss triggers e-stop escalation (timeout logic) + - Heartbeat recovery restores previous mode + - Heartbeat interval is sent at ~100 ms + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_heartbeat_timeout.py -v +""" + +import time +import struct +import threading +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + ORIN_CMD_HEARTBEAT, + ORIN_CMD_ESTOP, + ORIN_CMD_MODE, + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + MAMBA_CMD_ESTOP, + MODE_IDLE, + MODE_DRIVE, + MODE_ESTOP, + build_heartbeat, + build_estop_cmd, + build_mode_cmd, + build_velocity_cmd, + parse_velocity_cmd, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, +) + +# Heartbeat timeout from orin_can.h: 500 ms +ORIN_HB_TIMEOUT_MS = 500 +ORIN_HB_TIMEOUT_S = ORIN_HB_TIMEOUT_MS / 1000.0 + +# Expected heartbeat interval +HB_INTERVAL_MS = 100 +HB_INTERVAL_S = HB_INTERVAL_MS / 1000.0 + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class HeartbeatSimulator: + """ + Simulate periodic heartbeat injection into a MockCANBus. + + Call start() to begin sending heartbeats every interval_s. + Call stop() to cease — after ORIN_HB_TIMEOUT_S the firmware would declare + Orin offline. + """ + + def __init__(self, bus: MockCANBus, interval_s: float = HB_INTERVAL_S): + self._bus = bus + self._interval_s = interval_s + self._seq = 0 + self._running = False + self._thread: threading.Thread | None = None + + def start(self): + self._running = True + self._thread = threading.Thread(target=self._run, daemon=True) + self._thread.start() + + def stop(self): + self._running = False + + def _run(self): + while self._running: + self._bus.inject( + ORIN_CMD_HEARTBEAT, + build_heartbeat(self._seq), + is_extended_id=False, + ) + self._seq += 1 + time.sleep(self._interval_s) + + +def _simulate_estop_on_timeout(bus: MockCANBus) -> None: + """ + Simulate the firmware-side logic: when heartbeat timeout expires, + the FC sends an e-stop command by setting estop mode on the Mamba bus. + We model this as the bridge sending zero velocity + ESTOP mode. + """ + + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_ESTOP))) + bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestHeartbeatLoss: + def test_heartbeat_loss_triggers_estop(self, mock_can_bus): + """ + After heartbeat ceases, the bridge must command zero velocity and + set ESTOP mode. We simulate this directly using _simulate_estop_on_timeout. + """ + # First confirm the bus is clean + assert len(mock_can_bus.get_sent_frames()) == 0 + + # Simulate bridge detecting timeout and escalating + _simulate_estop_on_timeout(mock_can_bus) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1, "Zero velocity not sent after timeout" + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l) < 1e-5, "Left not zero on timeout" + assert abs(r) < 1e-5, "Right not zero on timeout" + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any( + bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames + ), "ESTOP mode not asserted on heartbeat timeout" + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert len(estop_frames) >= 1, "ESTOP command not sent" + assert bytes(estop_frames[0].data) == b"\x01" + + def test_heartbeat_loss_zero_velocity(self, mock_can_bus): + """Zero velocity frame must appear among sent frames after timeout.""" + _simulate_estop_on_timeout(mock_can_bus) + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1 + for f in vel_frames: + l, r = parse_velocity_cmd(bytes(f.data)) + assert abs(l) < 1e-5 + assert abs(r) < 1e-5 + + +class TestHeartbeatRecovery: + def test_heartbeat_recovery_restores_drive_mode(self, mock_can_bus): + """ + After heartbeat loss + recovery, drive commands must be accepted again. + We simulate: ESTOP → clear estop → send drive → verify velocity frame. + """ + + class _Msg: + def __init__(self, arb_id, data): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = False + + # Phase 1: timeout → estop + _simulate_estop_on_timeout(mock_can_bus) + mock_can_bus.reset() + + # Phase 2: recovery — clear estop, restore drive mode + mock_can_bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(False))) + mock_can_bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(MODE_DRIVE))) + mock_can_bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.5, 0.5))) + + estop_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_ESTOP) + assert any(bytes(f.data) == b"\x00" for f in estop_frames), \ + "ESTOP clear not sent on recovery" + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any( + bytes(f.data) == bytes([MODE_DRIVE]) for f in mode_frames + ), "DRIVE mode not restored after recovery" + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1 + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l - 0.5) < 1e-4 + + def test_heartbeat_sequence_increments(self, mock_can_bus): + """Heartbeat payloads must have incrementing sequence numbers.""" + payloads = [] + for seq in range(5): + mock_can_bus.inject( + ORIN_CMD_HEARTBEAT, + build_heartbeat(seq), + is_extended_id=False, + ) + + for i in range(5): + frame = mock_can_bus.recv(timeout=0.05) + assert frame is not None + (seq_val,) = struct.unpack(">I", bytes(frame.data)) + assert seq_val == i, f"Expected seq {i}, got {seq_val}" + + +class TestHeartbeatInterval: + def test_heartbeat_interval_approx_100ms(self, mock_can_bus): + """ + Start HeartbeatSimulator, collect timestamps over ~300 ms, and verify + the average interval is within 20% of 100 ms. + """ + sim = HeartbeatSimulator(mock_can_bus, interval_s=0.1) + sim.start() + time.sleep(0.35) + sim.stop() + + timestamps = [] + while True: + frame = mock_can_bus.recv(timeout=0.01) + if frame is None: + break + if frame.arbitration_id == ORIN_CMD_HEARTBEAT: + timestamps.append(frame.timestamp) + + assert len(timestamps) >= 2, "Not enough heartbeat frames captured" + + intervals = [ + timestamps[i + 1] - timestamps[i] + for i in range(len(timestamps) - 1) + ] + avg_interval = sum(intervals) / len(intervals) + # Allow ±30 ms tolerance (OS scheduling jitter in CI) + assert 0.07 <= avg_interval <= 0.13, \ + f"Average heartbeat interval {avg_interval*1000:.1f} ms not ~100 ms" + + def test_heartbeat_payload_is_4_bytes(self, mock_can_bus): + """Heartbeat payload must be exactly 4 bytes (uint32 sequence).""" + for seq in (0, 1, 0xFFFFFFFF): + payload = build_heartbeat(seq) + assert len(payload) == 4, \ + f"Heartbeat payload length {len(payload)} != 4" diff --git a/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py new file mode 100644 index 0000000..d87ab29 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_can_e2e_test/test/test_mode_switching.py @@ -0,0 +1,236 @@ +#!/usr/bin/env python3 +""" +test_mode_switching.py — Mode transition integration tests. + +Covers: + - idle → drive: drive commands become accepted + - drive → estop: immediate motor stop + - MODE frame byte values match protocol constants + - Unknown mode byte is ignored (no state change) + +No ROS2 or real CAN hardware required. +Run with: python -m pytest test/test_mode_switching.py -v +""" + +import struct +import pytest + +from saltybot_can_e2e_test.can_mock import MockCANBus +from saltybot_can_e2e_test.protocol_defs import ( + MAMBA_CMD_VELOCITY, + MAMBA_CMD_MODE, + MAMBA_CMD_ESTOP, + MODE_IDLE, + MODE_DRIVE, + MODE_ESTOP, + build_mode_cmd, + build_velocity_cmd, + parse_velocity_cmd, +) +from saltybot_can_bridge.mamba_protocol import ( + encode_velocity_cmd, + encode_mode_cmd, + encode_estop_cmd, +) + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +class _Msg: + def __init__(self, arb_id: int, data: bytes, is_extended_id: bool = False): + self.arbitration_id = arb_id + self.data = bytearray(data) + self.is_extended_id = is_extended_id + + +class ModeStateMachine: + """ + Minimal state machine tracking mode transitions and gating commands. + """ + + def __init__(self, bus: MockCANBus): + self._bus = bus + self._mode = MODE_IDLE + + def set_mode(self, mode: int) -> bool: + """ + Transition to mode. Returns True if accepted, False if invalid. + Invalid mode values (not 0, 1, 2) are ignored. + """ + if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + return False # silently ignore + + prev_mode = self._mode + self._mode = mode + self._bus.send(_Msg(MAMBA_CMD_MODE, encode_mode_cmd(mode))) + + # Side-effects of entering ESTOP from DRIVE + if mode == MODE_ESTOP and prev_mode == MODE_DRIVE: + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(0.0, 0.0))) + self._bus.send(_Msg(MAMBA_CMD_ESTOP, encode_estop_cmd(True))) + + return True + + def send_drive(self, left_mps: float, right_mps: float) -> bool: + """ + Send a velocity command. Returns True if forwarded, False if blocked. + """ + if self._mode != MODE_DRIVE: + return False + self._bus.send(_Msg(MAMBA_CMD_VELOCITY, encode_velocity_cmd(left_mps, right_mps))) + return True + + @property + def mode(self) -> int: + return self._mode + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + +class TestIdleToDrive: + def test_idle_to_drive_mode_frame(self, mock_can_bus): + """Transitioning to DRIVE must emit a MODE=drive frame.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert len(mode_frames) == 1 + assert bytes(mode_frames[0].data) == bytes([MODE_DRIVE]) + + def test_idle_blocks_drive_commands(self, mock_can_bus): + """In IDLE mode, drive commands must be suppressed.""" + sm = ModeStateMachine(mock_can_bus) + # Attempt drive without entering DRIVE mode + forwarded = sm.send_drive(1.0, 1.0) + assert forwarded is False, "Drive cmd should be blocked in IDLE mode" + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 0 + + def test_drive_mode_allows_commands(self, mock_can_bus): + """After entering DRIVE mode, velocity commands must be forwarded.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + mock_can_bus.reset() + + forwarded = sm.send_drive(0.5, 0.5) + assert forwarded is True + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 1 + l, r = parse_velocity_cmd(bytes(vel_frames[0].data)) + assert abs(l - 0.5) < 1e-4 + assert abs(r - 0.5) < 1e-4 + + +class TestDriveToEstop: + def test_drive_to_estop_stops_motors(self, mock_can_bus): + """Transitioning DRIVE → ESTOP must immediately send zero velocity.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + sm.send_drive(1.0, 1.0) + mock_can_bus.reset() + + sm.set_mode(MODE_ESTOP) + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) >= 1, "No velocity frame on DRIVE→ESTOP transition" + l, r = parse_velocity_cmd(bytes(vel_frames[-1].data)) + assert abs(l) < 1e-5, f"Left motor {l} not zero after ESTOP" + assert abs(r) < 1e-5, f"Right motor {r} not zero after ESTOP" + + def test_drive_to_estop_mode_frame(self, mock_can_bus): + """DRIVE → ESTOP must broadcast MODE=estop.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + sm.set_mode(MODE_ESTOP) + + mode_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_MODE) + assert any(bytes(f.data) == bytes([MODE_ESTOP]) for f in mode_frames) + + def test_estop_blocks_subsequent_drive(self, mock_can_bus): + """After DRIVE → ESTOP, drive commands must be blocked.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + sm.set_mode(MODE_ESTOP) + mock_can_bus.reset() + + forwarded = sm.send_drive(1.0, 1.0) + assert forwarded is False + + vel_frames = mock_can_bus.get_sent_frames_by_id(MAMBA_CMD_VELOCITY) + assert len(vel_frames) == 0 + + +class TestModeCommandEncoding: + def test_mode_idle_byte(self, mock_can_bus): + """MODE_IDLE must encode as 0x00.""" + assert encode_mode_cmd(MODE_IDLE) == b"\x00" + + def test_mode_drive_byte(self, mock_can_bus): + """MODE_DRIVE must encode as 0x01.""" + assert encode_mode_cmd(MODE_DRIVE) == b"\x01" + + def test_mode_estop_byte(self, mock_can_bus): + """MODE_ESTOP must encode as 0x02.""" + assert encode_mode_cmd(MODE_ESTOP) == b"\x02" + + def test_mode_frame_length(self, mock_can_bus): + """Mode command payload must be exactly 1 byte.""" + for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + payload = encode_mode_cmd(mode) + assert len(payload) == 1, f"Mode {mode} payload length {len(payload)} != 1" + + def test_protocol_defs_build_mode_cmd_matches(self): + """build_mode_cmd in protocol_defs must produce identical bytes.""" + for mode in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): + assert build_mode_cmd(mode) == encode_mode_cmd(mode), \ + f"protocol_defs.build_mode_cmd({mode}) != mamba_protocol.encode_mode_cmd({mode})" + + +class TestInvalidMode: + def test_invalid_mode_byte_ignored(self, mock_can_bus): + """Unknown mode byte (e.g. 0xFF) must be rejected — no state change.""" + sm = ModeStateMachine(mock_can_bus) + sm.set_mode(MODE_DRIVE) + initial_mode = sm.mode + mock_can_bus.reset() + + accepted = sm.set_mode(0xFF) + assert accepted is False, "Invalid mode 0xFF should be rejected" + assert sm.mode == initial_mode, "Mode changed despite invalid value" + assert len(mock_can_bus.get_sent_frames()) == 0, \ + "Frames sent for invalid mode command" + + def test_invalid_mode_99_ignored(self, mock_can_bus): + """Mode 99 must be rejected.""" + sm = ModeStateMachine(mock_can_bus) + accepted = sm.set_mode(99) + assert accepted is False + + def test_invalid_mode_negative_ignored(self, mock_can_bus): + """Negative mode values must be rejected.""" + sm = ModeStateMachine(mock_can_bus) + accepted = sm.set_mode(-1) + assert accepted is False + + def test_mamba_protocol_invalid_mode_raises(self): + """mamba_protocol.encode_mode_cmd must raise on invalid mode.""" + with pytest.raises(ValueError): + encode_mode_cmd(99) + with pytest.raises(ValueError): + encode_mode_cmd(-1) + + +@pytest.mark.parametrize("mode,expected_byte", [ + (MODE_IDLE, b"\x00"), + (MODE_DRIVE, b"\x01"), + (MODE_ESTOP, b"\x02"), +]) +def test_mode_encoding_parametrized(mode, expected_byte): + """Parametrized check that all mode constants encode to the right byte.""" + assert encode_mode_cmd(mode) == expected_byte