真机开工体检四件套：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,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())
|
||||
Reference in New Issue
Block a user