sl-jetson
7c4e46aaa1
saltybot_bridge ROS2 Python package (ament_python):
- serial_bridge_node.py: reads USB CDC JSON telemetry from STM32F722 at 50Hz
Parses exact firmware format: {"p","r","e","ig","m","s","y"} (all ×10 ints)
State enum: 0=DISARMED, 1=ARMED, 2=TILT_FAULT (matched to balance_state_t)
- Publishes sensor_msgs/Imu on /saltybot/imu (pitch/roll/yaw as angular_velocity)
- Publishes std_msgs/String on /saltybot/balance_state (full PID JSON diagnostics)
- Publishes diagnostic_msgs/DiagnosticArray on /diagnostics (OK/WARN/ERROR by state)
- Auto-reconnects on serial disconnect; IMU fault frames → ERROR diagnostic
- launch/bridge.launch.py with serial_port + baud_rate launch args
- config/bridge_params.yaml (921600 baud, /dev/ttyACM0)
- test/test_parse.py: 8 unit tests covering normal, fault, edge cases (all pass)
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
98 lines
3.1 KiB
Python
98 lines
3.1 KiB
Python
"""
|
|
Unit tests for STM32 telemetry parsing logic.
|
|
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
|
"""
|
|
|
|
import json
|
|
import pytest
|
|
|
|
|
|
# ── Minimal stub so we can test parsing without a ROS2 runtime ───────────────
|
|
|
|
def parse_frame(raw: bytes):
|
|
"""Mirror of the parsing logic in serial_bridge_node._parse_and_publish."""
|
|
text = raw.decode("ascii", errors="ignore").strip()
|
|
if not text:
|
|
return None
|
|
data = json.loads(text)
|
|
|
|
if "err" in data:
|
|
return {"type": "fault", "errno": data["err"]}
|
|
|
|
required = ("p", "r", "e", "ig", "m", "s", "y")
|
|
if not all(k in data for k in required):
|
|
raise ValueError(f"Incomplete frame: {data}")
|
|
|
|
return {
|
|
"type": "telemetry",
|
|
"pitch_deg": data["p"] / 10.0,
|
|
"roll_deg": data["r"] / 10.0,
|
|
"yaw_deg": data["y"] / 10.0,
|
|
"pid_error_deg": data["e"] / 10.0,
|
|
"integral": data["ig"] / 10.0,
|
|
"motor_cmd": int(data["m"]),
|
|
"state": int(data["s"]),
|
|
}
|
|
|
|
|
|
# ── Tests ─────────────────────────────────────────────────────────────────────
|
|
|
|
def test_normal_frame():
|
|
raw = b'{"p":125,"r":-30,"e":50,"ig":20,"m":350,"s":1,"y":0}\n'
|
|
result = parse_frame(raw)
|
|
assert result["type"] == "telemetry"
|
|
assert result["pitch_deg"] == pytest.approx(12.5)
|
|
assert result["roll_deg"] == pytest.approx(-3.0)
|
|
assert result["yaw_deg"] == pytest.approx(0.0)
|
|
assert result["pid_error_deg"] == pytest.approx(5.0)
|
|
assert result["integral"] == pytest.approx(2.0)
|
|
assert result["motor_cmd"] == 350
|
|
assert result["state"] == 1 # ARMED
|
|
|
|
|
|
def test_fault_frame():
|
|
raw = b'{"err":-1}\n'
|
|
result = parse_frame(raw)
|
|
assert result["type"] == "fault"
|
|
assert result["errno"] == -1
|
|
|
|
|
|
def test_zero_frame():
|
|
raw = b'{"p":0,"r":0,"e":0,"ig":0,"m":0,"s":0,"y":0}\n'
|
|
result = parse_frame(raw)
|
|
assert result["type"] == "telemetry"
|
|
assert result["pitch_deg"] == pytest.approx(0.0)
|
|
assert result["state"] == 0 # DISARMED
|
|
|
|
|
|
def test_tilt_fault_state():
|
|
raw = b'{"p":450,"r":0,"e":400,"ig":999,"m":1000,"s":2,"y":0}\n'
|
|
result = parse_frame(raw)
|
|
assert result["state"] == 2 # TILT_FAULT
|
|
assert result["pitch_deg"] == pytest.approx(45.0)
|
|
assert result["motor_cmd"] == 1000
|
|
|
|
|
|
def test_negative_motor_cmd():
|
|
raw = b'{"p":-100,"r":0,"e":-100,"ig":-50,"m":-750,"s":1,"y":10}\n'
|
|
result = parse_frame(raw)
|
|
assert result["motor_cmd"] == -750
|
|
assert result["pitch_deg"] == pytest.approx(-10.0)
|
|
|
|
|
|
def test_incomplete_frame_raises():
|
|
raw = b'{"p":100,"r":0}\n'
|
|
with pytest.raises(ValueError, match="Incomplete frame"):
|
|
parse_frame(raw)
|
|
|
|
|
|
def test_empty_line_returns_none():
|
|
assert parse_frame(b"\n") is None
|
|
assert parse_frame(b" \n") is None
|
|
|
|
|
|
def test_corrupt_frame_raises_json_error():
|
|
raw = b'not_json\n'
|
|
with pytest.raises(json.JSONDecodeError):
|
|
parse_frame(raw)
|