真机开工体检四件套：USB 链路 / V4L 设备 / RDK 状态 / 手眼几何

这四个都是「现场看着正常、结果不对」的故障源，共同特征是**不报错**，所以要把它们
从流程里拎出来单独判，并且给出可执行的下一步，而不是只报一个失败。

scripts/lib/usb_health.py
  RealSense 协商到 480Mbps（USB 2.0）时能出图、不报错，只是帧率掉到 1/2 且
  深度不稳定。这里解析 sysfs 拓扑，把「速率对不对」独立于「有没有图」来判，
  并在 --watch 下统计掉线/重连次数（线/口/供电 Hub 的问题会表现为反复抖）。

scripts/lib/v4l_health.py
  触觉相机有两类完全不同的坏法，处置方式相反，以前都只表现为「没有 /dev/video*」：
    · unplugged —— sysfs 里也没有，是没插/线坏
    · wedged    —— sysfs 里在、/dev/video* 没有，是 USB 端点了卡死（要复位或重插）
  2026-09-28 现场左指就是 wedged（1-3.4.1 在 uvcvideo 下，video 节点缺失）。

scripts/lib/rdk_status.py
  ros2_control 卡在 wait_for_operational 时，现象是「栈起来了但关节全是 0、
  流程一直等」。原因全在 OperationalStatus 上，而它只有 RDK 读得到。
  这里把状态翻成人话 + 处置（Auto(Remote) / 松急停 / 清故障）。
  判据取自厂商头文件原话：IN_MANUAL_MODE = "need to switch to Auto (Remote) mode"。

scripts/lib/real_geometry_check.py
  手眼矩阵与悬停几何。手眼写坏不会报错，只会让位姿整体偏几毫米到几厘米；
  判据：R 正交（|RᵀR−I| ≤ 2e-3）、det(R)=+1、平移量级在 10–500mm。
  悬停几何直接复用 gauge_fsm.hover_z_guard（流程里唯一判定处，不另写一套）；
  工件面未标定（site.yaml 写 0.0 = 未知）时**明确报 FAIL 并说明判不了**，
  而不是按 0.0 算出一个假的「已修正」结论。

测试：test_usb_health / test_v4l_health / test_real_geometry_check 共 33 例，
全部钉在「判据要能报错」上（故意弄坏的输入必须被抓到）。

顺带 .gitignore 补 log_real/：真机各终端日志是运行产物，与 log_sim/ 对称。

scripts/lib/rdk_status.py

@@ -0,0 +1,175 @@
+#!/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())

scripts/lib/real_geometry_check.py

@@ -0,0 +1,202 @@
+#!/usr/bin/env python3
+"""开工前的**几何自检**：手眼标定矩阵 + 悬停高度会不会把工具捅进工件。
+
+这两件事都属于"配置/文件看着没问题，但一跑就把工件推走"的那一类，而且
+**征兆出现在别的环节**（S05 残差恒不收敛、看着像检测坏了）。所以放在开工前
+用几何查一遍，比在流程里翻日志便宜得多。
+
+1) 手眼矩阵自洽：``handeye/results/<SN>_eye_in_hand.yaml`` 的 ``T_4x4``
+   必须是刚体变换 —— R 正交、det=+1、平移量级合理（几厘米到几十厘米）。
+   标定中断/写坏了半个文件时，矩阵会是"差不多但不正交"的，用它算出来的位姿
+   会整体偏几毫米到几厘米，而**不会**报任何错。
+
+2) 悬停高度 vs 工具长：判据不在这里重写 —— 直接调用 ``hover_z_guard``
+   （``arm_application.gauge_fsm``），那是流程里唯一的判定处。
+   2026-09-24 的教训：悬停高按**相机**视距定，而工具比相机长 ⇒ 悬停时规尖
+   已经钻进工件 25mm，S05 横向对准把工件拖走 92mm。这里提前喊出来。
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import math
+import sys
+from pathlib import Path
+from typing import Any, Dict, List, Optional, Sequence
+
+ROOT = Path(__file__).resolve().parents[2]
+SCRIPTS_LIB = ROOT / "scripts" / "lib"
+
+# 平移量级合理区间 [m]：腕部相机 eye-in-hand 的平移是"法兰到相机"的距离。
+T_MIN_M = 0.01
+T_MAX_M = 0.50
+# 正交性容差：R^T R 与 I 的每个元素偏差上限
+ORTHO_TOL = 2e-3
+
+
+def _matmul3(a: Sequence[Sequence[float]], b: Sequence[Sequence[float]]) -> List[List[float]]:
+    return [
+        [sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)] for i in range(3)
+    ]
+
+
+def check_transform(T: Any) -> List[str]:
+    """检查 4x4 齐次矩阵是不是一个像样的刚体变换。返回问题列表（空=没问题）。"""
+    problems: List[str] = []
+    try:
+        rows = [list(map(float, r)) for r in T]
+    except (TypeError, ValueError) as exc:
+        return [f"T_4x4 里有非数字：{exc}"]
+    if len(rows) != 4 or any(len(r) != 4 for r in rows):
+        return [f"T_4x4 必须是 4x4，实际 {len(rows)}x{len(rows[0]) if rows else 0}"]
+    if any(abs(rows[3][i] - v) > 1e-9 for i, v in enumerate((0.0, 0.0, 0.0, 1.0))):
+        problems.append(f"最后一行应该是 [0,0,0,1]，实际 {rows[3]}")
+
+    R = [r[:3] for r in rows[:3]]
+    # R^T R 应该等于 I
+    Rt = [[R[j][i] for j in range(3)] for i in range(3)]
+    RtR = _matmul3(Rt, R)
+    worst = max(
+        abs(RtR[i][j] - (1.0 if i == j else 0.0)) for i in range(3) for j in range(3)
+    )
+    if worst > ORTHO_TOL:
+        problems.append(
+            f"旋转部分不正交（|RᵀR−I| 最大 {worst:.2e} > {ORTHO_TOL:.0e}）"
+            "—— 标定文件写坏了或标定没收敛；这种矩阵不会报错，只会让位姿整体偏"
+        )
+    det = (
+        R[0][0] * (R[1][1] * R[2][2] - R[1][2] * R[2][1])
+        - R[0][1] * (R[1][0] * R[2][2] - R[1][2] * R[2][0])
+        + R[0][2] * (R[1][0] * R[2][1] - R[1][1] * R[2][0])
+    )
+    if abs(det - 1.0) > 1e-3:
+        problems.append(f"det(R)={det:.4f}，应该是 +1（−1 表示镜像/坐标系搞反了）")
+
+    t = [rows[i][3] for i in range(3)]
+    norm = math.sqrt(sum(v * v for v in t))
+    if not (T_MIN_M <= norm <= T_MAX_M):
+        problems.append(
+            f"平移量级 {norm * 1000:.1f}mm 不在合理区间 "
+            f"[{T_MIN_M * 1000:.0f}, {T_MAX_M * 1000:.0f}]mm —— "
+            "要么单位不是米，要么标定结果跑飞了"
+        )
+    return problems
+
+
+def load_T(path: Path) -> Any:
+    import yaml
+
+    doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    if "T_4x4" not in doc:
+        raise KeyError(f"{path} 里没有 T_4x4（键：{sorted(doc)}）")
+    return doc["T_4x4"]
+
+
+def geometry_report(site_path: Path, sn: str) -> Dict[str, Any]:
+    """跑一遍两项几何检查，返回结构化结论。"""
+    sys.path.insert(0, str(SCRIPTS_LIB))
+    import export_site_env as ex
+
+    sys.path.insert(0, str(ROOT / "install" / "arm_application" / "lib" / "python3.11" / "site-packages"))
+    from arm_application.gauge_config import from_site
+    from arm_application.gauge_fsm import hover_z_guard
+
+    data = ex.load_site(site_path)
+    merged, _robot = ex.apply_active_arm(data, sn)
+    cfg = from_site(merged)
+
+    out: Dict[str, Any] = {"handeye": {}, "hover": {}}
+
+    he_file = Path(str((merged.get("paths") or {}).get("handeye_file") or ""))
+    out["handeye"]["file"] = str(he_file)
+    if not he_file.is_file():
+        out["handeye"]["problems"] = [f"手眼文件不存在：{he_file}"]
+    else:
+        try:
+            T = load_T(he_file)
+            out["handeye"]["translation_m"] = [T[i][3] for i in range(3)]
+            out["handeye"]["problems"] = check_transform(T)
+        except Exception as exc:  # noqa: BLE001
+            out["handeye"]["problems"] = [f"读手眼文件失败：{exc}"]
+
+    servo = merged.get("servo") or {}
+    stage2 = merged.get("stage2") or {}
+    hover = servo.get("hole_hover_z_m")
+    hover = float(hover) if hover is not None else float(stage2.get("hover_z_m") or 0.30)
+    gap = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
+    out["hover"] = {
+        "hole_hover_z_m": hover,
+        "tool_tip_below_flange_m": cfg.tool_tip_below_flange_m,
+        "workpiece_surface_z_m": cfg.workpiece_surface_z_m,
+        "final_gap_m": gap,
+        "suggested_z_m": hover,
+        "problems": [],
+        "uncalibrated_surface": cfg.workpiece_surface_z_m <= 0.0,
+    }
+    if out["hover"]["uncalibrated_surface"]:
+        # site.yaml 用 0.0 表示"还不知道"（见 gauge_config 的默认值注释）。这时
+        # hover_z_guard 拿不到真正的工件面，算出来的结论是**假的** —— 与其报一个
+        # 假的 WARN，不如直接说"这一项现在判不了，得先标定"。
+        out["hover"]["problems"] = [
+            "工件上表面 z 没标定（site.yaml 写 0.0 = 未知）—— 悬停高度**没有**可靠的"
+            "几何依据。判据要把「工具长」和「工件面」比，缺一个就算不出来；"
+            "而 S05 的横向对准正好是在悬停高度上做的：悬停时工具尖端低于工件面就会把"
+            "工件推走（2026-09-24 实测推走 92mm，之后残差恒 13mm 永不收敛，"
+            "看着像检测坏了）。"
+            "修法：跑一次二阶段定位（bash scripts/run_stage2_locate.sh --arm 89），"
+            "它会把实测的工件面 z 打出来，把这个值填回 "
+            "robots[<sn>].gauge_assembly.workpiece_surface_z_m，再重启流程。"
+        ]
+    else:
+        _fixed, why = hover_z_guard(
+            hover, cfg.workpiece_surface_z_m, cfg.tool_tip_below_flange_m, gap
+        )
+        out["hover"]["suggested_z_m"] = _fixed
+        out["hover"]["problems"] = [why] if why else []
+    return out
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+    ap = argparse.ArgumentParser(description="开工前几何自检（手眼 + 悬停高度）")
+    ap.add_argument("--sn", default="")
+    ap.add_argument("--site", default=str(ROOT / "config" / "site.yaml"))
+    ap.add_argument("--json", action="store_true")
+    args = ap.parse_args(argv)
+
+    rep = geometry_report(Path(args.site), args.sn)
+    if args.json:
+        print(json.dumps(rep, ensure_ascii=False, indent=2))
+    problems: List[str] = []
+    he = rep["handeye"]
+    print(f"  手眼：{he.get('file')}")
+    if he.get("translation_m"):
+        t = he["translation_m"]
+        print(f"    平移 [{t[0]:+.4f}, {t[1]:+.4f}, {t[2]:+.4f}] m")
+    for p in he.get("problems") or []:
+        print(f"    [FAIL] {p}")
+        problems.append(p)
+    if not (he.get("problems") or []):
+        print("    [OK  ] 矩阵自洽（正交、det=+1、量级合理）")
+
+    hv = rep["hover"]
+    print(
+        f"  悬停几何：法兰 {hv['hole_hover_z_m']:.4f} − 工具长 "
+        f"{hv['tool_tip_below_flange_m'] * 1000:.1f}mm → 尖端 "
+        f"{hv['hole_hover_z_m'] - hv['tool_tip_below_flange_m']:.4f}，"
+        f"工件面 {hv['workpiece_surface_z_m']:.4f}"
+        + ("（未标定）" if hv.get("uncalibrated_surface") else "")
+    )
+    for p in hv.get("problems") or []:
+        print(f"    [FAIL] {p}")
+        problems.append(p)
+    if not (hv.get("problems") or []):
+        print(
+            f"    [OK  ] 悬停时工具尖端在工件面上方（几何修正值 "
+            f"{hv['suggested_z_m']:.4f}）"
+        )
+    return 1 if problems else 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/lib/usb_health.py

@@ -0,0 +1,272 @@
+#!/usr/bin/env python3
+"""USB 链路体检：RealSense 的**协商速率**与**掉线/掉速**事件。
+
+为什么单独做这个检查
+--------------------
+2026-09-28 现场实测（D435 顶置相机）：相机原本协商到 USB 3.2、1280x720@30
+跑得好好的，中途日志出现
+
+    xioctl(UVCIOC_CTRL_QUERY) failed ... No such file or directory
+
+也就是设备**从总线上消失**，几秒后重新枚举 —— 但这次是 USB **2.1**。
+USB3 接口里 SuperSpeed 是一对独立的差分针脚，它们一旦接触不良（线松、座子磨损、
+线被机械臂拽过），USB2 的针脚往往还能通：于是设备"还在"，只是**悄悄掉速**。
+
+这种故障软件侧查不出来：
+  · ``lsusb`` 照样列出这台相机（它不显示速率）；
+  · 相机节点照样启动，只是分辨率/帧率被降级（日志里一句 WARN 很容易被刷掉）；
+  · 于是现象变成"流程里相机帧过期 / 粗定位找不到工件"，看起来像视觉算法的问题。
+
+一开始就报"协商速率不对"，能把这一类问题从"查算法"挪回"插紧线"。
+
+判定口径（这里就是**唯一一处**）
+--------------------------------
+  · 5000 Mbps  → USB3，OK（D435 可用 1280x720@30）
+  · 480 Mbps   → USB2，RealSense 会掉到 640x480@15 —— WARN 并给出拔插建议
+  · 12 / 1.5   → USB 全速/低速，串口转换器正常，不告警
+
+用法::
+
+    python3 scripts/lib/usb_health.py                  # 单次体检
+    python3 scripts/lib/usb_health.py --watch 10       # 盯 10 秒，报掉线/掉速次数
+    python3 scripts/lib/usb_health.py --json           # 给脚本消费
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import time
+from dataclasses import dataclass, asdict
+from pathlib import Path
+from typing import Dict, Iterable, List, Optional, Sequence, Tuple
+
+SYS_USB = Path("/sys/bus/usb/devices")
+
+# RealSense 的 product 字符串形如 "Intel(R) RealSense(TM) Depth Camera 435"
+REALSENSE_TOKEN = "RealSense"
+# 型号 → 期望的最小协商速率 [Mbps]
+REALSENSE_MODELS: Tuple[Tuple[str, int], ...] = (
+    ("435", 5000),
+    ("455", 5000),
+    ("415", 5000),
+    ("405", 5000),
+)
+
+USB3_MBPS = 5000
+USB2_MBPS = 480
+
+# 见到 480Mbps 时的排查口径。相机节点 realsense_color_pub 里也有一句等价的
+# 运行时提示（USB_RESEAT_HINT）—— test_usb_health.py 会钉住两处都提到
+# "lsusb -t"，防止两份说法各自演化成互相矛盾的建议。
+USB_RESEAT_HINT = (
+    "只协商到 480Mbps 通常是 SuperSpeed 那一对针脚没接上，不是相机坏了："
+    "拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口。"
+    "核对速率：lsusb -t（设备后面写 480M 还是 5000M）。"
+)
+
+
+@dataclass(frozen=True)
+class UsbDevice:
+    """一个 USB 设备（按 sysfs 的拓扑路径标识，如 ``1-3.2``）。"""
+
+    path: str
+    vid: str
+    pid: str
+    product: str
+    speed_mbps: Optional[int]
+    serial: str = ""
+
+    @property
+    def is_realsense(self) -> bool:
+        return REALSENSE_TOKEN.lower() in self.product.lower()
+
+    @property
+    def model(self) -> str:
+        """RealSense 型号（``d435`` / ``d405``）；不是 RealSense 就返回空串。"""
+        if not self.is_realsense:
+            return ""
+        for token, _ in REALSENSE_MODELS:
+            if token in self.product:
+                return f"d{token}"
+        return "realsense"
+
+
+@dataclass(frozen=True)
+class Finding:
+    """一条体检结论。``level`` 只允许 ok/warn/error，便于脚本判定成败。"""
+
+    level: str
+    text: str
+
+
+def classify_speed(speed_mbps: Optional[int]) -> str:
+    """把协商速率归类成 ``usb3`` / ``usb2`` / ``low`` / ``unknown``。
+
+    12 和 1.5 Mbps 是 USB 全速/低速（串口转换器、HID），属于正常，不是"掉速"。
+    """
+    if speed_mbps is None:
+        return "unknown"
+    if speed_mbps >= USB3_MBPS:
+        return "usb3"
+    if speed_mbps == USB2_MBPS:
+        return "usb2"
+    if speed_mbps <= 12:
+        return "low"
+    return "other"
+
+
+def _read(path: Path) -> str:
+    try:
+        return path.read_text(encoding="utf-8", errors="replace").strip()
+    except OSError:
+        return ""
+
+
+def _parse_speed(raw: str) -> Optional[int]:
+    """sysfs 的 speed 单位是 Mbps，可能带小数（``1.5``）。"""
+    try:
+        return int(round(float(raw)))
+    except (TypeError, ValueError):
+        return None
+
+
+def read_devices(root: Path = SYS_USB) -> List[UsbDevice]:
+    """列出当前 USB 设备（不需要 root）。"""
+    out: List[UsbDevice] = []
+    if not root.is_dir():
+        return out
+    for entry in sorted(root.iterdir()):
+        vid = _read(entry / "idVendor")
+        if not vid:
+            continue  # 不是设备节点（接口/端口目录没有 idVendor）
+        out.append(
+            UsbDevice(
+                path=entry.name,
+                vid=vid,
+                pid=_read(entry / "idProduct"),
+                product=_read(entry / "product"),
+                speed_mbps=_parse_speed(_read(entry / "speed")),
+                serial=_read(entry / "serial"),
+            )
+        )
+    return out
+
+
+def findings_for(devices: Sequence[UsbDevice]) -> List[Finding]:
+    """对一批设备给出体检结论（纯函数，便于单测）。"""
+    out: List[Finding] = []
+    cams = [d for d in devices if d.is_realsense]
+    if not cams:
+        out.append(
+            Finding(
+                "error",
+                "一台 RealSense 都没枚举到。拔插一次；确认没有 "
+                "realsense-viewer / 其它进程占着相机。",
+            )
+        )
+        return out
+
+    for cam in cams:
+        kind = classify_speed(cam.speed_mbps)
+        label = f"{cam.model or 'RealSense'}（{cam.path}）"
+        if kind == "usb3":
+            out.append(Finding("ok", f"{label} 协商 {cam.speed_mbps} Mbps（USB3）"))
+        elif kind == "usb2":
+            out.append(
+                Finding(
+                    "warn",
+                    f"{label} 只协商到 {cam.speed_mbps} Mbps（USB2）—— "
+                    f"图会掉到 640x480@15。{USB_RESEAT_HINT}",
+                )
+            )
+        else:
+            out.append(
+                Finding(
+                    "error",
+                    f"{label} 协商速率异常：{cam.speed_mbps} Mbps。"
+                    "换线/换口后重试。",
+                )
+            )
+    return out
+
+
+def has_errors(findings: Iterable[Finding]) -> bool:
+    return any(f.level == "error" for f in findings)
+
+
+def flap_events(snapshots: Sequence[Sequence[str]]) -> List[str]:
+    """从连续几次"设备路径快照"里找出掉线/重连事件。
+
+    ``snapshots`` 是每轮采样到的 RealSense 设备路径列表。返回人话描述，
+    空列表 = 没抖。
+    """
+    events: List[str] = []
+    for i in range(1, len(snapshots)):
+        before, after = set(snapshots[i - 1]), set(snapshots[i])
+        gone = sorted(before - after)
+        back = sorted(after - before)
+        if gone:
+            events.append(f"第 {i} 次采样：消失 {', '.join(gone)}")
+        if back:
+            events.append(f"第 {i} 次采样：出现 {', '.join(back)}")
+    return events
+
+
+def watch_realsense(
+    seconds: float, interval_s: float = 1.0, root: Path = SYS_USB
+) -> List[List[str]]:
+    """在 ``seconds`` 秒内反复采样 RealSense 的设备路径。"""
+    snaps: List[List[str]] = []
+    deadline = time.monotonic() + max(0.0, seconds)
+    while True:
+        snaps.append(sorted(d.path for d in read_devices(root) if d.is_realsense))
+        if time.monotonic() >= deadline:
+            break
+        time.sleep(interval_s)
+    return snaps
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+    ap = argparse.ArgumentParser(description="USB 链路体检（RealSense 速率/掉线）")
+    ap.add_argument("--watch", type=float, default=0.0, help="盯多少秒（默认单次）")
+    ap.add_argument("--json", action="store_true", help="输出 JSON")
+    args = ap.parse_args(argv)
+
+    devices = read_devices()
+    findings = findings_for(devices)
+    snaps = watch_realsense(args.watch) if args.watch > 0 else []
+    flaps = flap_events(snaps) if snaps else []
+    if flaps:
+        findings.append(
+            Finding(
+                "error",
+                f"{len(snaps)} 次采样里 RealSense 掉线/重连 {len(flaps)} 次："
+                + "；".join(flaps)
+                + "。这属于**物理链路**问题（线/座子/供电 Hub），"
+                "不是软件 —— 先插紧/换线再跑流程。",
+            )
+        )
+
+    payload: Dict[str, object] = {
+        "devices": [asdict(d) for d in devices if d.is_realsense],
+        "findings": [asdict(f) for f in findings],
+        "flaps": flaps,
+    }
+    if args.json:
+        print(json.dumps(payload, ensure_ascii=False, indent=2))
+    else:
+        for d in devices:
+            if d.is_realsense:
+                print(
+                    f"  {d.model or d.product:8} {d.path:8} "
+                    f"{str(d.speed_mbps) + ' Mbps':>10}  vid:pid={d.vid}:{d.pid}"
+                )
+        for f in findings:
+            mark = {"ok": "OK  ", "warn": "WARN", "error": "FAIL"}[f.level]
+            print(f"  [{mark}] {f.text}")
+    return 1 if has_errors(findings) else 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/lib/v4l_health.py

@@ -0,0 +1,178 @@
+#!/usr/bin/env python3
+"""触觉相机（V4L2）的设备存在性体检 —— 分清三种"没有图像"。
+
+为什么要分三种（2026-09-28 现场，代价是半小时）
+-----------------------------------------------
+左指触觉那次故障，症状只有一个："``/tactile/left/status`` 一条都不发"。
+但底层其实是三种**完全不同**的情况，处理方式也完全不同：
+
+1. ``未插`` —— ``/dev/v4l/by-path/...`` 这个软链就不存在。
+   → 作用：插线/换口（物理）。
+
+2. ``内核卡死``（``wedged``）—— **在总线上、但拿不到 /dev/video* 节点**。
+   实测路径：相机先出现"开得开、读不到帧"（``select() timeout`` 刷屏），
+   重连逻辑反复 open 同一个设备之后，设备进入内核级卡死；用框架自带的
+   ``sudo /usr/local/bin/orisys-usb-reset <devpath>``（unbind/bind 白名单脚本）
+   复位 —— 若 unbind 成功而 bind 卡住，就会变成现在这个状态：
+   ``/sys/bus/usb/devices/1-3.4.1`` 还在、接口还挂在 ``uvcvideo`` 上，
+   但 ``/dev/video*`` 一个都没有。**软件侧已经没救了**，只能：
+   在真终端里带密码跑一次 ``orisys-usb-reset``，或拔插一次该相机。
+
+3. ``可读但无帧`` —— 节点在、open 得到，但一帧都读不出来
+   （日志里 ``select() timeout``）。→ 多半是同一条 USB2 总线被别的流占满；
+   把抢带宽的设备（顶置 D435）挪到 USB3 口通常就好了。
+
+把这三者混成一句"触觉没数据"，就会去查错的方向：查 SDK、查 ros2 话题、
+查配置。所以这里给出**唯一一处**判定口径。
+"""
+
+from __future__ import annotations
+
+import argparse
+import json
+import os
+import re
+from dataclasses import asdict, dataclass
+from pathlib import Path
+from typing import List, Optional, Sequence
+
+V4L_BY_PATH = Path("/dev/v4l/by-path")
+SYS_USB = Path("/sys/bus/usb/devices")
+
+# /dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0
+#                                  │    │     └── 接口 1.0
+#                                  │    └── USB 端口链 3.4.1  ← 我们只认这一段
+#                                  └── 根 Hub 口（不是 USB 总线号！）
+# sysfs 里的设备名是 ``<bus>-3.4.1``，bus 号（1 还是 2）取不到的时候不能瞎猜，
+# 所以下面一律用 ``*-3.4.1`` 去 glob —— 端口链在同一条 PCI 控制器上是唯一的。
+_BY_PATH_RE = re.compile(r"usb-\d+:(\d+(?:\.\d+)*):")
+
+
+def parse_usb_portchain(by_path: str) -> str:
+    """从 by-path 软链名里取出**端口链**（``3.4.1``）；取不到返回空串。"""
+    m = _BY_PATH_RE.search(str(by_path))
+    return m.group(1) if m else ""
+
+
+def find_usb_devpath(portchain: str, root: Path = SYS_USB) -> str:
+    """端口链 → sysfs 里的完整设备名（``1-3.4.1``），给 orisys-usb-reset 用。
+
+    取不到（设备真的不在总线上）返回空串。**不要**拿 ``Mode.IDLE`` 猜 bus 号。
+    """
+    if not portchain or not root.is_dir():
+        return ""
+    hits = sorted(p.name for p in root.glob(f"*-{portchain}") if p.is_dir())
+    return hits[0] if hits else ""
+
+
+def usb_device_present(portchain: str, root: Path = SYS_USB) -> bool:
+    """该端口链上有没有 USB 设备（不管内核有没有给它 /dev/video*）。"""
+    return bool(find_usb_devpath(portchain, root))
+
+
+def classify(video_nodes: Sequence[str], usb_present: bool) -> str:
+    """``ok`` / ``wedged`` / ``unplugged`` —— 见模块开头那三种情况。"""
+    if video_nodes:
+        return "ok"
+    return "wedged" if usb_present else "unplugged"
+
+
+def video_nodes_for(by_path: str, root: Path = V4L_BY_PATH) -> List[str]:
+    """该 by-path 前缀下实际存在的 /dev/video* 节点（真读文件系统）。
+
+    比的是**文件名**而不是整条路径：``root`` 是可注入的（测试用假目录），
+    拿整条路径去 startswith 会永远匹配不上 —— 那种写法会让这个检查
+    "永远通过"，比不检查更坏。
+    """
+    if not root.is_dir():
+        return []
+    stem = Path(str(by_path).rsplit("-video-index", 1)[0]).name
+    out: List[str] = []
+    for entry in root.iterdir():
+        if entry.name.startswith(stem) and os.path.exists(entry):
+            out.append(str(entry))
+    return sorted(out)
+
+
+@dataclass(frozen=True)
+class Check:
+    name: str
+    by_path: str
+    devpath: str
+    state: str
+    nodes: List[str]
+
+    @property
+    def level(self) -> str:
+        return {"ok": "ok", "wedged": "error", "unplugged": "error"}[self.state]
+
+    def text(self) -> str:
+        if self.state == "ok":
+            return f"{self.name}：正常（{len(self.nodes)} 个节点）"
+        if self.state == "wedged":
+            return (
+                f"{self.name}：设备在总线上（{self.devpath}）但**没有 /dev/video* 节点** "
+                "—— 内核级卡死，软件侧救不回来。到真终端里跑 "
+                f"`sudo /usr/local/bin/orisys-usb-reset {self.devpath}`（要密码），"
+                "或把这只相机拔插一次；然后重启触觉节点。"
+            )
+        return f"{self.name}：没有插（{self.by_path} 不存在）—— 插线/换 USB 口。"
+
+
+def check_devices(
+    pairs: Sequence[tuple[str, str]],
+    v4l_root: Path = V4L_BY_PATH,
+    usb_root: Path = SYS_USB,
+) -> List[Check]:
+    """``pairs`` = [(名字, by-path 或空)]，返回每只的体检结论。
+
+    两个 root 都可注入，测试不需要真机（也就不会因为环境不同而"永远通过"）。
+    """
+    out: List[Check] = []
+    for name, by_path in pairs:
+        if not by_path:
+            out.append(Check(name, "", "", "unplugged", []))
+            continue
+        nodes = video_nodes_for(by_path, v4l_root)
+        portchain = parse_usb_portchain(by_path)
+        devpath = find_usb_devpath(portchain, usb_root)
+        out.append(
+            Check(
+                name=name,
+                by_path=by_path,
+                devpath=devpath or portchain,
+                state=classify(nodes, bool(devpath)),
+                nodes=nodes,
+            )
+        )
+    return out
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+    ap = argparse.ArgumentParser(description="V4L2 触觉相机存在性体检")
+    ap.add_argument(
+        "--dev",
+        action="append",
+        default=[],
+        metavar="NAME=PATH",
+        help="名字=by-path，可重复",
+    )
+    ap.add_argument("--json", action="store_true")
+    args = ap.parse_args(argv)
+
+    pairs = []
+    for item in args.dev:
+        name, _, path = str(item).partition("=")
+        pairs.append((name.strip() or "v4l", path.strip()))
+    checks = check_devices(pairs)
+
+    if args.json:
+        print(json.dumps([asdict(c) for c in checks], ensure_ascii=False, indent=2))
+    for c in checks:
+        mark = {"ok": "OK  ", "error": "FAIL"}[c.level]
+        print(f"  [{mark}] {c.text()}")
+    return 1 if any(c.level == "error" for c in checks) else 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())