ros-zdt-x57s-single-motor：用 ROS 2 控制一台 ZDT X57Sapp/bricks/ros_gateway/gateway.py
# SPDX-License-Identifier: MIT
import json
import os
import queue
import threading
import time
from arduino.app_utils import brick
from websockets.exceptions import ConnectionClosed
from websockets.sync.server import serve
from .protocol import (
ProtocolError,
build_message,
decode_message,
validate_cmd_vel,
validate_heartbeat,
validate_hello,
validate_motor_enable,
validate_motor_set_speed,
validate_motor_stop,
validate_mode_change,
validate_sequence,
)
@brick
class RosGateway:
"""App Lab 与原生 ROS 2 之间的 WebSocket 网关。"""
def __init__(
self,
host=None,
port=None,
path=None,
node_name=None,
max_vx=None,
max_vy=None,
max_wz=None,
max_motor_rpm=None,
command_timeout_ms=None,
heartbeat_timeout_ms=None,
outbound_queue_size=32,
):
"""
@description : 创建 ROS Gateway 并读取 App Lab 注入的配置变量
@param host : 容器内监听地址，None 时读取环境变量
@param port : WebSocket 监听端口，None 时读取环境变量
@param path : WebSocket 请求路径，None 时读取环境变量
@param node_name : 握手响应中的App节点名称
@param max_vx : vx 绝对值上限
@param max_vy : vy 绝对值上限
@param max_wz : wz 绝对值上限
@param max_motor_rpm : 单电机转速绝对值上限，单位RPM
@param command_timeout_ms : 速度指令超时毫秒数
@param heartbeat_timeout_ms : 连接心跳超时毫秒数
@param outbound_queue_size : 有界出站队列长度
@return : 无返回值
"""
self._host = host or os.getenv("ROS_GATEWAY_HOST", "0.0.0.0")
self._port = self._read_int(port, "ROS_GATEWAY_PORT", 8765, 1, 65535)
self._path = path or os.getenv("ROS_GATEWAY_PATH", "/ros")
self._node_name = node_name or os.getenv(
"ROS_GATEWAY_NODE_NAME",
"arduino-app",
)
self._limits = {
"vx": self._read_float(max_vx, "ROS_GATEWAY_MAX_VX", 0.8, 0.0),
"vy": self._read_float(max_vy, "ROS_GATEWAY_MAX_VY", 0.8, 0.0),
"wz": self._read_float(max_wz, "ROS_GATEWAY_MAX_WZ", 1.5, 0.0),
}
self._max_motor_rpm = self._read_int(
max_motor_rpm,
"ROS_GATEWAY_MAX_MOTOR_RPM",
60,
1,
3000,
)
self._command_timeout_ms = self._read_int(
command_timeout_ms,
"ROS_GATEWAY_COMMAND_TIMEOUT_MS",
300,
1,
60_000,
)
self._heartbeat_timeout_ms = self._read_int(
heartbeat_timeout_ms,
"ROS_GATEWAY_HEARTBEAT_TIMEOUT_MS",
3000,
self._command_timeout_ms,
120_000,
)
self._outbound = queue.Queue(maxsize=max(1, int(outbound_queue_size)))
self._state_lock = threading.RLock()
self._stop_event = threading.Event()
self._ready_event = threading.Event()
self._server = None
self._server_thread = None
self._active_connection = None
self._connected = False
self._client_node = None
self._mode = "IDLE"
self._last_sequence = -1
self._outbound_sequence = 0
self._last_rx_monotonic = 0.0
self._last_cmd_monotonic = 0.0
self._last_server_heartbeat = 0.0
self._watchdog_triggered = True
self._server_error = None
self._dropped_messages = 0
self._cmd_vel_callback = None
self._motor_enable_callback = None
self._motor_speed_callback = None
self._motor_stop_callback = None
self._mode_change_callback = None
self._stop_callback = None
def start(self):
"""
@description : 启动非阻塞 WebSocket 服务线程
@param : 无参数
@return : 无返回值
"""
if self._server_thread and self._server_thread.is_alive():
return
self._stop_event.clear()
self._ready_event.clear()
self._server_thread = threading.Thread(
target=self._run_server,
name="ros-gateway-server",
daemon=True,
)
self._server_thread.start()
if not self._ready_event.wait(timeout=5.0):
print("[ros_gateway] server did not become ready within 5 seconds", flush=True)
def stop(self):
"""
@description : 关闭连接、释放端口并等待服务线程退出
@param : 无参数
@return : 无返回值
"""
self._stop_event.set()
with self._state_lock:
server = self._server
if server is not None:
server.shutdown(reason="App Lab application is stopping")
if self._server_thread and self._server_thread is not threading.current_thread():
self._server_thread.join(timeout=5.0)
self._invoke_stop("app_shutdown")
def on_cmd_vel(self, callback):
"""
@description : 注册已通过安全校验的 cmd_vel 回调
@param callback : 接收规范化速度字典的函数
@return : 当前 RosGateway 实例
"""
self._cmd_vel_callback = callback
return self
def on_motor_enable(self, callback):
"""
@description : 注册已通过校验的单电机使能或失能回调
@param callback : 接收包含enabled字段的命令并返回是否接受的函数
@return : 当前RosGateway实例
"""
self._motor_enable_callback = callback
return self
def on_motor_set_speed(self, callback):
"""
@description : 注册已通过校验的单电机RPM速度回调
@param callback : 接收rpm和acceleration_level字段并返回是否接受的函数
@return : 当前RosGateway实例
"""
self._motor_speed_callback = callback
return self
def on_motor_stop(self, callback):
"""
@description : 注册ROS 2主动发起的单电机安全停车回调
@param callback : 接收规范化停车命令并返回是否接受的函数
@return : 当前RosGateway实例
"""
self._motor_stop_callback = callback
return self
def on_mode_change(self, callback):
"""
@description : 注册模式切换回调
@param callback : 接收目标模式并返回是否允许的函数
@return : 当前 RosGateway 实例
"""
self._mode_change_callback = callback
return self
def on_stop(self, callback):
"""
@description : 注册通信异常或命令超时的统一安全停车回调
@param callback : 接收停车原因字符串的函数
@return : 当前 RosGateway 实例
"""
self._stop_callback = callback
return self
def is_ros_connected(self):
"""
@description : 查询是否存在已完成握手的 ROS 2 客户端
@param : 无参数
@return : 已连接返回 True，否则返回 False
"""
with self._state_lock:
return self._connected
def get_status(self):
"""
@description : 获取连接、模式、队列和服务错误状态快照
@param : 无参数
@return : 状态字典
"""
with self._state_lock:
return {
"connected": self._connected,
"client_node": self._client_node,
"mode": self._mode,
"server_ready": self._ready_event.is_set(),
"server_error": self._server_error,
"queued_messages": self._outbound.qsize(),
"dropped_messages": self._dropped_messages,
}
def publish_base_state(self, state):
"""
@description : 校验并将模拟或真实底盘状态加入有界发送队列
@param state : 底盘状态字段字典
@return : 成功入队返回 True，无客户端或失败返回 False
"""
required_fields = {
"mode",
"enabled",
"wheel_position",
"wheel_velocity",
"battery_voltage",
"estop",
"fault_code",
}
missing_fields = required_fields - state.keys()
if missing_fields:
raise ValueError(f"base_state missing fields: {sorted(missing_fields)}")
self._validate_four_values("wheel_position", state["wheel_position"])
self._validate_four_values("wheel_velocity", state["wheel_velocity"])
return self._enqueue("base_state", **state)
def publish_motor_state(self, state):
"""
@description : 校验并发布一台ZDT电机的实时状态
@param state : 包含motor_id、speed_rpm、enabled、communication_ok和error的字典
@return : 成功入队返回True，无客户端或失败返回False
"""
required_fields = {
"motor_id",
"speed_rpm",
"enabled",
"communication_ok",
"error",
}
missing_fields = required_fields - state.keys()
if missing_fields:
raise ValueError(f"motor_state missing fields: {sorted(missing_fields)}")
if type(state["motor_id"]) is not int or not 1 <= state["motor_id"] <= 255:
raise ValueError("motor_id must be an integer in range 1-255")
if type(state["speed_rpm"]) is not int:
raise ValueError("speed_rpm must be an integer")
if type(state["enabled"]) is not bool:
raise ValueError("enabled must be a boolean")
if type(state["communication_ok"]) is not bool:
raise ValueError("communication_ok must be a boolean")
if not isinstance(state["error"], str):
raise ValueError("error must be a string")
return self._enqueue("motor_state", **state)
def publish_imu(self, imu):
"""
@description : 将 IMU 状态加入有界发送队列
@param imu : orientation、angular_velocity、linear_acceleration 字典
@return : 成功入队返回 True，无客户端或失败返回 False
"""
self._validate_vector("orientation", imu.get("orientation"), 4)
self._validate_vector("angular_velocity", imu.get("angular_velocity"), 3)
self._validate_vector("linear_acceleration", imu.get("linear_acceleration"), 3)
return self._enqueue("imu", **imu)
def publish_diagnostics(self, diagnostics):
"""
@description : 将诊断字典加入有界发送队列
@param diagnostics : 可 JSON 序列化的诊断字段
@return : 成功入队返回 True，无客户端或失败返回 False
"""
if not isinstance(diagnostics, dict):
raise ValueError("diagnostics must be a dictionary")
return self._enqueue("diagnostics", **diagnostics)
@staticmethod
def _read_int(explicit_value, environment_name, default_value, minimum, maximum):
"""
@description : 从显式参数或环境变量读取受范围约束的整数
@param explicit_value : 调用方显式提供的值
@param environment_name : 环境变量名称
@param default_value : 默认值
@param minimum : 最小允许值
@param maximum : 最大允许值
@return : 校验后的整数
"""
value = explicit_value
if value is None:
value = os.getenv(environment_name, str(default_value))
parsed = int(value)
if parsed < minimum or parsed > maximum:
raise ValueError(f"{environment_name} must be between {minimum} and {maximum}")
return parsed
@staticmethod
def _read_float(explicit_value, environment_name, default_value, minimum):
"""
@description : 从显式参数或环境变量读取有下限约束的浮点数
@param explicit_value : 调用方显式提供的值
@param environment_name : 环境变量名称
@param default_value : 默认值
@param minimum : 最小允许值
@return : 校验后的浮点数
"""
value = explicit_value
if value is None:
value = os.getenv(environment_name, str(default_value))
parsed = float(value)
if parsed <= minimum:
raise ValueError(f"{environment_name} must be greater than {minimum}")
return parsed
@staticmethod
def _validate_vector(field_name, value, expected_length):
"""
@description : 校验固定长度数值数组
@param field_name : 用于错误信息的字段名
@param value : 待校验数组
@param expected_length : 期望数组长度
@return : 无返回值
"""
if not isinstance(value, (list, tuple)) or len(value) != expected_length:
raise ValueError(f"{field_name} must contain {expected_length} values")
for item in value:
if isinstance(item, bool) or not isinstance(item, (int, float)):
raise ValueError(f"{field_name} must contain only numbers")
@classmethod
def _validate_four_values(cls, field_name, value):
"""
@description : 校验四轮位置或速度数组
@param field_name : 用于错误信息的字段名
@param value : 待校验四元素数组
@return : 无返回值
"""
cls._validate_vector(field_name, value, 4)
def _run_server(self):
"""
@description : 在专用线程中绑定端口并运行同步 WebSocket 服务
@param : 无参数
@return : 无返回值
"""
try:
with serve(
self._handle_connection,
self._host,
self._port,
compression=None,
ping_interval=1.0,
ping_timeout=1.0,
close_timeout=2.0,
max_size=16 * 1024,
max_queue=16,
) as server:
with self._state_lock:
self._server = server
self._server_error = None
self._ready_event.set()
print(
f"[ros_gateway] listening on ws://{self._host}:{self._port}{self._path}",
flush=True,
)
server.serve_forever()
except Exception as exc:
with self._state_lock:
self._server_error = f"{type(exc).__name__}: {exc}"
self._ready_event.set()
print(f"[ros_gateway] server failed: {type(exc).__name__}: {exc}", flush=True)
finally:
with self._state_lock:
self._server = None
self._ready_event.clear()
def _handle_connection(self, websocket):
"""
@description : 处理单个 WebSocket 客户端的握手、收发和安全清理
@param websocket : websockets 同步服务端连接对象
@return : 无返回值
"""
request_path = websocket.request.path.split("?", 1)[0]
if request_path != self._path:
websocket.close(1008, f"expected path {self._path}")
return
with self._state_lock:
if self._active_connection is not None:
websocket.close(1013, "another ROS 2 client already owns the gateway")
return
self._active_connection = websocket
was_connected = False
disconnect_reason = "connection_closed"
try:
raw_hello = websocket.recv(timeout=2.0)
hello = decode_message(raw_hello)
client_node = validate_hello(hello)
current = time.monotonic()
with self._state_lock:
self._connected = True
self._client_node = client_node
self._last_sequence = -1
self._last_rx_monotonic = current
self._last_cmd_monotonic = 0.0
self._last_server_heartbeat = current
self._watchdog_triggered = True
was_connected = True
self._send_json(
websocket,
build_message(
"hello",
role="app",
node=self._node_name,
),
)
print(f"[ros_gateway] ROS 2 connected: node={client_node}", flush=True)
while not self._stop_event.is_set():
self._service_timers(websocket)
self._drain_outbound(websocket)
try:
raw_message = websocket.recv(timeout=0.05)
except TimeoutError:
continue
self._process_message(websocket, raw_message)
except ProtocolError as exc:
disconnect_reason = exc.code
self._send_error(websocket, exc)
websocket.close(1008, str(exc))
except ConnectionClosed as exc:
disconnect_reason = f"connection_closed:{exc.code}"
except Exception as exc:
disconnect_reason = f"handler_error:{type(exc).__name__}"
print(f"[ros_gateway] connection handler error: {type(exc).__name__}: {exc}", flush=True)
finally:
with self._state_lock:
if self._active_connection is websocket:
self._active_connection = None
self._connected = False
self._client_node = None
self._mode = "IDLE"
self._clear_outbound_queue()
if was_connected:
print(f"[ros_gateway] ROS 2 disconnected: {disconnect_reason}", flush=True)
self._invoke_stop(disconnect_reason)
def _process_message(self, websocket, raw_message):
"""
@description : 分派一个客户端消息并执行对应协议校验
@param websocket : 当前活动 WebSocket 连接
@param raw_message : 原始文本消息
@return : 无返回值
"""
try:
message = decode_message(raw_message)
message_type = message["type"]
if message_type == "hello":
raise ProtocolError("duplicate_hello", "hello is only valid as the first message")
with self._state_lock:
sequence = validate_sequence(message, self._last_sequence)
if message_type == "heartbeat":
validate_heartbeat(message)
with self._state_lock:
self._last_sequence = sequence
self._last_rx_monotonic = time.monotonic()
return
if message_type == "mode_change":
target_mode = validate_mode_change(message)
accepted = True
if self._mode_change_callback is not None:
accepted = self._mode_change_callback(target_mode) is not False
if accepted:
with self._state_lock:
self._mode = target_mode
with self._state_lock:
self._last_sequence = sequence
self._last_rx_monotonic = time.monotonic()
self._send_json(
websocket,
build_message(
"ack",
sequence=sequence,
command="mode_change",
accepted=accepted,
mode=self._mode,
),
)
return
if message_type == "cmd_vel":
with self._state_lock:
mode = self._mode
command = validate_cmd_vel(
message,
mode,
self._limits,
self._command_timeout_ms,
)
with self._state_lock:
current = time.monotonic()
self._last_sequence = sequence
self._last_rx_monotonic = current
self._last_cmd_monotonic = current
self._watchdog_triggered = False
if self._cmd_vel_callback is not None:
self._cmd_vel_callback(command)
return
if message_type == "motor_enable":
with self._state_lock:
mode = self._mode
command = validate_motor_enable(message, mode)
accepted = self._invoke_command_callback(
"motor_enable",
self._motor_enable_callback,
command,
)
with self._state_lock:
self._last_sequence = sequence
self._last_rx_monotonic = time.monotonic()
if not command["enabled"]:
self._watchdog_triggered = True
self._last_cmd_monotonic = 0.0
self._send_json(
websocket,
build_message(
"ack",
sequence=sequence,
command="motor_enable",
accepted=accepted,
mode=self._mode,
),
)
return
if message_type == "motor_set_speed":
with self._state_lock:
mode = self._mode
command = validate_motor_set_speed(
message,
mode,
self._max_motor_rpm,
self._command_timeout_ms,
)
accepted = self._invoke_command_callback(
"motor_set_speed",
self._motor_speed_callback,
command,
)
with self._state_lock:
current = time.monotonic()
self._last_sequence = sequence
self._last_rx_monotonic = current
if accepted and command["rpm"] != 0:
self._last_cmd_monotonic = current
self._watchdog_triggered = False
elif accepted:
self._last_cmd_monotonic = 0.0
self._watchdog_triggered = True
self._send_json(
websocket,
build_message(
"ack",
sequence=sequence,
command="motor_set_speed",
accepted=accepted,
mode=self._mode,
),
)
return
if message_type == "motor_stop":
command = validate_motor_stop(message)
accepted = self._invoke_command_callback(
"motor_stop",
self._motor_stop_callback,
command,
)
with self._state_lock:
self._last_sequence = sequence
self._last_rx_monotonic = time.monotonic()
self._last_cmd_monotonic = 0.0
self._watchdog_triggered = True
self._send_json(
websocket,
build_message(
"ack",
sequence=sequence,
command="motor_stop",
accepted=accepted,
mode=self._mode,
),
)
return
raise ProtocolError(
"unknown_type",
f"unsupported message type: {message_type}",
sequence,
)
except ProtocolError as exc:
self._send_error(websocket, exc)
def _service_timers(self, websocket):
"""
@description : 发送服务端心跳并执行连接与速度命令看门狗
@param websocket : 当前活动 WebSocket 连接
@return : 无返回值
"""
current = time.monotonic()
with self._state_lock:
last_rx = self._last_rx_monotonic
last_cmd = self._last_cmd_monotonic
watchdog_triggered = self._watchdog_triggered
last_heartbeat = self._last_server_heartbeat
if current - last_rx > self._heartbeat_timeout_ms / 1000.0:
raise ProtocolError("heartbeat_timeout", "client heartbeat timed out")
if last_cmd > 0.0 and not watchdog_triggered:
if current - last_cmd > self._command_timeout_ms / 1000.0:
with self._state_lock:
self._watchdog_triggered = True
self._invoke_stop("motion_command_timeout")
if current - last_heartbeat >= 1.0:
self._send_json(websocket, self._next_message("heartbeat"))
with self._state_lock:
self._last_server_heartbeat = current
def _enqueue(self, message_type, **fields):
"""
@description : 将出站消息加入有界队列，满时丢弃最旧消息
@param message_type : 出站消息类型
@param fields : 出站消息字段
@return : 有活动客户端时返回 True，否则返回 False
"""
if not self.is_ros_connected():
return False
message = self._next_message(message_type, **fields)
try:
self._outbound.put_nowait(message)
except queue.Full:
try:
self._outbound.get_nowait()
except queue.Empty:
pass
self._outbound.put_nowait(message)
with self._state_lock:
self._dropped_messages += 1
return True
def _next_message(self, message_type, **fields):
"""
@description : 分配严格递增的服务端序号并构造消息
@param message_type : 出站消息类型
@param fields : 出站消息字段
@return : 已构造的消息字典
"""
with self._state_lock:
self._outbound_sequence += 1
sequence = self._outbound_sequence
return build_message(message_type, sequence=sequence, **fields)
def _drain_outbound(self, websocket):
"""
@description : 在连接处理线程中发送当前队列内的全部消息
@param websocket : 当前活动 WebSocket 连接
@return : 无返回值
"""
while True:
try:
message = self._outbound.get_nowait()
except queue.Empty:
return
self._send_json(websocket, message)
def _send_error(self, websocket, error):
"""
@description : 向客户端发送结构化协议错误
@param websocket : 当前活动 WebSocket 连接
@param error : ProtocolError 实例
@return : 无返回值
"""
payload = build_message(
"error",
code=error.code,
message=str(error),
)
if error.seq is not None:
payload["seq"] = error.seq
self._send_json(websocket, payload)
print(f"[ros_gateway] rejected message: {error.code}: {error}", flush=True)
@staticmethod
def _send_json(websocket, message):
"""
@description : 将字典编码为紧凑 UTF-8 JSON 文本并发送
@param websocket : 当前活动 WebSocket 连接
@param message : 待发送消息字典
@return : 无返回值
"""
websocket.send(json.dumps(message, ensure_ascii=False, separators=(",", ":")))
def _invoke_stop(self, reason):
"""
@description : 安全调用统一停车回调并隔离回调异常
@param reason : 停车原因
@return : 无返回值
"""
if self._stop_callback is None:
return
try:
self._stop_callback(reason)
except Exception as exc:
print(f"[ros_gateway] stop callback failed: {type(exc).__name__}: {exc}", flush=True)
@staticmethod
def _invoke_command_callback(command_name, callback, command):
"""
@description : 隔离硬件命令回调异常并统一解释回调接受结果
@param command_name : 用于日志和错误报文的命令名称
@param callback : 已注册的硬件命令回调
@param command : 已完成协议校验的命令字典
@return : 回调未显式返回False时返回True
"""
if callback is None:
return False
try:
return callback(command) is not False
except Exception as exc:
print(
f"[ros_gateway] {command_name} callback failed: "
f"{type(exc).__name__}: {exc}",
flush=True,
)
raise ProtocolError(
"command_failed",
f"{command_name} execution failed: {exc}",
command.get("seq"),
) from exc
def _clear_outbound_queue(self):
"""
@description : 清空断线客户端尚未发送的出站消息
@param : 无参数
@return : 无返回值
"""
while True:
try:
self._outbound.get_nowait()
except queue.Empty:
return