feat(arch): align CAN/UART bridges with SAUL-TEE-SYSTEM-REFERENCE.md spec
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 <noreply@anthropic.com>
This commit is contained in:
parent
cfd5a15b3e
commit
f66035cf81
@ -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` |
|
| 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 |
|
| UART | Pins | Baud | Assignment | Notes |
|
||||||
|------|------|------|------------|-------|
|
|------|------|------|------------|-------|
|
||||||
|
|||||||
@ -1,30 +1,16 @@
|
|||||||
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (Issue #119)
|
# stm32_cmd_params.yaml — Configuration for stm32_cmd_node (ESP32-S3 IO bridge)
|
||||||
# Binary-framed Jetson↔STM32 bridge at 921600 baud.
|
# 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 ────────────────────────────────────────────────────────────────
|
# ── Serial port ────────────────────────────────────────────────────────────────
|
||||||
# Use /dev/stm32-bridge if the udev rule is applied:
|
# Use /dev/esp32-io if udev rule is applied (see jetson/docs/udev-rules.md).
|
||||||
# SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",
|
# ESP32-S3 IO appears as USB-JTAG/Serial device; no external UART bridge needed.
|
||||||
# SYMLINK+="stm32-bridge", MODE="0660", GROUP="dialout"
|
serial_port: /dev/esp32-io
|
||||||
serial_port: /dev/ttyACM0
|
baud_rate: 460800
|
||||||
baud_rate: 921600
|
reconnect_delay: 2.0 # seconds between reconnect attempts
|
||||||
reconnect_delay: 2.0 # seconds between USB reconnect attempts
|
|
||||||
|
|
||||||
# ── Heartbeat ─────────────────────────────────────────────────────────────────
|
# ── Heartbeat ─────────────────────────────────────────────────────────────────
|
||||||
# HEARTBEAT frame sent every heartbeat_period seconds.
|
# HEARTBEAT (0x20) sent every heartbeat_period.
|
||||||
# STM32 fires watchdog and reverts to safe state if no frame received for 500ms.
|
# ESP32 IO watchdog fires if no heartbeat for ~500 ms.
|
||||||
heartbeat_period: 0.2 # 200ms → well within 500ms STM32 watchdog
|
heartbeat_period: 0.2 # 200 ms → well within 500 ms 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
|
|
||||||
|
|||||||
@ -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:
|
Usage:
|
||||||
# Default (binary protocol, bidirectional):
|
|
||||||
ros2 launch saltybot_bridge stm32_cmd.launch.py
|
ros2 launch saltybot_bridge stm32_cmd.launch.py
|
||||||
|
ros2 launch saltybot_bridge stm32_cmd.launch.py serial_port:=/dev/ttyACM0
|
||||||
# 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
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import os
|
import os
|
||||||
@ -24,11 +24,8 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
|
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
|
||||||
|
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
DeclareLaunchArgument("serial_port", default_value="/dev/esp32-io"),
|
||||||
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
DeclareLaunchArgument("baud_rate", default_value="460800"),
|
||||||
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("heartbeat_period", default_value="0.2"),
|
||||||
|
|
||||||
Node(
|
Node(
|
||||||
@ -42,9 +39,6 @@ def generate_launch_description() -> LaunchDescription:
|
|||||||
{
|
{
|
||||||
"serial_port": LaunchConfiguration("serial_port"),
|
"serial_port": LaunchConfiguration("serial_port"),
|
||||||
"baud_rate": LaunchConfiguration("baud_rate"),
|
"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"),
|
"heartbeat_period": LaunchConfiguration("heartbeat_period"),
|
||||||
},
|
},
|
||||||
],
|
],
|
||||||
|
|||||||
@ -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
|
Connects to the ESP32-S3 IO board via USB-CDC (/dev/esp32-io) using the
|
||||||
framing protocol (STX/TYPE/LEN/PAYLOAD/CRC16/ETX) at 921600 baud.
|
inter-board binary protocol (docs/SAUL-TEE-SYSTEM-REFERENCE.md §5).
|
||||||
|
|
||||||
TX commands (Jetson → STM32):
|
This node is NOT the primary drive path (that is CAN via can_bridge_node).
|
||||||
SPEED_STEER — 50 Hz from /cmd_vel subscription
|
It handles auxiliary I/O: RC monitoring, sensor data, LED/output control.
|
||||||
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
|
|
||||||
|
|
||||||
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):
|
RX from ESP32 IO:
|
||||||
IMU → /saltybot/imu (sensor_msgs/Imu)
|
RC_CHANNELS (0x01) → /saltybot/rc_channels (std_msgs/String JSON)
|
||||||
BATTERY → /saltybot/telemetry/battery (std_msgs/String JSON)
|
SENSORS (0x02) → /saltybot/sensors (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)
|
|
||||||
|
|
||||||
Auto-reconnect: USB disconnect is detected when serial.read() raises; node
|
TX to ESP32 IO:
|
||||||
continuously retries at reconnect_delay interval.
|
LED_CMD (0x10) ← /saltybot/leds (std_msgs/String JSON)
|
||||||
|
OUTPUT_CMD (0x11) ← /saltybot/outputs (std_msgs/String JSON)
|
||||||
This node owns /dev/ttyACM0 exclusively — do NOT run alongside
|
HEARTBEAT (0x20) — sent every heartbeat_period (keep IO watchdog alive)
|
||||||
serial_bridge_node or saltybot_cmd_node on the same port.
|
|
||||||
|
|
||||||
Parameters (config/stm32_cmd_params.yaml):
|
Parameters (config/stm32_cmd_params.yaml):
|
||||||
serial_port /dev/ttyACM0
|
serial_port /dev/esp32-io
|
||||||
baud_rate 921600
|
baud_rate 460800
|
||||||
reconnect_delay 2.0 (seconds)
|
reconnect_delay 2.0
|
||||||
heartbeat_period 0.2 (seconds)
|
heartbeat_period 0.2 (ESP32 IO watchdog fires at ~500 ms)
|
||||||
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)
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
import json
|
import json
|
||||||
import math
|
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
|
|
||||||
@ -50,119 +37,69 @@ from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
|||||||
import serial
|
import serial
|
||||||
|
|
||||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
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_msgs.msg import String
|
||||||
from std_srvs.srv import SetBool, Trigger
|
|
||||||
|
|
||||||
from .stm32_protocol import (
|
from .stm32_protocol import (
|
||||||
|
BAUD_RATE,
|
||||||
FrameParser,
|
FrameParser,
|
||||||
ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame,
|
RcChannels,
|
||||||
encode_heartbeat, encode_speed_steer, encode_arm, encode_set_mode,
|
SensorData,
|
||||||
encode_pid_update,
|
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):
|
class Stm32CmdNode(Node):
|
||||||
"""Binary-framed Jetson↔STM32 bridge node."""
|
"""Orin ↔ ESP32-S3 IO auxiliary bridge node."""
|
||||||
|
|
||||||
def __init__(self) -> None:
|
def __init__(self) -> None:
|
||||||
super().__init__("stm32_cmd_node")
|
super().__init__("stm32_cmd_node")
|
||||||
|
|
||||||
# ── Parameters ────────────────────────────────────────────────────────
|
# ── Parameters ────────────────────────────────────────────────────
|
||||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
self.declare_parameter("serial_port", "/dev/esp32-io")
|
||||||
self.declare_parameter("baud_rate", 921600)
|
self.declare_parameter("baud_rate", BAUD_RATE)
|
||||||
self.declare_parameter("reconnect_delay", 2.0)
|
self.declare_parameter("reconnect_delay", 2.0)
|
||||||
self.declare_parameter("heartbeat_period", 0.2)
|
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
|
self._port_name = self.get_parameter("serial_port").value
|
||||||
baud = self.get_parameter("baud_rate").value
|
self._baud = self.get_parameter("baud_rate").value
|
||||||
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
||||||
self._hb_period = self.get_parameter("heartbeat_period").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 ───────────────────────────────────────────────────────────────
|
# ── QoS ───────────────────────────────────────────────────────────
|
||||||
sensor_qos = QoSProfile(
|
|
||||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
|
||||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
|
||||||
)
|
|
||||||
rel_qos = QoSProfile(
|
rel_qos = QoSProfile(
|
||||||
reliability=ReliabilityPolicy.RELIABLE,
|
reliability=ReliabilityPolicy.RELIABLE,
|
||||||
history=HistoryPolicy.KEEP_LAST, depth=10,
|
history=HistoryPolicy.KEEP_LAST, depth=10,
|
||||||
)
|
)
|
||||||
|
|
||||||
# ── Publishers ────────────────────────────────────────────────────────
|
# ── Publishers ────────────────────────────────────────────────────
|
||||||
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos)
|
self._rc_pub = self.create_publisher(String, "/saltybot/rc_channels", rel_qos)
|
||||||
self._arm_pub = self.create_publisher(String, "/saltybot/arm_state", rel_qos)
|
self._sens_pub = self.create_publisher(String, "/saltybot/sensors", 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)
|
self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", rel_qos)
|
||||||
|
|
||||||
# ── Subscribers ───────────────────────────────────────────────────────
|
# ── Subscriptions ─────────────────────────────────────────────────
|
||||||
self._cmd_vel_sub = self.create_subscription(
|
self.create_subscription(String, "/saltybot/leds", self._on_leds, rel_qos)
|
||||||
Twist, "/cmd_vel", self._on_cmd_vel, rel_qos,
|
self.create_subscription(String, "/saltybot/outputs", self._on_outputs, rel_qos)
|
||||||
)
|
|
||||||
self._pid_sub = self.create_subscription(
|
|
||||||
String, "/saltybot/pid_update", self._on_pid_update, rel_qos,
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Services ──────────────────────────────────────────────────────────
|
# ── Serial state ──────────────────────────────────────────────────
|
||||||
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
|
|
||||||
self._ser: serial.Serial | None = None
|
self._ser: serial.Serial | None = None
|
||||||
self._ser_lock = threading.Lock()
|
self._ser_lock = threading.Lock()
|
||||||
self._parser = FrameParser()
|
self._parser = FrameParser()
|
||||||
|
self._rx_count = 0
|
||||||
|
|
||||||
# ── TX state ──────────────────────────────────────────────────────────
|
# ── Open serial and start timers ──────────────────────────────────
|
||||||
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 ──────────────────────────────────────
|
|
||||||
self._open_serial()
|
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)
|
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)
|
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(
|
self.get_logger().info(
|
||||||
f"stm32_cmd_node started — {port} @ {baud} baud | "
|
f"stm32_cmd_node started — {self._port_name} @ {self._baud} baud"
|
||||||
f"HB {int(self._hb_period * 1000)}ms | WD {int(self._wd_timeout * 1000)}ms"
|
|
||||||
)
|
)
|
||||||
|
|
||||||
# ── Serial management ─────────────────────────────────────────────────────
|
# ── Serial management ─────────────────────────────────────────────────
|
||||||
|
|
||||||
def _open_serial(self) -> bool:
|
def _open_serial(self) -> bool:
|
||||||
with self._ser_lock:
|
with self._ser_lock:
|
||||||
@ -170,7 +107,7 @@ class Stm32CmdNode(Node):
|
|||||||
self._ser = serial.Serial(
|
self._ser = serial.Serial(
|
||||||
port=self._port_name,
|
port=self._port_name,
|
||||||
baudrate=self._baud,
|
baudrate=self._baud,
|
||||||
timeout=0.005, # non-blocking reads
|
timeout=0.005,
|
||||||
write_timeout=0.1,
|
write_timeout=0.1,
|
||||||
)
|
)
|
||||||
self._ser.reset_input_buffer()
|
self._ser.reset_input_buffer()
|
||||||
@ -185,17 +122,7 @@ class Stm32CmdNode(Node):
|
|||||||
self._ser = None
|
self._ser = None
|
||||||
return False
|
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:
|
def _write(self, data: bytes) -> bool:
|
||||||
"""Thread-safe serial write. Returns False if port is not open."""
|
|
||||||
with self._ser_lock:
|
with self._ser_lock:
|
||||||
if self._ser is None or not self._ser.is_open:
|
if self._ser is None or not self._ser.is_open:
|
||||||
return False
|
return False
|
||||||
@ -207,16 +134,15 @@ class Stm32CmdNode(Node):
|
|||||||
self._ser = None
|
self._ser = None
|
||||||
return False
|
return False
|
||||||
|
|
||||||
# ── RX — read callback ────────────────────────────────────────────────────
|
# ── RX ────────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
def _read_cb(self) -> None:
|
def _read_cb(self) -> None:
|
||||||
"""Read bytes from serial and feed them to the frame parser."""
|
|
||||||
raw: bytes | None = None
|
raw: bytes | None = None
|
||||||
reconnect_needed = False
|
reconnect = False
|
||||||
|
|
||||||
with self._ser_lock:
|
with self._ser_lock:
|
||||||
if self._ser is None or not self._ser.is_open:
|
if self._ser is None or not self._ser.is_open:
|
||||||
reconnect_needed = True
|
reconnect = True
|
||||||
else:
|
else:
|
||||||
try:
|
try:
|
||||||
n = self._ser.in_waiting
|
n = self._ser.in_waiting
|
||||||
@ -225,9 +151,9 @@ class Stm32CmdNode(Node):
|
|||||||
except serial.SerialException as exc:
|
except serial.SerialException as exc:
|
||||||
self.get_logger().error(f"Serial read error: {exc}")
|
self.get_logger().error(f"Serial read error: {exc}")
|
||||||
self._ser = None
|
self._ser = None
|
||||||
reconnect_needed = True
|
reconnect = True
|
||||||
|
|
||||||
if reconnect_needed:
|
if reconnect:
|
||||||
self.get_logger().warn(
|
self.get_logger().warn(
|
||||||
"Serial disconnected — will retry",
|
"Serial disconnected — will retry",
|
||||||
throttle_duration_sec=self._reconnect_delay,
|
throttle_duration_sec=self._reconnect_delay,
|
||||||
@ -240,230 +166,105 @@ class Stm32CmdNode(Node):
|
|||||||
return
|
return
|
||||||
|
|
||||||
for byte in raw:
|
for byte in raw:
|
||||||
frame = self._parser.feed(byte)
|
msg = self._parser.feed(byte)
|
||||||
if frame is not None:
|
if msg is not None:
|
||||||
self._rx_frame_count += 1
|
self._rx_count += 1
|
||||||
self._dispatch_frame(frame)
|
self._dispatch(msg)
|
||||||
|
|
||||||
def _dispatch_frame(self, frame) -> None:
|
def _dispatch(self, msg) -> None:
|
||||||
"""Route a decoded frame to the appropriate publisher."""
|
|
||||||
now = self.get_clock().now().to_msg()
|
now = self.get_clock().now().to_msg()
|
||||||
|
ts = f"{now.sec}.{now.nanosec:09d}"
|
||||||
|
|
||||||
if isinstance(frame, ImuFrame):
|
if isinstance(msg, RcChannels):
|
||||||
self._publish_imu(frame, now)
|
out = String()
|
||||||
|
out.data = json.dumps({
|
||||||
|
"channels": msg.channels,
|
||||||
|
"source": msg.source,
|
||||||
|
"ts": ts,
|
||||||
|
})
|
||||||
|
self._rc_pub.publish(out)
|
||||||
|
|
||||||
elif isinstance(frame, BatteryFrame):
|
elif isinstance(msg, SensorData):
|
||||||
self._publish_battery(frame, now)
|
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):
|
elif isinstance(msg, tuple):
|
||||||
self._publish_motor_rpm(frame, now)
|
type_code, _ = msg
|
||||||
|
self.get_logger().debug(f"Unknown inter-board type 0x{type_code:02X}")
|
||||||
|
|
||||||
elif isinstance(frame, ArmStateFrame):
|
# ── TX ────────────────────────────────────────────────────────────────
|
||||||
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,
|
|
||||||
)
|
|
||||||
|
|
||||||
def _heartbeat_cb(self) -> None:
|
def _heartbeat_cb(self) -> None:
|
||||||
"""Send HEARTBEAT every heartbeat_period (default 200ms)."""
|
|
||||||
self._write(encode_heartbeat())
|
self._write(encode_heartbeat())
|
||||||
|
|
||||||
def _watchdog_cb(self) -> None:
|
def _on_leds(self, msg: String) -> None:
|
||||||
"""Send zero-speed if /cmd_vel silent for watchdog_timeout seconds."""
|
"""Parse JSON {"pattern":N,"r":R,"g":G,"b":B} and send LED_CMD."""
|
||||||
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."""
|
|
||||||
try:
|
try:
|
||||||
data = json.loads(msg.data)
|
d = json.loads(msg.data)
|
||||||
kp = float(data["kp"])
|
frame = encode_led_cmd(
|
||||||
ki = float(data["ki"])
|
int(d.get("pattern", 0)),
|
||||||
kd = float(data["kd"])
|
int(d.get("r", 0)),
|
||||||
except (ValueError, KeyError, json.JSONDecodeError) as exc:
|
int(d.get("g", 0)),
|
||||||
self.get_logger().error(f"Bad PID update JSON: {exc}")
|
int(d.get("b", 0)),
|
||||||
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'}"
|
|
||||||
)
|
)
|
||||||
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):
|
def _on_outputs(self, msg: String) -> None:
|
||||||
"""SetBool: data maps to mode byte (True=1, False=0)."""
|
"""Parse JSON {"horn":bool,"buzzer":bool,"headlight":0-255,"fan":0-255}."""
|
||||||
mode = 1 if request.data else 0
|
try:
|
||||||
frame = encode_set_mode(mode)
|
d = json.loads(msg.data)
|
||||||
ok = self._write(frame)
|
frame = encode_output_cmd(
|
||||||
response.success = ok
|
bool(d.get("horn", False)),
|
||||||
response.message = f"mode={mode}" if ok else "serial not open"
|
bool(d.get("buzzer", False)),
|
||||||
return response
|
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:
|
def _publish_diagnostics(self) -> None:
|
||||||
diag = DiagnosticArray()
|
diag = DiagnosticArray()
|
||||||
diag.header.stamp = self.get_clock().now().to_msg()
|
diag.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
|
||||||
status = DiagnosticStatus()
|
status = DiagnosticStatus()
|
||||||
status.name = "saltybot/stm32_cmd_node"
|
status.name = "saltybot/esp32_io_bridge"
|
||||||
status.hardware_id = "stm32f722"
|
status.hardware_id = "esp32-s3-io"
|
||||||
|
|
||||||
port_ok = self._ser is not None and self._ser.is_open
|
port_ok = self._ser is not None and self._ser.is_open
|
||||||
if port_ok:
|
status.level = DiagnosticStatus.OK if port_ok else DiagnosticStatus.ERROR
|
||||||
status.level = DiagnosticStatus.OK
|
status.message = "Serial OK" if port_ok else f"Disconnected: {self._port_name}"
|
||||||
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 = [
|
status.values = [
|
||||||
KeyValue(key="serial_port", value=self._port_name),
|
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="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="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)
|
diag.status.append(status)
|
||||||
self._diag_pub.publish(diag)
|
self._diag_pub.publish(diag)
|
||||||
|
|
||||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
# ── Lifecycle ─────────────────────────────────────────────────────────
|
||||||
|
|
||||||
def destroy_node(self) -> None:
|
def destroy_node(self) -> None:
|
||||||
# Send zero-speed + disarm on shutdown
|
self._write(encode_heartbeat(state=0))
|
||||||
self._write(encode_speed_steer(0, 0))
|
with self._ser_lock:
|
||||||
self._write(encode_arm(False))
|
if self._ser and self._ser.is_open:
|
||||||
self._close_serial()
|
try:
|
||||||
|
self._ser.close()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
self._ser = None
|
||||||
super().destroy_node()
|
super().destroy_node()
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -1,26 +1,34 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""
|
"""
|
||||||
can_bridge_node.py — ROS2 node bridging the Jetson Orin to the ESP32-S3 BALANCE
|
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
|
Subscriptions
|
||||||
-------------
|
-------------
|
||||||
/cmd_vel geometry_msgs/Twist → ORIN_CMD_DRIVE (0x300)
|
/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
|
Publications
|
||||||
------------
|
------------
|
||||||
/can/imu sensor_msgs/Imu from FC_STATUS (0x400) pitch
|
/saltybot/attitude std_msgs/String (JSON) pitch, speed, yaw_rate, state
|
||||||
/can/battery sensor_msgs/BatteryState from FC_STATUS (0x400) vbat_mv
|
/saltybot/balance_state std_msgs/String (JSON) alias of /saltybot/attitude
|
||||||
/can/vesc/left/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
/can/battery sensor_msgs/BatteryState vbat_mv, fault, rssi
|
||||||
/can/vesc/right/state std_msgs/Float32MultiArray from FC_VESC (0x401)
|
/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"
|
/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 threading
|
||||||
import time
|
import time
|
||||||
from typing import Optional
|
from typing import Optional
|
||||||
@ -29,42 +37,32 @@ import can
|
|||||||
import rclpy
|
import rclpy
|
||||||
from geometry_msgs.msg import Twist
|
from geometry_msgs.msg import Twist
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import BatteryState
|
||||||
from sensor_msgs.msg import BatteryState, Imu
|
|
||||||
from std_msgs.msg import Bool, Float32MultiArray, String
|
from std_msgs.msg import Bool, Float32MultiArray, String
|
||||||
|
|
||||||
from saltybot_can_bridge.mamba_protocol import (
|
from saltybot_can_bridge.mamba_protocol import (
|
||||||
# Orin → BALANCE command IDs
|
|
||||||
ORIN_CMD_DRIVE,
|
ORIN_CMD_DRIVE,
|
||||||
ORIN_CMD_MODE,
|
ORIN_CMD_ARM,
|
||||||
ORIN_CMD_ESTOP,
|
ORIN_CMD_ESTOP,
|
||||||
# BALANCE → Orin telemetry IDs
|
ESP32_TELEM_ATTITUDE,
|
||||||
FC_STATUS,
|
ESP32_TELEM_BATTERY,
|
||||||
FC_VESC,
|
|
||||||
# VESC node IDs
|
|
||||||
VESC_LEFT_ID,
|
VESC_LEFT_ID,
|
||||||
VESC_RIGHT_ID,
|
VESC_RIGHT_ID,
|
||||||
VESC_STATUS_1,
|
VESC_STATUS_1,
|
||||||
# Mode constants
|
|
||||||
MODE_DRIVE,
|
MODE_DRIVE,
|
||||||
MODE_ESTOP,
|
|
||||||
MODE_IDLE,
|
MODE_IDLE,
|
||||||
# Encoders
|
|
||||||
encode_drive_cmd,
|
encode_drive_cmd,
|
||||||
encode_mode_cmd,
|
encode_arm_cmd,
|
||||||
encode_estop_cmd,
|
encode_estop_cmd,
|
||||||
encode_led_cmd,
|
decode_attitude,
|
||||||
# Decoders
|
decode_battery,
|
||||||
decode_fc_status,
|
|
||||||
decode_fc_vesc,
|
|
||||||
decode_vesc_status1,
|
decode_vesc_status1,
|
||||||
decode_vesc_can_id,
|
|
||||||
)
|
)
|
||||||
|
|
||||||
# Reconnect attempt interval when CAN bus is lost
|
# Reconnect attempt interval when CAN bus is lost
|
||||||
_RECONNECT_INTERVAL_S: float = 5.0
|
_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
|
_WATCHDOG_HZ: float = 10.0
|
||||||
|
|
||||||
|
|
||||||
@ -76,37 +74,37 @@ class CanBridgeNode(Node):
|
|||||||
|
|
||||||
# ── Parameters ────────────────────────────────────────────────────
|
# ── Parameters ────────────────────────────────────────────────────
|
||||||
self.declare_parameter("can_interface", "slcan0")
|
self.declare_parameter("can_interface", "slcan0")
|
||||||
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID) # 56
|
self.declare_parameter("left_vesc_can_id", VESC_LEFT_ID)
|
||||||
self.declare_parameter("right_vesc_can_id", VESC_RIGHT_ID) # 68
|
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.declare_parameter("command_timeout_s", 0.5)
|
||||||
|
|
||||||
self._iface: str = self.get_parameter("can_interface").value
|
self._iface = self.get_parameter("can_interface").value
|
||||||
self._left_vesc_id: int = self.get_parameter("left_vesc_can_id").value
|
self._left_vesc_id = self.get_parameter("left_vesc_can_id").value
|
||||||
self._right_vesc_id: int = self.get_parameter("right_vesc_can_id").value
|
self._right_vesc_id = self.get_parameter("right_vesc_can_id").value
|
||||||
self._cmd_timeout: float = self.get_parameter("command_timeout_s").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 ─────────────────────────────────────────────────────────
|
# ── State ─────────────────────────────────────────────────────────
|
||||||
self._bus: Optional[can.BusABC] = None
|
self._bus: Optional[can.BusABC] = None
|
||||||
self._connected: bool = False
|
self._connected: bool = False
|
||||||
self._last_cmd_time: float = time.monotonic()
|
self._last_cmd_time: float = time.monotonic()
|
||||||
self._lock = threading.Lock() # protects _bus / _connected
|
self._lock = threading.Lock()
|
||||||
|
|
||||||
# ── Publishers ────────────────────────────────────────────────────
|
# ── 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_battery = self.create_publisher(BatteryState, "/can/battery", 10)
|
||||||
self._pub_vesc_left = self.create_publisher(
|
self._pub_vesc_left = self.create_publisher(Float32MultiArray,"/can/vesc/left/state", 10)
|
||||||
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_right = self.create_publisher(
|
|
||||||
Float32MultiArray, "/can/vesc/right/state", 10
|
|
||||||
)
|
|
||||||
self._pub_status = self.create_publisher(
|
|
||||||
String, "/can/connection_status", 10
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── Subscriptions ─────────────────────────────────────────────────
|
# ── Subscriptions ─────────────────────────────────────────────────
|
||||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_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, "/estop", self._estop_cb, 10)
|
||||||
|
self.create_subscription(Bool, "/saltybot/arm", self._arm_cb, 10)
|
||||||
|
|
||||||
# ── Timers ────────────────────────────────────────────────────────
|
# ── Timers ────────────────────────────────────────────────────────
|
||||||
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
self.create_timer(1.0 / _WATCHDOG_HZ, self._watchdog_cb)
|
||||||
@ -123,22 +121,18 @@ class CanBridgeNode(Node):
|
|||||||
|
|
||||||
self.get_logger().info(
|
self.get_logger().info(
|
||||||
f"can_bridge_node ready — iface={self._iface} "
|
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 ──────────────────────────────────────────────
|
# ── Connection management ──────────────────────────────────────────────
|
||||||
|
|
||||||
def _try_connect(self) -> None:
|
def _try_connect(self) -> None:
|
||||||
"""Attempt to open the CAN interface; silently skip if already connected."""
|
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if self._connected:
|
if self._connected:
|
||||||
return
|
return
|
||||||
try:
|
try:
|
||||||
bus = can.interface.Bus(
|
self._bus = can.interface.Bus(channel=self._iface, bustype="socketcan")
|
||||||
channel=self._iface,
|
|
||||||
bustype="socketcan",
|
|
||||||
)
|
|
||||||
self._bus = bus
|
|
||||||
self._connected = True
|
self._connected = True
|
||||||
self.get_logger().info(f"CAN bus connected: {self._iface}")
|
self.get_logger().info(f"CAN bus connected: {self._iface}")
|
||||||
self._publish_status("connected")
|
self._publish_status("connected")
|
||||||
@ -151,12 +145,10 @@ class CanBridgeNode(Node):
|
|||||||
self._publish_status("disconnected")
|
self._publish_status("disconnected")
|
||||||
|
|
||||||
def _reconnect_cb(self) -> None:
|
def _reconnect_cb(self) -> None:
|
||||||
"""Periodic timer: try to reconnect when disconnected."""
|
|
||||||
if not self._connected:
|
if not self._connected:
|
||||||
self._try_connect()
|
self._try_connect()
|
||||||
|
|
||||||
def _handle_can_error(self, exc: Exception, context: str) -> None:
|
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}")
|
self.get_logger().warning(f"CAN error in {context}: {exc}")
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if self._bus is not None:
|
if self._bus is not None:
|
||||||
@ -171,59 +163,51 @@ class CanBridgeNode(Node):
|
|||||||
# ── ROS callbacks ─────────────────────────────────────────────────────
|
# ── ROS callbacks ─────────────────────────────────────────────────────
|
||||||
|
|
||||||
def _cmd_vel_cb(self, msg: Twist) -> None:
|
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()
|
self._last_cmd_time = time.monotonic()
|
||||||
|
|
||||||
if not self._connected:
|
if not self._connected:
|
||||||
return
|
return
|
||||||
|
speed = int(max(-1000.0, min(1000.0, msg.linear.x * self._speed_scale)))
|
||||||
# Differential drive decomposition — individual wheel speeds in m/s.
|
steer = int(max(-1000.0, min(1000.0, msg.angular.z * self._steer_scale)))
|
||||||
# The VESC nodes interpret linear velocity directly; angular is handled
|
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(speed, steer, MODE_DRIVE), "cmd_vel")
|
||||||
# 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")
|
|
||||||
|
|
||||||
def _estop_cb(self, msg: Bool) -> None:
|
def _estop_cb(self, msg: Bool) -> None:
|
||||||
"""Forward /estop to ESP32 BALANCE over CAN."""
|
"""Forward /estop to ESP32 BALANCE over CAN."""
|
||||||
if not self._connected:
|
if not self._connected:
|
||||||
return
|
return
|
||||||
self._send_can(ORIN_CMD_ESTOP, encode_estop_cmd(stop=msg.data), "estop")
|
|
||||||
if msg.data:
|
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")
|
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 ──────────────────────────────────────────────────────────
|
# ── Watchdog ──────────────────────────────────────────────────────────
|
||||||
|
|
||||||
def _watchdog_cb(self) -> None:
|
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:
|
if not self._connected:
|
||||||
return
|
return
|
||||||
elapsed = time.monotonic() - self._last_cmd_time
|
if time.monotonic() - self._last_cmd_time > self._cmd_timeout:
|
||||||
if elapsed > self._cmd_timeout:
|
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "watchdog")
|
||||||
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")
|
|
||||||
|
|
||||||
# ── CAN send helper ───────────────────────────────────────────────────
|
# ── CAN send helper ───────────────────────────────────────────────────
|
||||||
|
|
||||||
def _send_can(self, arb_id: int, data: bytes, context: str) -> None:
|
def _send_can(self, arb_id: int, data: bytes, context: str,
|
||||||
"""Send a standard CAN frame; handle errors gracefully."""
|
extended: bool = False) -> None:
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if not self._connected or self._bus is None:
|
if not self._connected or self._bus is None:
|
||||||
return
|
return
|
||||||
bus = self._bus
|
bus = self._bus
|
||||||
|
msg = can.Message(arbitration_id=arb_id, data=data,
|
||||||
msg = can.Message(
|
is_extended_id=extended)
|
||||||
arbitration_id=arb_id,
|
|
||||||
data=data,
|
|
||||||
is_extended_id=False,
|
|
||||||
)
|
|
||||||
try:
|
try:
|
||||||
bus.send(msg, timeout=0.05)
|
bus.send(msg, timeout=0.05)
|
||||||
except can.CanError as exc:
|
except can.CanError as exc:
|
||||||
@ -232,59 +216,41 @@ class CanBridgeNode(Node):
|
|||||||
# ── Background CAN reader ─────────────────────────────────────────────
|
# ── Background CAN reader ─────────────────────────────────────────────
|
||||||
|
|
||||||
def _reader_loop(self) -> None:
|
def _reader_loop(self) -> None:
|
||||||
"""
|
|
||||||
Blocking CAN read loop executed in a daemon thread.
|
|
||||||
Dispatches incoming frames to the appropriate handler.
|
|
||||||
"""
|
|
||||||
while rclpy.ok():
|
while rclpy.ok():
|
||||||
with self._lock:
|
with self._lock:
|
||||||
connected = self._connected
|
connected, bus = self._connected, self._bus
|
||||||
bus = self._bus
|
|
||||||
|
|
||||||
if not connected or bus is None:
|
if not connected or bus is None:
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
continue
|
continue
|
||||||
|
|
||||||
try:
|
try:
|
||||||
frame = bus.recv(timeout=0.5)
|
frame = bus.recv(timeout=0.5)
|
||||||
except can.CanError as exc:
|
except can.CanError as exc:
|
||||||
self._handle_can_error(exc, "reader_loop recv")
|
self._handle_can_error(exc, "reader_loop recv")
|
||||||
continue
|
continue
|
||||||
|
|
||||||
if frame is None:
|
if frame is None:
|
||||||
# Timeout — no frame within 0.5 s, loop again
|
|
||||||
continue
|
continue
|
||||||
|
|
||||||
self._dispatch_frame(frame)
|
self._dispatch_frame(frame)
|
||||||
|
|
||||||
def _dispatch_frame(self, frame: can.Message) -> None:
|
def _dispatch_frame(self, frame: can.Message) -> None:
|
||||||
"""Route an incoming CAN frame to the correct publisher."""
|
|
||||||
arb_id = frame.arbitration_id
|
arb_id = frame.arbitration_id
|
||||||
data = bytes(frame.data)
|
data = bytes(frame.data)
|
||||||
|
vesc_l = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
||||||
# VESC STATUS_1 CAN IDs: (VESC_STATUS_1 << 8) | node_id
|
vesc_r = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
||||||
_vesc_left_status1 = (VESC_STATUS_1 << 8) | self._left_vesc_id
|
|
||||||
_vesc_right_status1 = (VESC_STATUS_1 << 8) | self._right_vesc_id
|
|
||||||
|
|
||||||
try:
|
try:
|
||||||
if arb_id == FC_STATUS:
|
if arb_id == ESP32_TELEM_ATTITUDE:
|
||||||
self._handle_fc_status(data)
|
self._handle_attitude(data)
|
||||||
|
elif arb_id == ESP32_TELEM_BATTERY:
|
||||||
elif arb_id == FC_VESC:
|
self._handle_battery(data)
|
||||||
self._handle_fc_vesc(data)
|
elif arb_id == vesc_l:
|
||||||
|
t = decode_vesc_status1(self._left_vesc_id, data)
|
||||||
elif arb_id == _vesc_left_status1:
|
m = Float32MultiArray()
|
||||||
telem = decode_vesc_status1(self._left_vesc_id, data)
|
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||||
msg = Float32MultiArray()
|
self._pub_vesc_left.publish(m)
|
||||||
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
elif arb_id == vesc_r:
|
||||||
self._pub_vesc_left.publish(msg)
|
t = decode_vesc_status1(self._right_vesc_id, data)
|
||||||
|
m = Float32MultiArray()
|
||||||
elif arb_id == _vesc_right_status1:
|
m.data = [t.erpm, t.duty, 0.0, t.current]
|
||||||
telem = decode_vesc_status1(self._right_vesc_id, data)
|
self._pub_vesc_right.publish(m)
|
||||||
msg = Float32MultiArray()
|
|
||||||
msg.data = [telem.erpm, telem.duty, 0.0, telem.current]
|
|
||||||
self._pub_vesc_right.publish(msg)
|
|
||||||
|
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
self.get_logger().warning(
|
self.get_logger().warning(
|
||||||
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
|
f"Error parsing CAN frame 0x{arb_id:03X}: {exc}"
|
||||||
@ -292,38 +258,35 @@ class CanBridgeNode(Node):
|
|||||||
|
|
||||||
# ── Frame handlers ────────────────────────────────────────────────────
|
# ── Frame handlers ────────────────────────────────────────────────────
|
||||||
|
|
||||||
def _handle_fc_status(self, data: bytes) -> None:
|
_STATE_LABEL = {0: "IDLE", 1: "RUNNING", 2: "FAULT"}
|
||||||
"""FC_STATUS (0x400): pitch, motor_cmd, vbat_mv, state, flags."""
|
|
||||||
telem = decode_fc_status(data)
|
|
||||||
|
|
||||||
# Publish pitch as IMU (orientation only — yaw/roll unknown from FC_STATUS)
|
def _handle_attitude(self, data: bytes) -> None:
|
||||||
imu_msg = Imu()
|
"""ATTITUDE (0x400): pitch, speed, yaw_rate, state, flags → /saltybot/attitude."""
|
||||||
imu_msg.header.stamp = self.get_clock().now().to_msg()
|
t = decode_attitude(data)
|
||||||
imu_msg.header.frame_id = "imu_link"
|
now = self.get_clock().now().to_msg()
|
||||||
# Only pitch is available; publish as angular velocity placeholder
|
payload = {
|
||||||
imu_msg.angular_velocity.y = telem.pitch_deg # degrees, not rad/s
|
"pitch_deg": round(t.pitch_deg, 2),
|
||||||
imu_msg.orientation_covariance[0] = -1.0 # covariance unknown
|
"speed_mps": round(t.speed, 3),
|
||||||
self._pub_imu.publish(imu_msg)
|
"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)
|
def _handle_battery(self, data: bytes) -> None:
|
||||||
bat_msg = BatteryState()
|
"""BATTERY (0x401): vbat_mv, fault_code, rssi → /can/battery."""
|
||||||
bat_msg.header.stamp = imu_msg.header.stamp
|
t = decode_battery(data)
|
||||||
bat_msg.voltage = telem.vbat_mv / 1000.0
|
msg = BatteryState()
|
||||||
bat_msg.present = True
|
msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
bat_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
msg.voltage = t.vbat_mv / 1000.0
|
||||||
self._pub_battery.publish(bat_msg)
|
msg.present = True
|
||||||
|
msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING
|
||||||
def _handle_fc_vesc(self, data: bytes) -> None:
|
self._pub_battery.publish(msg)
|
||||||
"""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)
|
|
||||||
|
|
||||||
# ── Status helper ─────────────────────────────────────────────────────
|
# ── Status helper ─────────────────────────────────────────────────────
|
||||||
|
|
||||||
@ -335,11 +298,10 @@ class CanBridgeNode(Node):
|
|||||||
# ── Shutdown ──────────────────────────────────────────────────────────
|
# ── Shutdown ──────────────────────────────────────────────────────────
|
||||||
|
|
||||||
def destroy_node(self) -> None:
|
def destroy_node(self) -> None:
|
||||||
"""Send zero velocity and shut down the CAN bus cleanly."""
|
|
||||||
if self._connected and self._bus is not None:
|
if self._connected and self._bus is not None:
|
||||||
try:
|
try:
|
||||||
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0.0, 0.0), "shutdown")
|
self._send_can(ORIN_CMD_DRIVE, encode_drive_cmd(0, 0, MODE_IDLE), "shutdown")
|
||||||
self._send_can(ORIN_CMD_MODE, encode_mode_cmd(MODE_IDLE), "shutdown")
|
self._send_can(ORIN_CMD_ARM, encode_arm_cmd(False), "shutdown")
|
||||||
except Exception:
|
except Exception:
|
||||||
pass
|
pass
|
||||||
try:
|
try:
|
||||||
@ -349,8 +311,6 @@ class CanBridgeNode(Node):
|
|||||||
super().destroy_node()
|
super().destroy_node()
|
||||||
|
|
||||||
|
|
||||||
# ---------------------------------------------------------------------------
|
|
||||||
|
|
||||||
def main(args=None) -> None:
|
def main(args=None) -> None:
|
||||||
rclpy.init(args=args)
|
rclpy.init(args=args)
|
||||||
node = CanBridgeNode()
|
node = CanBridgeNode()
|
||||||
|
|||||||
@ -1,31 +1,29 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
"""mamba_protocol.py — CAN frame codec for SAUL-TEE ESP32 BALANCE + VESC.
|
"""mamba_protocol.py — CAN frame codec for Orin ↔ ESP32-S3 BALANCE.
|
||||||
|
|
||||||
Spec source: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 & §8 (2026-04-04)
|
|
||||||
|
|
||||||
|
Spec: docs/SAUL-TEE-SYSTEM-REFERENCE.md §6 (2026-04-04)
|
||||||
File name retained for import compatibility.
|
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) ───────────────────────────────────────────
|
── Orin → ESP32 BALANCE (commands) ───────────────────────────────────────────
|
||||||
0x300 DRIVE 8 B [left_mps f32 LE][right_mps f32 LE] velocity setpoint
|
0x300 DRIVE 8 B [speed:i16 BE][steer:i16 BE][mode:u8][flags:u8][_:u16]
|
||||||
0x301 MODE 1 B [mode u8] 0=idle 1=drive 2=estop
|
0x301 ARM 1 B [arm:u8] 0x00=DISARM 0x01=ARM
|
||||||
0x302 ESTOP 1 B [flags u8] bit0=stop bit1=clear
|
0x302 PID_SET 8 B [kp:f16 BE][ki:f16 BE][kd:f16 BE][_:u16]
|
||||||
0x303 LED 4 B [pattern u8][r u8][g u8][b u8]
|
0x303 ESTOP 1 B [0xE5] magic byte — cuts all motors immediately
|
||||||
|
|
||||||
── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
|
── ESP32 BALANCE → Orin (telemetry) ──────────────────────────────────────────
|
||||||
0x400 FC_STATUS 8 B [pitch_x10 i16][motor_cmd i16][vbat_mv u16][state u8][flags u8] 10 Hz
|
0x400 ATTITUDE 8 B [pitch:f16 BE][speed:f16 BE][yaw_rate:f16 BE][state:u8][flags:u8]
|
||||||
0x401 FC_VESC 8 B [l_rpm_x10 i16][r_rpm_x10 i16][l_cur_x10 i16][r_cur_x10 i16] 10 Hz
|
0x401 BATTERY 4 B [vbat_mv:u16 BE][fault_code:u8][rssi:i8]
|
||||||
|
|
||||||
── VESC standard CAN frames (29-bit extended IDs) ────────────────────────────
|
speed/steer range: −1000..+1000 (motor units). f16 = IEEE 754 half-precision.
|
||||||
VESC node IDs: Left = 56, Right = 68
|
|
||||||
ID = (packet_type << 8) | node_id
|
|
||||||
|
|
||||||
SET_RPM cmd=3 payload: rpm i32 big-endian
|
VESC standard extended CAN (29-bit IDs = packet_type<<8 | node_id):
|
||||||
STATUS cmd=9 payload: erpm i32, current i16, duty i16
|
Left VESC node ID = 56 (0x38)
|
||||||
STATUS_4 cmd=16 payload: temp_fet×10 i16, temp_motor×10 i16,
|
Right VESC node ID = 68 (0x44)
|
||||||
current_in×10 i16, pid_pos×50 i16
|
STATUS_1 cmd=9 erpm i32 BE, current i16 (/10 A), duty i16 (/1000)
|
||||||
STATUS_5 cmd=27 payload: tacho i32, vbat×10 i16
|
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
|
import struct
|
||||||
@ -35,60 +33,76 @@ from dataclasses import dataclass
|
|||||||
|
|
||||||
# Orin → ESP32 BALANCE
|
# Orin → ESP32 BALANCE
|
||||||
ORIN_CMD_DRIVE: int = 0x300
|
ORIN_CMD_DRIVE: int = 0x300
|
||||||
ORIN_CMD_MODE: int = 0x301
|
ORIN_CMD_ARM: int = 0x301
|
||||||
ORIN_CMD_ESTOP: int = 0x302
|
ORIN_CMD_PID: int = 0x302
|
||||||
ORIN_CMD_LED: int = 0x303
|
ORIN_CMD_ESTOP: int = 0x303
|
||||||
|
|
||||||
# ESP32 BALANCE → Orin
|
# ESP32 BALANCE → Orin
|
||||||
FC_STATUS: int = 0x400
|
ESP32_TELEM_ATTITUDE: int = 0x400
|
||||||
FC_VESC: int = 0x401
|
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_LEFT_ID: int = 56
|
||||||
VESC_RIGHT_ID: int = 68
|
VESC_RIGHT_ID: int = 68
|
||||||
|
|
||||||
# VESC CAN packet types (standard VESC protocol)
|
# VESC packet types
|
||||||
VESC_CMD_SET_RPM: int = 3
|
|
||||||
VESC_STATUS_1: int = 9
|
VESC_STATUS_1: int = 9
|
||||||
VESC_STATUS_4: int = 16
|
VESC_STATUS_4: int = 16
|
||||||
VESC_STATUS_5: int = 27
|
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 ──────────────────────────────────────────────────────────────
|
# ── Data classes ──────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class DriveCmd:
|
class AttitudeTelemetry:
|
||||||
left_mps: float = 0.0 # m/s, positive = forward
|
"""Decoded ATTITUDE (0x400) from ESP32 BALANCE."""
|
||||||
right_mps: float = 0.0
|
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
|
||||||
@dataclass
|
state: int = 0 # 0=IDLE 1=RUNNING 2=FAULT
|
||||||
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
|
|
||||||
flags: int = 0 # error bitmask
|
flags: int = 0 # error bitmask
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class FcVesc:
|
class BatteryTelemetry:
|
||||||
"""Decoded FC_VESC (0x401) VESC aggregate telemetry."""
|
"""Decoded BATTERY (0x401) from ESP32 BALANCE."""
|
||||||
left_rpm: float = 0.0 # left_rpm_x10 / 10.0
|
vbat_mv: int = 0 # millivolts
|
||||||
right_rpm: float = 0.0
|
fault_code: int = 0 # 0 = OK
|
||||||
left_cur: float = 0.0 # left_cur_x10 / 10.0 (amps)
|
rssi: int = 0 # RC signal dBm (signed)
|
||||||
right_cur: float = 0.0
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class PidGains:
|
||||||
|
"""Balance PID gains."""
|
||||||
|
kp: float = 0.0
|
||||||
|
ki: float = 0.0
|
||||||
|
kd: float = 0.0
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class VescStatus1:
|
class VescStatus1:
|
||||||
"""Decoded VESC STATUS (cmd=9) frame."""
|
"""Decoded VESC STATUS (cmd=9) — direct from VESC."""
|
||||||
node_id: int = 0
|
node_id: int = 0
|
||||||
erpm: float = 0.0
|
erpm: float = 0.0
|
||||||
current: float = 0.0 # A
|
current: float = 0.0 # A
|
||||||
@ -97,7 +111,7 @@ class VescStatus1:
|
|||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class VescStatus4:
|
class VescStatus4:
|
||||||
"""Decoded VESC STATUS_4 (cmd=16) frame."""
|
"""Decoded VESC STATUS_4 (cmd=16)."""
|
||||||
node_id: int = 0
|
node_id: int = 0
|
||||||
temp_fet_c: float = 0.0
|
temp_fet_c: float = 0.0
|
||||||
temp_motor_c: float = 0.0
|
temp_motor_c: float = 0.0
|
||||||
@ -106,112 +120,98 @@ class VescStatus4:
|
|||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class VescStatus5:
|
class VescStatus5:
|
||||||
"""Decoded VESC STATUS_5 (cmd=27) frame."""
|
"""Decoded VESC STATUS_5 (cmd=27)."""
|
||||||
node_id: int = 0
|
node_id: int = 0
|
||||||
tacho: int = 0
|
tacho: int = 0
|
||||||
vbat_v: float = 0.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 ───────────────────────────────────────────────────
|
# ── Orin → BALANCE encoders ───────────────────────────────────────────────────
|
||||||
|
|
||||||
def encode_drive_cmd(left_mps: float, right_mps: float) -> bytes:
|
def encode_drive_cmd(speed: int, steer: int,
|
||||||
"""Encode ORIN_CMD_DRIVE (0x300) — 8 bytes, float32 LE."""
|
mode: int = MODE_DRIVE, flags: int = 0) -> bytes:
|
||||||
return struct.pack("<ff", float(left_mps), float(right_mps))
|
"""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:
|
def encode_arm_cmd(arm: bool) -> bytes:
|
||||||
"""Encode ORIN_CMD_MODE (0x301) — 1 byte."""
|
"""Encode ORIN_CMD_ARM (0x301) — 1 byte."""
|
||||||
if mode not in (MODE_IDLE, MODE_DRIVE, MODE_ESTOP):
|
return struct.pack("B", 0x01 if arm else 0x00)
|
||||||
raise ValueError(f"Invalid mode {mode!r}")
|
|
||||||
return struct.pack("B", mode)
|
|
||||||
|
|
||||||
|
|
||||||
def encode_estop_cmd(stop: bool = True, clear: bool = False) -> bytes:
|
def encode_pid_cmd(kp: float, ki: float, kd: float) -> bytes:
|
||||||
"""Encode ORIN_CMD_ESTOP (0x302) — 1 byte flags (bit0=stop, bit1=clear)."""
|
"""Encode ORIN_CMD_PID (0x302) — 8 bytes (3× half-float + 2-byte pad)."""
|
||||||
flags = (0x01 if stop else 0) | (0x02 if clear else 0)
|
return struct.pack(_FMT_PID, float(kp), float(ki), float(kd), 0)
|
||||||
return struct.pack("B", flags)
|
|
||||||
|
|
||||||
|
|
||||||
def encode_led_cmd(pattern: int, r: int, g: int, b: int) -> bytes:
|
def encode_estop_cmd() -> bytes:
|
||||||
"""Encode ORIN_CMD_LED (0x303) — 4 bytes."""
|
"""Encode ORIN_CMD_ESTOP (0x303) — 1 byte magic 0xE5."""
|
||||||
return struct.pack("BBBB", pattern & 0xFF, r & 0xFF, g & 0xFF, b & 0xFF)
|
return struct.pack("B", _ESTOP_MAGIC)
|
||||||
|
|
||||||
|
|
||||||
# ── 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))
|
|
||||||
|
|
||||||
|
|
||||||
# ── BALANCE → Orin decoders ───────────────────────────────────────────────────
|
# ── BALANCE → Orin decoders ───────────────────────────────────────────────────
|
||||||
|
|
||||||
def decode_fc_status(data: bytes) -> FcStatus:
|
def decode_attitude(data: bytes) -> AttitudeTelemetry:
|
||||||
"""Decode FC_STATUS (0x400) — 8 bytes."""
|
"""Decode ATTITUDE (0x400) — 8 bytes."""
|
||||||
if len(data) < 8:
|
if len(data) < 8:
|
||||||
raise ValueError(f"FC_STATUS expects 8 bytes, got {len(data)}")
|
raise ValueError(f"ATTITUDE expects ≥8 bytes, got {len(data)}")
|
||||||
pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hHHBB", data)
|
pitch, speed, yaw_rate, state, flags = struct.unpack_from(_FMT_ATTITUDE, data)
|
||||||
return FcStatus(
|
return AttitudeTelemetry(pitch_deg=pitch, speed=speed, yaw_rate=yaw_rate,
|
||||||
pitch_deg=pitch_x10 / 10.0,
|
state=state, flags=flags)
|
||||||
motor_cmd=motor_cmd,
|
|
||||||
vbat_mv=vbat_mv,
|
|
||||||
state=state,
|
|
||||||
flags=flags,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def decode_fc_vesc(data: bytes) -> FcVesc:
|
def decode_battery(data: bytes) -> BatteryTelemetry:
|
||||||
"""Decode FC_VESC (0x401) — 8 bytes."""
|
"""Decode BATTERY (0x401) — 4 bytes."""
|
||||||
if len(data) < 8:
|
if len(data) < 4:
|
||||||
raise ValueError(f"FC_VESC expects 8 bytes, got {len(data)}")
|
raise ValueError(f"BATTERY expects ≥4 bytes, got {len(data)}")
|
||||||
l_rpm, r_rpm, l_cur, r_cur = struct.unpack_from(">hhhh", data)
|
vbat, fault, rssi = struct.unpack_from(_FMT_BATTERY, data)
|
||||||
return FcVesc(
|
return BatteryTelemetry(vbat_mv=vbat, fault_code=fault, rssi=rssi)
|
||||||
left_rpm=l_rpm / 10.0,
|
|
||||||
right_rpm=r_rpm / 10.0,
|
|
||||||
left_cur=l_cur / 10.0,
|
|
||||||
right_cur=r_cur / 10.0,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
# ── 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)."""
|
"""Split a VESC extended CAN ID into (packet_type, node_id)."""
|
||||||
return (can_id >> 8) & 0xFF, can_id & 0xFF
|
return (can_id >> 8) & 0xFF, can_id & 0xFF
|
||||||
|
|
||||||
|
|
||||||
def decode_vesc_status1(node_id: int, data: bytes) -> VescStatus1:
|
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])
|
erpm, cur_x10, duty_x1000 = struct.unpack_from(">ihh", data[:8])
|
||||||
return VescStatus1(
|
return VescStatus1(node_id=node_id, erpm=float(erpm),
|
||||||
node_id=node_id,
|
current=cur_x10 / 10.0, duty=duty_x1000 / 1000.0)
|
||||||
erpm=float(erpm),
|
|
||||||
current=cur_x10 / 10.0,
|
|
||||||
duty=duty_x1000 / 1000.0,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
def decode_vesc_status4(node_id: int, data: bytes) -> VescStatus4:
|
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])
|
tfet, tmot, cur_in, _ = struct.unpack_from(">hhhh", data[:8])
|
||||||
return VescStatus4(
|
return VescStatus4(node_id=node_id, temp_fet_c=tfet / 10.0,
|
||||||
node_id=node_id,
|
temp_motor_c=tmot / 10.0, current_in=cur_in / 10.0)
|
||||||
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:
|
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])
|
tacho, vbat_x10 = struct.unpack_from(">ih", data[:6])
|
||||||
return VescStatus5(node_id=node_id, tacho=tacho, vbat_v=vbat_x10 / 10.0)
|
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))
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user