seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(arch): CAN/UART bridge update for SAUL-TEE ESP32 architecture #714

Merged
sl-jetson merged 2 commits from sl-controls/esp32-can-bridge-update into main 2026-04-04 09:00:10 -04:00
Conversation 0 Commits 2 Files Changed 7 +277 -529
5 changed files with 277 additions and 529 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit fda6ab99ff - Show all commits

13
docs/wiring-diagram.md
View File

@ -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)
```
┌─────────────────────────────────────────────────────────────────────┐
@ -138,7 +145,7 @@ BATTERY (36V) ──┬── VESC Left (36V direct -> BLDC left motor)
| CANable2 | USB-CAN | USB-A | `/dev/canable2` -> `slcan0` |
## ESP32-S3 BALANCE UART/CAN Summary
## FC UART Summary (MAMBA F722S — OBSOLETE)
| Interface | Pins | Baud/Rate | Assignment | Notes |
|-----------|------|-----------|------------|-------|

38
jetson/ros2_ws/src/saltybot_bridge/config/stm32_cmd_params.yaml
View File

@ -1,30 +1,16 @@
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
# Binary-framed Jetson↔ESP32 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/esp32-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 ESP32 BALANCE 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

26
jetson/ros2_ws/src/saltybot_bridge/launch/stm32_cmd.launch.py
View File

@ -1,14 +1,14 @@
"""stm32_cmd.launch.py — Launch the binary-framed ESP32 BALANCE 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,11 +24,8 @@ 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("serial_port", default_value="/dev/esp32-io"),
DeclareLaunchArgument("baud_rate", default_value="460800"),
DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
Node(
@ -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"),
},
],

431
jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/stm32_cmd_node.py
View File

@ -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 (ESP32 BALANCE 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↔ESP32 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)
# ── 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._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._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.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 ESP32 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"ESP32 BALANCE 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"])
except (ValueError, KeyError, json.JSONDecodeError) as exc:
self.get_logger().error(f"Bad PID update 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")
# ── Services ──────────────────────────────────────────────────────────────
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'}"
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)),
)
return response
except (ValueError, KeyError, json.JSONDecodeError) as exc:
self.get_logger().error(f"Bad /saltybot/leds JSON: {exc}")
return
self._write(frame)
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
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)
# ── 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 = "esp32"
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.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_frame_count)),
KeyValue(key="rx_frames", value=str(self._rx_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, "?")),
]
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()

264
jetson/ros2_ws/src/saltybot_can_bridge/saltybot_can_bridge/can_bridge_node.py
View File

@ -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)
/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)
/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.balance_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
@ -76,37 +74,37 @@ class CanBridgeNode(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("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_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
)
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(Bool, "/saltybot/arm", self._arm_cb, 10)
# ── Timers ────────────────────────────────────────────────────────
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
@ -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
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()