sl-jetson
22aaeb02cf
STM32 firmware (C): - include/jetson_cmd.h: protocol constants (HB_TIMEOUT_MS=500, SPEED_MAX_DEG=4°), API for jetson_cmd_process/is_active/steer/sp_offset - src/jetson_cmd.c: main-loop parser for buffered C<spd>,<str> frames; setpoint offset = speed/1000 * 4°; steer clamped ±1000 - lib/USB_CDC/src/usbd_cdc_if.c: add H (heartbeat) and C (drive cmd) to CDC_Receive ISR — follows existing pattern: H updates jetson_hb_tick in ISR, C copied to jetson_cmd_buf for main-loop sscanf (avoids sscanf in IRQ) - src/main.c: integrate jetson_cmd — process buffered frame, apply setpoint offset around balance_update(), inject steer into motor_driver_update() only when heartbeat alive (fallback: steer=0, setpoint unchanged) ROS2 (Python): - saltybot_cmd_node.py: full bidirectional node — owns serial port, handles telemetry RX → topics AND /cmd_vel TX → C<spd>,<str>\n + H\n heartbeat 200ms timer; sends C0,0\n on shutdown; speed/steer_scale configurable - serial_bridge_node.py: add write_serial() helper for extensibility - launch/bridge.launch.py: mode arg (bidirectional|rx_only) selects node - config/bridge_params.yaml: heartbeat_period, speed_scale, steer_scale docs - test/test_cmd.py: 13 tests — zero, full fwd/rev, turn clamping, combined - setup.py: saltybot_cmd_node entry point All 21 tests pass (8 parse + 13 cmd). Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
100 lines
2.9 KiB
Python
100 lines
2.9 KiB
Python
"""
|
|
Unit tests for Jetson→STM32 command serialization logic.
|
|
Tests Twist→speed/steer conversion and frame formatting.
|
|
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
|
|
"""
|
|
|
|
import pytest
|
|
|
|
|
|
# ── Minimal stubs (no ROS2 runtime needed) ───────────────────────────────────
|
|
|
|
def _clamp(v, lo, hi):
|
|
return max(lo, min(hi, v))
|
|
|
|
|
|
def twist_to_frame(linear_x, angular_z, speed_scale=1000.0, steer_scale=-500.0):
|
|
"""Mirror of SaltybotCmdNode._cmd_vel_cb frame building."""
|
|
speed = int(_clamp(linear_x * speed_scale, -1000.0, 1000.0))
|
|
steer = int(_clamp(angular_z * steer_scale, -1000.0, 1000.0))
|
|
return f"C{speed},{steer}\n".encode("ascii"), speed, steer
|
|
|
|
|
|
# ── Frame format tests ────────────────────────────────────────────────────────
|
|
|
|
def test_zero_twist_produces_zero_cmd():
|
|
frame, speed, steer = twist_to_frame(0.0, 0.0)
|
|
assert frame == b"C0,0\n"
|
|
assert speed == 0
|
|
assert steer == 0
|
|
|
|
|
|
def test_full_forward():
|
|
frame, speed, steer = twist_to_frame(1.0, 0.0)
|
|
assert frame == b"C1000,0\n"
|
|
assert speed == 1000
|
|
|
|
|
|
def test_full_reverse():
|
|
frame, speed, steer = twist_to_frame(-1.0, 0.0)
|
|
assert frame == b"C-1000,0\n"
|
|
assert speed == -1000
|
|
|
|
|
|
def test_left_turn_positive_angular_z():
|
|
# Default steer_scale=-500: +angular.z → negative steer
|
|
frame, speed, steer = twist_to_frame(0.0, 1.0)
|
|
assert steer == -500
|
|
assert b"C0,-500\n" == frame
|
|
|
|
|
|
def test_right_turn_negative_angular_z():
|
|
frame, speed, steer = twist_to_frame(0.0, -1.0)
|
|
assert steer == 500
|
|
assert b"C0,500\n" == frame
|
|
|
|
|
|
def test_speed_clamped_at_max():
|
|
_, speed, _ = twist_to_frame(5.0, 0.0) # 5 m/s >> 1 m/s max
|
|
assert speed == 1000
|
|
|
|
|
|
def test_speed_clamped_at_min():
|
|
_, speed, _ = twist_to_frame(-5.0, 0.0)
|
|
assert speed == -1000
|
|
|
|
|
|
def test_steer_clamped_at_max():
|
|
# angular.z=-5 rad/s with steer_scale=-500 → +2500 → clamped to +1000
|
|
_, _, steer = twist_to_frame(0.0, -5.0)
|
|
assert steer == 1000
|
|
|
|
|
|
def test_steer_clamped_at_min():
|
|
_, _, steer = twist_to_frame(0.0, 5.0)
|
|
assert steer == -1000
|
|
|
|
|
|
def test_combined_motion():
|
|
frame, speed, steer = twist_to_frame(0.5, -0.4)
|
|
assert speed == 500
|
|
assert steer == int(_clamp(-0.4 * -500.0, -1000.0, 1000.0)) # +200
|
|
assert frame == b"C500,200\n"
|
|
|
|
|
|
def test_custom_scales():
|
|
# speed_scale=500 → 1 m/s = 500 ESC units
|
|
frame, speed, steer = twist_to_frame(1.0, 0.0, speed_scale=500.0, steer_scale=-250.0)
|
|
assert speed == 500
|
|
assert frame == b"C500,0\n"
|
|
|
|
|
|
def test_heartbeat_frame():
|
|
assert b"H\n" == b"H\n" # constant — just verifies expected bytes
|
|
|
|
|
|
def test_zero_cmd_frame():
|
|
"""C0,0\\n must be sent on shutdown."""
|
|
frame, _, _ = twist_to_frame(0.0, 0.0)
|
|
assert frame == b"C0,0\n"
|