无人车 UDP Agent
Raspberry Pi 远程控制中间件 · STM32 串行协议 · JSON 命令接口
UDP 服务器
STM32 C30D
多线程运动
安全停止
JSON 命令
概述
本文档介绍 Raspberry Pi 上的 rover_agent.py 程序。该代理接收来自 PC 任务计算机的 UDP 命令，将其转换为 Wheeltec STM32 串行包格式，并通过 USB 串行发送至无人车电机控制器。
PC mission script
-> UDP JSON command
Raspberry Pi 4B rover_agent.py
-> USB serial packet
STM32 motor controller
-> motors and steering
这使 PC 端的任务脚本保持简单，只发送高层命令：
ping · stop · prime · move 网络设置
LISTEN_IP = "0.0.0.0"
LISTEN_PORT = 15000
使用
在 LQ3/以太网设置中，PC 任务脚本应发送命令到树莓派以太网 IP，如
0.0.0.0 表示代理通过树莓派的任何活跃网络接口（以太网或 Wi-Fi）接收命令。在 LQ3/以太网设置中，PC 任务脚本应发送命令到树莓派以太网 IP，如
192.168.1.102。 串口设置
MOTOR_PORT = "/dev/serial/by-id/usb-1a86_USB_Single_Serial_5C2C061781-if00"
BAUD = 115200
使用稳定的
/dev/serial/by-id/... 路径优于 /dev/ttyACM0，因为重启或 USB 重连后 ACM 设备编号可能互换。 STM32 包格式
电机控制器期望 11 字节数据包：
7B 00 00 XX XX 00 00 ZZ ZZ CHECKSUM 7D
| 字段 | 说明 |
|---|---|
7B | 帧头 |
00 00 | 保留字节 |
XX XX | X 速度，16位有符号，mm/s |
00 00 | Y 速度（此处未使用） |
ZZ ZZ | Z 速度/转向，16位有符号 |
CHECKSUM | 前 9 字节的 XOR 校验和 |
7D | 帧尾 |
x_speed 控制前进/后退：正 = 前进，负 = 后退，0 = 停止z_speed 控制转向：正/负 = 左/右转（取决于配置），0 = 直行
有符号 16 位转换
def signed16(value):
value = int(max(-1000, min(1000, value)))
if value < 0:
value = 65536 + value
return value
100 → 0x0064
300 → 0x012C
-100 → 0xFF9C
脚本将值限制在
300 → 0x012C
-100 → 0xFF9C
脚本将值限制在
-1000 到 1000 之间以确保安全。
停止行为
send_drive(0, 0)
X 速度 = 0，Z 速度 = 0
stop_rover() 发送中性包并打印 [ROVER] STOP Prime 行为
for _ in range(10):
send_drive(0, 0)
time.sleep(0.1)
for _ in range(5):
send_drive(120, 0)
time.sleep(0.1)
for _ in range(10):
send_drive(0, 0)
time.sleep(0.1)
警告 无人车可能在启动时轻微移动：120 mm/s 约 0.5 秒。
如果启动移动不可接受，不要自动调用
如果启动移动不可接受，不要自动调用
prime_rover()，改为仅在安全时从 PC 发送手动 prime 命令。 支持的 UDP 命令
Ping
{"cmd": "ping"}
响应：
{"ok": true, "status": "alive"} Stop
{"cmd": "stop"}
响应：
{"ok": true, "status": "stopped"} Prime
{"cmd": "prime"}
响应：
{"ok": true, "status": "primed"} Move
{"cmd": "move", "x": 200, "z": 0, "duration": 2}
含义：前进 200 mm/s，无转向，持续 2 秒，然后停止。
响应：
响应：
{"ok": true, "status": "moving", "x": 200, "z": 0, "duration": 2}
运动线程
def run_motion(x_speed, z_speed, duration):...
运动在后台线程中执行，每
0.10 秒重复发送运动包，确保 STM32 在运动期间持续接收新鲜命令。时长结束或收到停止命令时发送最终 STOP。
安全行为
脚本以
• 设置停止事件
• 向无人车发送 STOP
• 关闭串口
• 干净退出
Ctrl-C 退出或收到终止信号时执行 exit_handler()：• 设置停止事件
• 向无人车发送 STOP
• 关闭串口
• 干净退出
通过 防止脚本停止后无人车继续移动。
PC 测试命令
Windows PowerShell
# Ping
py -c "import socket,json; s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM); s.settimeout(2); s.sendto(json.dumps(dict(cmd='ping')).encode(),('192.168.1.102',15000)); print(s.recvfrom(4096))"
# Move
py -c "import socket,json; s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM); s.settimeout(2); s.sendto(json.dumps(dict(cmd='move',x=200,z=0,duration=2)).encode(),('192.168.1.102',15000)); print(s.recvfrom(4096))"
# Stop
py -c "import socket,json; s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM); s.settimeout(2); s.sendto(json.dumps(dict(cmd='stop')).encode(),('192.168.1.102',15000)); print(s.recvfrom(4096))"
完整脚本
#!/usr/bin/env python3
import socket
import json
import time
import signal
import sys
import serial
import threading
MOTOR_PORT = "/dev/serial/by-id/usb-1a86_USB_Single_Serial_5C2C061781-if00"
BAUD = 115200
LISTEN_IP = "0.0.0.0"
LISTEN_PORT = 15000
SEND_INTERVAL = 0.10
ser = None
motion_lock = threading.Lock()
motion_stop_event = threading.Event()
motion_thread = None
def signed16(value):
value = int(max(-1000, min(1000, value)))
if value < 0:
value = 65536 + value
return value
def make_drive_cmd(x_speed, z_speed):
x = signed16(x_speed)
z = signed16(z_speed)
packet = [
0x7B,
0x00,
0x00,
(x >> 8) & 0xFF,
x & 0xFF,
0x00,
0x00,
(z >> 8) & 0xFF,
z & 0xFF,
]
checksum = 0
for b in packet:
checksum ^= b
packet.append(checksum)
packet.append(0x7D)
return bytes(packet)
def send_drive(x_speed, z_speed):
data = make_drive_cmd(x_speed, z_speed)
ser.write(data)
ser.flush()
def stop_rover():
send_drive(0, 0)
print("[ROVER] STOP", flush=True)
def prime_rover():
print("[ROVER] Priming STM32 serial control...", flush=True)
for _ in range(10):
send_drive(0, 0)
time.sleep(0.1)
for _ in range(5):
send_drive(120, 0)
time.sleep(0.1)
for _ in range(10):
send_drive(0, 0)
time.sleep(0.1)
print("[ROVER] Prime complete", flush=True)
def run_motion(x_speed, z_speed, duration):
print("[ROVER] MOVE x=%d z=%d duration=%.1f" % (x_speed, z_speed, duration), flush=True)
end_time = time.time() + duration
while time.time() < end_time:
if motion_stop_event.is_set():
break
send_drive(x_speed, z_speed)
time.sleep(SEND_INTERVAL)
stop_rover()
def start_motion(x_speed, z_speed, duration):
global motion_thread
with motion_lock:
motion_stop_event.set()
if motion_thread and motion_thread.is_alive():
motion_thread.join(timeout=1)
motion_stop_event.clear()
motion_thread = threading.Thread(
target=run_motion,
args=(x_speed, z_speed, duration),
daemon=True
)
motion_thread.start()
def handle_command(cmd):
name = cmd.get("cmd")
if name == "stop":
motion_stop_event.set()
stop_rover()
return {"ok": True, "status": "stopped"}
if name == "prime":
motion_stop_event.set()
prime_rover()
return {"ok": True, "status": "primed"}
if name == "move":
x = int(cmd.get("x", 0))
z = int(cmd.get("z", 0))
duration = float(cmd.get("duration", 0))
if duration <= 0:
return {"ok": False, "error": "duration must be > 0"}
start_motion(x, z, duration)
return {"ok": True, "status": "moving", "x": x, "z": z, "duration": duration}
if name == "ping":
return {"ok": True, "status": "alive"}
return {"ok": False, "error": "unknown command"}
def exit_handler(sig=None, frame=None):
print("\n[EXIT] STOP", flush=True)
try:
motion_stop_event.set()
stop_rover()
except Exception:
pass
try:
if ser:
ser.close()
except Exception:
pass
sys.exit(0)
signal.signal(signal.SIGINT, exit_handler)
signal.signal(signal.SIGTERM, exit_handler)
print("[ROVER] Connecting STM32...", flush=True)
ser = serial.Serial(MOTOR_PORT, BAUD, timeout=1)
time.sleep(2)
print("[ROVER] STM32 OK", flush=True)
prime_rover()
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((LISTEN_IP, LISTEN_PORT))
print("[ROVER] UDP listening on %s:%d" % (LISTEN_IP, LISTEN_PORT), flush=True)
while True:
data, addr = sock.recvfrom(4096)
try:
cmd = json.loads(data.decode("utf-8"))
print("[ROVER] RX from", addr, cmd, flush=True)
reply = handle_command(cmd)
except Exception as exc:
reply = {"ok": False, "error": str(exc)}
try:
sock.sendto(json.dumps(reply).encode("utf-8"), addr)
except Exception:
pass
平台：Raspberry Pi 4B · STM32 C30D · Ubuntu 22.04 · Python 3 · UDP
(c) 卓博泰科技 · Rover UDP Agent 完整文档
(c) 卓博泰科技 · Rover UDP Agent 完整文档