feat: Jetson→STM32 command protocol — /cmd_vel to serial (Phase 2)
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>
This commit is contained in:
parent
76dc03db1b
commit
22aaeb02cf
76
include/jetson_cmd.h
Normal file
76
include/jetson_cmd.h
Normal file
@ -0,0 +1,76 @@
|
||||
#ifndef JETSON_CMD_H
|
||||
#define JETSON_CMD_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* Jetson→STM32 command protocol over USB CDC (bidirectional, same /dev/ttyACM0)
|
||||
*
|
||||
* Commands (newline-terminated ASCII, sent by Jetson):
|
||||
* H\n — heartbeat (every 200ms). Must arrive within 500ms or
|
||||
* jetson_cmd_is_active() returns false → steer reverts to 0.
|
||||
* C<spd>,<str>\n — drive command: speed -1000..+1000, steer -1000..+1000.
|
||||
* Also refreshes the heartbeat timer.
|
||||
*
|
||||
* Speed→setpoint:
|
||||
* Speed is converted to a setpoint offset (degrees) before calling balance_update().
|
||||
* Positive speed → forward tilt → robot moves forward.
|
||||
* Max offset is ±JETSON_SPEED_MAX_DEG (see below).
|
||||
*
|
||||
* Steer:
|
||||
* Passed directly to motor_driver_update() as steer_cmd.
|
||||
* Motor driver ramps and clamps with balance headroom (see motor_driver.h).
|
||||
*
|
||||
* Integration pattern in main.c (after the cdc_cmd_ready block):
|
||||
*
|
||||
* // Process buffered C command (parsed here, not in ISR)
|
||||
* if (jetson_cmd_ready) { jetson_cmd_ready = 0; jetson_cmd_process(); }
|
||||
*
|
||||
* // Apply setpoint offset and steer when active
|
||||
* float base_sp = bal.setpoint;
|
||||
* if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
|
||||
* balance_update(&bal, &imu, dt);
|
||||
* bal.setpoint = base_sp;
|
||||
*
|
||||
* // Steer injection in 50Hz ESC block
|
||||
* int16_t jsteer = jetson_cmd_is_active(now) ? jetson_cmd_steer() : 0;
|
||||
* motor_driver_update(&motors, bal.motor_cmd, jsteer, now);
|
||||
*/
|
||||
|
||||
/* Heartbeat timeout: if no H or C within this window, commands deactivate */
|
||||
#define JETSON_HB_TIMEOUT_MS 500
|
||||
|
||||
/* Max setpoint offset from Jetson speed command (speed=1000 → +N degrees tilt) */
|
||||
#define JETSON_SPEED_MAX_DEG 4.0f /* ±4° → enough for ~0.5 m/s */
|
||||
|
||||
/*
|
||||
* jetson_cmd_process()
|
||||
* Call from main loop (NOT ISR) when jetson_cmd_ready is set.
|
||||
* Parses jetson_cmd_buf (the C<spd>,<str> frame) with sscanf.
|
||||
*/
|
||||
void jetson_cmd_process(void);
|
||||
|
||||
/*
|
||||
* jetson_cmd_is_active(now)
|
||||
* Returns true if a heartbeat (H or C command) arrived within JETSON_HB_TIMEOUT_MS.
|
||||
* If false, main loop should fall back to RC or zero steer.
|
||||
*/
|
||||
bool jetson_cmd_is_active(uint32_t now_ms);
|
||||
|
||||
/* Current steer command after latest C frame, clamped to ±1000 */
|
||||
int16_t jetson_cmd_steer(void);
|
||||
|
||||
/* Setpoint offset (degrees) derived from latest speed command. */
|
||||
float jetson_cmd_sp_offset(void);
|
||||
|
||||
/*
|
||||
* Externals — declared here, defined in usbd_cdc_if.c alongside the other
|
||||
* CDC volatile flags (cdc_streaming, cdc_arm_request, etc.).
|
||||
* Main loop checks jetson_cmd_ready; ISR sets it.
|
||||
*/
|
||||
extern volatile uint8_t jetson_cmd_ready; /* set by ISR on C frame */
|
||||
extern volatile char jetson_cmd_buf[32]; /* C<spd>,<str>\0 from ISR */
|
||||
extern volatile uint32_t jetson_hb_tick; /* HAL_GetTick() of last H or C */
|
||||
|
||||
#endif /* JETSON_CMD_H */
|
||||
@ -1,8 +1,25 @@
|
||||
stm32_serial_bridge:
|
||||
ros__parameters:
|
||||
# STM32 USB CDC device node
|
||||
# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied
|
||||
serial_port: /dev/ttyACM0
|
||||
baud_rate: 921600
|
||||
timeout: 0.1 # serial readline timeout (seconds)
|
||||
reconnect_delay: 2.0 # seconds between reconnect attempts on disconnect
|
||||
# saltybot_bridge parameters
|
||||
# Used by both serial_bridge_node (RX-only) and saltybot_cmd_node (bidirectional)
|
||||
|
||||
# ── Serial ─────────────────────────────────────────────────────────────────────
|
||||
# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied.
|
||||
serial_port: /dev/ttyACM0
|
||||
baud_rate: 921600
|
||||
timeout: 0.05 # serial readline timeout (seconds)
|
||||
reconnect_delay: 2.0 # seconds between reconnect attempts on serial disconnect
|
||||
|
||||
# ── saltybot_cmd_node (bidirectional) only ─────────────────────────────────────
|
||||
|
||||
# Heartbeat: H\n sent every heartbeat_period seconds.
|
||||
# STM32 reverts steer to 0 after JETSON_HB_TIMEOUT_MS (500ms) without heartbeat.
|
||||
heartbeat_period: 0.2 # seconds (= 200ms)
|
||||
|
||||
# Twist → ESC command 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]
|
||||
#
|
||||
# Tune speed_scale for max desired forward speed (1 m/s → 1000 ESC units at default).
|
||||
# steer_scale is negative because ROS2 +angular.z = CCW but ESC positive steer
|
||||
# may mean right-turn — verify on hardware and flip sign if needed.
|
||||
speed_scale: 1000.0
|
||||
steer_scale: -500.0
|
||||
|
||||
@ -1,34 +1,75 @@
|
||||
"""
|
||||
saltybot_bridge launch file.
|
||||
|
||||
Two deployment modes:
|
||||
|
||||
1. Full bidirectional (recommended for Nav2):
|
||||
ros2 launch saltybot_bridge bridge.launch.py mode:=bidirectional
|
||||
Starts saltybot_cmd_node — owns serial port, handles both RX telemetry
|
||||
and TX /cmd_vel → STM32 commands + heartbeat.
|
||||
|
||||
2. RX-only (telemetry monitor, no drive commands):
|
||||
ros2 launch saltybot_bridge bridge.launch.py mode:=rx_only
|
||||
Starts serial_bridge_node — telemetry RX only. Use when you want to
|
||||
observe telemetry without commanding the robot.
|
||||
|
||||
Note: never run both nodes simultaneously on the same serial port.
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/ttyACM0",
|
||||
description="STM32 USB CDC device node",
|
||||
)
|
||||
baud_rate_arg = DeclareLaunchArgument(
|
||||
"baud_rate",
|
||||
default_value="921600",
|
||||
description="Serial baud rate",
|
||||
)
|
||||
def _launch_nodes(context, *args, **kwargs):
|
||||
mode = LaunchConfiguration("mode").perform(context)
|
||||
port = LaunchConfiguration("serial_port").perform(context)
|
||||
baud = LaunchConfiguration("baud_rate").perform(context)
|
||||
spd_scale = LaunchConfiguration("speed_scale").perform(context)
|
||||
str_scale = LaunchConfiguration("steer_scale").perform(context)
|
||||
|
||||
bridge_node = Node(
|
||||
params = {
|
||||
"serial_port": port,
|
||||
"baud_rate": int(baud),
|
||||
"timeout": 0.05,
|
||||
"reconnect_delay": 2.0,
|
||||
}
|
||||
|
||||
if mode == "rx_only":
|
||||
return [Node(
|
||||
package="saltybot_bridge",
|
||||
executable="serial_bridge_node",
|
||||
name="stm32_serial_bridge",
|
||||
output="screen",
|
||||
parameters=[params],
|
||||
)]
|
||||
|
||||
# bidirectional (default)
|
||||
params.update({
|
||||
"heartbeat_period": 0.2,
|
||||
"speed_scale": float(spd_scale),
|
||||
"steer_scale": float(str_scale),
|
||||
})
|
||||
return [Node(
|
||||
package="saltybot_bridge",
|
||||
executable="serial_bridge_node",
|
||||
name="stm32_serial_bridge",
|
||||
executable="saltybot_cmd_node",
|
||||
name="saltybot_cmd",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"serial_port": LaunchConfiguration("serial_port"),
|
||||
"baud_rate": LaunchConfiguration("baud_rate"),
|
||||
"timeout": 0.1,
|
||||
"reconnect_delay": 2.0,
|
||||
}
|
||||
],
|
||||
)
|
||||
parameters=[params],
|
||||
)]
|
||||
|
||||
return LaunchDescription([serial_port_arg, baud_rate_arg, bridge_node])
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument("mode", default_value="bidirectional",
|
||||
description="bidirectional | rx_only"),
|
||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0",
|
||||
description="STM32 USB CDC device node"),
|
||||
DeclareLaunchArgument("baud_rate", default_value="921600"),
|
||||
DeclareLaunchArgument("speed_scale", default_value="1000.0",
|
||||
description="m/s → ESC units (linear.x scale)"),
|
||||
DeclareLaunchArgument("steer_scale", default_value="-500.0",
|
||||
description="rad/s → ESC units (angular.z scale, neg=flip)"),
|
||||
OpaqueFunction(function=_launch_nodes),
|
||||
])
|
||||
|
||||
@ -0,0 +1,344 @@
|
||||
"""
|
||||
saltybot_cmd_node — full bidirectional STM32↔Jetson bridge
|
||||
Combines telemetry RX (from serial_bridge_node) with drive command TX.
|
||||
|
||||
Owns /dev/ttyACM0 exclusively — do NOT run alongside serial_bridge_node.
|
||||
|
||||
RX path (50Hz from STM32):
|
||||
JSON telemetry → /saltybot/imu, /saltybot/balance_state, /diagnostics
|
||||
|
||||
TX path:
|
||||
/cmd_vel (geometry_msgs/Twist) → C<speed>,<steer>\\n → STM32
|
||||
Heartbeat timer (200ms) → H\\n → STM32
|
||||
|
||||
Protocol:
|
||||
H\\n — heartbeat. STM32 reverts steer to 0 if gap > 500ms.
|
||||
C<spd>,<str>\\n — drive command. speed/steer: -1000..+1000 integers.
|
||||
C command also refreshes STM32 heartbeat timer.
|
||||
|
||||
Twist mapping (configurable via ROS2 params):
|
||||
speed = clamp(linear.x * speed_scale, -1000, 1000)
|
||||
steer = clamp(angular.z * steer_scale, -1000, 1000)
|
||||
Default scales: speed_scale=1000.0 (1 m/s → 1000), steer_scale=-500.0
|
||||
Negative steer_scale because ROS2 +angular.z = CCW but ESC steer convention
|
||||
may differ — tune in config/bridge_params.yaml.
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import threading
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
import serial
|
||||
|
||||
from geometry_msgs.msg import Twist
|
||||
from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import String
|
||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
|
||||
_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
||||
IMU_FRAME_ID = "imu_link"
|
||||
|
||||
|
||||
def _clamp(v: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, v))
|
||||
|
||||
|
||||
class SaltybotCmdNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__("saltybot_cmd")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
||||
self.declare_parameter("baud_rate", 921600)
|
||||
self.declare_parameter("timeout", 0.05)
|
||||
self.declare_parameter("reconnect_delay", 2.0)
|
||||
self.declare_parameter("heartbeat_period", 0.2) # seconds
|
||||
self.declare_parameter("speed_scale", 1000.0) # m/s → ESC units
|
||||
self.declare_parameter("steer_scale", -500.0) # rad/s → ESC units
|
||||
|
||||
port = self.get_parameter("serial_port").value
|
||||
baud = self.get_parameter("baud_rate").value
|
||||
timeout = self.get_parameter("timeout").value
|
||||
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
||||
self._hb_period = self.get_parameter("heartbeat_period").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)
|
||||
reliable_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST, depth=10)
|
||||
|
||||
# ── Publishers (telemetry RX path) ────────────────────────────────────
|
||||
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos)
|
||||
self._balance_pub = self.create_publisher(String, "/saltybot/balance_state", reliable_qos)
|
||||
self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", reliable_qos)
|
||||
|
||||
# ── Subscriber (cmd TX path) ──────────────────────────────────────────
|
||||
self._cmd_vel_sub = self.create_subscription(
|
||||
Twist, "/cmd_vel", self._cmd_vel_cb, reliable_qos)
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────────
|
||||
self._port_name = port
|
||||
self._baud = baud
|
||||
self._timeout = timeout
|
||||
self._ser: serial.Serial | None = None
|
||||
self._ser_lock = threading.Lock() # guards _ser for RX/TX threads
|
||||
self._frame_count = 0
|
||||
self._error_count = 0
|
||||
self._last_state = -1
|
||||
self._last_speed = 0
|
||||
self._last_steer = 0
|
||||
|
||||
# ── Open serial ───────────────────────────────────────────────────────
|
||||
self._open_serial()
|
||||
|
||||
# ── Timers ────────────────────────────────────────────────────────────
|
||||
# Telemetry read at 100Hz (STM32 sends at 50Hz)
|
||||
self._read_timer = self.create_timer(0.01, self._read_cb)
|
||||
# Heartbeat TX at configured period (default 200ms)
|
||||
self._hb_timer = self.create_timer(self._hb_period, self._heartbeat_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f"saltybot_cmd started — {port} @ {baud} baud "
|
||||
f"(HB {int(self._hb_period*1000)}ms, "
|
||||
f"speed_scale={self._speed_scale}, steer_scale={self._steer_scale})"
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────────
|
||||
|
||||
def _open_serial(self) -> bool:
|
||||
with self._ser_lock:
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
port=self._port_name,
|
||||
baudrate=self._baud,
|
||||
timeout=self._timeout,
|
||||
)
|
||||
self._ser.reset_input_buffer()
|
||||
self.get_logger().info(f"Serial open: {self._port_name}")
|
||||
return True
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Cannot open {self._port_name}: {exc}")
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
def _close_serial(self):
|
||||
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 not open."""
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
return False
|
||||
try:
|
||||
self._ser.write(data)
|
||||
return True
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial write error: {exc}")
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
# ── RX — telemetry read ───────────────────────────────────────────────────
|
||||
|
||||
def _read_cb(self):
|
||||
with self._ser_lock:
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
pass
|
||||
else:
|
||||
try:
|
||||
lines = []
|
||||
while self._ser.in_waiting:
|
||||
raw = self._ser.readline()
|
||||
if raw:
|
||||
lines.append(raw)
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial read error: {exc}")
|
||||
self._ser = None
|
||||
lines = []
|
||||
|
||||
# Parse outside the lock
|
||||
for raw in lines:
|
||||
self._parse_and_publish(raw)
|
||||
|
||||
# Reconnect if port lost
|
||||
with self._ser_lock:
|
||||
if self._ser is None:
|
||||
self.get_logger().warn(
|
||||
"Serial lost — reconnecting…",
|
||||
throttle_duration_sec=self._reconnect_delay,
|
||||
)
|
||||
if self._ser is None:
|
||||
self._open_serial()
|
||||
|
||||
def _parse_and_publish(self, raw: bytes):
|
||||
try:
|
||||
text = raw.decode("ascii", errors="ignore").strip()
|
||||
if not text:
|
||||
return
|
||||
data = json.loads(text)
|
||||
except (ValueError, UnicodeDecodeError) as exc:
|
||||
self.get_logger().debug(f"Parse error ({exc}): {raw!r}")
|
||||
self._error_count += 1
|
||||
return
|
||||
|
||||
now = self.get_clock().now().to_msg()
|
||||
|
||||
if "err" in data:
|
||||
self._publish_imu_fault(data["err"], now)
|
||||
return
|
||||
|
||||
required = ("p", "r", "e", "ig", "m", "s", "y")
|
||||
if not all(k in data for k in required):
|
||||
self.get_logger().debug(f"Incomplete frame: {text}")
|
||||
return
|
||||
|
||||
pitch_deg = data["p"] / 10.0
|
||||
roll_deg = data["r"] / 10.0
|
||||
yaw_deg = data["y"] / 10.0
|
||||
error_deg = data["e"] / 10.0
|
||||
integral = data["ig"] / 10.0
|
||||
motor_cmd = float(data["m"])
|
||||
state = int(data["s"])
|
||||
self._frame_count += 1
|
||||
|
||||
self._publish_imu(pitch_deg, roll_deg, yaw_deg, now)
|
||||
self._publish_balance_state(
|
||||
pitch_deg, roll_deg, yaw_deg,
|
||||
error_deg, integral, motor_cmd, state, now,
|
||||
)
|
||||
if state != self._last_state:
|
||||
self.get_logger().info(
|
||||
f"Balance state → {_STATE_LABEL.get(state, f'UNKNOWN({state})')}"
|
||||
)
|
||||
self._last_state = state
|
||||
|
||||
def _publish_imu(self, pitch_deg, roll_deg, yaw_deg, stamp):
|
||||
msg = Imu()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = IMU_FRAME_ID
|
||||
msg.orientation_covariance[0] = -1.0
|
||||
msg.angular_velocity.x = math.radians(pitch_deg)
|
||||
msg.angular_velocity.y = math.radians(roll_deg)
|
||||
msg.angular_velocity.z = math.radians(yaw_deg)
|
||||
cov = math.radians(0.5) ** 2
|
||||
msg.angular_velocity_covariance[0] = cov
|
||||
msg.angular_velocity_covariance[4] = cov
|
||||
msg.angular_velocity_covariance[8] = cov
|
||||
msg.linear_acceleration_covariance[0] = -1.0
|
||||
self._imu_pub.publish(msg)
|
||||
|
||||
def _publish_balance_state(
|
||||
self, pitch, roll, yaw, error, integral, motor_cmd, state, stamp
|
||||
):
|
||||
state_label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
|
||||
payload = {
|
||||
"stamp": f"{stamp.sec}.{stamp.nanosec:09d}",
|
||||
"state": state_label,
|
||||
"pitch_deg": round(pitch, 1),
|
||||
"roll_deg": round(roll, 1),
|
||||
"yaw_deg": round(yaw, 1),
|
||||
"pid_error_deg":round(error, 1),
|
||||
"integral": round(integral, 1),
|
||||
"motor_cmd": int(motor_cmd),
|
||||
"jetson_speed": self._last_speed,
|
||||
"jetson_steer": self._last_steer,
|
||||
"frames": self._frame_count,
|
||||
"parse_errors": self._error_count,
|
||||
}
|
||||
str_msg = String()
|
||||
str_msg.data = json.dumps(payload)
|
||||
self._balance_pub.publish(str_msg)
|
||||
|
||||
diag = DiagnosticArray()
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/balance_controller"
|
||||
status.hardware_id = "stm32f722"
|
||||
status.message = state_label
|
||||
if state == 1:
|
||||
status.level = DiagnosticStatus.OK
|
||||
elif state == 0:
|
||||
status.level = DiagnosticStatus.WARN
|
||||
else:
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.values = [
|
||||
KeyValue(key="pitch_deg", value=f"{pitch:.1f}"),
|
||||
KeyValue(key="roll_deg", value=f"{roll:.1f}"),
|
||||
KeyValue(key="yaw_deg", value=f"{yaw:.1f}"),
|
||||
KeyValue(key="pid_error_deg", value=f"{error:.1f}"),
|
||||
KeyValue(key="integral", value=f"{integral:.1f}"),
|
||||
KeyValue(key="motor_cmd", value=f"{int(motor_cmd)}"),
|
||||
KeyValue(key="jetson_speed", value=str(self._last_speed)),
|
||||
KeyValue(key="jetson_steer", value=str(self._last_steer)),
|
||||
KeyValue(key="state", value=state_label),
|
||||
]
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
|
||||
def _publish_imu_fault(self, errno: int, stamp):
|
||||
diag = DiagnosticArray()
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
status.hardware_id = "stm32f722"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"STM32 IMU fault: errno={errno}")
|
||||
|
||||
# ── TX — command send ─────────────────────────────────────────────────────
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist):
|
||||
"""Convert Twist to C<speed>,<steer>\\n and send over serial."""
|
||||
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
|
||||
frame = f"C{speed},{steer}\n".encode("ascii")
|
||||
if not self._write(frame):
|
||||
self.get_logger().warn(
|
||||
"Cannot send cmd — serial not open",
|
||||
throttle_duration_sec=2.0,
|
||||
)
|
||||
|
||||
def _heartbeat_cb(self):
|
||||
"""Send H\\n heartbeat. STM32 reverts steer to 0 if gap > 500ms."""
|
||||
self._write(b"H\n")
|
||||
|
||||
# ── Lifecycle ─────────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self):
|
||||
# Send zero command on shutdown so robot doesn't run away
|
||||
self._write(b"C0,0\n")
|
||||
self._close_serial()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = SaltybotCmdNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -115,6 +115,22 @@ class SerialBridgeNode(Node):
|
||||
pass
|
||||
self._ser = None
|
||||
|
||||
def write_serial(self, data: bytes) -> bool:
|
||||
"""
|
||||
Send raw bytes to STM32 over the open serial port.
|
||||
Returns False if port is not open (caller should handle gracefully).
|
||||
Note: for bidirectional use prefer saltybot_cmd_node which owns TX natively.
|
||||
"""
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
return False
|
||||
try:
|
||||
self._ser.write(data)
|
||||
return True
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial write error: {exc}")
|
||||
self._close_serial()
|
||||
return False
|
||||
|
||||
# ── Read callback ─────────────────────────────────────────────────────────
|
||||
|
||||
def _read_cb(self):
|
||||
|
||||
@ -21,7 +21,10 @@ setup(
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
# RX-only telemetry bridge (does not send commands)
|
||||
"serial_bridge_node = saltybot_bridge.serial_bridge_node:main",
|
||||
# Full bidirectional bridge: telemetry RX + /cmd_vel TX + heartbeat
|
||||
"saltybot_cmd_node = saltybot_bridge.saltybot_cmd_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
|
||||
99
jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
Normal file
99
jetson/ros2_ws/src/saltybot_bridge/test/test_cmd.py
Normal file
@ -0,0 +1,99 @@
|
||||
"""
|
||||
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"
|
||||
@ -16,6 +16,16 @@ volatile uint8_t cdc_disarm_request = 0; /* set by D command */
|
||||
volatile uint8_t cdc_cmd_ready = 0;
|
||||
volatile char cdc_cmd_buf[32];
|
||||
|
||||
/*
|
||||
* Jetson command buffer (bidirectional protocol).
|
||||
* 'H'\n — heartbeat, ISR updates jetson_hb_tick only (no buf copy needed).
|
||||
* 'C'<s>,<t>\n — drive command: ISR copies to buf, main loop parses with sscanf.
|
||||
* jetson_hb_tick is also refreshed on every C command.
|
||||
*/
|
||||
volatile uint8_t jetson_cmd_ready = 0;
|
||||
volatile char jetson_cmd_buf[32];
|
||||
volatile uint32_t jetson_hb_tick = 0; /* HAL_GetTick() of last H or C */
|
||||
|
||||
/*
|
||||
* USB TX/RX buffers grouped into a single 512-byte aligned struct so that
|
||||
* one MPU region (configured in usbd_conf.c) can mark them non-cacheable.
|
||||
@ -141,6 +151,23 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
|
||||
cdc_cmd_ready = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
/* Jetson heartbeat — just refresh the tick, no buffer copy needed */
|
||||
case 'H':
|
||||
jetson_hb_tick = HAL_GetTick();
|
||||
break;
|
||||
|
||||
/* Jetson drive command: C<speed>,<steer>\n
|
||||
* Copy to buffer; main loop parses ints (keeps sscanf out of ISR). */
|
||||
case 'C': {
|
||||
uint32_t copy_len = *len < 31 ? *len : 31;
|
||||
for (uint32_t i = 0; i < copy_len; i++) jetson_cmd_buf[i] = (char)buf[i];
|
||||
jetson_cmd_buf[copy_len] = '\0';
|
||||
jetson_hb_tick = HAL_GetTick(); /* C command also refreshes heartbeat */
|
||||
jetson_cmd_ready = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
default: break;
|
||||
}
|
||||
|
||||
|
||||
55
src/jetson_cmd.c
Normal file
55
src/jetson_cmd.c
Normal file
@ -0,0 +1,55 @@
|
||||
#include "jetson_cmd.h"
|
||||
#include <stdio.h>
|
||||
|
||||
/*
|
||||
* Parsed drive state — updated by jetson_cmd_process() in the main loop.
|
||||
* Raw fields are ints parsed from "C<speed>,<steer>\n".
|
||||
*/
|
||||
static volatile int16_t jcmd_speed = 0; /* -1000..+1000 */
|
||||
static volatile int16_t jcmd_steer = 0; /* -1000..+1000 */
|
||||
|
||||
/* Clamp helper (avoids including math.h) */
|
||||
static int16_t clamp16(int v, int lo, int hi) {
|
||||
if (v < lo) return (int16_t)lo;
|
||||
if (v > hi) return (int16_t)hi;
|
||||
return (int16_t)v;
|
||||
}
|
||||
|
||||
/*
|
||||
* Called from main loop when jetson_cmd_ready is set.
|
||||
* Parses jetson_cmd_buf — safe to use sscanf here (not in ISR).
|
||||
* The ISR only copies bytes and sets the ready flag.
|
||||
*/
|
||||
void jetson_cmd_process(void) {
|
||||
int speed = 0, steer = 0;
|
||||
/* buf format: "C<speed>,<steer>" — skip leading 'C' */
|
||||
if (sscanf((const char *)jetson_cmd_buf + 1, "%d,%d", &speed, &steer) == 2) {
|
||||
jcmd_speed = clamp16(speed, -1000, 1000);
|
||||
jcmd_steer = clamp16(steer, -1000, 1000);
|
||||
}
|
||||
/* If parse fails, keep previous values — don't zero-out mid-motion */
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns true if the last heartbeat (H or C command) arrived within
|
||||
* JETSON_HB_TIMEOUT_MS. jetson_hb_tick is updated in the ISR.
|
||||
*/
|
||||
bool jetson_cmd_is_active(uint32_t now_ms) {
|
||||
/* jetson_hb_tick == 0 means we've never received any command */
|
||||
if (jetson_hb_tick == 0) return false;
|
||||
return (now_ms - jetson_hb_tick) < JETSON_HB_TIMEOUT_MS;
|
||||
}
|
||||
|
||||
/* Steer command for motor_driver_update() */
|
||||
int16_t jetson_cmd_steer(void) {
|
||||
return jcmd_steer;
|
||||
}
|
||||
|
||||
/*
|
||||
* Convert speed command to balance setpoint offset (degrees).
|
||||
* Positive speed → lean forward → robot moves forward.
|
||||
* Scaled linearly: speed=1000 → +JETSON_SPEED_MAX_DEG degrees.
|
||||
*/
|
||||
float jetson_cmd_sp_offset(void) {
|
||||
return ((float)jcmd_speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user