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,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())
@@ -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("<HH", packet, 4)
+        self.assertEqual(length, len(asdu))
+        self.assertEqual(rid, 0x1234)
+        self.assertEqual(packet[8], FMT_JSON)
+        self.assertEqual(packet[9], 0)
+        self.assertEqual(packet[10], 0x01)
+        self.assertEqual(packet[11:16], b"\x00" * 5)
+
+    def test_asdu_is_valid_json_with_numeric_ids(self):
+        asdu = build_asdu(0x00100001, 0x00100002, {"X": 0.5})
+        doc = json.loads(asdu)
+        self.assertEqual(doc["PatrolDevice"]["Type"], 0x00100001)
+        self.assertEqual(doc["PatrolDevice"]["Command"], 0x00100002)
+        self.assertEqual(doc["PatrolDevice"]["Items"], {"X": 0.5})
+        self.assertRegex(doc["PatrolDevice"]["Time"], r"^\d{4}-\d{2}-\d{2} \d{2}:\d{2}:\d{2}$")
+
+    def test_parse_single(self):
+        packet = make_frame(T_MOTION, C_AXIS, {"X": 0.1}, report_id=7)
+        buf = bytearray(packet)
+        frames = parse_frames(buf)
+        self.assertEqual(len(frames), 1)
+        frame = frames[0]
+        self.assertEqual((frame.type, frame.command, frame.report_id), (T_MOTION, C_AXIS, 7))
+        self.assertEqual(frame.items, {"X": 0.1})
+        self.assertEqual(buf, bytearray())
+
+    def test_parse_split_across_reads(self):
+        packet = make_frame(T_HEARTBEAT, C_STATUS, {"BasicStatus": {"MotionState": 1}})
+        buf = bytearray()
+        frames = []
+        for i in range(0, len(packet), 5):
+            buf.extend(packet[i:i + 5])
+            frames.extend(parse_frames(buf))
+        self.assertEqual(len(frames), 1)
+        self.assertEqual(frames[0].items["BasicStatus"]["MotionState"], 1)
+
+    def test_parse_multiple_in_one_read(self):
+        data = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1}) + make_frame(
+            T_MOTION, C_STATUS, {"i": 2}
+        )
+        frames = parse_frames(bytearray(data))
+        self.assertEqual([f.items["i"] for f in frames], [1, 2])
+
+    def test_resync_after_garbage(self):
+        packet = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1})
+        frames = parse_frames(bytearray(b"\x00\xff\xeb\x91" + packet))
+        self.assertEqual(len(frames), 1)
+        self.assertEqual(frames[0].items, {"i": 1})
+
+    def test_bad_json_is_skipped(self):
+        asdu = b'{"PatrolDevice": not-json'
+        bad = build_frame(asdu)
+        good = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1})
+        frames = parse_frames(bytearray(bad + good))
+        self.assertEqual(len(frames), 1)
+        self.assertEqual(frames[0].items, {"i": 1})
+
+    def test_real_device_response(self):
+        # README 中实测返回的原始报文（无 ErrorMessage）
+        payload = (
+            '{"PatrolDevice":{"Command":5,"Items":{"ErrorCode":0},'
+            '"Time":"2026-10-29 19:50:18.403","Type":1048676}}'
+        ).encode()
+        buf = bytearray(build_frame(payload, report_id=3))
+        frames = parse_frames(buf)
+        self.assertEqual(len(frames), 1)
+        self.assertEqual(frames[0].type, 0x00100064)
+        self.assertEqual(frames[0].command, 5)
+        self.assertEqual(frames[0].items, {"ErrorCode": 0})
+        self.assertEqual(frames[0].report_id, 3)
+
+    def test_axis_asdu_too_long_raises(self):
+        with self.assertRaises(ValueError):
+            build_frame(b"x" * (s10.MAX_ASDU + 1))
+
+
+class ClientTest(unittest.TestCase):
+    def setUp(self):
+        self.client = S10Client()
+
+    def tearDown(self):
+        self.client.close()
+
+    def test_command_error(self):
+        err = S10CommandError(0xE006, "Mismatched request client")
+        self.assertIn("0xE006", str(err))
+        self.assertEqual(err.code, 0xE006)
+
+    def test_move_range_validation(self):
+        class FakeSock:
+            def sendall(self, _packet):
+                pass
+
+        self.client._sock = FakeSock()  # type: ignore[assignment]
+        with self.assertRaises(ValueError):
+            self.client.move(x=1.5)
+        with self.assertRaises(ValueError):
+            self.client.drive_velocity(x=2.0)
+        self.client.move(x=0.5, yaw=-0.5)  # 合法，不应抛异常
+
+    def test_successful_request_resolves_pending(self):
+        client = self.client
+        sent = []
+
+        class FakeSock:
+            def sendall(self, packet):
+                sent.append(packet)
+                rid = struct.unpack_from("<H", packet, 6)[0]
+                # 模拟机器人在同一连接上异步回包
+                threading.Thread(
+                    target=lambda: client._dispatch(
+                        Frame(T_MOTION, C_AXIS, "", {"ErrorCode": 0}, rid, FMT_JSON, 0, 1)
+                    ),
+                    daemon=True,
+                ).start()
+
+        client._sock = FakeSock()  # type: ignore[assignment]
+        result = client._request(T_MOTION, C_AXIS, {"X": 0.0}, timeout=2)
+        self.assertEqual(result, {"ErrorCode": 0})
+        self.assertEqual(len(sent), 1)
+
+    def test_error_response_raises(self):
+        client = self.client
+
+        class FakeSock:
+            def sendall(self, packet):
+                rid = struct.unpack_from("<H", packet, 6)[0]
+                client._dispatch(
+                    Frame(T_MOTION, C_AXIS, "",
+                          {"ErrorCode": 0xE006, "ErrorMessage": "Mismatched request client"},
+                          rid, FMT_JSON, 0, 1)
+                )
+
+        client._sock = FakeSock()  # type: ignore[assignment]
+        with self.assertRaises(S10CommandError) as ctx:
+            client._request(T_MOTION, C_AXIS, {}, timeout=2)
+        self.assertEqual(ctx.exception.code, 0xE006)
+
+    def test_status_frames_update_snapshot(self):
+        client = self.client
+        client._dispatch(
+            Frame(T_HEARTBEAT, C_STATUS, "",
+                  {"BasicStatus": {"MotionState": 17, "Gait": 0x1001, "HES": 0}},
+                  0, FMT_JSON, 0, 1)
+        )
+        self.assertEqual(client.basic_status["MotionState"], 17)
+        self.assertEqual(client.wait_motion_state({17}, timeout=0.5), 17)
+
+    def test_fault_start_and_stop(self):
+        client = self.client
+        client._dispatch(
+            Frame(T_FAULT, C_STATUS, "",
+                  {"ErrorList": [{"Code": 0x8014, "Type": 1, "Name": "joint_position_over_limit",
+                                  "Severities": [4]}]},
+                  0, FMT_JSON, 0, 1)
+        )
+        self.assertEqual(len(client.active_errors), 1)
+        client._dispatch(
+            Frame(T_FAULT, C_STATUS, "",
+                  {"ErrorList": [{"Code": 0x8014, "Type": 2}]}, 0, FMT_JSON, 0, 1)
+        )
+        self.assertEqual(client.active_errors, [])
+
+    def test_estop_aborts_wait(self):
+        client = self.client
+        client._dispatch(
+            Frame(T_HEARTBEAT, C_STATUS, "",
+                  {"BasicStatus": {"MotionState": 1, "HES": 1}}, 0, FMT_JSON, 0, 1)
+        )
+        with self.assertRaises(s10.S10Error):
+            client.wait_motion_state({17}, timeout=0.5)
+
+    def test_wait_timeout(self):
+        with self.assertRaises(s10.S10Timeout):
+            self.client.wait_motion_state({17}, timeout=0.2)
+
+
+if __name__ == "__main__":
+    unittest.main(verbosity=2)