真机开工体检四件套：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/ 对称。
This commit is contained in:
arm_dev
@@ -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())
|
||||
Reference in New Issue
Block a user