真机开工体检四件套：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/ 对称。

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) == []

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"}

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