ros_contorl/scripts/lib/usb_health.py

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