From 5e2017de697fdb25dc47f43a8d6b461726297e43 Mon Sep 17 00:00:00 2001 From: CaoWangrenbo Date: Thu, 29 Oct 2026 20:31:30 +0800 Subject: [PATCH] Initial commit: S10 remote control module, demos, and docs - s10_robot.py: protocol framing, mTLS client, heartbeat, status and control APIs - demo_stand_rotate_lie.py: stand, rotate in place, lie down with safe recovery - status_probe.py: read-only status subscription - tests: protocol unit tests and local fake robot - docs: remote control command reference --- .gitignore | 15 + README.md | 86 +++++ demo_stand_rotate_lie.py | 241 ++++++++++++++ s10_robot.py | 693 +++++++++++++++++++++++++++++++++++++++ status_probe.py | 154 +++++++++ tests/fake_robot.py | 231 +++++++++++++ tests/test_protocol.py | 228 +++++++++++++ 山猫S10-遥控指令.md | 467 ++++++++++++++++++++++++++ 8 files changed, 2115 insertions(+) create mode 100644 .gitignore create mode 100644 README.md create mode 100644 demo_stand_rotate_lie.py create mode 100644 s10_robot.py create mode 100644 status_probe.py create mode 100644 tests/fake_robot.py create mode 100644 tests/test_protocol.py create mode 100644 山猫S10-遥控指令.md diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0939eb3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,15 @@ +# Python +.venv/ +__pycache__/ +*.py[cod] +*.egg-info/ + +# 证书与私钥（不入库） +certs/ +*.key +*.crt +*.pem + +# 日志与临时文件 +*.log +.DS_Store diff --git a/README.md b/README.md new file mode 100644 index 0000000..4e6b621 --- /dev/null +++ b/README.md @@ -0,0 +1,86 @@ +# 山猫 S10 遥控示例 + +基于《山猫S10 软件开发指南》和 DeepRoboticsLab 2.5.4 的连接实测，提供协议实现与演示： + +| 文件 | 说明 | +|---|---| +| `s10_robot.py` | 遥控通信模块（标准库，无第三方依赖） | +| `demo_stand_rotate_lie.py` | 演示：起立 → 原地旋转 → 停止 → 趴下恢复 | +| `status_probe.py` | **只读**状态订阅：只发心跳，不下发任何控制指令 | +| `tests/test_protocol.py` | 协议层单元测试 | +| `tests/fake_robot.py` | 本地假机器人（TLS + 协议仿真），用于无真机联调 | +| `山猫S10-遥控指令.md` | 指令整理文档 | + +## 环境 + +```bash +uv venv .venv +uv run python -m unittest discover -s tests -v # 17 项测试 +``` + +证书默认按顺序查找：`--certs` → `$S10_CERTS` → `./certs` → +`~/.zcode/workspace/draft-183385f3-.../backup/certs`。目录内需要 +`client_chain.crt`、`client.key`、`root_ca.crt`。 + +## 只读观察真机 + +```bash +uv run python status_probe.py --host 10.21.33.103 --seconds 10 +``` + +只发送心跳（≥1Hz）以开启状态上报，不会切换模式、站立或发轴指令。 + +## 跑动作演示 + +```bash +uv run python demo_stand_rotate_lie.py --seconds 5 --yaw 0.3 +``` + +参数：`--yaw` 偏航比例 [-1,1]（正值逆时针）、`--seconds` 旋转时长、 +`--rate` 轴指令频率（默认 10Hz）。任何异常、Ctrl-C 或超时都会先发全 0 +停止，若仍在站立/RL 状态则自动趴下，最后关闭连接。 + +## 无真机联调 + +```bash +uv run python tests/fake_robot.py --port 30003 --once +# 另一个终端 +uv run python demo_stand_rotate_lie.py --host 127.0.0.1 --seconds 3 +``` + +假机器人会模拟：心跳后 5Hz 上报、站立后自动进入 RL 控制（17）、 +趴下（4）、步态/模式更新，轴指令只记录不回包。 + +## 模块用法 + +```python +from s10_robot import S10Client, MotionState, Gait, Mode + +with S10Client(host="10.21.33.103") as robot: # 证书用默认路径 + robot.set_mode(Mode.NORMAL) + robot.stand() + robot.wait_motion_state({MotionState.RL}, timeout=20) + robot.set_gait(Gait.BASIC) + + for _ in range(50): # 10Hz 持续 5 秒 + robot.move(yaw=0.3) + time.sleep(0.1) + robot.stop() + + robot.lie_down() + robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=20) +``` + +主要方法：`heartbeat` / `set_mode` / `set_motion_state`(`stand`/`lie_down`/`damp`) / +`set_gait` / `move`（轴指令）/ `drive_velocity`（速度指令）/ `stop` / +`set_led` / `charge` / `sdk_mode` / `sleep_mode`；状态读取 `basic_status`、 +`motion_status`、`motor_status`、`active_errors` 和 `wait_basic` / +`wait_motion_state`。 + +## 注意 + +- 轴指令、速度指令**不返回响应帧**，只能通过状态上报确认；轴指令要求 2 秒内来自同一客户端，所以要在同一连接里循环下发。 +- 心跳中断 2 秒机器人就停止上报，需要重连。 +- 证书客户端有效期到 2027-04-03，过期需重新获取。 +- 脚本默认不严格校验服务端证书链（根 CA 缺 keyUsage，Python 3.13+ 严格模式会拒绝），需要时加 `--verify`。 +- 标准库不支持 DTLS，30004/UDP 需要额外依赖（如 python-mbedtls）；30003/TCP 与之等价。 diff --git a/demo_stand_rotate_lie.py b/demo_stand_rotate_lie.py new file mode 100644 index 0000000..8296176 --- /dev/null +++ b/demo_stand_rotate_lie.py @@ -0,0 +1,241 @@ +#!/usr/bin/env python3 +"""演示：起立 → 原地旋转一段时间 → 停止 → 趴下恢复。 + +用法:: + + uv run python demo_stand_rotate_lie.py --seconds 5 --yaw 0.3 + + # 指定证书目录和机器人地址 + uv run python demo_stand_rotate_lie.py --certs /path/to/certs --host 10.21.33.103 + +流程与《山猫S10 软件开发指南》的遥控时序一致： + +1. mTLS 连接 30003/TCP，开启 2Hz 心跳（机器人随后开始主动上报状态） +2. 切到常规模式 +3. 下发站立，等待进入 RL 控制状态（ASDU 站立完成后会自动进入） +4. 切到基础步态（必须在完全停止时切换） +5. 以 10Hz 持续下发 Yaw 轴指令，原地旋转指定秒数 +6. 下发全 0 停止，再下发趴下，等待回到趴卧/待机状态 + +任何异常、Ctrl-C 或超时都会走安全收尾：先发全 0 停止，若仍处于站立/RL +状态则下发趴下，最后关闭连接。 +""" + +from __future__ import annotations + +import argparse +import logging +import os +import signal +import sys +import threading +import time +from pathlib import Path + +from s10_robot import ( + Gait, + Mode, + MotionState, + S10Client, + S10CommandError, + S10Error, + S10Timeout, +) + +log = logging.getLogger("demo") + +ABORT = threading.Event() + + +def _handle_signal(signum, _frame): + log.warning("收到信号 %s，准备安全收尾", signal.Signals(signum).name) + ABORT.set() + + +def resolve_certs(explicit: str | None) -> Path: + candidates = [] + if explicit: + candidates.append(Path(explicit)) + if os.environ.get("S10_CERTS"): + candidates.append(Path(os.environ["S10_CERTS"])) + candidates.append(Path("certs")) + candidates.append( + Path.home() + / ".zcode/workspace/draft-183385f3-4f14-43ad-ab22-e5deace0e05c/backup/certs" + ) + for path in candidates: + if (path / "client_chain.crt").is_file() and (path / "client.key").is_file(): + return path + raise SystemExit( + "找不到证书目录，请用 --certs 指定包含 client_chain.crt / client.key / root_ca.crt 的目录" + ) + + +def describe(robot: S10Client) -> str: + basic = robot.basic_status + return ( + f"MotionState={basic.get('MotionState')} Gait={basic.get('Gait')} " + f"Mode={basic.get('ControlUsageMode')} HES={basic.get('HES')}" + ) + + +def check_alive(robot: S10Client) -> None: + basic = robot.basic_status + if basic.get("HES") == 1: + raise S10Error("硬急停已触发，终止运动") + errors = robot.active_errors + for err in errors: + if max(err.get("Severities") or [0]) >= 5: + raise S10Error(f"出现 FATAL 故障: {err.get('Name')} ({err.get('Code')})") + + +def rotate_in_place(robot: S10Client, yaw: float, seconds: float, rate_hz: float) -> None: + """以固定频率持续下发 Yaw 轴指令，旋转指定时间。""" + period = 1.0 / rate_hz + deadline = time.monotonic() + seconds + next_tick = time.monotonic() + sent = 0 + log.info("开始原地旋转: yaw=%+.2f, %g 秒, %g Hz", yaw, seconds, rate_hz) + while time.monotonic() < deadline: + if ABORT.is_set(): + log.warning("旋转被中断") + break + check_alive(robot) + state = robot.basic_status.get("MotionState") + if state != MotionState.RL: + raise S10Error(f"旋转过程中运动状态异常: MotionState={state}（预期 {MotionState.RL}）") + robot.move(yaw=yaw) + sent += 1 + next_tick += period + time.sleep(max(0.0, next_tick - time.monotonic())) + log.info("旋转结束，共下发 %d 帧轴指令", sent) + + +def safe_recover(robot: S10Client) -> None: + """异常/中断后的安全收尾：停止移动，必要时趴下。""" + if not robot.connected: + return + try: + robot.stop() + except S10Error as exc: + log.warning("发送停止指令失败: %s", exc) + state = robot.basic_status.get("MotionState") + if state in (MotionState.STANDING, MotionState.RL, MotionState.JOINT_DAMP): + try: + log.info("安全收尾：下发趴下") + robot.lie_down() + robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=15) + log.info("已趴下: %s", describe(robot)) + except (S10Error, S10Timeout) as exc: + log.error("趴下失败，请手动接管: %s", exc) + + +def run(args: argparse.Namespace) -> int: + certs = resolve_certs(args.certs) + log.info("证书目录: %s", certs) + + robot = S10Client( + host=args.host, + port=args.port, + cert=str(certs / "client_chain.crt"), + key=str(certs / "client.key"), + ca=str(certs / "root_ca.crt"), + verify=args.verify, + heartbeat_hz=args.heartbeat_hz, + request_timeout=args.timeout, + ) + + try: + robot.connect() + log.info("等待首次状态上报…") + try: + robot.wait_basic("MotionState", range(-2, 0x1002), timeout=5.0) + except S10Timeout: + log.warning("5 秒内未收到状态上报，继续尝试下发指令") + log.info("初始状态: %s", describe(robot)) + + if ABORT.is_set(): + return 130 + + log.info("① 切换到常规模式") + robot.set_mode(Mode.NORMAL) + try: + robot.wait_mode({Mode.NORMAL}, timeout=3) + except S10Timeout: + log.warning("模式回读超时，继续") + + log.info("② 下发站立") + robot.stand() + log.info(" 等待进入 RL 控制状态（最多 25 秒）…") + robot.wait_motion_state({MotionState.RL}, timeout=25) + log.info(" 已站立并进入 RL 控制: %s", describe(robot)) + + log.info("③ 切换到基础步态 0x%04X", Gait.BASIC) + robot.set_gait(Gait.BASIC) + time.sleep(0.3) + + log.info("④ 原地旋转") + rotate_in_place(robot, args.yaw, args.seconds, args.rate) + + log.info("⑤ 停止移动，等待完全静止") + robot.stop() + robot.stop() + time.sleep(0.5) + + log.info("⑥ 下发趴下") + robot.lie_down() + state = robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=20) + log.info(" 已恢复: MotionState=%s（%s）", state, describe(robot)) + return 0 + + except S10CommandError as exc: + log.error("机器人拒绝指令: %s", exc) + return 2 + except (S10Error, S10Timeout) as exc: + log.error("执行失败: %s", exc) + return 1 + finally: + safe_recover(robot) + robot.close() + log.info("连接已关闭") + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description="山猫 S10：起立 → 原地旋转 → 趴下 恢复演示", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument("--host", default="10.21.33.103", help="机器人地址") + parser.add_argument("--port", type=int, default=30003, help="TLS 端口") + parser.add_argument("--certs", default=None, help="证书目录（含 client_chain.crt/client.key/root_ca.crt）") + parser.add_argument("--yaw", type=float, default=0.3, + help="偏航比例 [-1,1]，正值为逆时针") + parser.add_argument("--seconds", type=float, default=5.0, help="旋转时长（秒）") + parser.add_argument("--rate", type=float, default=10.0, help="轴指令下发频率（Hz）") + parser.add_argument("--heartbeat-hz", type=float, default=2.0, help="心跳频率（Hz）") + parser.add_argument("--timeout", type=float, default=5.0, help="单条指令响应超时（秒）") + parser.add_argument("--verify", action="store_true", help="严格校验服务端证书链") + parser.add_argument("-v", "--verbose", action="store_true", help="输出调试日志") + args = parser.parse_args(argv) + + if not -1.0 <= args.yaw <= 1.0: + parser.error("--yaw 必须在 [-1, 1] 之间") + if args.seconds <= 0: + parser.error("--seconds 必须大于 0") + if args.rate <= 0: + parser.error("--rate 必须大于 0") + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s.%(msecs)03d %(levelname)-7s %(name)s: %(message)s", + datefmt="%H:%M:%S", + ) + + signal.signal(signal.SIGINT, _handle_signal) + signal.signal(signal.SIGTERM, _handle_signal) + + return run(args) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/s10_robot.py b/s10_robot.py new file mode 100644 index 0000000..74c7e27 --- /dev/null +++ b/s10_robot.py @@ -0,0 +1,693 @@ +#!/usr/bin/env python3 +"""山猫 S10（Deep Robotics）遥控通信模块。 + +仅依赖标准库。实现《山猫S10 软件开发指南》第 1 章中遥控所需的部分： + +* 16 字节帧头 + JSON ASDU 的封装与解包（支持 TCP 流式拆包/粘包） +* mTLS 连接（TLS 1.3，客户端证书双向认证） +* 心跳保活与后台状态接收线程 +* 控制指令：使用模式、运动状态、步态、轴指令、速度指令、 + 灯语、自主充电、运动 SDK、休眠 +* 状态回读：基础状态 / 运控状态 / 设备状态 / 异常状态 / 导航状态 + +限制：只处理单包 ASDU（ASDU 长度 <= 1000 字节），所有控制指令都在此范围内； +多包拼接（总长最大 65535）未实现。 + +示例:: + + with S10Client(cert="certs/client_chain.crt", + key="certs/client.key", + ca="certs/root_ca.crt") as robot: + robot.stand() + robot.wait_motion_state({MotionState.RL}, timeout=20) + robot.move(yaw=0.3) # 轴指令，需持续下发 + robot.stop() + robot.lie_down() +""" + +from __future__ import annotations + +import json +import logging +import socket +import ssl +import struct +import threading +import time +from dataclasses import dataclass, field +from typing import Any, Iterable + +__all__ = [ + "S10Client", + "S10Error", + "S10Timeout", + "S10CommandError", + "MotionState", + "Gait", + "Mode", + "Charge", + "Frame", + "build_frame", + "parse_frames", +] + +log = logging.getLogger("s10_robot") + +# --------------------------------------------------------------------------- +# 协议常量 +# --------------------------------------------------------------------------- + +SYNC = b"\xeb\x91\xeb\x90" +HEADER_LEN = 16 +FMT_XML = 0x00 +FMT_JSON = 0x01 +PROTO_VERSION = 0x01 +MAX_ASDU = 1000 + +# Type 字段 +T_MOTION = 0x00100001 # 运控：状态/步态/轴指令/速度指令，也用于运控状态上报 +T_MODE = 0x00100002 # 使用模式/休眠/设备状态上报 +T_NAV = 0x00100003 # 导航状态 +T_CHARGE = 0x00100004 # 自主充电 +T_LED = 0x00100005 # 灯语 / 运动 SDK +T_HEARTBEAT = 0x00100064 # 心跳 / 基础状态上报 +T_FAULT = 0x0010007F # 异常状态上报 + +# Command 字段 +C_HEARTBEAT = 0x00000005 +C_MODE_SET = 0x00500002 +C_MOTION_STATE = 0x00200002 +C_GAIT = 0x00300002 +C_AXIS = 0x00100002 +C_VELOCITY = 0x00110002 +C_CHARGE = 0x00100002 +C_LED = 0x00200002 +C_SDK = 0x00300002 +C_SLEEP = 0x00400002 +C_STATUS = 0x00F00000 # 所有主动上报的 Command + +# 上报类别 +_STATUS_KEYS = { + (T_HEARTBEAT, C_STATUS): "basic", + (T_MOTION, C_STATUS): "motion", + (T_MODE, C_STATUS): "device", + (T_FAULT, C_STATUS): "fault", + (T_NAV, C_STATUS): "nav", +} + + +class MotionState: + """MotionState / MotionParam 取值。""" + + SOFT_ESTOP = -2 + DEFAULT = 0 # 初始 / 趴卧 + STANDING = 1 + JOINT_DAMP = 2 # 关节阻尼 / 软急停 + LYING = 4 + CALIBRATE = 5 + RL = 17 + DAMP_LYING = 0x1001 + + +class Gait: + """GaitParam / Gait 取值。""" + + BASIC = 0x1001 # 常规运动模式 + HIGH_PLATFORM = 0x1002 # 高台（仅在状态上报中出现） + FLAT_NAV = 0x3002 # 导航运动模式 + STAIR_NAV = 0x3003 # 导航运动模式 + + +class Mode: + """ControlUsageMode 取值。""" + + NORMAL = 0 + NAV = 1 + ASSIST = 2 + + +class Charge: + """Charge 动作取值。""" + + STOP = 0 + START = 1 + RESET = 2 # 强制断电并清除错误，慎用 + + +# --------------------------------------------------------------------------- +# 异常 +# --------------------------------------------------------------------------- + + +class S10Error(RuntimeError): + """通信或协议层错误。""" + + +class S10Timeout(S10Error): + """等待响应或状态超时。""" + + +class S10CommandError(S10Error): + """机器人返回非 0 ErrorCode。""" + + def __init__(self, code: int, message: str | None, frame: "Frame | None" = None): + self.code = code + self.message = message + self.frame = frame + super().__init__(f"ErrorCode=0x{code:04X} ({code}) {message or ''}".strip()) + + +# --------------------------------------------------------------------------- +# 帧结构 +# --------------------------------------------------------------------------- + + +@dataclass +class Frame: + type: int + command: int + time: str + items: dict[str, Any] + report_id: int + fmt: int + seq: int + version: int + payload: bytes = b"" + + @property + def is_status(self) -> bool: + return self.command == C_STATUS + + +def _to_int(value: Any) -> int: + if isinstance(value, bool): + return int(value) + if isinstance(value, int): + return value + if isinstance(value, float): + return int(value) + if isinstance(value, str): + text = value.strip().replace(" ", "") + return int(text, 0) + raise ValueError(f"无法转换为整数: {value!r}") + + +def build_frame(asdu: bytes, report_id: int = 0, seq: int = 0, fmt: int = FMT_JSON) -> bytes: + """把 ASDU 字节封装成 16 字节头 + 载荷。""" + if len(asdu) > MAX_ASDU: + raise ValueError(f"单帧 ASDU 超长: {len(asdu)} > {MAX_ASDU}") + header = SYNC + header += struct.pack(" bytes: + """构造 JSON 格式的 ASDU 载荷。""" + body = { + "PatrolDevice": { + "Type": type_, + "Command": command, + "Time": time_str or time.strftime("%Y-%m-%d %H:%M:%S"), + "Items": items or {}, + } + } + return json.dumps(body, separators=(",", ":"), ensure_ascii=False).encode("utf-8") + + +def _parse_payload(payload: bytes) -> tuple[int, int, str, dict[str, Any]]: + doc = json.loads(payload.decode("utf-8")) + body = doc.get("PatrolDevice", doc) + if not isinstance(body, dict): + raise ValueError("ASDU 根节点格式异常") + type_ = _to_int(body.get("Type", -1)) + command = _to_int(body.get("Command", -1)) + time_str = str(body.get("Time", "")) + items = body.get("Items") or {} + if not isinstance(items, dict): + raise ValueError("Items 不是对象") + return type_, command, time_str, items + + +def parse_frames(buf: bytearray) -> list[Frame]: + """从缓冲区中提取所有完整帧，剩余的半个帧留在 buf 里。 + + 遇到垃圾数据时通过搜索同步字符重新对齐。 + """ + frames: list[Frame] = [] + while True: + idx = buf.find(SYNC) + if idx < 0: + # 保留尾部 3 字节，避免同步字符被切开 + if len(buf) > 3: + del buf[: len(buf) - 3] + return frames + if idx: + log.debug("丢弃 %d 字节非同步数据", idx) + del buf[:idx] + if len(buf) < HEADER_LEN: + return frames + + length, report_id = struct.unpack_from(" MAX_ASDU * 65: # 明显异常，跳过这个同步字符重新找 + log.warning("长度字段异常 (%d)，重新同步", length) + del buf[:4] + continue + total = HEADER_LEN + length + if len(buf) < total: + return frames + + raw = bytes(buf[:total]) + del buf[:total] + payload = raw[HEADER_LEN:] + try: + type_, command, time_str, items = _parse_payload(payload) + except Exception as exc: # 坏包不应中断接收 + log.warning("ASDU 解析失败: %s; payload=%r", exc, payload[:200]) + continue + + frames.append( + Frame( + type=type_, + command=command, + time=time_str, + items=items, + report_id=report_id, + fmt=raw[8], + seq=raw[9], + version=raw[10], + payload=payload, + ) + ) + + +# --------------------------------------------------------------------------- +# 客户端 +# --------------------------------------------------------------------------- + + +@dataclass +class _Pending: + type: int + command: int + event: threading.Event = field(default_factory=threading.Event) + frame: Frame | None = None + + +class S10Client: + """山猫 S10 遥控客户端。 + + 单条 TCP/TLS 连接内完成心跳、指令下发和状态接收。轴指令/速度指令要求 + 2 秒内来自同一客户端，因此不要为每条指令重新建连。 + """ + + def __init__( + self, + host: str = "10.21.33.103", + port: int = 30003, + *, + cert: str = "certs/client_chain.crt", + key: str = "certs/client.key", + ca: str = "certs/root_ca.crt", + verify: bool = False, + server_name: str = "server.local", + heartbeat_hz: float = 2.0, + request_timeout: float = 5.0, + connect_timeout: float = 8.0, + ): + self.host = host + self.port = port + self.cert = cert + self.key = key + self.ca = ca + self.verify = verify + self.server_name = server_name + self.heartbeat_hz = heartbeat_hz + self.request_timeout = request_timeout + self.connect_timeout = connect_timeout + + self._sock: ssl.SSLSocket | None = None + self._running = False + self._wlock = threading.Lock() + self._id_lock = threading.Lock() + self._report_id = 0 + self._pending: dict[int, _Pending] = {} + self._pending_lock = threading.Lock() + + self._cond = threading.Condition() + self._status: dict[str, dict[str, Any]] = { + "basic": {}, "motion": {}, "device": {}, "fault": {}, "nav": {} + } + self._status_ts: dict[str, float] = {} + self._errors: dict[int, dict[str, Any]] = {} + + self._recv_thread: threading.Thread | None = None + self._hb_thread: threading.Thread | None = None + self._last_rx = 0.0 + self._last_hb = 0.0 + + # -- 连接管理 --------------------------------------------------------- + + def connect(self) -> "S10Client": + if self._sock is not None: + return self + ctx = ssl.create_default_context(ssl.Purpose.SERVER_AUTH, cafile=self.ca) + ctx.check_hostname = False # 服务端证书 CN=server.local，而连接用的是 IP + ctx.verify_mode = ssl.CERT_REQUIRED if self.verify else ssl.CERT_NONE + ctx.load_cert_chain(self.cert, self.key) + + raw = socket.create_connection((self.host, self.port), timeout=self.connect_timeout) + raw.settimeout(0.5) + try: + sock = ctx.wrap_socket(raw, server_hostname=self.server_name) + except Exception: + raw.close() + raise + self._sock = sock + if not self.verify: + log.warning("未严格校验服务端证书链（--verify 可开启，部分 Python 版本会因根 CA 缺 keyUsage 失败）") + log.info("已连接 %s:%s，%s %s", self.host, self.port, + sock.version(), sock.cipher()[0]) + + self._running = True + self._last_rx = time.monotonic() + self._recv_thread = threading.Thread(target=self._recv_loop, name="s10-recv", daemon=True) + self._hb_thread = threading.Thread(target=self._hb_loop, name="s10-hb", daemon=True) + self._recv_thread.start() + self._hb_thread.start() + self.heartbeat() + return self + + def close(self) -> None: + self._running = False + sock, self._sock = self._sock, None + if sock is not None: + try: + sock.shutdown(socket.SHUT_RDWR) + except (OSError, AttributeError): + pass + try: + sock.close() + except (OSError, AttributeError): + pass + for thread in (self._recv_thread, self._hb_thread): + if thread is not None and thread.is_alive(): + thread.join(timeout=2.0) + with self._pending_lock: + for pending in self._pending.values(): + pending.event.set() + self._pending.clear() + + def __enter__(self) -> "S10Client": + return self.connect() + + def __exit__(self, exc_type, exc, tb) -> None: + self.close() + + @property + def connected(self) -> bool: + return self._running and self._sock is not None + + # -- 收发 ------------------------------------------------------------- + + def _next_report_id(self) -> int: + with self._id_lock: + rid = self._report_id + self._report_id = (self._report_id + 1) & 0xFFFF + return rid + + def _send_nowait(self, type_: int, command: int, items: dict[str, Any]) -> None: + sock = self._sock + if sock is None: + raise S10Error("未连接") + packet = build_frame(build_asdu(type_, command, items), self._next_report_id()) + with self._wlock: + try: + sock.sendall(packet) + except OSError as exc: + raise S10Error(f"发送失败: {exc}") from exc + + def _request(self, type_: int, command: int, items: dict[str, Any] | None = None, + timeout: float | None = None) -> dict[str, Any]: + rid = self._next_report_id() + pending = _Pending(type_, command) + with self._pending_lock: + self._pending[rid] = pending + try: + packet = build_frame(build_asdu(type_, command, items), rid) + sock = self._sock + if sock is None: + raise S10Error("未连接") + with self._wlock: + sock.sendall(packet) + if not pending.event.wait(timeout or self.request_timeout): + raise S10Timeout( + f"等待响应超时 Type=0x{type_:08X} Command=0x{command:08X}" + ) + frame = pending.frame + finally: + with self._pending_lock: + self._pending.pop(rid, None) + + assert frame is not None + code = frame.items.get("ErrorCode") + if code is not None and int(code) != 0: + raise S10CommandError(int(code), frame.items.get("ErrorMessage"), frame) + return frame.items + + def _recv_loop(self) -> None: + buf = bytearray() + while self._running: + sock = self._sock + if sock is None: + break + try: + chunk = sock.recv(65535) + except socket.timeout: + continue + except (OSError, ssl.SSLError) as exc: + if self._running: + log.warning("接收失败: %s", exc) + break + if not chunk: + log.warning("对端关闭连接") + break + self._last_rx = time.monotonic() + buf.extend(chunk) + for frame in parse_frames(buf): + self._dispatch(frame) + self._running = False + + def _dispatch(self, frame: Frame) -> None: + if not frame.is_status: + with self._pending_lock: + pending = self._pending.get(frame.report_id) + if pending and pending.type == frame.type and pending.command == frame.command: + pending.frame = frame + pending.event.set() + return + + key = _STATUS_KEYS.get((frame.type, frame.command)) + if key is None: + log.debug("忽略未识别帧 Type=0x%08X Command=0x%08X", frame.type, frame.command) + return + + with self._cond: + if key == "fault": + self._apply_faults(frame.items) + elif key == "basic": + # Items.BasicStatus.*，部分固件可能直接平铺 + inner = frame.items.get("BasicStatus") + self._status[key] = dict(inner if isinstance(inner, dict) else frame.items) + else: + self._status[key] = dict(frame.items) + self._status_ts[key] = time.monotonic() + if key == "basic": + state = self._status["basic"].get("MotionState") + hes = self._status["basic"].get("HES") + log.debug("基础状态 MotionState=%s Gait=%s HES=%s", + state, self._status["basic"].get("Gait"), hes) + self._cond.notify_all() + + def _apply_faults(self, items: dict[str, Any]) -> None: + errors = items.get("ErrorList") + if isinstance(errors, dict): + errors = [errors] + if not isinstance(errors, list): + self._status["fault"] = dict(items) + return + for err in errors: + code = int(err.get("Code", 0)) + etype = int(err.get("Type", 1)) + if etype == 2: + self._errors.pop(code, None) + else: + self._errors[code] = err + self._status["fault"] = dict(items) + self._status["fault"]["ActiveErrors"] = list(self._errors.values()) + + def _hb_loop(self) -> None: + period = 1.0 / max(self.heartbeat_hz, 0.2) + while self._running: + try: + self.heartbeat() + except S10Error as exc: + if self._running: + log.warning("心跳发送失败: %s", exc) + break + time.sleep(period) + + # -- 状态读取 --------------------------------------------------------- + + @property + def basic_status(self) -> dict[str, Any]: + with self._cond: + return dict(self._status["basic"]) + + @property + def motion_status(self) -> dict[str, Any]: + """运控状态的 MotionStatus 部分（速度、姿态、关节角等）。""" + with self._cond: + items = self._status["motion"] + inner = items.get("MotionStatus") + return dict(inner if isinstance(inner, dict) else items) + + @property + def motor_status(self) -> dict[str, Any]: + with self._cond: + inner = self._status["motion"].get("MotorStatus") + return dict(inner) if isinstance(inner, dict) else {} + + @property + def device_status(self) -> dict[str, Any]: + with self._cond: + return dict(self._status["device"]) + + @property + def active_errors(self) -> list[dict[str, Any]]: + with self._cond: + return [dict(e) for e in self._errors.values()] + + @property + def status_age(self) -> float: + """最近一次收到基础状态距今的秒数；尚未收到时为 inf。""" + last = self._status_ts.get("basic") + return float("inf") if last is None else time.monotonic() - last + + def wait_basic(self, field_name: str, values: Iterable[Any], timeout: float = 10.0, + check_estop: bool = True) -> Any: + """等待基础状态中某字段取到指定值之一。""" + wanted = set(values) + + def ready() -> bool: + basic = self._status["basic"] + if check_estop and basic.get("HES") == 1: + raise S10Error("硬急停已触发（HES=1）") + return basic.get(field_name) in wanted + + with self._cond: + ok = self._cond.wait_for(ready, timeout) + value = self._status["basic"].get(field_name) + if not ok: + raise S10Timeout(f"等待 {field_name} in {sorted(wanted)} 超时，当前={value!r}") + return value + + def wait_motion_state(self, states: Iterable[int], timeout: float = 20.0, + check_estop: bool = True) -> int: + return self.wait_basic("MotionState", states, timeout, check_estop) + + def wait_gait(self, gaits: Iterable[int], timeout: float = 10.0) -> int: + return self.wait_basic("Gait", gaits, timeout) + + def wait_mode(self, modes: Iterable[int], timeout: float = 10.0) -> int: + return self.wait_basic("ControlUsageMode", modes, timeout) + + # -- 控制指令 --------------------------------------------------------- + + def heartbeat(self) -> None: + """发送一次心跳。机器人只向持续发心跳的 IP:端口 上报状态。""" + self._send_nowait(T_HEARTBEAT, C_HEARTBEAT, {}) + + def set_mode(self, mode: int) -> dict[str, Any]: + """切换使用模式（Mode.NORMAL / NAV / ASSIST）。""" + return self._request(T_MODE, C_MODE_SET, {"Mode": int(mode)}) + + def set_motion_state(self, state: int) -> dict[str, Any]: + """转换运动状态（见 MotionState）。软急停/空闲不可下发。""" + return self._request(T_MOTION, C_MOTION_STATE, {"MotionParam": int(state)}) + + def stand(self) -> dict[str, Any]: + return self.set_motion_state(MotionState.STANDING) + + def lie_down(self) -> dict[str, Any]: + return self.set_motion_state(MotionState.LYING) + + def damp(self) -> dict[str, Any]: + """进入关节阻尼状态（可作为软急停后的安全态）。""" + return self.set_motion_state(MotionState.JOINT_DAMP) + + def set_gait(self, gait: int) -> dict[str, Any]: + """切换步态。必须在机器人完全停止后下发。""" + return self._request(T_MOTION, C_GAIT, {"GaitParam": int(gait)}) + + def move(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, + roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0) -> None: + """轴指令：各轴为相对最大速度的比例，取值 [-1, 1]，无响应帧。 + + 仅常规/辅助模式有效；基础步态只响应 X、Y、Yaw。 + """ + values = (x, y, z, roll, pitch, yaw) + if any(v < -1.0 or v > 1.0 for v in values): + raise ValueError(f"轴指令参数必须位于 [-1, 1]: {values}") + self._send_nowait(T_MOTION, C_AXIS, { + "X": float(x), "Y": float(y), "Z": float(z), + "Roll": float(roll), "Pitch": float(pitch), "Yaw": float(yaw), + }) + + def drive_velocity(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, + roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0) -> None: + """速度指令：物理量（m/s、rad/s），仅导航模式生效，无响应帧。""" + if abs(x) > 1.67 or abs(y) > 0.4 or abs(yaw) > 1.0: + raise ValueError(f"速度指令超限: X±1.67 Y±0.4 Yaw±1.0, got {x},{y},{yaw}") + self._send_nowait(T_MOTION, C_VELOCITY, { + "X": float(x), "Y": float(y), "Z": float(z), + "Roll": float(roll), "Pitch": float(pitch), "Yaw": float(yaw), + }) + + def stop(self) -> None: + """下发全 0 轴指令，让机器人停止移动。""" + self.move() + + def set_led(self, head: dict[str, Any] | None = None, + tail: dict[str, Any] | None = None, + custom: bool = True) -> dict[str, Any]: + """设置头灯/尾灯。每项形如 {"Type": 0, "Color": [1], "Cycle": 1}。""" + leds = [] + for spec in (head, tail): + if spec is not None: + leds.append({ + "Type": int(spec.get("Type", 0)), + "Color": list(spec.get("Color", [1])), + "Cycle": int(spec.get("Cycle", 1)), + }) + return self._request(T_LED, C_LED, {"CustomMode": bool(custom), "Led": leds}) + + def charge(self, action: int) -> dict[str, Any]: + """自主充电：0 结束 / 1 开始 / 2 清除状态（慎用）。""" + return self._request(T_CHARGE, C_CHARGE, {"Charge": int(action)}) + + def sdk_mode(self, enable: bool, frequency: int = 100) -> dict[str, Any]: + """开启/关闭运动 SDK 模式。频率需在 [1,200] 且能整除 1000。""" + if enable and not (1 <= frequency <= 200 and 1000 % frequency == 0): + raise ValueError(f"Frequency 非法: {frequency}") + return self._request(T_LED, C_SDK, + {"SDKEnable": bool(enable), "Frequency": int(frequency)}) + + def sleep_mode(self, sleep: bool, auto: bool = True, minutes: int = 5) -> dict[str, Any]: + """设置休眠/自动休眠。minutes 为心跳断联后的等待时间。""" + return self._request(T_MODE, C_SLEEP, + {"Sleep": bool(sleep), "Auto": bool(auto), "Time": int(minutes)}) diff --git a/status_probe.py b/status_probe.py new file mode 100644 index 0000000..9b34f6a --- /dev/null +++ b/status_probe.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +"""只读状态订阅：连接山猫 S10，仅发送心跳并打印主动上报的状态。 + +该脚本**不会**下发任何控制指令（不切模式、不站立、不发轴指令）， +只做心跳保活和状态读取，可安全用于观察真机当前状态。 + +用法:: + + uv run python status_probe.py --seconds 10 + uv run python status_probe.py --host 10.21.33.103 --port 30003 -v +""" + +from __future__ import annotations + +import argparse +import json +import logging +import os +import signal +import sys +import time +from pathlib import Path + +from s10_robot import S10Client, S10Error + +log = logging.getLogger("probe") + +MOTION_STATE_NAMES = { + -2: "软急停", + 0: "默认/趴卧", + 1: "站立", + 2: "关节阻尼", + 4: "趴下", + 5: "标零", + 17: "RL控制", + 0x1001: "阻尼趴下", +} +GAIT_NAMES = {0x1001: "基础", 0x1002: "高台", 0x3002: "平地(导航)", 0x3003: "楼梯(导航)"} +MODE_NAMES = {0: "常规", 1: "导航", 2: "辅助"} + + +def resolve_certs(explicit: str | None) -> Path: + for candidate in ( + Path(explicit) if explicit else None, + Path(os.environ["S10_CERTS"]) if os.environ.get("S10_CERTS") else None, + Path("certs"), + Path.home() / ".zcode/workspace/draft-183385f3-4f14-43ad-ab22-e5deace0e05c/backup/certs", + ): + if candidate and (candidate / "client_chain.crt").is_file(): + return candidate + raise SystemExit("找不到证书目录，请用 --certs 指定") + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description="山猫 S10 只读状态订阅") + parser.add_argument("--host", default="10.21.33.103") + parser.add_argument("--port", type=int, default=30003) + parser.add_argument("--certs", default=None) + parser.add_argument("--seconds", type=float, default=10.0, help="订阅时长") + parser.add_argument("--interval", type=float, default=1.0, help="状态摘要打印间隔") + parser.add_argument("--verify", action="store_true") + parser.add_argument("-v", "--verbose", action="store_true") + args = parser.parse_args(argv) + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s.%(msecs)03d %(levelname)-7s %(name)s: %(message)s", + datefmt="%H:%M:%S", + ) + logging.getLogger("s10_robot").setLevel(logging.INFO if not args.verbose else logging.DEBUG) + + certs = resolve_certs(args.certs) + robot = S10Client( + host=args.host, + port=args.port, + cert=str(certs / "client_chain.crt"), + key=str(certs / "client.key"), + ca=str(certs / "root_ca.crt"), + verify=args.verify, + ) + + deadline = time.monotonic() + args.seconds + last_summary = 0.0 + last_state = None + last_errors: list[int] = [] + try: + robot.connect() + while time.monotonic() < deadline: + time.sleep(0.1) + basic = robot.basic_status + if not basic: + continue # 还没收到首次上报 + state = basic.get("MotionState") + if state != last_state: + last_state = state + log.info( + "状态变化: MotionState=%s(%s) Gait=%s(%s) Mode=%s(%s) HES=%s Charge=%s", + state, MOTION_STATE_NAMES.get(state, "?"), + basic.get("Gait"), GAIT_NAMES.get(basic.get("Gait"), "?"), + basic.get("ControlUsageMode"), + MODE_NAMES.get(basic.get("ControlUsageMode"), "?"), + basic.get("HES"), basic.get("Charge"), + ) + errors = robot.active_errors + codes = sorted(int(e.get("Code", 0)) for e in errors) + if codes != last_errors: + last_errors = codes + for err in errors: + log.warning( + "告警 Code=0x%04X Name=%s Severity=%s Resources=%s", + int(err.get("Code", 0)), err.get("Name"), + err.get("Severities"), err.get("Resources"), + ) + if not errors: + log.info("告警已全部解除") + now = time.monotonic() + if now - last_summary >= args.interval: + last_summary = now + motion = robot.motion_status + print( + "[%s] state=%s gait=%s mode=%s hes=%s | " + "vx=%.2f vy=%.2f wz=%.2f height=%.3f " + "roll=%.3f pitch=%.3f yaw=%.3f | errors=%d | age=%.1fs" + % ( + time.strftime("%H:%M:%S"), + MOTION_STATE_NAMES.get(state, state), + GAIT_NAMES.get(basic.get("Gait"), basic.get("Gait")), + MODE_NAMES.get(basic.get("ControlUsageMode"), basic.get("ControlUsageMode")), + basic.get("HES"), + float(motion.get("LinearX", 0.0)), + float(motion.get("LinearY", 0.0)), + float(motion.get("AngularZ", 0.0)), + float(motion.get("Height", 0.0)), + float(motion.get("Roll", 0.0)), + float(motion.get("Pitch", 0.0)), + float(motion.get("Yaw", 0.0)), + len(robot.active_errors), + robot.status_age, + ), + flush=True, + ) + return 0 + except S10Error as exc: + log.error("订阅失败: %s", exc) + return 1 + except KeyboardInterrupt: + return 130 + finally: + robot.close() + + +if __name__ == "__main__": + signal.signal(signal.SIGTERM, lambda *_: sys.exit(143)) + sys.exit(main()) diff --git a/tests/fake_robot.py b/tests/fake_robot.py new file mode 100644 index 0000000..1a7512c --- /dev/null +++ b/tests/fake_robot.py @@ -0,0 +1,231 @@ +#!/usr/bin/env python3 +"""本地假机器人：用于在没有真机时跑通 demo。 + +启动一个 TLS 服务端，按《山猫S10 软件开发指南》的协议响应控制指令，并 +周期性上报基础状态。证书用 openssl 现场生成（自签名，客户端需 verify=False）。 + +用法:: + + uv run python tests/fake_robot.py --port 30003 + + # 另一个终端 + uv run python demo_stand_rotate_lie.py --host 127.0.0.1 --seconds 3 + +行为模拟： +* 心跳 -> ErrorCode 0，并开始 5Hz 上报 BasicStatus +* 使用模式切换 -> 更新 ControlUsageMode +* 站立 -> 0.8 秒后 MotionState 变为 17（RL 控制） +* 趴下 -> 0.8 秒后 MotionState 变为 4 +* 步态切换 -> 更新 Gait +* 轴指令/速度指令 -> 不回包，仅记录最后一帧 +""" + +from __future__ import annotations + +import argparse +import logging +import socket +import ssl +import subprocess +import sys +import tempfile +import threading +import time +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent.parent)) + +import s10_robot as s10 + +log = logging.getLogger("fake") + + +def make_cert(directory: Path) -> tuple[Path, Path]: + crt = directory / "server.crt" + key = directory / "server.key" + subprocess.run( + [ + "openssl", "req", "-x509", "-newkey", "rsa:2048", "-nodes", + "-keyout", str(key), "-out", str(crt), "-days", "1", + "-subj", "/CN=server.local", + "-addext", "subjectAltName=DNS:server.local,IP:127.0.0.1", + ], + check=True, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + ) + return crt, key + + +class RobotState: + def __init__(self): + self.lock = threading.Lock() + self.motion_state = 0 + self.gait = 0x1001 + self.mode = 0 + self.hes = 0 + self.last_axis = {} + self.axis_frames = 0 + self.reporting = False + + def snapshot(self) -> dict: + with self.lock: + return { + "MotionState": self.motion_state, + "Gait": self.gait, + "ControlUsageMode": self.mode, + "HES": self.hes, + "PowerManagement": 0, + "Sleep": 0, + "Model": "CD1", + "Version": "STD", + "Charge": 0, + } + + +def handle_command(state: RobotState, frame: s10.Frame) -> dict | None: + if frame.type == s10.T_HEARTBEAT and frame.command == s10.C_HEARTBEAT: + state.reporting = True + return {"ErrorCode": 0} + + if frame.type == s10.T_MODE and frame.command == s10.C_MODE_SET: + with state.lock: + state.mode = int(frame.items.get("Mode", 0)) + log.info("使用模式 -> %s", state.mode) + return {"ErrorCode": 0} + + if frame.type == s10.T_MOTION and frame.command == s10.C_MOTION_STATE: + target = int(frame.items.get("MotionParam", 0)) + log.info("运动状态指令 -> %s", target) + + def transition(): + time.sleep(0.8) + with state.lock: + # ASDU 站立完成后会自动进入 RL 控制状态 + state.motion_state = 17 if target == 1 else target + log.info("运动状态已变为 %s", state.motion_state) + + threading.Thread(target=transition, daemon=True).start() + return {"ErrorCode": 0} + + if frame.type == s10.T_MOTION and frame.command == s10.C_GAIT: + with state.lock: + state.gait = int(frame.items.get("GaitParam", 0)) + log.info("步态 -> 0x%04X", state.gait) + return {"ErrorCode": 0} + + if frame.type == s10.T_MOTION and frame.command in (s10.C_AXIS, s10.C_VELOCITY): + with state.lock: + state.last_axis = dict(frame.items) + state.axis_frames += 1 + n = state.axis_frames + if n % 10 == 1: + log.info("轴指令 #%d: %s", n, state.last_axis) + return None # 轴指令无响应帧 + + if frame.type == s10.T_MODE and frame.command == s10.C_SLEEP: + return {"ErrorCode": 0} + + return {"ErrorCode": 0} + + +def serve_connection(conn: ssl.SSLSocket, addr, state: RobotState, stop: threading.Event, + send_lock: threading.Lock) -> None: + log.info("客户端接入 %s", addr) + buf = bytearray() + try: + while not stop.is_set(): + conn.settimeout(0.2) + try: + chunk = conn.recv(65535) + except socket.timeout: + continue + if not chunk: + break + buf.extend(chunk) + for frame in s10.parse_frames(buf): + response = handle_command(state, frame) + if response is not None: + reply = s10.build_frame( + s10.build_asdu(frame.type, frame.command, response), + report_id=frame.report_id, + ) + with send_lock: + conn.sendall(reply) + except (OSError, ssl.SSLError) as exc: + log.info("连接结束: %s", exc) + finally: + try: + conn.close() + except OSError: + pass + log.info("客户端断开") + + +def reporter(conn: ssl.SSLSocket, state: RobotState, stop: threading.Event, + send_lock: threading.Lock, period: float = 0.2) -> None: + next_t = time.monotonic() + while not stop.is_set(): + if not state.reporting: + stop.wait(0.05) + next_t = time.monotonic() + continue + payload = s10.build_asdu( + s10.T_HEARTBEAT, s10.C_STATUS, {"BasicStatus": state.snapshot()} + ) + try: + with send_lock: + conn.sendall(s10.build_frame(payload)) + except (OSError, ssl.SSLError): + return + next_t += period + stop.wait(max(0.0, next_t - time.monotonic())) + + +def main() -> int: + parser = argparse.ArgumentParser(description="山猫 S10 本地假机器人") + parser.add_argument("--host", default="127.0.0.1") + parser.add_argument("--port", type=int, default=30003) + parser.add_argument("--once", action="store_true", help="处理一个连接后退出") + parser.add_argument("-v", "--verbose", action="store_true") + args = parser.parse_args() + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s.%(msecs)03d %(levelname)-7s %(name)s: %(message)s", + datefmt="%H:%M:%S", + ) + + with tempfile.TemporaryDirectory() as tmp: + crt, key = make_cert(Path(tmp)) + ctx = ssl.SSLContext(ssl.PROTOCOL_TLS_SERVER) + ctx.load_cert_chain(crt, key) + ctx.verify_mode = ssl.CERT_NONE # 不校验客户端证书，便于本地联调 + + state = RobotState() + stop = threading.Event() + + with socket.create_server((args.host, args.port), reuse_port=False) as server: + log.info("假机器人监听 %s:%d（TLS）", args.host, args.port) + while not stop.is_set(): + raw, addr = server.accept() + try: + conn = ctx.wrap_socket(raw, server_side=True) + except ssl.SSLError as exc: + log.warning("TLS 握手失败: %s", exc) + raw.close() + continue + send_lock = threading.Lock() + rep = threading.Thread( + target=reporter, args=(conn, state, stop, send_lock), daemon=True + ) + rep.start() + serve_connection(conn, addr, state, stop, send_lock) + rep.join(timeout=1.0) + if args.once: + break + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tests/test_protocol.py b/tests/test_protocol.py new file mode 100644 index 0000000..afc6c3d --- /dev/null +++ b/tests/test_protocol.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +"""s10_robot 协议层单元测试（不连机器人）。 + +运行:: + + uv run python -m unittest discover -s tests -v +""" + +from __future__ import annotations + +import json +import struct +import sys +import threading +import unittest +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent.parent)) + +import s10_robot as s10 +from s10_robot import ( + C_AXIS, + C_HEARTBEAT, + C_STATUS, + FMT_JSON, + HEADER_LEN, + SYNC, + T_HEARTBEAT, + T_FAULT, + T_MOTION, + Frame, + S10Client, + S10CommandError, + build_asdu, + build_frame, + parse_frames, +) + + +def make_frame(type_: int, command: int, items: dict, report_id: int = 0) -> bytes: + return build_frame(build_asdu(type_, command, items), report_id) + + +class FrameTest(unittest.TestCase): + def test_header_layout(self): + asdu = build_asdu(0x00100064, 5, {}) + packet = build_frame(asdu, report_id=0x1234) + self.assertEqual(packet[:4], SYNC) + self.assertEqual(len(packet), HEADER_LEN + len(asdu)) + length, rid = struct.unpack_from(" **资料依据** +> - 《山猫S10 软件开发指南》V1.0.2（2026-10-13）第 1 章「本体监控协议」 +> - 《DeepRoboticsLab 2.5.4 备份与密钥分析》（2026-10-29）连接与加密实测部分 +> +> **整理日期：**2026-10-29 +> +> 本文只整理「遥控（手动/半自主）控制」所需的内容：连接方式、帧格式、控制类指令、必要的状态回读。 +> 导航任务下发、CPU/电池等设备监控、ROS2 话题不在本文范围内。 + +--- + +## 0. 一分钟速查 + +| 功能 | Type | Command | 关键 Items | 响应 | +|---|---|---|---|---| +| 心跳（保活 + 开启状态上报） | `0x00100064` | `0x00000005` | 无 | 有 | +| 使用模式切换 | `0x00100002` | `0x00500002` | `Mode` | 有 | +| 运动状态转换（站/趴/阻尼） | `0x00100001` | `0x00200002` | `MotionParam` | 有 | +| 运动步态切换 | `0x00100001` | `0x00300002` | `GaitParam` | 有 | +| 运动控制（轴指令） | `0x00100001` | `0x00100002` | `X/Y/Z/Roll/Pitch/Yaw` | **无响应帧** | +| 运动控制（速度指令） | `0x00100001` | `0x00110002` | `X/Y/Z/Roll/Pitch/Yaw` | **无响应帧** | +| 自定义灯语 | `0x00100005` | `0x00200002` | `CustomMode`、`Led[]` | 有 | +| 自主充电 | `0x00100004` | `0x00100002` | `Charge` | 有 | +| 运动 SDK 模式 | `0x00100005` | `0x00300002` | `SDKEnable`、`Frequency` | 有 | +| 休眠模式设置 | `0x00100002` | `0x00400002` | `Sleep`、`Auto`、`Time` | 有 | + +十进制换算（JSON 里必须写数字，不能写带空格的十六进制）： + +| 十六进制 | 十进制 | 十六进制 | 十进制 | +|---|---|---|---| +| `0x00100001` | 1048577 | `0x00000005` | 5 | +| `0x00100002` | 1048578 | `0x00100002` | 1048578 | +| `0x00100004` | 1048580 | `0x00110002` | 1114114 | +| `0x00100005` | 1048581 | `0x00200002` | 2097154 | +| `0x00100064` | 1048676 | `0x00300002` | 3145730 | +| `0x0010007f` | 1048703 | `0x00400002` | 4194306 | +| `0x00f00000` | 15728640 | `0x00500002` | 5242882 | + +--- + +## 1. 连接与帧格式 + +### 1.1 连接参数 + +| 项目 | 值 | +|---|---| +| 机器人地址（实测） | `10.21.33.103` | +| TLS（TCP + 加密） | 端口 **30003** | +| DTLS（UDP + 加密） | 端口 **30004** | +| 角色 | 机器人为服务端，上位机/外部板卡为客户端 | +| 加密 | 默认开启；关闭需修改 `robotserver` 配置文件（联系技术支持），系统更新不会覆盖该配置 | + +**安全机制是 mTLS 双向认证**（实测结论，不是可选的）： + +- 客户端必须提供 `client_chain.crt` + `client.key`，服务端用同一 CA 链验证； +- 不带客户端证书，服务端直接返回 `TLSV13_ALERT_CERTIFICATE_REQUIRED`； +- 明文 TCP 会被服务端用 TLS alert 拒绝（`15 03 03 00 02 02 16`）； +- 实测协商结果：TLSv1.3 / `TLS_AES_256_GCM_SHA384`，服务端证书 `CN=server.local`，可用导出的 `root_ca.crt` 校验通过。 + +证书材料来自 App 的 `assets/certs/`，运行时复制到 `filesDir/certs/`： + +| 文件 | 说明 | +|---|---| +| `client.key` | 客户端私钥，RSA 2048 / PKCS#8 / PEM，无口令 | +| `client_chain.crt` | 客户端证书链（client.local ← TLS CA ← Project CA ← Root CA） | +| `root_ca.crt` | 根 CA，有效期 2026-04-03 ~ 2036-03-31 | + +> 客户端叶子证书有效期 2026-04-03 ~ 2027-04-03，过期后需重新获取。 +> TCP 与 DTLS 复用同一套证书和同一个 TLS 上下文，不存在单独的对称密钥或 PSK。 + +### 1.2 协议帧（16 字节头 + ASDU） + +| 偏移 | 长度 | 字段 | 说明 | +|---|---|---|---| +| 0 | 4 | 同步字符 | 固定 `EB 91 EB 90` | +| 4 | 2 | 长度 | ASDU 字节数，**小端**；单帧 ASDU ≤ 1000 字节，分包总长 ≤ 65535 | +| 6 | 2 | 报文 ID | 小端，请求/响应同值，0 → 65535 循环；分包须一致 | +| 8 | 1 | ASDU 格式 | `0x00` = XML，`0x01` = JSON | +| 9 | 1 | 当前包编号 | 从 0 递增，≤ 255 | +| 10 | 1 | 协议版本 | 固定 `0x01` | +| 11 | 5 | 预留 | 全 0 | + +### 1.3 ASDU 结构 + +根节点固定为 `PatrolDevice`，四个字段： + +| 字段 | 含义 | +|---|---| +| `Type` | 消息类型 | +| `Command` | 消息命令码 | +| `Time` | 本地时区发送时间，`YYYY-MM-DD HH:MM:SS`（实测响应带毫秒，如 `2026-10-29 19:50:18.403`，两种均可） | +| `Items` | 参数项 | + +推荐 JSON（官方说明：JSON 处理更快、支持的消息类型更全）。 + +### 1.4 数值写法（容易踩坑） + +官方文档示例里写的是 `"Type": 0x0010 0064` 这种**带空格的十六进制，这不是合法 JSON**。实际发送必须用整数： + +```json +{"PatrolDevice":{"Type":1048676,"Command":5,"Time":"2026-01-01 00:00:00","Items":{}}} +``` + +--- + +## 2. 手动遥控操作时序 + +``` +① 建立 mTLS 连接 (30003/TCP 或 30004/UDP) + │ +② 心跳 0x00100064 / 0x00000005，频率 ≥ 1Hz（持续保活） + │ └─ 机器人开始向该 IP:端口 上报状态（2Hz/10Hz/1Hz） + │ +③ 使用模式 0x00100002 / 0x00500002 Mode=0 常规（手动） + │ +④ 运动状态 0x00100001 / 0x00200002 MotionParam=1 站立 + │ └─ ASDU 下发站立完成后会自动进入 RL 控制状态 + │ +⑤ 步态 0x00100001 / 0x00300002 GaitParam=0x1001 基础 + │ （必须在机器人完全停止后下发） + │ +⑥ 轴指令 0x00100001 / 0x00100002 持续下发 X/Y/Yaw（建议 ≥10Hz） + │ （无响应帧，靠状态上报确认） + │ +⑦ 结束：轴指令全 0 → 趴下 MotionParam=4，或直接断开 +``` + +**导航（半自主）路线**与上面的差异： + +``` +③ Mode=1 导航模式 +⑤ GaitParam=0x3002（平地导航）/ 0x3003（楼梯导航，须在进楼梯前切换） +⑥ 改用速度指令 0x00100001 / 0x00110002（单位 m/s、rad/s，仅导航模式生效，建议 10Hz） +``` + +**关键约束** + +- 心跳必须持续发：连续 **2 秒** 未收到就关闭主动上报，需重新建立连接。 +- 轴指令/速度指令要求 **2 秒内来自同一客户端**，否则返回 `0xE006`。 +- 步态切换必须在机器人**完全停止**后下发，否则指令被缓存、停机后才执行。 +- 站立后：走 **ASDU** 会自动进 RL 控制，可直接发速度/步态；走 **ROS2 话题**不会自动进，需要再补一条 RL 控制指令。 +- 轴指令和速度指令**不返回响应帧**，只能靠状态上报判断是否生效。 + +--- + +## 3. 控制类指令详解 + +### 3.1 心跳指令 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100064` | `0x00000005` | 心跳，Items 为空 | + +```json +{"PatrolDevice":{"Type":1048676,"Command":5,"Time":"2026-01-01 00:00:00","Items":{}}} +``` + +- 建议 ≥ 1Hz。 +- 机器人会向**持续发送心跳的 IP 和端口**主动上报实时状态与异常状态。 +- 超时 2 秒关闭主动上报，超时后需要重新建立链接。 + +### 3.2 使用模式切换 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100002` | `0x00500002` | 切换机器人使用模式 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `Mode` | int | `0` 常规模式 / `1` 导航模式 / `2` 辅助模式 | + +```json +{"PatrolDevice":{"Type":1048578,"Command":5242882,"Time":"2026-01-01 00:00:00","Items":{"Mode":0}}} +``` + +- 常规模式下支持轴指令；导航模式下支持导航任务。 +- 切换结果需通过状态上报 `ControlUsageMode` 确认。 + +### 3.3 运动状态转换 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100001` | `0x00200002` | 转换机器人运动状态 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `MotionParam` | int | `-2` 软急停 / `0` 默认（初始、趴卧）/ `1` 站立 / `2` 关节阻尼 / `4` 趴下 / `5` 标零 / `17` RL 控制 / `0x1001` 阻尼趴下 | + +```json +{"PatrolDevice":{"Type":1048577,"Command":2097154,"Time":"2026-01-01 00:00:00","Items":{"MotionParam":1}}} +``` + +> 「空闲」与「软急停」只能查询，**不能通过该指令下发转换**。 +> 用途：`MotionParam=1` 站立、`4` 趴下是最常用的两条；`2` 关节阻尼可作为软急停后的安全态。 + +### 3.4 运动步态切换 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100001` | `0x00300002` | 切换运动步态 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `GaitParam` | int | `0x1001` 基础（常规）/ `0x1003` 楼梯（常规）/ `0x3002` 平地（导航）/ `0x3003` 楼梯（导航） | + +```json +{"PatrolDevice":{"Type":1048577,"Command":3145730,"Time":"2026-01-01 00:00:00","Items":{"GaitParam":4097}}} +``` + +- 步态切换会自动切到对应运动模式，运动模式切换也会自动切到默认步态。 +- 二次开发做自主导航时**必须**用导航专用步态：平地 `0x3002`，楼梯 `0x3003`，且楼梯步态要在**进楼梯前**切换，严禁进楼梯后再切。 +- 必须停机后下发。 + +### 3.5 运动控制（轴指令） + +| Type | Command | 说明 | +|---|---|---| +| `0x00100001` | `0x00100002` | 六自由度速度比例控制，**无响应帧** | + +```json +{"PatrolDevice":{"Type":1048577,"Command":1048578,"Time":"2026-01-01 00:00:00","Items":{"X":0.0,"Y":0.0,"Z":0.0,"Roll":0.0,"Pitch":0.0,"Yaw":0.0}}} +``` + +| 参数 | 含义 | 单位 | 范围 | +|---|---|---|---| +| `X` | 前后速度 | %（相对最大速度比例） | [-1, 1] | +| `Y` | 左右速度 | % | [-1, 1] | +| `Z` | 高度方向速度 | % | [-1, 1] | +| `Roll` | 翻滚角速度 | % | [-1, 1] | +| `Pitch` | 俯仰角速度 | % | [-1, 1] | +| `Yaw` | 偏航角速度 | % | [-1, 1] | + +- 仅常规模式和辅助模式可用（导航模式请用速度指令）。 +- **基础步态只响应 X、Y、Yaw**；Z、Roll、Pitch、Yaw 的额外自由度仅在特殊步态下有效。 +- 正方向按右手坐标系；X 正为前进、Y 正为左移、Yaw 正为逆时针。 +- 需要连续下发维持运动；停止时发全 0。 + +### 3.6 运动控制（速度指令） + +| Type | Command | 说明 | +|---|---|---| +| `0x00100001` | `0x00110002` | 真实物理量速度控制，**无响应帧** | + +```json +{"PatrolDevice":{"Type":1048577,"Command":1114114,"Time":"2026-01-01 00:00:00","Items":{"X":0.0,"Y":0.0,"Z":0.0,"Roll":0.0,"Pitch":0.0,"Yaw":0.0}}} +``` + +| 参数 | 含义 | 单位 | 范围 | +|---|---|---|---| +| `X` | 前后速度 | m/s | ±1.67 | +| `Y` | 左右速度 | m/s | ±0.4 | +| `Z` | 高度方向速度 | m/s | — | +| `Roll` | 翻滚角速度 | rad/s | — | +| `Pitch` | 俯仰角速度 | rad/s | — | +| `Yaw` | 偏航角速度 | rad/s | ±1.0 | + +- 字段与轴指令完全相同，差别是**不做速度换算、原样下发执行端**，单位为物理量。 +- **仅导航模式生效**，建议 10Hz 下发。 + +### 3.7 自定义灯语 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100005` | `0x00200002` | 分别设置头灯、尾灯的灯光模式/颜色/占空比 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `CustomMode` | bool | `true` 开启自定义 / `false` 关闭 | +| `Led[0]` | object | 头灯 | +| `Led[1]` | object | 尾灯 | +| `Led[].Type` | int | `0` 常亮 / `1` 填充式流水 / `2` 移动式流水 / `3` 渐变式流水 / `4` 呼吸 / `5` 单色闪烁 | +| `Led[].Color` | array\ | `0` 黑 / `1` 白 / `2` 绿 / `3` 蓝 | +| `Led[].Cycle` | int | 占空比，单位 s；仅呼吸、闪烁模式使用 | + +```json +{"PatrolDevice":{"Type":1048581,"Command":2097154,"Time":"2026-01-01 00:00:00","Items":{"CustomMode":true,"Led":[{"Type":0,"Color":[1],"Cycle":1},{"Type":0,"Color":[2],"Cycle":1}]}}} +``` + +> 官方原文的 JSON 示例缺逗号、`"Led"` 后直接跟数组，不是合法 JSON；上面是修正后的可直接发送版本。 + +### 3.8 自主充电 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100004` | `0x00100002` | 控制自主充电 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `Charge` | int | `0` 结束充电 / `1` 开始充电 / `2` 清除充电状态、恢复空闲 | + +```json +{"PatrolDevice":{"Type":1048580,"Command":1048578,"Time":"2026-01-01 00:00:00","Items":{"Charge":1}}} +``` + +> `Charge=2` 会强制切断充电电流、状态归零并清除相关错误记录，**有风险，慎用**。 + +### 3.9 运动 SDK 模式 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100005` | `0x00300002` | 进入/退出 SDK 运动控制模式 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `SDKEnable` | bool | `true` 开启 / `false` 关闭 | +| `Frequency` | int | 关节话题数据发布频率，[1, 200] 且能整除 1000 | + +```json +{"PatrolDevice":{"Type":1048581,"Command":3145730,"Time":"2026-01-01 00:00:00.000","Items":{"SDKEnable":true,"Frequency":100}}} +``` + +### 3.10 休眠模式设置 + +| Type | Command | 说明 | +|---|---|---| +| `0x00100002` | `0x00400002` | 设置休眠/自动休眠 | + +| 参数 | 类型 | 取值 | +|---|---|---| +| `Sleep` | bool | `true` 进入休眠 / `false` 唤醒 | +| `Auto` | bool | 到时间是否自动休眠 | +| `Time` | uint（min） | 心跳断联后等待多久休眠 | + +```json +{"PatrolDevice":{"Type":1048578,"Command":4194306,"Time":"2026-01-01 00:00:00.000","Items":{"Sleep":false,"Auto":true,"Time":5}}} +``` + +--- + +## 4. 遥控必须回读的状态 + +心跳建立后机器人主动上报。上报地址 = 最近发心跳的 IP:端口。 + +| 上报 | Type | Command | 频率 | 遥控关注点 | +|---|---|---|---|---| +| 基础状态 | `0x00100064` | `0x00f00000` | 2Hz | `MotionState`、`Gait`、`ControlUsageMode`、`HES`、`Charge` | +| 运控状态 | `0x00100001` | `0x00f00000` | 10Hz | 实际速度、姿态、关节角，用于闭环 | +| 设备状态 | `0x00100002` | `0x00f00000` | 2Hz | 电池、电机温度、外设使能 | +| 异常状态 | `0x0010007f` | `0x00f00000` | 2Hz | 故障列表，状态变化时额外上报 | +| 导航状态 | `0x00100003` | `0x00f00000` | 1Hz | 导航任务状态、定位状态 | + +### 4.1 基础状态 `BasicStatus` + +| 字段 | 含义 | 取值 | +|---|---|---| +| `MotionState` | 运动状态 | `-2` 软急停 / `0` 默认 / `1` 站立 / `2` 关节阻尼 / `4` 趴下 / `5` 标零 / `17` RL 控制 / `0x1001` 阻尼趴下 | +| `Gait` | 步态 | `0x1001` 基础 / `0x1002` 高台 / `0x3002` 平地（导航）/ `0x3003` 楼梯（导航） | +| `HES` | 硬急停 | `0` 未触发 / `1` 已触发 | +| `ControlUsageMode` | 使用模式 | `0` 常规 / `1` 导航 / `2` 辅助 | +| `PowerManagement` | 电源模式 | `0` 常规 / `1` 单电池 | +| `Sleep` | 休眠 | `0` 非休眠 / `1` 休眠 / `2` 进入休眠中 | +| `Model` | 型号 | 如 `CD1` | +| `Version` | 版本 | `STD` 标准 / `PRO` 增强 | +| `Charge` | 充电状态 | `0x0000` 空闲 / `0x0001` 前往电桩 / `0x0002` 充电中 / `0x0003` 退出电桩 / `0x0004` 异常 / `0x0005` 在桩未充电 / `0x0006` 线充 | + +> 注意：步态上报里有 `0x1002` 高台，但《运动步态切换》指令表没列出 `0x1002`；下发时以指令表为准。 + +### 4.2 运控状态 `MotionStatus`（10Hz，闭环用） + +| 字段 | 含义 | 单位 | +|---|---|---| +| `LinearX` / `LinearY` | 自身坐标系 x/y 线速度 | m/s | +| `AngularZ` | yaw 角速度 | rad/s | +| `Roll` / `Pitch` / `Yaw` | 姿态角 | rad | +| `OmegaX/Y/Z` | IMU 三轴角速度 | rad/s | +| `AccX/Y/Z` | 三轴加速度 | m/s² | +| `Height` | 机身高度 | m | +| `Payload` | 负载 | kg | +| `RemainMile` | 预测剩余里程 | km | +| `Gait` / `MotionState` | 当前步态 / 运动状态 | — | +| `MotorStatus.Joint` | 16 关节角度数组 | rad | + +`MotorStatus.Joint` 顺序（16 个）：LeftFrontHipX、LeftFrontHipY、LeftFrontKnee、LeftFrontWheel、RightFrontHipX、RightFrontHipY、RightFrontKnee、RightFrontWheel、LeftBackHipX、LeftBackHipY、LeftBackKnee、LeftBackWheel、RightBackHipX、RightBackHipY、RightBackKnee、RightBackWheel。 + +### 4.3 异常状态 `ErrorList` + +每组含 `Code`、`Name`、`Severities`（`3` WARN / `4` ERROR / `5` FATAL）、`Resources`、`Source`、`Timestamp`、`Type`（`1` 开始/等级变化，`2` 消除）。遥控常见故障码： + +| 码 | 含义 | 码 | 含义 | +|---|---|---|---| +| `0x8001` | 电机温度过高 | `0x8101` | 电池电量低 | +| `0x8002` | 驱动器温度过高 | `0x8108` | 电池温度过高 | +| `0x8014` | 关节位置超限 | `0x810A` | 放电电流过大 | +| `0x8016` | 关节数据无效 | `0x8201` | IMU 发布超时/无更新 | +| `0x8006` | 驱动器通信超时 | `0x8204` | IMU 数据无效 | +| `0x8301` | CPU 温度过高 | `0x8304` | 可用内存过低 | + +--- + +## 5. 通用返回与错误码 + +除轴指令、速度指令外，所有控制指令都会返回同 Type/Command 的通用状态响应： + +```json +{"PatrolDevice":{"Type":1048676,"Command":5,"Time":"2026-10-29 19:50:18.403","Items":{"ErrorCode":0,"ErrorMessage":"Success"}}} +``` + +| ErrorCode | ErrorMessage | 含义 | +|---|---|---| +| `0x0000` | Success | 成功 | +| `0xE001` | Unsupported data format | 数据格式不支持（非 XML/JSON） | +| `0xE002` | Data parsing failed | 解析失败（缺 PatrolDevice/Items/Type/Command） | +| `0xE003` | Unsupported protocol type | 找不到 Type+Command 对应协议 | +| `0xE004` | Missing necessary fields | 缺少必要字段 | +| `0xE005` | Mismatched data type | 字段类型不匹配 | +| `0xE006` | Mismatched request client | 请求客户端不匹配（如 2 秒内轴指令来自不同客户端） | +| `0xE007` | No operation permission | 无操作权限 | +| `0xE008` | Not allowed operation | 不允许的操作 | +| `0xE009` | Operation failure | 操作失败 | +| `0xE00A` | Unsupported function | 不支持的功能（如标准版不支持导航） | +| `0xE00B` | Internal error | 内部错误 | + +> 实测返回里 `Items` 可能只带 `ErrorCode` 而不带 `ErrorMessage`，解析时两者都要兼容。 + +--- + +## 6. 注意事项汇总 + +1. **心跳是前提**：≥1Hz，2 秒断流即关闭上报并需重连。 +2. **mTLS 是强制的**：无客户端证书连不上；明文 TCP 直接被拒。 +3. **JSON 数值必须写整数**，不能用文档示例里的 `0x0010 0064` 带空格写法。 +4. **轴指令/速度指令没有响应帧**，必须靠状态上报验证；且 2 秒内必须同一客户端。 +5. **轴指令 vs 速度指令**：轴指令 = [-1,1] 比例、常规/辅助模式；速度指令 = 物理量、仅导航模式。 +6. **步态切换要停机**，否则被缓存延后执行。 +7. **楼梯步态先进后退**：必须在进入楼梯前切到 `0x3003`，严禁楼梯上切换。 +8. **基础步态只有 X/Y/Yaw 有效**，Z/Roll/Pitch 需要特殊步态。 +9. **站立后模式差异**：ASDU 站立自动进 RL 控制；ROS2 话题站立需手动补 RL 控制。 +10. **`Charge=2` 有风险**：强制断电、状态归零、清错误记录。 +11. **导航功能仅 PRO 版**，标准版会返回 `0xE00A`。 +12. 报文 ID 请求/响应同值，便于把异步上报与请求响应区分开。 + +--- + +## 7. 配合 `send_adsu.py` 使用 + +`send_adsu.py` 已实现帧封装 + mTLS 连接，`--type/--command` 接受任意进制写法，`Items` 用 JSON 传入。 + +```bash +# 心跳 +python3 send_adsu.py --type 0x00100064 --command 0x00000005 --items-json '{}' + +# 常规模式 +python3 send_adsu.py --type 0x00100002 --command 0x00500002 --items-json '{"Mode":0}' + +# 站立 +python3 send_adsu.py --type 0x00100001 --command 0x00200002 --items-json '{"MotionParam":1}' + +# 基础步态 +python3 send_adsu.py --type 0x00100001 --command 0x00300002 --items-json '{"GaitParam":4097}' + +# 前进（轴指令，无响应，需脚本循环维持） +python3 send_adsu.py --type 0x00100001 --command 0x00100002 \ + --items-json '{"X":0.3,"Y":0.0,"Z":0.0,"Roll":0.0,"Pitch":0.0,"Yaw":0.0}' + +# 导航模式下的速度指令（m/s、rad/s） +python3 send_adsu.py --type 0x00100001 --command 0x00110002 \ + --items-json '{"X":0.5,"Y":0.0,"Z":0.0,"Roll":0.0,"Pitch":0.0,"Yaw":0.0}' + +# 趴下 +python3 send_adsu.py --type 0x00100001 --command 0x00200002 --items-json '{"MotionParam":4}' +``` + +脚本当前默认**不校验证书链**（根 CA 缺 keyUsage 扩展，Python 3.13+ 严格校验会拒绝）；已用 `openssl s_client` 单独确认服务端证书可信。需要严格校验时加 `--verify`，但部分 Python 版本会失败。 + +> 做遥控闭环时不要每条指令新建连接：轴指令要求 2 秒内同一客户端，应在同一个 TLS 会话里循环发送，并同时读状态上报。