From 98f427784984d262e272a8294de822ea5631abce Mon Sep 17 00:00:00 2001 From: arm_dev Date: Mon, 28 Sep 2026 10:17:32 +0800 Subject: [PATCH] =?UTF-8?q?=E7=9C=9F=E6=9C=BA=E5=BC=80=E5=B7=A5=E4=BD=93?= =?UTF-8?q?=E6=A3=80=E5=9B=9B=E4=BB=B6=E5=A5=97=EF=BC=9AUSB=20=E9=93=BE?= =?UTF-8?q?=E8=B7=AF=20/=20V4L=20=E8=AE=BE=E5=A4=87=20/=20RDK=20=E7=8A=B6?= =?UTF-8?q?=E6=80=81=20/=20=E6=89=8B=E7=9C=BC=E5=87=A0=E4=BD=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 这四个都是「现场看着正常、结果不对」的故障源，共同特征是**不报错**，所以要把它们 从流程里拎出来单独判，并且给出可执行的下一步，而不是只报一个失败。 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/ 对称。 --- .gitignore | 4 + scripts/lib/rdk_status.py | 175 +++++++++++ scripts/lib/real_geometry_check.py | 202 +++++++++++++ scripts/lib/usb_health.py | 272 ++++++++++++++++++ scripts/lib/v4l_health.py | 178 ++++++++++++ .../test/test_real_geometry_check.py | 104 +++++++ src/arm_application/test/test_usb_health.py | 154 ++++++++++ src/arm_application/test/test_v4l_health.py | 111 +++++++ 8 files changed, 1200 insertions(+) create mode 100644 scripts/lib/rdk_status.py create mode 100644 scripts/lib/real_geometry_check.py create mode 100644 scripts/lib/usb_health.py create mode 100644 scripts/lib/v4l_health.py create mode 100644 src/arm_application/test/test_real_geometry_check.py create mode 100644 src/arm_application/test/test_usb_health.py create mode 100644 src/arm_application/test/test_v4l_health.py diff --git a/.gitignore b/.gitignore index a3bf20a..172fe3e 100644 --- a/.gitignore +++ b/.gitignore @@ -28,6 +28,10 @@ log/ build_sim/ install_sim/ log_sim/ +# 真机各终端（T1 栈 / T2 相机 / T3 触觉 / T4 夹爪）的日志与截图。 +# 与 log_sim/ 对称：日志是运行产物，不进 git；跑完要留档的证据（如相机截图） +# 单独放到 docs/ 或 PR 里，不要指望从 log_real/ 里找历史。 +log_real/ .colcon/ # Python diff --git a/scripts/lib/rdk_status.py b/scripts/lib/rdk_status.py new file mode 100644 index 0000000..9eb7dbf --- /dev/null +++ b/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()) diff --git a/scripts/lib/real_geometry_check.py b/scripts/lib/real_geometry_check.py new file mode 100644 index 0000000..690bff7 --- /dev/null +++ b/scripts/lib/real_geometry_check.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +"""开工前的**几何自检**：手眼标定矩阵 + 悬停高度会不会把工具捅进工件。 + +这两件事都属于"配置/文件看着没问题，但一跑就把工件推走"的那一类，而且 +**征兆出现在别的环节**（S05 残差恒不收敛、看着像检测坏了）。所以放在开工前 +用几何查一遍，比在流程里翻日志便宜得多。 + +1) 手眼矩阵自洽：``handeye/results/_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[].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()) diff --git a/scripts/lib/usb_health.py b/scripts/lib/usb_health.py new file mode 100644 index 0000000..7228168 --- /dev/null +++ b/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()) diff --git a/scripts/lib/v4l_health.py b/scripts/lib/v4l_health.py new file mode 100644 index 0000000..3a20365 --- /dev/null +++ b/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 ``（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 里的设备名是 ``-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()) diff --git a/src/arm_application/test/test_real_geometry_check.py b/src/arm_application/test/test_real_geometry_check.py new file mode 100644 index 0000000..eb08f06 --- /dev/null +++ b/src/arm_application/test/test_real_geometry_check.py @@ -0,0 +1,104 @@ +"""手眼矩阵的自洽判据必须能报错 —— 它是最容易"看着没问题"的一环。 + +一个"差不多但不正交"的矩阵（标定中断、文件写坏、少了最后一行）不会让任何东西 +报错：节点照样起来、照样算位姿，只是**整体偏几毫米到几厘米**，而现象出现在 +S05（残差恒不收敛），看起来像检测坏了。所以这里逐个钉住： + + · 形状不对（不是 4x4 / 最后一行不是 [0,0,0,1]） + · R 不正交（|RᵀR−I| 超容差） + · det(R) = −1（镜像：坐标系左右手搞反了） + · 平移量级离谱（单位不是米，或标定跑飞） + +外加一条"判据本身能失败"的测试：拿一个**故意坏的**矩阵喂进去，必须报错。 +""" + +from __future__ import annotations + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "scripts" / "lib")) + +from real_geometry_check import ORTHO_TOL, T_MAX_M, T_MIN_M, check_transform # noqa: E402 + +# 真机 89 的 handeye/results/89_eye_in_hand.yaml（取整到 1e-6） +GOOD_89 = [ + [0.01784499, -0.86878381, 0.49486994, -0.06085181], + [0.99977570, 0.02115147, 0.00108124, -0.00864320], + [-0.01140659, 0.49473964, 0.86896638, 0.12364093], + [0.0, 0.0, 0.0, 1.0], +] + + +def test_real_89_matrix_is_clean() -> None: + assert check_transform(GOOD_89) == [] + + +def test_identity_is_clean_except_translation_is_zero() -> None: + """单位矩阵的平移是 0 —— 那说明"没标定"，也要报出来。""" + problems = check_transform( + [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] + ) + assert len(problems) == 1 and "平移量级" in problems[0] + assert T_MIN_M > 0.0 + + +def test_wrong_shape_reports() -> None: + assert "4x4" in check_transform([[1, 0, 0], [0, 1, 0], [0, 0, 1]])[0] + assert "4x4" in check_transform([[1, 0, 0, 0]] * 2)[0] + + +def test_bad_last_row_reports() -> None: + bad = [row[:] for row in GOOD_89] + bad[3] = [0.0, 0.0, 0.001, 1.0] + assert any("最后一行" in p for p in check_transform(bad)) + + +def test_non_orthonormal_rotation_reports() -> None: + """把第一行放大 10% —— “差不多但不正交”的典型形态。""" + bad = [row[:] for row in GOOD_89] + bad[0][:3] = [v * 1.1 for v in bad[0][:3]] + problems = check_transform(bad) + assert any("不正交" in p for p in problems), problems + + +def test_ortho_tolerance_is_actually_tight_enough() -> None: + """容差不能宽到把"肉眼看不见的坏矩阵"放过。""" + bad = [row[:] for row in GOOD_89] + bad[0][:3] = [v * (1.0 + ORTHO_TOL * 5) for v in bad[0][:3]] + assert any("不正交" in p for p in check_transform(bad)) + + +def test_mirrored_rotation_reports_det_minus_one() -> None: + """右手系翻转：det=−1。现象是位姿整体镜像，很难靠肉眼发现。""" + mirrored = [ + [1.0, 0.0, 0.0, 0.05], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, -1.0, 0.12], # 翻一个轴 + [0.0, 0.0, 0.0, 1.0], + ] + problems = check_transform(mirrored) + assert any("det(R)" in p for p in problems), problems + + +def test_translation_out_of_range_reports() -> None: + far = [row[:] for row in GOOD_89] + far[0][3] = T_MAX_M * 3 + assert any("平移量级" in p for p in check_transform(far)) + # 整个平移向量缩到容差下限以下（只改一个分量不算 —— 量级看的是模长） + near = [row[:] for row in GOOD_89] + for i in range(3): + near[i][3] = T_MIN_M / 3 + assert any("平移量级" in p for p in check_transform(near)) + + +def test_non_numeric_reports_instead_of_crashing() -> None: + assert "非数字" in check_transform([["a", 0, 0, 0]] * 4)[0] + + +def test_the_predicate_can_actually_fail() -> None: + """判据要能报错：把好矩阵故意弄坏（整行缩 0.5%），必须能看出来。""" + bad = [row[:] for row in GOOD_89] + bad[0][:3] = [v * 1.005 for v in bad[0][:3]] + assert check_transform(bad) != [] + assert check_transform(GOOD_89) == [] diff --git a/src/arm_application/test/test_usb_health.py b/src/arm_application/test/test_usb_health.py new file mode 100644 index 0000000..f4cb5fc --- /dev/null +++ b/src/arm_application/test/test_usb_health.py @@ -0,0 +1,154 @@ +"""USB 链路体检的判据本身必须能报错（否则等于没检查）。 + +这些测试之所以重要：现场那台顶置 D435 是**悄悄掉速**的 —— 设备还在、节点还在 +发图，只是从 1280x720@30 掉到 640x480@15。软件里没有任何东西会"失败"，只有 +日志里一句被刷掉的 WARN。所以判据必须是显式的、可测的： + + · 5000 Mbps → OK + · 480 Mbps → WARN（并给出拔插建议） + · 一台都没有 → FAIL + · 8 秒内掉线重连 → FAIL（这是物理链路问题，不是软件） + +另外钉一条"不能各说各的"：相机节点 ``realsense_color_pub`` 的运行时提示和本 +lib 的提示必须给出**同一个核对手段**（lsusb -t），否则两边话术会各自演化。 +""" + +from __future__ import annotations + +import sys +from pathlib import Path + +import pytest + +ROOT = Path(__file__).resolve().parents[3] +SCRIPTS_LIB = ROOT / "scripts" / "lib" +sys.path.insert(0, str(SCRIPTS_LIB)) + +import usb_health # noqa: E402 +from usb_health import Finding, UsbDevice, classify_speed, findings_for, flap_events # noqa: E402 + + +def _cam(path: str, product: str, speed: int | None) -> UsbDevice: + return UsbDevice( + path=path, vid="8086", pid="0b07", product=product, speed_mbps=speed + ) + + +D435 = "Intel(R) RealSense(TM) Depth Camera 435" +D405 = "Intel(R) RealSense(TM) Depth Camera 405" + + +def _levels(findings) -> list: + return [f.level for f in findings] + + +def test_classify_speed_buckets() -> None: + assert classify_speed(5000) == "usb3" + assert classify_speed(10000) == "usb3" + assert classify_speed(480) == "usb2" + assert classify_speed(12) == "low" + assert classify_speed(1) == "low" + assert classify_speed(None) == "unknown" + + +def test_usb3_camera_is_ok() -> None: + findings = findings_for([_cam("2-3.4.3", D405, 5000)]) + assert _levels(findings) == ["ok"] + + +def test_usb2_camera_warns_and_tells_you_how_to_check() -> None: + findings = findings_for([_cam("1-3.2", D435, 480)]) + assert _levels(findings) == ["warn"] + text = findings[0].text + assert "480" in text + assert "lsusb -t" in text, "必须给出可执行的核对手段，不能只说'掉速了'" + + +def test_no_realsense_at_all_is_an_error() -> None: + findings = findings_for([]) + assert _levels(findings) == ["error"] + + +def test_serial_adapter_alone_still_reports_error() -> None: + """只有串口转换器、没有相机时必须 FAIL —— 不能把"有 USB 设备"当成有相机。""" + dongle = UsbDevice( + path="1-3.3", vid="067b", pid="2303", product="USB-Serial", speed_mbps=12 + ) + assert _levels(findings_for([dongle])) == ["error"] + + +def test_the_predicate_can_actually_fail() -> None: + """判据要能真报错：造一个 USB2 的相机，必须不是 ok。""" + assert "ok" not in _levels(findings_for([_cam("1-3.2", D435, 480)])) + assert usb_health.has_errors(findings_for([])) is True + assert usb_health.has_errors(findings_for([_cam("1-3.2", D435, 480)])) is False + + +def test_flap_events_detects_disappear_and_return() -> None: + snaps = [["2-3.4.3"], ["2-3.4.3"], [], ["2-3.4.3"]] + events = flap_events(snaps) + assert len(events) == 2, events + assert "消失" in events[0] and "出现" in events[1] + + +def test_flap_events_is_empty_when_stable() -> None: + assert flap_events([["1-3.2"], ["1-3.2"], ["1-3.2"]]) == [] + assert flap_events([]) == [] + assert flap_events([["1-3.2"]]) == [] + + +def test_flap_events_detects_a_port_change_without_disappearing() -> None: + """掉速的典型形态：设备没消失，但换了端口（USB3 → USB2 重新枚举）。""" + events = flap_events([["2-3.4.3"], ["1-3.2"]]) + assert len(events) == 2 + assert any("消失" in e and "2-3.4.3" in e for e in events) + assert any("出现" in e and "1-3.2" in e for e in events) + + +def test_read_devices_parses_a_fake_sysfs(tmp_path: Path) -> None: + """不依赖真机：用假 sysfs 目录验证解析（含 1.5 这种小数速率）。""" + dev = tmp_path / "1-3.2" + dev.mkdir() + (dev / "idVendor").write_text("8086\n", encoding="utf-8") + (dev / "idProduct").write_text("0b07\n", encoding="utf-8") + (dev / "product").write_text(D435 + "\n", encoding="utf-8") + (dev / "speed").write_text("480\n", encoding="utf-8") + iface = tmp_path / "1-3.2:1.0" # 接口目录没有 idVendor，必须被跳过 + iface.mkdir() + + devices = usb_health.read_devices(tmp_path) + assert [d.path for d in devices] == ["1-3.2"] + assert devices[0].is_realsense and devices[0].model == "d435" + assert devices[0].speed_mbps == 480 + + (dev / "speed").write_text("1.5\n", encoding="utf-8") + assert usb_health.read_devices(tmp_path)[0].speed_mbps == 2 + assert classify_speed(usb_health.read_devices(tmp_path)[0].speed_mbps) == "low" + + +def test_model_detection_does_not_confuse_435_and_405() -> None: + assert _cam("1-3.2", D435, 5000).model == "d435" + assert _cam("2-3.4.3", D405, 5000).model == "d405" + + +def test_two_sources_of_advice_agree_on_lsusb() -> None: + """相机节点的运行时提示与本 lib 的提示必须给出同一种核对手段。""" + node_py = ( + ROOT + / "src" + / "arm_application" + / "arm_application" + / "realsense_color_pub.py" + ).read_text(encoding="utf-8") + assert "lsusb -t" in node_py, ( + "realsense_color_pub 的 USB 提示里没有 lsusb -t —— 与 " + "scripts/lib/usb_health.py 的话术脱节了" + ) + assert "lsusb -t" in usb_health.USB_RESEAT_HINT + assert "SuperSpeed" in node_py and "SuperSpeed" in usb_health.USB_RESEAT_HINT + + +@pytest.mark.parametrize("level", ["ok", "warn", "error"]) +def test_finding_levels_are_from_a_fixed_vocabulary(level: str) -> None: + """级别只能是这三个词 —— 脚本按它们判定成败，多一个词就没人处理。""" + assert level in {"ok", "warn", "error"} diff --git a/src/arm_application/test/test_v4l_health.py b/src/arm_application/test/test_v4l_health.py new file mode 100644 index 0000000..2537675 --- /dev/null +++ b/src/arm_application/test/test_v4l_health.py @@ -0,0 +1,111 @@ +"""三种"没有图像"必须分开报：未插 / 内核卡死 / 可读但无帧。 + +现场那次（2026-09-28 左指触觉）就是混在一起了：``/tactile/left/status`` 一条不发， +我按"没插/驱动不对"查了很久，实际是： + + 设备在总线上（``1-3.4.1`` 还在、接口还挂在 uvcvideo 上） + 但 /dev/video* 一个都没有 ← 内核级卡死，只能 orisys-usb-reset 或拔插 + +所以这里的判据必须**能区分**这两种"没有节点"的情况 —— 测试里专门钉住 +``wedged != unplugged``：把它们合成一个词，就等于又把排查方向搞错一次。 +""" + +from __future__ import annotations + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "scripts" / "lib")) + +import v4l_health # noqa: E402 +from v4l_health import ( # noqa: E402 + check_devices, + classify, + find_usb_devpath, + parse_usb_portchain, +) + +LEFT_BY_PATH = ( + "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0" +) +RIGHT_BY_PATH = ( + "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.2:1.0-video-index0" +) + + +def test_portchain_parsing_matches_real_by_path_format() -> None: + """by-path 里 ``usb-0:3.4.1:1.0`` 的端口链是 3.4.1（0 是根 Hub 口，不是总线号）。""" + assert parse_usb_portchain(LEFT_BY_PATH) == "3.4.1" + assert parse_usb_portchain(RIGHT_BY_PATH) == "3.4.2" + assert parse_usb_portchain("/dev/video0") == "" + assert parse_usb_portchain("") == "" + + +def test_classify_distinguishes_wedged_from_unplugged() -> None: + assert classify(["a"], True) == "ok" + assert classify(["a"], False) == "ok" + # 这两种输入都是"没有节点"，但结论必须不同 + assert classify([], True) == "wedged" + assert classify([], False) == "unplugged" + assert classify([], True) != classify([], False) + + +def test_find_usb_devpath_picks_the_device_not_the_interface(tmp_path: Path) -> None: + """``*-3.4.1`` 只能匹配设备目录，不能匹配 ``1-3.4.1:1.0`` 这种接口目录。""" + (tmp_path / "1-3.4.1").mkdir() + (tmp_path / "1-3.4.1:1.0").mkdir() + assert find_usb_devpath("3.4.1", tmp_path) == "1-3.4.1" + assert find_usb_devpath("3.4.9", tmp_path) == "" + + +def test_check_devices_reports_ok_when_nodes_exist(tmp_path: Path) -> None: + v4l = tmp_path / "v4l" + v4l.mkdir() + (v4l / (Path(RIGHT_BY_PATH).name)).symlink_to(tmp_path / "fake-video-node") + (tmp_path / "fake-video-node").write_text("", encoding="utf-8") + usb = tmp_path / "usb" + (usb / "1-3.4.2").mkdir(parents=True) + + checks = check_devices([("右指", RIGHT_BY_PATH)], v4l_root=v4l, usb_root=usb) + assert checks[0].state == "ok" + assert checks[0].level == "ok" + + +def test_check_devices_reports_wedged_for_the_real_failure(tmp_path: Path) -> None: + """复刻现场：设备在总线上、video 节点没了 → wedged，并且给出复位命令。""" + v4l = tmp_path / "v4l" + v4l.mkdir() # 空的：一个 video 节点都没有 + usb = tmp_path / "usb" + (usb / "1-3.4.1").mkdir(parents=True) + + checks = check_devices([("左指", LEFT_BY_PATH)], v4l_root=v4l, usb_root=usb) + c = checks[0] + assert c.state == "wedged" + assert c.level == "error" + assert "orisys-usb-reset 1-3.4.1" in c.text(), "必须给出可直接执行的复位命令" + assert "拔插" in c.text(), "也要给出不需要 root 的办法" + + +def test_check_devices_reports_unplugged_when_device_is_gone(tmp_path: Path) -> None: + v4l = tmp_path / "v4l" + v4l.mkdir() + usb = tmp_path / "usb" + usb.mkdir() # 总线上也没有 + + checks = check_devices([("左指", LEFT_BY_PATH)], v4l_root=v4l, usb_root=usb) + assert checks[0].state == "unplugged" + assert "插线" in checks[0].text() + + +def test_empty_by_path_is_unplugged_not_ok(tmp_path: Path) -> None: + """配置里这一项是空的（没配）也不能算通过。""" + checks = check_devices([("左指", "")], v4l_root=tmp_path, usb_root=tmp_path) + assert checks[0].state == "unplugged" + assert checks[0].level == "error" + + +def test_main_exit_code_is_nonzero_on_error(tmp_path: Path, capsys) -> None: + """CLI 退出码必须能表示失败 —— 否则脚本里的 it 会被忽略掉。""" + code = v4l_health.main(["--dev", "左指=" + LEFT_BY_PATH]) + assert code == 1 + assert "orisys-usb-reset" in capsys.readouterr().out