ros_contorl/scripts/lib/v4l_health.py

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