ros_contorl/scripts/lib/rdk_status.py

#!/usr/bin/env python3
"""读一次机械臂的"能不能动"状态，并给出**人话结论**。

必须在 ``/home/cc/rdk_py310/bin/python`` 下跑（flexivrdk 与 ROS 的 3.11 ABI 不兼容）。

为什么要有这个脚本
------------------
``run_real.sh`` 起来之后，如果机器人没使能，``ros2_control`` 会**卡在**
``on_activate`` 里等 ``operational``（``wait_for_operational`` 是死循环重试）。
卡住的表现是：

    · ``ros2 control list_controllers`` → ``Could not contact service``（不是"没起来"）
    · ``/joint_states`` 一条都不发
    · 日志里每 5 秒一句 ``Waiting for operational (status: ...)``

实测（2026-09-28）：状态是 ``IN_MANUAL_MODE`` —— 注意 ``robot.mode()`` 返回的是
``Mode.IDLE``，**看不出**这点，必须读 ``operational_status()``。示教器那个 Auto/Manual
开关没有 RDK 接口可以远程切（Flexiv 的安全设计），所以只能人到现场拨。
这个脚本的价值就是把"卡住了"翻译成"去拨那个开关"，而不是让人去查 ROS。

输出
----
``--out FILE`` 写 ``KEY=VALUE``（RDK 自己的日志会混在 stdout 里，所以结果单独落文件，
不去解析第三方日志）。失败分类用 :func:`classify_state`（纯函数，可单测）。
"""

from __future__ import annotations

import argparse
import json
import sys
from pathlib import Path
from typing import Dict, List, Optional, Sequence, Tuple

# 结论级别：ok / warn / fail（与 scripts/lib/usb_health.py 同一套词）
LEVEL_OK = "ok"
LEVEL_WARN = "warn"
LEVEL_FAIL = "fail"

# 现场那句"必须人去拨"的说明。preflight 与文档引用同一句话。
MANUAL_HINT = (
    "示教器切到 Auto（Remote）—— RDK **没有**远程切 Manual→Auto 的接口，"
    "这是 Flexiv 的安全设计，必须人到现场拨。切完**不用**重启栈："
    "run_real.sh 里的 wait_for_operational 会自动接着 Enable。"
)


def classify_state(
    *,
    connected: bool,
    operational: bool = False,
    op_status: str = "",
    mode: str = "",
    fault: bool = False,
    estop_released: bool = True,
    error: str = "",
) -> Tuple[str, str, List[str]]:
    """把机器人状态翻成 ``(级别, 一句话结论, 建议动作)``。

    判定顺序是**故意的**：故障/急停要排在模式前面 —— 急停没复位时模式可能还是
    Auto，只报"模式不对"会让人白跑一趟。
    """
    if not connected:
        return (
            LEVEL_FAIL,
            f"连不上 RDK：{error or '未知原因'}",
            [
                "确认网线接的是控制柜用户口、PC 与机器人同网段（先跑 "
                "bash scripts/check_robot_network.sh）。",
                "确认示教器上已开启 Remote/Ethernet（RDK）。",
                "如果 ros2_control 已经在跑，RDK 可能被它占着 —— 先停掉栈再试。",
            ],
        )
    if fault:
        return (
            LEVEL_FAIL,
            "机器人有故障（fault）",
            [
                "在示教器上清故障；没有示教器就重启控制柜。",
                "栈里的 wait_for_operational 也会自动 ClearFault 重试。",
            ],
        )
    if not estop_released:
        return (
            LEVEL_FAIL,
            "急停没复位（estop_released=False）",
            ["松开示教器/控制柜上的急停按钮，再按面板上的复位。"],
        )
    upper = (op_status or "").upper()
    if "MANUAL" in upper:
        return (
            LEVEL_FAIL,
            f"示教器在 Manual 模式（operational_status={op_status}）—— "
            "机器人不会使能，整条流程会一直等着",
            [MANUAL_HINT],
        )
    if operational:
        return (LEVEL_OK, f"已使能、可接收指令（mode={mode}）", [])
    return (
        LEVEL_FAIL,
        f"未使能（operational_status={op_status or '?'}, mode={mode or '?'}）",
        [
            MANUAL_HINT,
            "确认没有未清的故障、急停已复位。",
        ],
    )


def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
    """连一次机器人并读状态。返回给 classify_state 的关键字参数。"""
    import flexivrdk  # 延迟导入：让本模块在没有 flexivrdk 的环境里也能被单测导入

    out: Dict[str, object] = {
        "connected": False,
        "operational": False,
        "op_status": "",
        "mode": "",
        "fault": False,
        "estop_released": True,
        "error": "",
    }
    try:
        if iface:
            robot = flexivrdk.Robot(sn, [iface], True, False)
        else:
            robot = flexivrdk.Robot(sn)
    except Exception as exc:  # noqa: BLE001
        out["error"] = str(exc)
        return out

    out["connected"] = True
    if enable:
        try:
            robot.Enable()
        except Exception as exc:  # noqa: BLE001
            out["error"] = f"Enable 失败: {exc}"

    def _safe(fn, default):
        try:
            return fn()
        except Exception:  # noqa: BLE001
            return default

    out["mode"] = str(_safe(robot.mode, ""))
    out["operational"] = bool(_safe(robot.operational, False))
    out["op_status"] = str(_safe(robot.operational_status, ""))
    out["fault"] = bool(_safe(robot.fault, False))
    out["estop_released"] = bool(_safe(robot.estop_released, True))
    return out


def main(argv: Optional[Sequence[str]] = None) -> int:
    ap = argparse.ArgumentParser(description="读一次机械臂状态并给结论")
    ap.add_argument("--sn", required=True)
    ap.add_argument("--iface", default="")
    ap.add_argument("--enable", action="store_true", help="顺手尝试 Enable（会真的使能）")
    ap.add_argument("--out", default="", help="把 KEY=VALUE 写到这个文件")
    ap.add_argument("--json", action="store_true", help="把结果打到 stdout（JSON）")
    args = ap.parse_args(argv)

    state = _probe(args.sn, args.iface, args.enable)
    level, summary, actions = classify_state(**state)  # type: ignore[arg-type]
    payload = {**state, "level": level, "summary": summary, "actions": actions}

    if args.out:
        lines = [f"{k}={v}" for k, v in payload.items() if k != "actions"]
        lines.append("actions=" + " | ".join(actions))
        Path(args.out).write_text("\n".join(lines) + "\n", encoding="utf-8")
    if args.json:
        print(json.dumps(payload, ensure_ascii=False, indent=2))
    return 0 if level == LEVEL_OK else 1


if __name__ == "__main__":
    sys.exit(main())