From f66035cf815ba3e951ba6c27dc369d2338f69edc Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 4 Apr 2026 08:32:00 -0400 Subject: [PATCH] feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Update CAN and serial bridge code to match authoritative protocol spec from docs/SAUL-TEE-SYSTEM-REFERENCE.md §5-6 (hal, 2026-04-04). mamba_protocol.py (CAN, Orin ↔ ESP32 BALANCE): - 0x300 DRIVE: [speed:i16][steer:i16][mode:u8][flags:u8][_:u16] — combined frame - 0x301 ARM: [arm:u8] - 0x302 PID: [kp:f16][ki:f16][kd:f16][_:u16] — half-float gains - 0x303 ESTOP: [0xE5] — magic byte cut - 0x400 ATTITUDE: [pitch:f16][speed:f16][yaw_rate:f16][state:u8][flags:u8] - 0x401 BATTERY: [vbat_mv:u16][fault_code:u8][rssi:i8] - Add VESC STATUS1/4/5 decode helpers; VESC IDs 56 (left) / 68 (right) can_bridge_node.py: - /cmd_vel → encode_drive_cmd (speed/steer int16, MODE_DRIVE) - /estop → encode_estop_cmd (magic 0xE5); clear → DISARM - /saltybot/arm → encode_arm_cmd (new subscription) - Watchdog sends DRIVE(0,0,MODE_IDLE) when /cmd_vel silent - ATTITUDE (0x400) → /saltybot/attitude + /saltybot/balance_state JSON - BATTERY (0x401) → /can/battery BatteryState - VESC STATUS1 frames → /can/vesc/left|right/state stm32_cmd_node.py — rewritten for inter-board protocol API: - Imports from updated stm32_protocol (BAUD_RATE=460800, new frame types) - RX: RcChannels → /saltybot/rc_channels, SensorData → /saltybot/sensors - TX: encode_led_cmd, encode_output_cmd from /saltybot/leds + /saltybot/outputs - HEARTBEAT (0x20) timer replaces old SPEED_STEER/ARM logic stm32_cmd_params.yaml: serial_port=/dev/esp32-io, baud=460800 stm32_cmd.launch.py: updated defaults and description Co-Authored-By: Claude Sonnet 4.6 --- docs/wiring-diagram.md | 13 +- .../config/stm32_cmd_params.yaml | 38 +- .../launch/stm32_cmd.launch.py | 28 +- .../saltybot_bridge/stm32_cmd_node.py | 447 +++++------------- .../saltybot_can_bridge/can_bridge_node.py | 280 +++++------ .../saltybot_can_bridge/mamba_protocol.py | 258 +++++----- 6 files changed, 406 insertions(+), 658 deletions(-) diff --git a/docs/wiring-diagram.md b/docs/wiring-diagram.md index 824e365..aaa15f5 100644 --- a/docs/wiring-diagram.md +++ b/docs/wiring-diagram.md @@ -1,6 +1,13 @@ -# SaltyLab Wiring Diagram +# SaltyLab / SAUL-TEE Wiring Reference -## System Overview +> ⚠️ **ARCHITECTURE CHANGE (2026-04-03):** Mamba F722S / STM32 retired. +> New stack: **ESP32-S3 BALANCE** + **ESP32-S3 IO** + VESCs on 500 kbps CAN. +> **Authoritative reference:** [`docs/SAUL-TEE-SYSTEM-REFERENCE.md`](SAUL-TEE-SYSTEM-REFERENCE.md) +> Historical STM32/Mamba wiring below is **obsolete** — retained for reference only. + +--- + +## ~~System Overview~~ (OBSOLETE — see SAUL-TEE-SYSTEM-REFERENCE.md) ``` ┌─────────────────────────────────────────────────────────────────────┐ @@ -139,7 +146,7 @@ BATTERY (36V) ──┬── Hoverboard ESC (36V direct) | 1TB NVMe | PCIe Gen3 ×4 | M.2 Key M | `/dev/nvme0n1` | -## FC UART Summary (MAMBA F722S) +## FC UART Summary (MAMBA F722S — OBSOLETE) | UART | Pins | Baud | Assignment | Notes | |------|------|------|------------|-------| diff --git a/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml b/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml index 8b65663..cca03a5 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml +++ b/jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml @@ -1,30 +1,16 @@ -# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119) -# Binary-framed Jetson↔STM32 bridge at 921600 baud. +# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge) +# Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud. +# Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] +# Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 # ── Serial port ──────────────────────────────────────────────────────────────── -# Use /dev/stm32-bridge if the udev rule is applied: -# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", -# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout" -serial_port: /dev/ttyACM0 -baud_rate: 921600 -reconnect_delay: 2.0 # seconds between USB reconnect attempts +# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md). +# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed. +serial_port: /dev/esp32-io +baud_rate: 460800 +reconnect_delay: 2.0 # seconds between reconnect attempts # ── Heartbeat ───────────────────────────────────────────────────────────────── -# HEARTBEAT frame sent every heartbeat_period seconds. -# STM32 fires watchdog and reverts to safe state if no frame received for 500ms. -heartbeat_period: 0.2 # 200ms → well within 500ms STM32 watchdog - -# ── Watchdog (Jetson-side) ──────────────────────────────────────────────────── -# If no /cmd_vel message received for watchdog_timeout seconds, -# send SPEED_STEER(0,0) to stop the robot. -watchdog_timeout: 0.5 # 500ms - -# ── Twist velocity scaling ──────────────────────────────────────────────────── -# speed = clamp(linear.x * speed_scale, -1000, 1000) (m/s → ESC units) -# steer = clamp(angular.z * steer_scale, -1000, 1000) (rad/s → ESC units) -# -# Default: 1 m/s → 1000 ESC units, ±2 rad/s → ±1000 steer. -# Negative steer_scale flips ROS2 CCW+ convention to match ESC steer direction. -# Tune speed_scale to set the physical top speed. -speed_scale: 1000.0 -steer_scale: -500.0 +# HEARTBEAT (0x20) sent every heartbeat_period. +# ESP32 IO watchdog fires if no heartbeat for ~500 ms. +heartbeat_period: 0.2 # 200 ms → well within 500 ms watchdog diff --git a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py b/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py index 2fa400a..5c0f1e0 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py +++ b/jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py @@ -1,14 +1,14 @@ -"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119). +"""stm32_cmd.launch.py — Launch the ESP32-S3 IO auxiliary bridge node. + +Connects to ESP32-S3 IO board via USB-CDC @ 460800 baud (inter-board protocol). +Handles RC monitoring, sensor data, LED/output commands. +Primary drive path uses CAN (can_bridge_node / saltybot_can_node), not this node. + +Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §5 Usage: - # Default (binary protocol, bidirectional): ros2 launch saltybot_bridge stm32_cmd.launch.py - - # Override serial port: - ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM1 - - # Custom velocity scales: - ros2 launch saltybot_bridge stm32_cmd.launch.py speed_scale:=800.0 steer_scale:=-400.0 + ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0 """ import os @@ -24,12 +24,9 @@ def generate_launch_description() -> LaunchDescription: params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml") return LaunchDescription([ - DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"), - DeclareLaunchArgument("baud_rate", default_value="921600"), - DeclareLaunchArgument("speed_scale", default_value="1000.0"), - DeclareLaunchArgument("steer_scale", default_value="-500.0"), - DeclareLaunchArgument("watchdog_timeout", default_value="0.5"), - DeclareLaunchArgument("heartbeat_period", default_value="0.2"), + DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"), + DeclareLaunchArgument("baud_rate", default_value="460800"), + DeclareLaunchArgument("heartbeat_period", default_value="0.2"), Node( package="saltybot_bridge", @@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription: { "serial_port": LaunchConfiguration("serial_port"), "baud_rate": LaunchConfiguration("baud_rate"), - "speed_scale": LaunchConfiguration("speed_scale"), - "steer_scale": LaunchConfiguration("steer_scale"), - "watchdog_timeout": LaunchConfiguration("watchdog_timeout"), "heartbeat_period": LaunchConfiguration("heartbeat_period"), }, ], diff --git a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py index 4e3d63c..af16a65 100644 --- a/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py +++ b/jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py @@ -1,45 +1,32 @@ -"""stm32_cmd_node.py — Full bidirectional binary-framed STM32↔Jetson bridge. +"""stm32_cmd_node.py — Orin ↔ ESP32-S3 IO auxiliary bridge node. -Issue #119: replaces the ASCII-protocol saltybot_cmd_node with a robust binary -framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud. +Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the +inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5). -TX commands (Jetson → STM32): - SPEED_STEER — 50 Hz from /cmd_vel subscription - HEARTBEAT — 200 ms timer (STM32 watchdog fires at 500 ms) - ARM — via /saltybot/arm service - SET_MODE — via /saltybot/set_mode service - PID_UPDATE — via /saltybot/pid_update topic +This node is NOT the primary drive path (that is CAN via can_bridge_node). +It handles auxiliary I/O: RC monitoring, sensor data, LED/output control. -Watchdog: if /cmd_vel is silent for 500 ms, send SPEED_STEER(0,0) and log warning. +Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8] @ 460800 baud -RX telemetry (STM32 → Jetson): - IMU → /saltybot/imu (sensor_msgs/Imu) - BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON) - MOTOR_RPM → /saltybot/telemetry/motor_rpm (std_msgs/String JSON) - ARM_STATE → /saltybot/arm_state (std_msgs/String JSON) - ERROR → /saltybot/error (std_msgs/String JSON) - All frames → /diagnostics (diagnostic_msgs/DiagnosticArray) +RX from ESP32 IO: + RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON) + SENSORS (0x02) → /saltybot/sensors (std_msgs/String JSON) -Auto-reconnect: USB disconnect is detected when serial.read() raises; node -continuously retries at reconnect_delay interval. - -This node owns /dev/ttyACM0 exclusively — do NOT run alongside -serial_bridge_node or saltybot_cmd_node on the same port. +TX to ESP32 IO: + LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON) + OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON) + HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive) Parameters (config/stm32_cmd_params.yaml): - serial_port /dev/ttyACM0 - baud_rate 921600 - reconnect_delay 2.0 (seconds) - heartbeat_period 0.2 (seconds) - watchdog_timeout 0.5 (seconds — no /cmd_vel → send zero-speed) - speed_scale 1000.0 (linear.x m/s → ESC units) - steer_scale -500.0 (angular.z rad/s → ESC units, neg to flip convention) + serial_port /dev/esp32-io + baud_rate 460800 + reconnect_delay 2.0 + heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms) """ from __future__ import annotations import json -import math import threading import time @@ -50,119 +37,69 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy import serial from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue -from geometry_msgs.msg import Twist -from sensor_msgs.msg import Imu from std_msgs.msg import String -from std_srvs.srv import SetBool, Trigger from .stm32_protocol import ( + BAUD_RATE, FrameParser, - ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame, - encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode, - encode_pid_update, + RcChannels, + SensorData, + encode_heartbeat, + encode_led_cmd, + encode_output_cmd, ) -# ── Constants ───────────────────────────────────────────────────────────────── - -IMU_FRAME_ID = "imu_link" -_ARM_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"} - - -def _clamp(v: float, lo: float, hi: float) -> float: - return max(lo, min(hi, v)) - - -# ── Node ────────────────────────────────────────────────────────────────────── class Stm32CmdNode(Node): - """Binary-framed Jetson↔STM32 bridge node.""" + """Orin ↔ ESP32-S3 IO auxiliary bridge node.""" def __init__(self) -> None: super().__init__("stm32_cmd_node") - # ── Parameters ──────────────────────────────────────────────────────── - self.declare_parameter("serial_port", "/dev/ttyACM0") - self.declare_parameter("baud_rate", 921600) + # ── Parameters ──────────────────────────────────────────────────── + self.declare_parameter("serial_port", "/dev/esp32-io") + self.declare_parameter("baud_rate", BAUD_RATE) self.declare_parameter("reconnect_delay", 2.0) self.declare_parameter("heartbeat_period", 0.2) - self.declare_parameter("watchdog_timeout", 0.5) - self.declare_parameter("speed_scale", 1000.0) - self.declare_parameter("steer_scale", -500.0) - port = self.get_parameter("serial_port").value - baud = self.get_parameter("baud_rate").value + self._port_name = self.get_parameter("serial_port").value + self._baud = self.get_parameter("baud_rate").value self._reconnect_delay = self.get_parameter("reconnect_delay").value self._hb_period = self.get_parameter("heartbeat_period").value - self._wd_timeout = self.get_parameter("watchdog_timeout").value - self._speed_scale = self.get_parameter("speed_scale").value - self._steer_scale = self.get_parameter("steer_scale").value - # ── QoS ─────────────────────────────────────────────────────────────── - sensor_qos = QoSProfile( - reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, depth=10, - ) + # ── QoS ─────────────────────────────────────────────────────────── rel_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, history=HistoryPolicy.KEEP_LAST, depth=10, ) - # ── Publishers ──────────────────────────────────────────────────────── - self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos) - self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos) - self._error_pub = self.create_publisher(String, "/saltybot/error", rel_qos) - self._battery_pub = self.create_publisher(String, "/saltybot/telemetry/battery", rel_qos) - self._rpm_pub = self.create_publisher(String, "/saltybot/telemetry/motor_rpm", rel_qos) - self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos) + # ── Publishers ──────────────────────────────────────────────────── + self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos) + self._sens_pub = self.create_publisher(String, "/saltybot/sensors", rel_qos) + self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos) - # ── Subscribers ─────────────────────────────────────────────────────── - self._cmd_vel_sub = self.create_subscription( - Twist, "/cmd_vel", self._on_cmd_vel, rel_qos, - ) - self._pid_sub = self.create_subscription( - String, "/saltybot/pid_update", self._on_pid_update, rel_qos, - ) + # ── Subscriptions ───────────────────────────────────────────────── + self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos) + self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos) - # ── Services ────────────────────────────────────────────────────────── - self._arm_srv = self.create_service(SetBool, "/saltybot/arm", self._svc_arm) - self._mode_srv = self.create_service(SetBool, "/saltybot/set_mode", self._svc_set_mode) - - # ── Serial state ────────────────────────────────────────────────────── - self._port_name = port - self._baud = baud + # ── Serial state ────────────────────────────────────────────────── self._ser: serial.Serial | None = None - self._ser_lock = threading.Lock() - self._parser = FrameParser() + self._ser_lock = threading.Lock() + self._parser = FrameParser() + self._rx_count = 0 - # ── TX state ────────────────────────────────────────────────────────── - self._last_speed = 0 - self._last_steer = 0 - self._last_cmd_t = time.monotonic() - self._watchdog_sent = False # tracks whether we already sent zero - - # ── Diagnostics state ────────────────────────────────────────────────── - self._last_arm_state = -1 - self._last_battery_mv = 0 - self._rx_frame_count = 0 - - # ── Open serial and start timers ────────────────────────────────────── + # ── Open serial and start timers ────────────────────────────────── self._open_serial() - # Read at 200 Hz (serial RX thread is better, but timer keeps ROS2 integration clean) - self._read_timer = self.create_timer(0.005, self._read_cb) - # Heartbeat TX + self._read_timer = self.create_timer(0.005, self._read_cb) self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb) - # Watchdog check (fires at 2× watchdog_timeout for quick detection) - self._wd_timer = self.create_timer(self._wd_timeout / 2, self._watchdog_cb) - # Periodic diagnostics - self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) + self._diag_timer = self.create_timer(1.0, self._publish_diagnostics) self.get_logger().info( - f"stm32_cmd_node started — {port} @ {baud} baud | " - f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms" + f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud" ) - # ── Serial management ───────────────────────────────────────────────────── + # ── Serial management ───────────────────────────────────────────────── def _open_serial(self) -> bool: with self._ser_lock: @@ -170,7 +107,7 @@ class Stm32CmdNode(Node): self._ser = serial.Serial( port=self._port_name, baudrate=self._baud, - timeout=0.005, # non-blocking reads + timeout=0.005, write_timeout=0.1, ) self._ser.reset_input_buffer() @@ -185,17 +122,7 @@ class Stm32CmdNode(Node): self._ser = None return False - def _close_serial(self) -> None: - with self._ser_lock: - if self._ser and self._ser.is_open: - try: - self._ser.close() - except Exception: - pass - self._ser = None - def _write(self, data: bytes) -> bool: - """Thread-safe serial write. Returns False if port is not open.""" with self._ser_lock: if self._ser is None or not self._ser.is_open: return False @@ -207,16 +134,15 @@ class Stm32CmdNode(Node): self._ser = None return False - # ── RX — read callback ──────────────────────────────────────────────────── + # ── RX ──────────────────────────────────────────────────────────────── def _read_cb(self) -> None: - """Read bytes from serial and feed them to the frame parser.""" raw: bytes | None = None - reconnect_needed = False + reconnect = False with self._ser_lock: if self._ser is None or not self._ser.is_open: - reconnect_needed = True + reconnect = True else: try: n = self._ser.in_waiting @@ -225,9 +151,9 @@ class Stm32CmdNode(Node): except serial.SerialException as exc: self.get_logger().error(f"Serial read error: {exc}") self._ser = None - reconnect_needed = True + reconnect = True - if reconnect_needed: + if reconnect: self.get_logger().warn( "Serial disconnected — will retry", throttle_duration_sec=self._reconnect_delay, @@ -240,230 +166,105 @@ class Stm32CmdNode(Node): return for byte in raw: - frame = self._parser.feed(byte) - if frame is not None: - self._rx_frame_count += 1 - self._dispatch_frame(frame) + msg = self._parser.feed(byte) + if msg is not None: + self._rx_count += 1 + self._dispatch(msg) - def _dispatch_frame(self, frame) -> None: - """Route a decoded frame to the appropriate publisher.""" + def _dispatch(self, msg) -> None: now = self.get_clock().now().to_msg() + ts = f"{now.sec}.{now.nanosec:09d}" - if isinstance(frame, ImuFrame): - self._publish_imu(frame, now) + if isinstance(msg, RcChannels): + out = String() + out.data = json.dumps({ + "channels": msg.channels, + "source": msg.source, + "ts": ts, + }) + self._rc_pub.publish(out) - elif isinstance(frame, BatteryFrame): - self._publish_battery(frame, now) + elif isinstance(msg, SensorData): + out = String() + out.data = json.dumps({ + "pressure_pa": msg.pressure_pa, + "temperature_c": msg.temperature_c, + "tof_mm": msg.tof_mm, + "ts": ts, + }) + self._sens_pub.publish(out) - elif isinstance(frame, MotorRpmFrame): - self._publish_motor_rpm(frame, now) + elif isinstance(msg, tuple): + type_code, _ = msg + self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}") - elif isinstance(frame, ArmStateFrame): - self._publish_arm_state(frame, now) - - elif isinstance(frame, ErrorFrame): - self._publish_error(frame, now) - - elif isinstance(frame, tuple): - type_code, payload = frame - self.get_logger().debug( - f"Unknown telemetry type 0x{type_code:02X} len={len(payload)}" - ) - - # ── Telemetry publishers ────────────────────────────────────────────────── - - def _publish_imu(self, frame: ImuFrame, stamp) -> None: - msg = Imu() - msg.header.stamp = stamp - msg.header.frame_id = IMU_FRAME_ID - - # orientation: unknown — signal with -1 in first covariance - msg.orientation_covariance[0] = -1.0 - - msg.angular_velocity.x = math.radians(frame.pitch_deg) - msg.angular_velocity.y = math.radians(frame.roll_deg) - msg.angular_velocity.z = math.radians(frame.yaw_deg) - cov = math.radians(0.3) ** 2 # ±0.3° noise estimate from STM32 BMI088 - msg.angular_velocity_covariance[0] = cov - msg.angular_velocity_covariance[4] = cov - msg.angular_velocity_covariance[8] = cov - - msg.linear_acceleration.x = frame.accel_x - msg.linear_acceleration.y = frame.accel_y - msg.linear_acceleration.z = frame.accel_z - acov = 0.05 ** 2 # ±0.05 m/s² noise - msg.linear_acceleration_covariance[0] = acov - msg.linear_acceleration_covariance[4] = acov - msg.linear_acceleration_covariance[8] = acov - - self._imu_pub.publish(msg) - - def _publish_battery(self, frame: BatteryFrame, stamp) -> None: - payload = { - "voltage_v": round(frame.voltage_mv / 1000.0, 3), - "voltage_mv": frame.voltage_mv, - "current_ma": frame.current_ma, - "soc_pct": frame.soc_pct, - "charging": frame.current_ma < -100, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - self._last_battery_mv = frame.voltage_mv - msg = String() - msg.data = json.dumps(payload) - self._battery_pub.publish(msg) - - def _publish_motor_rpm(self, frame: MotorRpmFrame, stamp) -> None: - payload = { - "left_rpm": frame.left_rpm, - "right_rpm": frame.right_rpm, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._rpm_pub.publish(msg) - - def _publish_arm_state(self, frame: ArmStateFrame, stamp) -> None: - label = _ARM_LABEL.get(frame.state, f"UNKNOWN({frame.state})") - if frame.state != self._last_arm_state: - self.get_logger().info(f"Arm state → {label} (flags=0x{frame.error_flags:02X})") - self._last_arm_state = frame.state - - payload = { - "state": frame.state, - "state_label": label, - "error_flags": frame.error_flags, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._arm_pub.publish(msg) - - def _publish_error(self, frame: ErrorFrame, stamp) -> None: - self.get_logger().error( - f"STM32 error code=0x{frame.error_code:02X} sub=0x{frame.subcode:02X}" - ) - payload = { - "error_code": frame.error_code, - "subcode": frame.subcode, - "ts": f"{stamp.sec}.{stamp.nanosec:09d}", - } - msg = String() - msg.data = json.dumps(payload) - self._error_pub.publish(msg) - - # ── TX — command send ───────────────────────────────────────────────────── - - def _on_cmd_vel(self, msg: Twist) -> None: - """Convert /cmd_vel Twist to SPEED_STEER frame at up to 50 Hz.""" - speed = int(_clamp(msg.linear.x * self._speed_scale, -1000.0, 1000.0)) - steer = int(_clamp(msg.angular.z * self._steer_scale, -1000.0, 1000.0)) - self._last_speed = speed - self._last_steer = steer - self._last_cmd_t = time.monotonic() - self._watchdog_sent = False - - frame = encode_speed_steer(speed, steer) - if not self._write(frame): - self.get_logger().warn( - "SPEED_STEER dropped — serial not open", - throttle_duration_sec=2.0, - ) + # ── TX ──────────────────────────────────────────────────────────────── def _heartbeat_cb(self) -> None: - """Send HEARTBEAT every heartbeat_period (default 200ms).""" self._write(encode_heartbeat()) - def _watchdog_cb(self) -> None: - """Send zero-speed if /cmd_vel silent for watchdog_timeout seconds.""" - if time.monotonic() - self._last_cmd_t >= self._wd_timeout: - if not self._watchdog_sent: - self.get_logger().warn( - f"No /cmd_vel for {self._wd_timeout:.1f}s — sending zero-speed" - ) - self._watchdog_sent = True - self._last_speed = 0 - self._last_steer = 0 - self._write(encode_speed_steer(0, 0)) - - def _on_pid_update(self, msg: String) -> None: - """Parse JSON /saltybot/pid_update and send PID_UPDATE frame.""" + def _on_leds(self, msg: String) -> None: + """Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD.""" try: - data = json.loads(msg.data) - kp = float(data["kp"]) - ki = float(data["ki"]) - kd = float(data["kd"]) + d = json.loads(msg.data) + frame = encode_led_cmd( + int(d.get("pattern", 0)), + int(d.get("r", 0)), + int(d.get("g", 0)), + int(d.get("b", 0)), + ) except (ValueError, KeyError, json.JSONDecodeError) as exc: - self.get_logger().error(f"Bad PID update JSON: {exc}") + self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}") return - frame = encode_pid_update(kp, ki, kd) - if self._write(frame): - self.get_logger().info(f"PID update: kp={kp}, ki={ki}, kd={kd}") - else: - self.get_logger().warn("PID_UPDATE dropped — serial not open") + self._write(frame) - # ── Services ────────────────────────────────────────────────────────────── + def _on_outputs(self, msg: String) -> None: + """Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}.""" + try: + d = json.loads(msg.data) + frame = encode_output_cmd( + bool(d.get("horn", False)), + bool(d.get("buzzer", False)), + int(d.get("headlight", 0)), + int(d.get("fan", 0)), + ) + except (ValueError, KeyError, json.JSONDecodeError) as exc: + self.get_logger().error(f"Bad /saltybot/outputs JSON: {exc}") + return + self._write(frame) - def _svc_arm(self, request: SetBool.Request, response: SetBool.Response): - """SetBool(True) = arm, SetBool(False) = disarm.""" - arm = request.data - frame = encode_arm(arm) - ok = self._write(frame) - response.success = ok - response.message = ("ARMED" if arm else "DISARMED") if ok else "serial not open" - self.get_logger().info( - f"ARM service: {'arm' if arm else 'disarm'} — {'sent' if ok else 'FAILED'}" - ) - return response - - def _svc_set_mode(self, request: SetBool.Request, response: SetBool.Response): - """SetBool: data maps to mode byte (True=1, False=0).""" - mode = 1 if request.data else 0 - frame = encode_set_mode(mode) - ok = self._write(frame) - response.success = ok - response.message = f"mode={mode}" if ok else "serial not open" - return response - - # ── Diagnostics ─────────────────────────────────────────────────────────── + # ── Diagnostics ─────────────────────────────────────────────────────── def _publish_diagnostics(self) -> None: diag = DiagnosticArray() diag.header.stamp = self.get_clock().now().to_msg() - status = DiagnosticStatus() - status.name = "saltybot/stm32_cmd_node" - status.hardware_id = "stm32f722" - + status.name = "saltybot/esp32_io_bridge" + status.hardware_id = "esp32-s3-io" port_ok = self._ser is not None and self._ser.is_open - if port_ok: - status.level = DiagnosticStatus.OK - status.message = "Serial OK" - else: - status.level = DiagnosticStatus.ERROR - status.message = f"Serial disconnected: {self._port_name}" - - wd_age = time.monotonic() - self._last_cmd_t - status.values = [ - KeyValue(key="serial_port", value=self._port_name), - KeyValue(key="port_open", value=str(port_ok)), - KeyValue(key="rx_frames", value=str(self._rx_frame_count)), - KeyValue(key="rx_errors", value=str(self._parser.frames_error)), - KeyValue(key="last_speed", value=str(self._last_speed)), - KeyValue(key="last_steer", value=str(self._last_steer)), - KeyValue(key="cmd_vel_age_s", value=f"{wd_age:.2f}"), - KeyValue(key="battery_mv", value=str(self._last_battery_mv)), - KeyValue(key="arm_state", value=_ARM_LABEL.get(self._last_arm_state, "?")), + status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR + status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}" + status.values = [ + KeyValue(key="serial_port", value=self._port_name), + KeyValue(key="baud_rate", value=str(self._baud)), + KeyValue(key="port_open", value=str(port_ok)), + KeyValue(key="rx_frames", value=str(self._rx_count)), + KeyValue(key="rx_errors", value=str(self._parser.frames_error)), ] diag.status.append(status) self._diag_pub.publish(diag) - # ── Lifecycle ───────────────────────────────────────────────────────────── + # ── Lifecycle ───────────────────────────────────────────────────────── def destroy_node(self) -> None: - # Send zero-speed + disarm on shutdown - self._write(encode_speed_steer(0, 0)) - self._write(encode_arm(False)) - self._close_serial() + self._write(encode_heartbeat(state=0)) + with self._ser_lock: + if self._ser and self._ser.is_open: + try: + self._ser.close() + except Exception: + pass + self._ser = None super().destroy_node() diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py index 8d2620c..f0e6b76 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py @@ -1,26 +1,34 @@ #!/usr/bin/env python3 """ can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE -board and VESC motor controllers over CAN bus (CANable2 / slcan0, 500 kbps). +board and VESC motor controllers over CAN bus (CANable 2.0 / slcan0, 500 kbps). -Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §4 & §6 (2026-04-04) +Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04) Subscriptions ------------- - /cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300) - /estop std_msgs/Bool → ORIN_CMD_ESTOP (0x302) + /cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300) + /estop std_msgs/Bool → ORIN_CMD_ESTOP (0x303) + /saltybot/arm std_msgs/Bool → ORIN_CMD_ARM (0x301) Publications ------------ - /can/imu sensor_msgs/Imu from FC_STATUS (0x400) pitch - /can/battery sensor_msgs/BatteryState from FC_STATUS (0x400) vbat_mv - /can/vesc/left/state std_msgs/Float32MultiArray from FC_VESC (0x401) - /can/vesc/right/state std_msgs/Float32MultiArray from FC_VESC (0x401) - /can/connection_status std_msgs/String "connected" | "disconnected" + /saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state + /saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude + /can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi + /can/vesc/left/state std_msgs/Float32MultiArray VESC STATUS_1 left + /can/vesc/right/state std_msgs/Float32MultiArray VESC STATUS_1 right + /can/connection_status std_msgs/String "connected" | "disconnected" -Issue: https://gitea.vayrette.com/seb/saltylab-firmware/issues/674 +Parameters +---------- + can_interface str CAN socket name (default: slcan0) + speed_scale float /cmd_vel linear.x (m/s) → motor units (default: 1000.0) + steer_scale float /cmd_vel angular.z (rad/s) → motor units (default: -500.0) + command_timeout_s float watchdog zero-vel threshold (default: 0.5) """ +import json import threading import time from typing import Optional @@ -29,42 +37,32 @@ import can import rclpy from geometry_msgs.msg import Twist from rclpy.node import Node - -from sensor_msgs.msg import BatteryState, Imu +from sensor_msgs.msg import BatteryState from std_msgs.msg import Bool, Float32MultiArray, String from saltybot_can_bridge.mamba_protocol import ( - # Orin → BALANCE command IDs ORIN_CMD_DRIVE, - ORIN_CMD_MODE, + ORIN_CMD_ARM, ORIN_CMD_ESTOP, - # BALANCE → Orin telemetry IDs - FC_STATUS, - FC_VESC, - # VESC node IDs + ESP32_TELEM_ATTITUDE, + ESP32_TELEM_BATTERY, VESC_LEFT_ID, VESC_RIGHT_ID, VESC_STATUS_1, - # Mode constants MODE_DRIVE, - MODE_ESTOP, MODE_IDLE, - # Encoders encode_drive_cmd, - encode_mode_cmd, + encode_arm_cmd, encode_estop_cmd, - encode_led_cmd, - # Decoders - decode_fc_status, - decode_fc_vesc, + decode_attitude, + decode_battery, decode_vesc_status1, - decode_vesc_can_id, ) # Reconnect attempt interval when CAN bus is lost _RECONNECT_INTERVAL_S: float = 5.0 -# Watchdog timer tick rate (Hz) +# Watchdog tick rate (Hz); sends zero DRIVE when /cmd_vel is silent _WATCHDOG_HZ: float = 10.0 @@ -75,41 +73,41 @@ class CanBridgeNode(Node): super().__init__("can_bridge_node") # ── Parameters ──────────────────────────────────────────────────── - self.declare_parameter("can_interface", "slcan0") - self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) # 56 - self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) # 68 + self.declare_parameter("can_interface", "slcan0") + self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) + self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) + self.declare_parameter("speed_scale", 1000.0) + self.declare_parameter("steer_scale", -500.0) self.declare_parameter("command_timeout_s", 0.5) - self._iface: str = self.get_parameter("can_interface").value - self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value - self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value - self._cmd_timeout: float = self.get_parameter("command_timeout_s").value + self._iface = self.get_parameter("can_interface").value + self._left_vesc_id = self.get_parameter("left_vesc_can_id").value + self._right_vesc_id = self.get_parameter("right_vesc_can_id").value + self._speed_scale = self.get_parameter("speed_scale").value + self._steer_scale = self.get_parameter("steer_scale").value + self._cmd_timeout = self.get_parameter("command_timeout_s").value # ── State ───────────────────────────────────────────────────────── self._bus: Optional[can.BusABC] = None self._connected: bool = False self._last_cmd_time: float = time.monotonic() - self._lock = threading.Lock() # protects _bus / _connected + self._lock = threading.Lock() # ── Publishers ──────────────────────────────────────────────────── - self._pub_imu = self.create_publisher(Imu, "/can/imu", 10) - self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) - self._pub_vesc_left = self.create_publisher( - Float32MultiArray, "/can/vesc/left/state", 10 - ) - self._pub_vesc_right = self.create_publisher( - Float32MultiArray, "/can/vesc/right/state", 10 - ) - self._pub_status = self.create_publisher( - String, "/can/connection_status", 10 - ) + self._pub_attitude = self.create_publisher(String, "/saltybot/attitude", 10) + self._pub_balance = self.create_publisher(String, "/saltybot/balance_state", 10) + self._pub_battery = self.create_publisher(BatteryState, "/can/battery", 10) + self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state", 10) + self._pub_vesc_right= self.create_publisher(Float32MultiArray,"/can/vesc/right/state", 10) + self._pub_status = self.create_publisher(String, "/can/connection_status", 10) # ── Subscriptions ───────────────────────────────────────────────── - self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) - self.create_subscription(Bool, "/estop", self._estop_cb, 10) + self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10) + self.create_subscription(Bool, "/estop", self._estop_cb, 10) + self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10) # ── Timers ──────────────────────────────────────────────────────── - self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) + self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb) self.create_timer(_RECONNECT_INTERVAL_S, self._reconnect_cb) # ── Open CAN ────────────────────────────────────────────────────── @@ -123,22 +121,18 @@ class CanBridgeNode(Node): self.get_logger().info( f"can_bridge_node ready — iface={self._iface} " - f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id}" + f"left_vesc={self._left_vesc_id} right_vesc={self._right_vesc_id} " + f"speed_scale={self._speed_scale} steer_scale={self._steer_scale}" ) # ── Connection management ────────────────────────────────────────────── def _try_connect(self) -> None: - """Attempt to open the CAN interface; silently skip if already connected.""" with self._lock: if self._connected: return try: - bus = can.interface.Bus( - channel=self._iface, - bustype="socketcan", - ) - self._bus = bus + self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan") self._connected = True self.get_logger().info(f"CAN bus connected: {self._iface}") self._publish_status("connected") @@ -151,12 +145,10 @@ class CanBridgeNode(Node): self._publish_status("disconnected") def _reconnect_cb(self) -> None: - """Periodic timer: try to reconnect when disconnected.""" if not self._connected: self._try_connect() def _handle_can_error(self, exc: Exception, context: str) -> None: - """Mark bus as disconnected on any CAN error.""" self.get_logger().warning(f"CAN error in {context}: {exc}") with self._lock: if self._bus is not None: @@ -171,59 +163,51 @@ class CanBridgeNode(Node): # ── ROS callbacks ───────────────────────────────────────────────────── def _cmd_vel_cb(self, msg: Twist) -> None: - """Convert /cmd_vel Twist to VESC speed commands over CAN.""" + """Convert /cmd_vel Twist to ORIN_CMD_DRIVE over CAN.""" self._last_cmd_time = time.monotonic() - if not self._connected: return - - # Differential drive decomposition — individual wheel speeds in m/s. - # The VESC nodes interpret linear velocity directly; angular is handled - # by the sign difference between left and right. - linear = msg.linear.x - angular = msg.angular.z - - # Differential drive decomposition (positive angular = CCW = left turn). - left_mps = linear - angular - right_mps = linear + angular - - self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(left_mps, right_mps), "cmd_vel") - self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_DRIVE), "cmd_vel mode") + speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale))) + steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale))) + self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel") def _estop_cb(self, msg: Bool) -> None: """Forward /estop to ESP32 BALANCE over CAN.""" if not self._connected: return - self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "estop") if msg.data: - self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_ESTOP), "estop mode") + self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(), "estop") self.get_logger().warning("E-stop asserted — sent ESTOP to ESP32 BALANCE") + else: + # Clear estop: send DISARM then re-ARM (let operator decide to re-arm) + self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "estop_clear") + + def _arm_cb(self, msg: Bool) -> None: + """Forward /saltybot/arm to ORIN_CMD_ARM.""" + if not self._connected: + return + self._send_can(ORIN_CMD_ARM, encode_arm_cmd(msg.data), "arm") + self.get_logger().info(f"ARM command: {'ARM' if msg.data else 'DISARM'}") # ── Watchdog ────────────────────────────────────────────────────────── def _watchdog_cb(self) -> None: - """If no /cmd_vel arrives within the timeout, send zero velocity.""" + """If /cmd_vel is silent for command_timeout_s, send zero DRIVE (acts as keepalive).""" if not self._connected: return - elapsed = time.monotonic() - self._last_cmd_time - if elapsed > self._cmd_timeout: - self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "watchdog zero-vel") - self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "watchdog idle") + if time.monotonic() - self._last_cmd_time > self._cmd_timeout: + self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog") # ── CAN send helper ─────────────────────────────────────────────────── - def _send_can(self, arb_id: int, data: bytes, context: str) -> None: - """Send a standard CAN frame; handle errors gracefully.""" + def _send_can(self, arb_id: int, data: bytes, context: str, + extended: bool = False) -> None: with self._lock: if not self._connected or self._bus is None: return bus = self._bus - - msg = can.Message( - arbitration_id=arb_id, - data=data, - is_extended_id=False, - ) + msg = can.Message(arbitration_id=arb_id, data=data, + is_extended_id=extended) try: bus.send(msg, timeout=0.05) except can.CanError as exc: @@ -232,59 +216,41 @@ class CanBridgeNode(Node): # ── Background CAN reader ───────────────────────────────────────────── def _reader_loop(self) -> None: - """ - Blocking CAN read loop executed in a daemon thread. - Dispatches incoming frames to the appropriate handler. - """ while rclpy.ok(): with self._lock: - connected = self._connected - bus = self._bus - + connected, bus = self._connected, self._bus if not connected or bus is None: time.sleep(0.1) continue - try: frame = bus.recv(timeout=0.5) except can.CanError as exc: self._handle_can_error(exc, "reader_loop recv") continue - if frame is None: - # Timeout — no frame within 0.5 s, loop again continue - self._dispatch_frame(frame) def _dispatch_frame(self, frame: can.Message) -> None: - """Route an incoming CAN frame to the correct publisher.""" arb_id = frame.arbitration_id - data = bytes(frame.data) - - # VESC STATUS_1 CAN IDs: (VESC_STATUS_1 << 8) | node_id - _vesc_left_status1 = (VESC_STATUS_1 << 8) | self._left_vesc_id - _vesc_right_status1 = (VESC_STATUS_1 << 8) | self._right_vesc_id - + data = bytes(frame.data) + vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id + vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id try: - if arb_id == FC_STATUS: - self._handle_fc_status(data) - - elif arb_id == FC_VESC: - self._handle_fc_vesc(data) - - elif arb_id == _vesc_left_status1: - telem = decode_vesc_status1(self._left_vesc_id, data) - msg = Float32MultiArray() - msg.data = [telem.erpm, telem.duty, 0.0, telem.current] - self._pub_vesc_left.publish(msg) - - elif arb_id == _vesc_right_status1: - telem = decode_vesc_status1(self._right_vesc_id, data) - msg = Float32MultiArray() - msg.data = [telem.erpm, telem.duty, 0.0, telem.current] - self._pub_vesc_right.publish(msg) - + if arb_id == ESP32_TELEM_ATTITUDE: + self._handle_attitude(data) + elif arb_id == ESP32_TELEM_BATTERY: + self._handle_battery(data) + elif arb_id == vesc_l: + t = decode_vesc_status1(self._left_vesc_id, data) + m = Float32MultiArray() + m.data = [t.erpm, t.duty, 0.0, t.current] + self._pub_vesc_left.publish(m) + elif arb_id == vesc_r: + t = decode_vesc_status1(self._right_vesc_id, data) + m = Float32MultiArray() + m.data = [t.erpm, t.duty, 0.0, t.current] + self._pub_vesc_right.publish(m) except Exception as exc: self.get_logger().warning( f"Error parsing CAN frame 0x{arb_id:03X}: {exc}" @@ -292,38 +258,35 @@ class CanBridgeNode(Node): # ── Frame handlers ──────────────────────────────────────────────────── - def _handle_fc_status(self, data: bytes) -> None: - """FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags.""" - telem = decode_fc_status(data) + _STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"} - # Publish pitch as IMU (orientation only — yaw/roll unknown from FC_STATUS) - imu_msg = Imu() - imu_msg.header.stamp = self.get_clock().now().to_msg() - imu_msg.header.frame_id = "imu_link" - # Only pitch is available; publish as angular velocity placeholder - imu_msg.angular_velocity.y = telem.pitch_deg # degrees, not rad/s - imu_msg.orientation_covariance[0] = -1.0 # covariance unknown - self._pub_imu.publish(imu_msg) + def _handle_attitude(self, data: bytes) -> None: + """ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude.""" + t = decode_attitude(data) + now = self.get_clock().now().to_msg() + payload = { + "pitch_deg": round(t.pitch_deg, 2), + "speed_mps": round(t.speed, 3), + "yaw_rate": round(t.yaw_rate, 3), + "state": t.state, + "state_label": self._STATE_LABEL.get(t.state, f"UNKNOWN({t.state})"), + "flags": t.flags, + "ts": f"{now.sec}.{now.nanosec:09d}", + } + msg = String() + msg.data = json.dumps(payload) + self._pub_attitude.publish(msg) + self._pub_balance.publish(msg) # keep /saltybot/balance_state alive - # Publish battery (vbat_mv → volts) - bat_msg = BatteryState() - bat_msg.header.stamp = imu_msg.header.stamp - bat_msg.voltage = telem.vbat_mv / 1000.0 - bat_msg.present = True - bat_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING - self._pub_battery.publish(bat_msg) - - def _handle_fc_vesc(self, data: bytes) -> None: - """FC_VESC (0x401): left/right RPM and current aggregated by BALANCE.""" - telem = decode_fc_vesc(data) - - left_msg = Float32MultiArray() - left_msg.data = [telem.left_rpm, 0.0, 0.0, telem.left_cur] - self._pub_vesc_left.publish(left_msg) - - right_msg = Float32MultiArray() - right_msg.data = [telem.right_rpm, 0.0, 0.0, telem.right_cur] - self._pub_vesc_right.publish(right_msg) + def _handle_battery(self, data: bytes) -> None: + """BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery.""" + t = decode_battery(data) + msg = BatteryState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.voltage = t.vbat_mv / 1000.0 + msg.present = True + msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + self._pub_battery.publish(msg) # ── Status helper ───────────────────────────────────────────────────── @@ -335,11 +298,10 @@ class CanBridgeNode(Node): # ── Shutdown ────────────────────────────────────────────────────────── def destroy_node(self) -> None: - """Send zero velocity and shut down the CAN bus cleanly.""" if self._connected and self._bus is not None: try: - self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "shutdown") - self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown") + self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown") + self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown") except Exception: pass try: @@ -349,8 +311,6 @@ class CanBridgeNode(Node): super().destroy_node() -# --------------------------------------------------------------------------- - def main(args=None) -> None: rclpy.init(args=args) node = CanBridgeNode() diff --git a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py index 9d74970..32985e2 100644 --- a/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py +++ b/jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/mamba_protocol.py @@ -1,31 +1,29 @@ #!/usr/bin/env python3 -"""mamba_protocol.py — CAN frame codec for SAUL-TEE ESP32 BALANCE + VESC. - -Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 & §8 (2026-04-04) +"""mamba_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE. +Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04) File name retained for import compatibility. -CAN bus: 500 kbps, CANable 2.0 (slcan0) on Orin. +CAN bus: 500 kbps, standard 11-bit IDs, CANable 2.0 (slcan0 / can0) on Orin. ── Orin → ESP32 BALANCE (commands) ─────────────────────────────────────────── - 0x300 DRIVE 8 B [left_mps f32 LE][right_mps f32 LE] velocity setpoint - 0x301 MODE 1 B [mode u8] 0=idle 1=drive 2=estop - 0x302 ESTOP 1 B [flags u8] bit0=stop bit1=clear - 0x303 LED 4 B [pattern u8][r u8][g u8][b u8] + 0x300 DRIVE 8 B [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16] + 0x301 ARM 1 B [arm:u8] 0x00=DISARM 0x01=ARM + 0x302 PID_SET 8 B [kp:f16 BE][ki:f16 BE][kd:f16 BE][_:u16] + 0x303 ESTOP 1 B [0xE5] magic byte — cuts all motors immediately ── ESP32 BALANCE → Orin (telemetry) ────────────────────────────────────────── - 0x400 FC_STATUS 8 B [pitch_x10 i16][motor_cmd i16][vbat_mv u16][state u8][flags u8] 10 Hz - 0x401 FC_VESC 8 B [l_rpm_x10 i16][r_rpm_x10 i16][l_cur_x10 i16][r_cur_x10 i16] 10 Hz + 0x400 ATTITUDE 8 B [pitch:f16 BE][speed:f16 BE][yaw_rate:f16 BE][state:u8][flags:u8] + 0x401 BATTERY 4 B [vbat_mv:u16 BE][fault_code:u8][rssi:i8] -── VESC standard CAN frames (29-bit extended IDs) ──────────────────────────── - VESC node IDs: Left = 56, Right = 68 - ID = (packet_type << 8) | node_id +speed/steer range: −1000..+1000 (motor units). f16 = IEEE 754 half-precision. - SET_RPM cmd=3 payload: rpm i32 big-endian - STATUS cmd=9 payload: erpm i32, current i16, duty i16 - STATUS_4 cmd=16 payload: temp_fet×10 i16, temp_motor×10 i16, - current_in×10 i16, pid_pos×50 i16 - STATUS_5 cmd=27 payload: tacho i32, vbat×10 i16 +VESC standard extended CAN (29-bit IDs = packet_type<<8 | node_id): + Left VESC node ID = 56 (0x38) + Right VESC node ID = 68 (0x44) + STATUS_1 cmd=9 erpm i32 BE, current i16 (/10 A), duty i16 (/1000) + STATUS_4 cmd=16 temp_fet i16 (/10 °C), temp_mot i16 (/10 °C), cur_in i16 (/10 A) + STATUS_5 cmd=27 tacho i32, vbat i16 (/10 V) """ import struct @@ -34,70 +32,86 @@ from dataclasses import dataclass # ── CAN IDs ─────────────────────────────────────────────────────────────────── # Orin → ESP32 BALANCE -ORIN_CMD_DRIVE: int = 0x300 -ORIN_CMD_MODE: int = 0x301 -ORIN_CMD_ESTOP: int = 0x302 -ORIN_CMD_LED: int = 0x303 +ORIN_CMD_DRIVE: int = 0x300 +ORIN_CMD_ARM: int = 0x301 +ORIN_CMD_PID: int = 0x302 +ORIN_CMD_ESTOP: int = 0x303 # ESP32 BALANCE → Orin -FC_STATUS: int = 0x400 -FC_VESC: int = 0x401 +ESP32_TELEM_ATTITUDE: int = 0x400 +ESP32_TELEM_BATTERY: int = 0x401 -# VESC node IDs (per docs/SAUL-TEE-SYSTEM-REFERENCE.md §8) +# Backward-compat aliases used by other nodes +FC_STATUS: int = ESP32_TELEM_ATTITUDE +FC_VESC: int = ESP32_TELEM_BATTERY + +# VESC node IDs VESC_LEFT_ID: int = 56 VESC_RIGHT_ID: int = 68 -# VESC CAN packet types (standard VESC protocol) -VESC_CMD_SET_RPM: int = 3 -VESC_STATUS_1: int = 9 -VESC_STATUS_4: int = 16 -VESC_STATUS_5: int = 27 +# VESC packet types +VESC_STATUS_1: int = 9 +VESC_STATUS_4: int = 16 +VESC_STATUS_5: int = 27 -# ── Mode constants ──────────────────────────────────────────────────────────── +# ── Mode constants (DRIVE frame mode byte) ───────────────────────────────────── + +MODE_IDLE: int = 0 # RC passthrough, Orin not injecting +MODE_DRIVE: int = 1 # Orin velocity commands +MODE_AUTONOMOUS: int = 2 # full autonomy +MODE_ESTOP: int = 2 # alias + +# ESTOP magic byte +_ESTOP_MAGIC: int = 0xE5 + +# ── Struct formats (big-endian) ──────────────────────────────────────────────── + +_FMT_DRIVE = ">hhBBH" # i16 speed, i16 steer, u8 mode, u8 flags, u16 pad +_FMT_PID = ">eeeH" # f16 kp, f16 ki, f16 kd, u16 pad +_FMT_ATTITUDE = ">eeeBB" # f16 pitch, f16 speed, f16 yaw_rate, u8 state, u8 flags +_FMT_BATTERY = ">HBb" # u16 vbat_mv, u8 fault_code, i8 rssi -MODE_IDLE: int = 0 -MODE_DRIVE: int = 1 -MODE_ESTOP: int = 2 # ── Data classes ────────────────────────────────────────────────────────────── @dataclass -class DriveCmd: - left_mps: float = 0.0 # m/s, positive = forward - right_mps: float = 0.0 - - -@dataclass -class FcStatus: - """Decoded FC_STATUS (0x400) telemetry from ESP32 BALANCE.""" - pitch_deg: float = 0.0 # pitch_x10 / 10.0 - motor_cmd: int = 0 # raw motor command output - vbat_mv: int = 0 # battery voltage in mV - state: int = 0 # 0=idle 1=running 2=fault +class AttitudeTelemetry: + """Decoded ATTITUDE (0x400) from ESP32 BALANCE.""" + pitch_deg: float = 0.0 # degrees, half-float + speed: float = 0.0 # m/s, half-float + yaw_rate: float = 0.0 # rad/s, half-float + state: int = 0 # 0=IDLE 1=RUNNING 2=FAULT flags: int = 0 # error bitmask @dataclass -class FcVesc: - """Decoded FC_VESC (0x401) VESC aggregate telemetry.""" - left_rpm: float = 0.0 # left_rpm_x10 / 10.0 - right_rpm: float = 0.0 - left_cur: float = 0.0 # left_cur_x10 / 10.0 (amps) - right_cur: float = 0.0 +class BatteryTelemetry: + """Decoded BATTERY (0x401) from ESP32 BALANCE.""" + vbat_mv: int = 0 # millivolts + fault_code: int = 0 # 0 = OK + rssi: int = 0 # RC signal dBm (signed) + + +@dataclass +class PidGains: + """Balance PID gains.""" + kp: float = 0.0 + ki: float = 0.0 + kd: float = 0.0 @dataclass class VescStatus1: - """Decoded VESC STATUS (cmd=9) frame.""" - node_id: int = 0 - erpm: float = 0.0 - current: float = 0.0 # A - duty: float = 0.0 # -1.0..+1.0 + """Decoded VESC STATUS (cmd=9) — direct from VESC.""" + node_id: int = 0 + erpm: float = 0.0 + current: float = 0.0 # A + duty: float = 0.0 # -1.0..+1.0 @dataclass class VescStatus4: - """Decoded VESC STATUS_4 (cmd=16) frame.""" + """Decoded VESC STATUS_4 (cmd=16).""" node_id: int = 0 temp_fet_c: float = 0.0 temp_motor_c: float = 0.0 @@ -106,112 +120,98 @@ class VescStatus4: @dataclass class VescStatus5: - """Decoded VESC STATUS_5 (cmd=27) frame.""" + """Decoded VESC STATUS_5 (cmd=27).""" node_id: int = 0 tacho: int = 0 vbat_v: float = 0.0 -@dataclass -class PidGains: - """Balance PID gains (Issue #693).""" - kp: float = 0.0 - ki: float = 0.0 - kd: float = 0.0 - - # ── Orin → BALANCE encoders ─────────────────────────────────────────────────── -def encode_drive_cmd(left_mps: float, right_mps: float) -> bytes: - """Encode ORIN_CMD_DRIVE (0x300) — 8 bytes, float32 LE.""" - return struct.pack(" bytes: + """Encode ORIN_CMD_DRIVE (0x300) — 8 bytes. + + speed: −1000..+1000 motor units (positive = forward) + steer: −1000..+1000 motor units (positive = right) + mode: MODE_IDLE / MODE_DRIVE / MODE_AUTONOMOUS + """ + speed = max(-1000, min(1000, int(speed))) + steer = max(-1000, min(1000, int(steer))) + return struct.pack(_FMT_DRIVE, speed, steer, mode & 0xFF, flags & 0xFF, 0) -def encode_mode_cmd(mode: int) -> bytes: - """Encode ORIN_CMD_MODE (0x301) — 1 byte.""" - if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP): - raise ValueError(f"Invalid mode {mode!r}") - return struct.pack("B", mode) +def encode_arm_cmd(arm: bool) -> bytes: + """Encode ORIN_CMD_ARM (0x301) — 1 byte.""" + return struct.pack("B", 0x01 if arm else 0x00) -def encode_estop_cmd(stop: bool = True, clear: bool = False) -> bytes: - """Encode ORIN_CMD_ESTOP (0x302) — 1 byte flags (bit0=stop, bit1=clear).""" - flags = (0x01 if stop else 0) | (0x02 if clear else 0) - return struct.pack("B", flags) +def encode_pid_cmd(kp: float, ki: float, kd: float) -> bytes: + """Encode ORIN_CMD_PID (0x302) — 8 bytes (3× half-float + 2-byte pad).""" + return struct.pack(_FMT_PID, float(kp), float(ki), float(kd), 0) -def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes: - """Encode ORIN_CMD_LED (0x303) — 4 bytes.""" - return struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF) - - -# ── VESC encoders ───────────────────────────────────────────────────────────── - -def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple[int, bytes]: - """Encode VESC SET_RPM command. Returns (can_id, payload).""" - can_id = (VESC_CMD_SET_RPM << 8) | (node_id & 0xFF) - return can_id, struct.pack(">i", int(rpm)) +def encode_estop_cmd() -> bytes: + """Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5.""" + return struct.pack("B", _ESTOP_MAGIC) # ── BALANCE → Orin decoders ─────────────────────────────────────────────────── -def decode_fc_status(data: bytes) -> FcStatus: - """Decode FC_STATUS (0x400) — 8 bytes.""" +def decode_attitude(data: bytes) -> AttitudeTelemetry: + """Decode ATTITUDE (0x400) — 8 bytes.""" if len(data) < 8: - raise ValueError(f"FC_STATUS expects 8 bytes, got {len(data)}") - pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hHHBB", data) - return FcStatus( - pitch_deg=pitch_x10 / 10.0, - motor_cmd=motor_cmd, - vbat_mv=vbat_mv, - state=state, - flags=flags, - ) + raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}") + pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data) + return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate, + state=state, flags=flags) -def decode_fc_vesc(data: bytes) -> FcVesc: - """Decode FC_VESC (0x401) — 8 bytes.""" - if len(data) < 8: - raise ValueError(f"FC_VESC expects 8 bytes, got {len(data)}") - l_rpm, r_rpm, l_cur, r_cur = struct.unpack_from(">hhhh", data) - return FcVesc( - left_rpm=l_rpm / 10.0, - right_rpm=r_rpm / 10.0, - left_cur=l_cur / 10.0, - right_cur=r_cur / 10.0, - ) +def decode_battery(data: bytes) -> BatteryTelemetry: + """Decode BATTERY (0x401) — 4 bytes.""" + if len(data) < 4: + raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}") + vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data) + return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi) -# ── VESC telemetry decoders ─────────────────────────────────────────────────── +# Backward-compat aliases +def decode_fc_status(data: bytes) -> AttitudeTelemetry: + return decode_attitude(data) -def decode_vesc_can_id(can_id: int) -> tuple[int, int]: + +def decode_fc_vesc(data: bytes) -> BatteryTelemetry: + return decode_battery(data) + + +# ── VESC CAN helpers ───────────────────────────────────────────────────────── + +def decode_vesc_can_id(can_id: int) -> tuple: """Split a VESC extended CAN ID into (packet_type, node_id).""" return (can_id >> 8) & 0xFF, can_id & 0xFF def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1: - """Decode VESC STATUS (cmd=9) — erpm i32, current i16, duty i16.""" + """Decode VESC STATUS (cmd=9): erpm i32, current i16(/10), duty i16(/1000).""" erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8]) - return VescStatus1( - node_id=node_id, - erpm=float(erpm), - current=cur_x10 / 10.0, - duty=duty_x1000 / 1000.0, - ) + return VescStatus1(node_id=node_id, erpm=float(erpm), + current=cur_x10 / 10.0, duty=duty_x1000 / 1000.0) def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4: - """Decode VESC STATUS_4 (cmd=16) — temp_fet×10, temp_motor×10, current_in×10.""" + """Decode VESC STATUS_4 (cmd=16): temp_fet, temp_mot, cur_in (all /10).""" tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8]) - return VescStatus4( - node_id=node_id, - temp_fet_c=tfet / 10.0, - temp_motor_c=tmot / 10.0, - current_in=cur_in / 10.0, - ) + return VescStatus4(node_id=node_id, temp_fet_c=tfet / 10.0, + temp_motor_c=tmot / 10.0, current_in=cur_in / 10.0) def decode_vesc_status5(node_id: int, data: bytes) -> VescStatus5: - """Decode VESC STATUS_5 (cmd=27) — tacho i32, vbat×10 i16.""" + """Decode VESC STATUS_5 (cmd=27): tacho i32, vbat i16 (/10 V).""" tacho, vbat_x10 = struct.unpack_from(">ih", data[:6]) return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0) + + +def encode_vesc_set_rpm(node_id: int, rpm: int) -> tuple: + """Encode VESC SET_RPM command. Returns (extended_can_id, payload).""" + can_id = (3 << 8) | (node_id & 0xFF) + return can_id, struct.pack(">i", int(rpm))