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
2026-10-29 20:31:30 +08:00
commit 5e2017de69
8 changed files with 2115 additions and 0 deletions
@@ -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())