真机开工体检四件套：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,175 @@
|
||||
#!/usr/bin/env python3
|
||||
"""读一次机械臂的"能不能动"状态，并给出**人话结论**。
|
||||
|
||||
必须在 ``/home/cc/rdk_py310/bin/python`` 下跑（flexivrdk 与 ROS 的 3.11 ABI 不兼容）。
|
||||
|
||||
为什么要有这个脚本
|
||||
------------------
|
||||
``run_real.sh`` 起来之后，如果机器人没使能，``ros2_control`` 会**卡在**
|
||||
``on_activate`` 里等 ``operational``（``wait_for_operational`` 是死循环重试）。
|
||||
卡住的表现是：
|
||||
|
||||
· ``ros2 control list_controllers`` → ``Could not contact service``（不是"没起来"）
|
||||
· ``/joint_states`` 一条都不发
|
||||
· 日志里每 5 秒一句 ``Waiting for operational (status: ...)``
|
||||
|
||||
实测（2026-09-28）：状态是 ``IN_MANUAL_MODE`` —— 注意 ``robot.mode()`` 返回的是
|
||||
``Mode.IDLE``，**看不出**这点，必须读 ``operational_status()``。示教器那个 Auto/Manual
|
||||
开关没有 RDK 接口可以远程切（Flexiv 的安全设计），所以只能人到现场拨。
|
||||
这个脚本的价值就是把"卡住了"翻译成"去拨那个开关"，而不是让人去查 ROS。
|
||||
|
||||
输出
|
||||
----
|
||||
``--out FILE`` 写 ``KEY=VALUE``（RDK 自己的日志会混在 stdout 里，所以结果单独落文件，
|
||||
不去解析第三方日志）。失败分类用 :func:`classify_state`（纯函数，可单测）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
# 结论级别：ok / warn / fail（与 scripts/lib/usb_health.py 同一套词）
|
||||
LEVEL_OK = "ok"
|
||||
LEVEL_WARN = "warn"
|
||||
LEVEL_FAIL = "fail"
|
||||
|
||||
# 现场那句"必须人去拨"的说明。preflight 与文档引用同一句话。
|
||||
MANUAL_HINT = (
|
||||
"示教器切到 Auto（Remote）—— RDK **没有**远程切 Manual→Auto 的接口，"
|
||||
"这是 Flexiv 的安全设计，必须人到现场拨。切完**不用**重启栈："
|
||||
"run_real.sh 里的 wait_for_operational 会自动接着 Enable。"
|
||||
)
|
||||
|
||||
|
||||
def classify_state(
|
||||
*,
|
||||
connected: bool,
|
||||
operational: bool = False,
|
||||
op_status: str = "",
|
||||
mode: str = "",
|
||||
fault: bool = False,
|
||||
estop_released: bool = True,
|
||||
error: str = "",
|
||||
) -> Tuple[str, str, List[str]]:
|
||||
"""把机器人状态翻成 ``(级别, 一句话结论, 建议动作)``。
|
||||
|
||||
判定顺序是**故意的**：故障/急停要排在模式前面 —— 急停没复位时模式可能还是
|
||||
Auto，只报"模式不对"会让人白跑一趟。
|
||||
"""
|
||||
if not connected:
|
||||
return (
|
||||
LEVEL_FAIL,
|
||||
f"连不上 RDK：{error or '未知原因'}",
|
||||
[
|
||||
"确认网线接的是控制柜用户口、PC 与机器人同网段（先跑 "
|
||||
"bash scripts/check_robot_network.sh）。",
|
||||
"确认示教器上已开启 Remote/Ethernet（RDK）。",
|
||||
"如果 ros2_control 已经在跑，RDK 可能被它占着 —— 先停掉栈再试。",
|
||||
],
|
||||
)
|
||||
if fault:
|
||||
return (
|
||||
LEVEL_FAIL,
|
||||
"机器人有故障（fault）",
|
||||
[
|
||||
"在示教器上清故障；没有示教器就重启控制柜。",
|
||||
"栈里的 wait_for_operational 也会自动 ClearFault 重试。",
|
||||
],
|
||||
)
|
||||
if not estop_released:
|
||||
return (
|
||||
LEVEL_FAIL,
|
||||
"急停没复位（estop_released=False）",
|
||||
["松开示教器/控制柜上的急停按钮，再按面板上的复位。"],
|
||||
)
|
||||
upper = (op_status or "").upper()
|
||||
if "MANUAL" in upper:
|
||||
return (
|
||||
LEVEL_FAIL,
|
||||
f"示教器在 Manual 模式（operational_status={op_status}）—— "
|
||||
"机器人不会使能，整条流程会一直等着",
|
||||
[MANUAL_HINT],
|
||||
)
|
||||
if operational:
|
||||
return (LEVEL_OK, f"已使能、可接收指令（mode={mode}）", [])
|
||||
return (
|
||||
LEVEL_FAIL,
|
||||
f"未使能（operational_status={op_status or '?'}, mode={mode or '?'}）",
|
||||
[
|
||||
MANUAL_HINT,
|
||||
"确认没有未清的故障、急停已复位。",
|
||||
],
|
||||
)
|
||||
|
||||
|
||||
def _probe(sn: str, iface: str, enable: bool) -> Dict[str, object]:
|
||||
"""连一次机器人并读状态。返回给 classify_state 的关键字参数。"""
|
||||
import flexivrdk # 延迟导入：让本模块在没有 flexivrdk 的环境里也能被单测导入
|
||||
|
||||
out: Dict[str, object] = {
|
||||
"connected": False,
|
||||
"operational": False,
|
||||
"op_status": "",
|
||||
"mode": "",
|
||||
"fault": False,
|
||||
"estop_released": True,
|
||||
"error": "",
|
||||
}
|
||||
try:
|
||||
if iface:
|
||||
robot = flexivrdk.Robot(sn, [iface], True, False)
|
||||
else:
|
||||
robot = flexivrdk.Robot(sn)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
out["error"] = str(exc)
|
||||
return out
|
||||
|
||||
out["connected"] = True
|
||||
if enable:
|
||||
try:
|
||||
robot.Enable()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
out["error"] = f"Enable 失败: {exc}"
|
||||
|
||||
def _safe(fn, default):
|
||||
try:
|
||||
return fn()
|
||||
except Exception: # noqa: BLE001
|
||||
return default
|
||||
|
||||
out["mode"] = str(_safe(robot.mode, ""))
|
||||
out["operational"] = bool(_safe(robot.operational, False))
|
||||
out["op_status"] = str(_safe(robot.operational_status, ""))
|
||||
out["fault"] = bool(_safe(robot.fault, False))
|
||||
out["estop_released"] = bool(_safe(robot.estop_released, True))
|
||||
return out
|
||||
|
||||
|
||||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||||
ap = argparse.ArgumentParser(description="读一次机械臂状态并给结论")
|
||||
ap.add_argument("--sn", required=True)
|
||||
ap.add_argument("--iface", default="")
|
||||
ap.add_argument("--enable", action="store_true", help="顺手尝试 Enable（会真的使能）")
|
||||
ap.add_argument("--out", default="", help="把 KEY=VALUE 写到这个文件")
|
||||
ap.add_argument("--json", action="store_true", help="把结果打到 stdout（JSON）")
|
||||
args = ap.parse_args(argv)
|
||||
|
||||
state = _probe(args.sn, args.iface, args.enable)
|
||||
level, summary, actions = classify_state(**state) # type: ignore[arg-type]
|
||||
payload = {**state, "level": level, "summary": summary, "actions": actions}
|
||||
|
||||
if args.out:
|
||||
lines = [f"{k}={v}" for k, v in payload.items() if k != "actions"]
|
||||
lines.append("actions=" + " | ".join(actions))
|
||||
Path(args.out).write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
if args.json:
|
||||
print(json.dumps(payload, ensure_ascii=False, indent=2))
|
||||
return 0 if level == LEVEL_OK else 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user