sl-jetson
30be0d8cd3
Add stm32_protocol.py — pure-Python binary frame codec: - Frame: STX(0x02)+TYPE+LEN+PAYLOAD+CRC16-CCITT(BE)+ETX(0x03) - CRC covers TYPE+LEN+PAYLOAD; polynomial 0x1021, init 0xFFFF - Encoders: HEARTBEAT, SPEED_STEER(-1000..+1000 int16), ARM, SET_MODE, PID_UPDATE - Telemetry decoders: ImuFrame, BatteryFrame, MotorRpmFrame, ArmStateFrame, ErrorFrame - FrameParser: streaming byte-by-byte state machine, resync on corrupt data Add stm32_cmd_node.py — ROS2 bidirectional bridge node: - /cmd_vel → SPEED_STEER at up to 50 Hz - HEARTBEAT timer (default 200ms); STM32 watchdog fires at 500ms - Jetson-side watchdog: no /cmd_vel for 500ms → send SPEED_STEER(0,0) - /saltybot/arm service (SetBool) → ARM frame - /saltybot/pid_update topic → PID_UPDATE frame - Telemetry RX: IMU→/saltybot/imu, BATTERY→/saltybot/telemetry/battery, MOTOR_RPM→/saltybot/telemetry/motor_rpm, ARM_STATE→/saltybot/arm_state, ERROR→/saltybot/error - Auto-reconnect on USB disconnect (serial.SerialException caught) - Zero-speed + disarm sent on node shutdown - /diagnostics with serial health, frame counts, last cmd age Add test_stm32_protocol.py (60+ tests): - CRC16-CCITT correctness, known test vectors - All encoder output structures and payload values - FrameParser: all telemetry types, bad CRC, bad ETX, resync, streaming, oversized payload, frame counters, reset Add test_stm32_cmd_node.py (30+ tests): - MockSerial: TX/RX byte-level testing without hardware - Speed/steer clamping, scaling, frame structure - Watchdog fires/doesn't fire relative to timeout - CRC error counted, resync after garbage Add stm32_cmd_params.yaml, stm32_cmd.launch.py. Update package.xml (add std_srvs, geometry_msgs deps). Update setup.py (add stm32_cmd_node entry point + new config/launch). Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
53 lines
2.1 KiB
Python
53 lines
2.1 KiB
Python
"""stm32_cmd.launch.py — Launch the binary-framed STM32 command node (Issue #119).
|
|
|
|
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
|
|
"""
|
|
|
|
import os
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def generate_launch_description() -> LaunchDescription:
|
|
pkg = get_package_share_directory("saltybot_bridge")
|
|
params_file = os.path.join(pkg, "config", "stm32_cmd_params.yaml")
|
|
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
|
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
|
DeclareLaunchArgument("speed_scale", default_value="1000.0"),
|
|
DeclareLaunchArgument("steer_scale", default_value="-500.0"),
|
|
DeclareLaunchArgument("watchdog_timeout", default_value="0.5"),
|
|
DeclareLaunchArgument("heartbeat_period", default_value="0.2"),
|
|
|
|
Node(
|
|
package="saltybot_bridge",
|
|
executable="stm32_cmd_node",
|
|
name="stm32_cmd_node",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
parameters=[
|
|
params_file,
|
|
{
|
|
"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"),
|
|
},
|
|
],
|
|
),
|
|
])
|