DeepRoboticsLab-S10-proto/status_probe.py

#!/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())