arm_dev
baf7601995
上一轮残留的那四条孤儿（real_trajectory_adapter / named_pose_server /
arm_status_node / joint_state_publisher）在预检里**一条都报不出来**：
老的六条模式只覆盖 ros2_control_node 和 stack.launch，而它们的特征恰恰是
"栈的主体已经没了、只剩它们"，所以两条老模式全都匹配不到。
补两条：
· `install/arm_control/lib/arm_control/` —— 一次覆盖该包全部节点
（用 install/ 而不是 install_sim/：仿真同名节点不该被真机体检误报，
"install_sim/" 不含子串 "install/"，这条性质用测试钉住了）；
· `joint_state_publisher` —— mock 专用节点，真机上出现 = 上一轮 mock 栈的
残留。它发的**静态** /joint_states 会盖掉真实关节角，是"控制器一切正常、
TF 显示臂没动"这个假象的元凶，所以说明里把它写清楚。
人工动作也改成先指向新的 stop_real.sh（它真能带走这些孤儿），保留手动 pkill 兜底。
新增 3 项单测（含"不许误报 install_sim/"的负向用例）。
Co-authored-by: Cursor <cursoragent@cursor.com>
751 lines
31 KiB
Python
751 lines
31 KiB
Python
#!/usr/bin/env python3
|
||
"""真机开工前的**一次性体检**：把「看起来都起来了，其实动不了」的原因一次说清。
|
||
|
||
为什么必须有这个
|
||
----------------
|
||
2026-09-28 无人值守实测，一次开工踩了三个**互不相干、现象却很像**的坑：
|
||
|
||
1. 示教器还在 Manual 模式 → ``ros2_control`` 永远卡在 wait_for_operational，
|
||
栈起来了、话题在、``/joint_states`` 是空的（全是 0）。现场看起来像"驱动没配好"。
|
||
2. 总线供电的小 Hub 在内核里卡死 → 挂它下面的四台设备一起消失
|
||
（腕部 D405 + 顶置 D435 + 左右视触觉）。现场看起来像"相机坏了"。
|
||
3. 上一轮的栈没退干净 → spawner 进程永远等一个不存在的 controller_manager，
|
||
这一轮再起栈，报错指向别处。
|
||
|
||
单独看每一个都不难，但它们**同时存在**时，日志里全是别人的症状，人会先怀疑软件。
|
||
所以这里把它们收成一条命令，按「根因在前、症状在后」输出，并给出**人工要做什么**。
|
||
|
||
设计约束（都是踩过的）
|
||
----------------------
|
||
* **绝不挂住**。任何子进程都有超时，sysfs 读取有超时（见 ``usb_health.SysfsReader``）。
|
||
一个卡死的 USB 设备曾让体检自己挂到超时被杀 —— 那等于什么都没查。
|
||
* **单项失败不连坐**。某个检查抛异常时把它转成一条结论，其余照跑。
|
||
* **人工动作去重后按优先级排**，因为现场是按顺序做的。
|
||
|
||
用法::
|
||
|
||
bash scripts/run_real_preflight.sh --arm 89
|
||
python3 scripts/lib/real_preflight.py --sn Rizon4-063989 # 已在真机环境里时
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import json
|
||
import os
|
||
import shutil
|
||
import subprocess
|
||
import sys
|
||
from dataclasses import dataclass, field, asdict
|
||
from pathlib import Path
|
||
from typing import Any, Callable, Dict, List, Optional, Sequence, Tuple
|
||
|
||
ROOT = Path(__file__).resolve().parents[2]
|
||
SCRIPTS_LIB = ROOT / "scripts" / "lib"
|
||
sys.path.insert(0, str(SCRIPTS_LIB))
|
||
|
||
# 人工动作的措辞只有这一个来源：同一个文件也被 rdk_status.py import，
|
||
# 两处字面一致才能被 Report.manual_actions 去重成一条。
|
||
from manual_actions import NET_UNREACHABLE_ACTION # noqa: E402
|
||
|
||
LEVEL_OK = "ok"
|
||
LEVEL_WARN = "warn"
|
||
LEVEL_FAIL = "fail"
|
||
|
||
#: 级别排序：fail 最先报
|
||
_LEVEL_RANK = {LEVEL_FAIL: 0, LEVEL_WARN: 1, LEVEL_OK: 2}
|
||
|
||
DEFAULT_RDK_PYTHON = os.environ.get("RDK_PYTHON") or "/home/cc/rdk_py310/bin/python"
|
||
|
||
#: 上一轮没退干净的进程特征。留着它们会让这一轮的错误指向别处：
|
||
#: · ros2_control_node 占着 RDK（同一个 SN 是独占访问）
|
||
#: · spawner 永远在等一个不存在的 controller_manager，日志里全是 WARN
|
||
#: · 相机节点占着 USB（USB 是独占的，新进程只会报"打不开设备"）
|
||
LEFTOVER_PATTERNS: Tuple[Tuple[str, str], ...] = (
|
||
("ros2_control_node", "ros2_control 还在跑（它占着 RDK，RDK 对同一台臂是独占的）"),
|
||
("stack.launch", "上一次的 arm_bringup 栈还在"),
|
||
("ros2 run arm_application realsense_color_pub", "相机节点还在跑（USB 独占）"),
|
||
("arm_application/tactile", "触觉服务还在跑（V4L 独占）"),
|
||
("dh_pgea", "夹爪节点还在跑（串口独占）"),
|
||
("controller_manager/spawner", "孤儿 spawner 还在等 controller_manager"),
|
||
# ↓ 2026-09-28 实测补的：这一族**独立于** ros2_control_node 存活。
|
||
# `ros2 launch` 被 Ctrl-C / 崩掉时，子进程不一定跟着退；而下面这几条
|
||
# 恰恰是"栈的主体已经没了、只剩它们"的那个状态 —— 上面两条模式全都
|
||
# 匹配不到（`ros2_control_node` 已退、`stack.launch` 的命令行也没了）。
|
||
# 留着的危害不是"占资源"，而是**冒名顶替**：残留的
|
||
# real_trajectory_adapter 照样订阅 /trajectory、named_pose_server 照样
|
||
# 提供 /set_named_pose、arm_status_node 照样发 /arm_status，于是新一轮
|
||
# 的请求被 DDS 负载均衡到"残留的那个"，日志上看不出任何异常，现象是
|
||
# "指令发了、臂没动"或"状态跳来跳去"。仿真侧早在 sim_procs.sh 的注释里
|
||
# 记过同一个坑（两个适配器同时跑 → 开口恒为 0mm），真机这边的表当时没同步。
|
||
# 用 `install/`（不是 install_sim/）前缀：仿真的同名节点在 install_sim/ 下，
|
||
# 不该被真机体检误报。
|
||
(
|
||
"install/arm_control/lib/arm_control/",
|
||
"arm_control 的节点还在跑（trajectory_bridge / real_trajectory_adapter / "
|
||
"named_pose_server / arm_status_node 会继续接发 /trajectory、/set_named_pose、"
|
||
"/arm_status，新一轮的调用可能被发给残留的那个）",
|
||
),
|
||
(
|
||
"joint_state_publisher",
|
||
"joint_state_publisher 还在跑（**mock 专用**节点：它发的 /joint_states 会盖掉"
|
||
"真实关节角，TF 看着像'臂根本没动'，而控制器那边其实一切正常）",
|
||
),
|
||
)
|
||
|
||
|
||
@dataclass
|
||
class Check:
|
||
"""一项检查的结论。``level`` 只允许 ok/warn/fail。"""
|
||
|
||
key: str
|
||
title: str
|
||
level: str
|
||
lines: List[str] = field(default_factory=list)
|
||
#: 需要**人工**做的事（软件做不了的）。会被汇总、去重后按检查顺序输出。
|
||
actions: List[str] = field(default_factory=list)
|
||
|
||
def as_dict(self) -> Dict[str, Any]:
|
||
return asdict(self)
|
||
|
||
|
||
@dataclass
|
||
class Report:
|
||
checks: List[Check] = field(default_factory=list)
|
||
|
||
@property
|
||
def level(self) -> str:
|
||
if any(c.level == LEVEL_FAIL for c in self.checks):
|
||
return LEVEL_FAIL
|
||
if any(c.level == LEVEL_WARN for c in self.checks):
|
||
return LEVEL_WARN
|
||
return LEVEL_OK
|
||
|
||
def manual_actions(self) -> List[str]:
|
||
"""汇总人工动作，去重且保持检查顺序（= 现场该做的顺序）。"""
|
||
out: List[str] = []
|
||
for c in self.checks:
|
||
for a in c.actions:
|
||
if a not in out:
|
||
out.append(a)
|
||
return out
|
||
|
||
def as_dict(self) -> Dict[str, Any]:
|
||
return {
|
||
"level": self.level,
|
||
"checks": [c.as_dict() for c in self.checks],
|
||
"manual_actions": self.manual_actions(),
|
||
}
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 单项检查
|
||
# ---------------------------------------------------------------------------
|
||
|
||
|
||
def _json_tail(text: str) -> Dict[str, Any]:
|
||
"""从一段可能混着日志的文本里取**最后一个完整的 JSON 对象**。
|
||
|
||
为什么不 ``json.loads(text[text.find('{'):])``：RDK 的 C++ 日志里也有花括号，
|
||
JSON 后面还可能拖日志行，那样解析必然失败 —— 失败之后只能把原始文本打出来，
|
||
结论就丢了（实测就是这个现象）。``raw_decode`` 允许后面有剩余文本，
|
||
所以逐个候选起点试一遍、取最后成功的那个即可。
|
||
"""
|
||
dec = json.JSONDecoder()
|
||
best: Dict[str, Any] = {}
|
||
for i, ch in enumerate(text):
|
||
if ch != "{":
|
||
continue
|
||
try:
|
||
obj, _end = dec.raw_decode(text[i:])
|
||
except json.JSONDecodeError:
|
||
continue
|
||
if isinstance(obj, dict):
|
||
best = obj
|
||
return best
|
||
|
||
|
||
def _run(cmd: Sequence[str], timeout_s: float) -> Tuple[int, str, str]:
|
||
"""跑一条命令并**一定有超时**。返回 (退出码, stdout, stderr)。
|
||
|
||
超时返回码 124（与 GNU timeout 一致）。体检里任何一处不能无限等 ——
|
||
卡死的 USB / 半连的机器人都会让普通的 subprocess.run 永远不返回。
|
||
|
||
stdout / stderr **分开返回**（不合并）：RDK 自己的日志走 stderr，
|
||
而 ``rdk_status.py --json`` 的 JSON 在 stdout。合并之后 JSON 后面会拖一串
|
||
日志，``json.loads`` 必然失败 —— 实测踩过，表现为"体检说读不到 RDK 状态"，
|
||
而实际那句话就在输出里。
|
||
"""
|
||
try:
|
||
p = subprocess.run(
|
||
list(cmd), capture_output=True, text=True, timeout=timeout_s
|
||
)
|
||
except subprocess.TimeoutExpired as exc:
|
||
out = exc.stdout or ""
|
||
err = exc.stderr or ""
|
||
if isinstance(out, bytes): # pragma: no cover - text=True 时不会走到
|
||
out = out.decode(errors="replace")
|
||
if isinstance(err, bytes): # pragma: no cover
|
||
err = err.decode(errors="replace")
|
||
return 124, out, (err + f"\n超时（{timeout_s:.0f}s）：{' '.join(cmd)}")
|
||
except FileNotFoundError as exc:
|
||
return 127, "", str(exc)
|
||
return p.returncode, p.stdout or "", p.stderr or ""
|
||
|
||
|
||
def check_leftovers(
|
||
ps_output: str, running_cmds: Sequence[str] = ()
|
||
) -> Check:
|
||
"""上一轮的残留进程。
|
||
|
||
``running_cmds`` 是**当前进程自己**的命令行（用于排除自己）——
|
||
以前用 ``pkill -f`` 清理时把自己的 shell 也匹配掉了，教训是：
|
||
匹配规则必须能区分"别人的进程"和"我这条命令"。这里改成显式排除。
|
||
"""
|
||
hits: List[str] = []
|
||
for line in ps_output.splitlines():
|
||
if not line.strip():
|
||
continue
|
||
if any(frag in line for frag in running_cmds):
|
||
continue
|
||
for pat, why in LEFTOVER_PATTERNS:
|
||
if pat in line and why not in hits:
|
||
hits.append(why)
|
||
if not hits:
|
||
return Check("leftovers", "遗留进程", LEVEL_OK, ["没有上一轮残留的进程"])
|
||
return Check(
|
||
"leftovers",
|
||
"遗留进程",
|
||
LEVEL_WARN,
|
||
["上一轮有进程没退干净，这一轮的现象会指向别处："] + [f" · {h}" for h in hits],
|
||
actions=[
|
||
"先清干净再起栈：bash scripts/stop_real.sh"
|
||
"（按 install/ 前缀兜底扫 + 按 ROS_DOMAIN_ID 避开仿真进程，能带走上面这些孤儿；"
|
||
"没有它时手动清：pkill -f '[r]os2_control_node'；"
|
||
"pkill -f '[a]rm_bringup.*stack.launch'；pkill -f '[s]pawner'；"
|
||
"pkill -f '[a]rm_control/lib/arm_control'；pkill -f '[j]oint_state_publisher'）"
|
||
],
|
||
)
|
||
|
||
|
||
def check_network(ping_code: int, ping_out: str, arp_lines: Sequence[str], ip: str) -> Check:
|
||
"""机器人是否在网里。
|
||
|
||
额外查一条**现场特有的坑**：两台臂的 ARP 指向同一个 MAC。2026-09-28 实测
|
||
到一次：``.100``（91）与 ``.101``（89）在 ARP 表里是**同一个 MAC**
|
||
``88:88:88:88:87:88``，而 ping ``.100`` 稳定、ping ``.101`` 丢 50~90%，
|
||
且**先 ping 一次 .100 之后 .101 丢包立刻加重**。这是同一台设备（或两台同
|
||
MAC 的设备）在同一 L2 段上争抢转发 —— 它不是"网络不通"，而是"通得不稳"，
|
||
足以让 RDK 的握手时好时坏。
|
||
|
||
这条只涉及**目标 IP 自己**的撞车才报；与目标无关的撞车只在 PING 不通时
|
||
作为解释提一句，避免正常的双机配置被误报。
|
||
"""
|
||
lines: List[str] = []
|
||
actions: List[str] = []
|
||
level = LEVEL_OK
|
||
|
||
last = [ln for ln in ping_out.splitlines() if ln.strip()]
|
||
if ping_code == 0:
|
||
lines.append(f"ping {ip}: 通")
|
||
else:
|
||
level = LEVEL_FAIL
|
||
lines.append(f"ping {ip}: 不通")
|
||
if last:
|
||
lines.append(f" {last[-1].strip()[:160]}")
|
||
actions.append(NET_UNREACHABLE_ACTION)
|
||
|
||
# 同一 MAC 对应多个 IP：按目标 IP 是否牵涉其中决定措辞与级别。
|
||
macs: Dict[str, List[str]] = {}
|
||
for line in arp_lines:
|
||
parts = line.split()
|
||
if len(parts) >= 5 and parts[0].startswith("192.168."):
|
||
macs.setdefault(parts[4].lower(), []).append(parts[0])
|
||
for mac, ips in macs.items():
|
||
if len(ips) < 2:
|
||
continue
|
||
involves_target = ip in ips
|
||
lines.append(
|
||
f" ARP：{' / '.join(sorted(ips))} 指向同一个 MAC {mac}"
|
||
)
|
||
if involves_target:
|
||
# 目标币自己牵涉其中：这是"通得不稳"的根因候选，比 ping 失败更值钱。
|
||
if level == LEVEL_OK:
|
||
level = LEVEL_WARN
|
||
actions.append(
|
||
f"两个 IP（{' / '.join(sorted(ips))}）在 ARP 里是同一个 MAC —— 要么是"
|
||
"同一台设备配了两个地址，要么是两台臂的 MAC 出厂就相同（同一 L2 段"
|
||
"会互相抢转发，表现为 ping 通但丢包、RDK 时连时断）。"
|
||
"先把另一台臂的网线拔掉/换到独立网段，只留当前这台，再重跑体检。"
|
||
)
|
||
if not involves_target and level == LEVEL_FAIL:
|
||
lines.append(" （与目标 IP 无关，仅供参考）")
|
||
break
|
||
|
||
return Check("network", "机器人在网", level, lines, actions=actions)
|
||
|
||
|
||
def check_arm_rdk(payload: Dict[str, Any], returncode: int) -> Check:
|
||
"""机械臂能不能被使能（这是"能不能动"的唯一权威判据）。
|
||
|
||
状态词与处置来自厂商 ``flexiv/rdk/data.hpp`` 的原话：
|
||
``IN_MANUAL_MODE`` = "In Manual mode, need to switch to Auto (Remote) mode."
|
||
"""
|
||
if returncode == 124:
|
||
return Check(
|
||
"arm",
|
||
"机械臂 RDK",
|
||
LEVEL_FAIL,
|
||
["读 RDK 状态超时 —— 通常是别的进程占着 RDK（见上一条遗留进程）"],
|
||
actions=["先清掉占用 RDK 的进程（ros2_control / 上一轮的栈），再重试"],
|
||
)
|
||
level = str(payload.get("level") or LEVEL_FAIL)
|
||
lines = [str(payload.get("summary") or payload.get("error") or "")]
|
||
if payload.get("connected"):
|
||
lines.append(
|
||
f" mode={payload.get('mode', '?')} "
|
||
f"operational={payload.get('operational')} "
|
||
f"op_status={payload.get('op_status', '?')} "
|
||
f"fault={payload.get('fault')} estop_released={payload.get('estop_released')}"
|
||
)
|
||
return Check(
|
||
"arm",
|
||
"机械臂 RDK",
|
||
level if level in _LEVEL_RANK else LEVEL_FAIL,
|
||
lines,
|
||
actions=[str(a) for a in (payload.get("actions") or [])],
|
||
)
|
||
|
||
|
||
def check_usb(reader: Any = None) -> Check:
|
||
"""USB 链路：RealSense 协商速率，以及 Hub 是否在内核里卡死。"""
|
||
import usb_health
|
||
|
||
rd = reader or usb_health.SysfsReader()
|
||
devices = usb_health.read_devices(reader=rd)
|
||
findings = usb_health.findings_for(devices, rd.blocked)
|
||
level = LEVEL_FAIL if usb_health.has_errors(findings) else (
|
||
LEVEL_WARN if any(f.level == LEVEL_WARN for f in findings) else LEVEL_OK
|
||
)
|
||
actions = []
|
||
if rd.blocked:
|
||
actions.append(
|
||
"某个 USB Hub 在内核里卡死（见上面 FAIL）：给这个 Hub 断电重启，"
|
||
"或把设备改插到主板后面板直连的口；软件复位需要 root"
|
||
)
|
||
if any(f.level == "error" and "都没枚举到" in f.text for f in findings):
|
||
actions.append("一台 RealSense 都没枚举到：拔插一次，并确认没有被别的进程占着")
|
||
return Check(
|
||
"usb",
|
||
"USB 链路（相机）",
|
||
level,
|
||
[f"[{f.level}] {f.text}" for f in findings],
|
||
actions=actions,
|
||
)
|
||
|
||
|
||
def check_v4l(pairs: Sequence[Tuple[str, str]], v4l_root: Optional[Path] = None) -> Check:
|
||
"""视触觉的 V4L2 设备在不在，以及是"没插"还是"卡死"。"""
|
||
import v4l_health
|
||
|
||
if not pairs:
|
||
return Check("v4l", "触觉相机（V4L2）", LEVEL_OK, ["本臂没有配触觉，跳过"])
|
||
checks = (
|
||
v4l_health.check_devices(pairs)
|
||
if v4l_root is None
|
||
else v4l_health.check_devices(pairs, v4l_root=v4l_root)
|
||
)
|
||
level = LEVEL_FAIL if any(c.level == "error" for c in checks) else LEVEL_OK
|
||
actions = []
|
||
if any(c.state == "wedged" for c in checks):
|
||
actions.append(
|
||
"触觉相机在内核里卡死（sysfs 还在、/dev/video* 没了）：`sudo -n "
|
||
"/usr/local/bin/orisys-usb-reset <usb-path>` 就是为这种情况准备的"
|
||
"（免密白名单）。**必须加 `timeout 15`** —— 实测这一句会卡在写 unbind "
|
||
"上不返回，无人值守时会永久卡住；它跑完设备仍不在总线上就只剩物理"
|
||
"处理了（拔插该相机 / 给这个 Hub 断电重启）"
|
||
)
|
||
elif level == LEVEL_FAIL:
|
||
actions.append("触觉相机没枚举到：确认插好（by-path 是按口认的，换口就会失配）")
|
||
return Check(
|
||
"v4l", "触觉相机（V4L2）", level, [c.text() for c in checks], actions=actions
|
||
)
|
||
|
||
|
||
def check_cameras(
|
||
site_path: Path, sn: str, devices: Optional[Sequence[Any]] = None
|
||
) -> Check:
|
||
"""本臂配置的**腕部相机**（eye-in-hand 那只）必须在 USB 上。
|
||
|
||
为什么单独一条判据
|
||
------------------
|
||
``check_usb`` 只看 RealSense 的**协商速率**，对"这只相机**根本不在了**"
|
||
没有话可说 —— 它遍历的是**发现到的**设备。2026-09-28 现场实测到这只：
|
||
|
||
lsusb 里只剩顶置 D435，d405（89 的腕部相机、装配全程唯一的位置来源）
|
||
整个消失，而体检报告**全绿**。
|
||
|
||
也就是说，把相机碰掉了、或者它所在的 Hub 掉电了，报告照样说"可以开工"。
|
||
缺 D405 之后，踩的是完全另一类错误（点云为空 / 找不到工件），排查方向
|
||
会被带偏很远。所以这里按 site.yaml 的**配置意图**去核在不在：
|
||
``robots[<sn>].camera.name`` → ``cameras.<name>.model``，比的是型号。
|
||
|
||
为什么比型号而不是序列号：实测这台 D435 的 sysfs ``serial`` 是**空串**
|
||
（UVC 不一定把序列号透出来），拿序列号判会永远判"不在"。型号
|
||
（``d405`` / ``d435``）来自 USB product 串，稳定得多。序列号只用来
|
||
在**型号对得上**时做一次"是不是本臂那只"的提醒（WARN，不阻断）。
|
||
"""
|
||
import export_site_env as ex
|
||
|
||
try:
|
||
data = ex.load_site(site_path)
|
||
_merged, robot = ex.apply_active_arm(data, sn)
|
||
except Exception as exc: # noqa: BLE001
|
||
return Check(
|
||
"cameras",
|
||
"腕部相机（USB）",
|
||
LEVEL_WARN,
|
||
[f"读不了 {site_path} 或找不到 SN={sn}：{type(exc).__name__}: {exc}"],
|
||
)
|
||
|
||
cam_name = str(((robot.get("camera") or {}) or {}).get("name") or "")
|
||
if not cam_name:
|
||
return Check(
|
||
"cameras", "腕部相机（USB）", LEVEL_OK, ["本臂没配腕部相机（camera.name），跳过"]
|
||
)
|
||
cam = (data.get("cameras") or {}).get(cam_name) or {}
|
||
if "sim" in cam_name or "sim" in str(cam.get("role") or ""):
|
||
return Check(
|
||
"cameras",
|
||
"腕部相机（USB）",
|
||
LEVEL_OK,
|
||
[f"本臂腕部相机 {cam_name} 是仿真源（真机不读），跳过"],
|
||
)
|
||
|
||
want_model = str(cam.get("model") or "").lower()
|
||
want_serial = str(cam.get("serial") or "")
|
||
if not want_model:
|
||
return Check(
|
||
"cameras",
|
||
"腕部相机（USB）",
|
||
LEVEL_WARN,
|
||
[f"cameras.{cam_name} 没写 model，无法核对是否插着"],
|
||
actions=[f"在 site.yaml 的 cameras.{cam_name} 里补上 model（如 d405）"],
|
||
)
|
||
|
||
if devices is None:
|
||
import usb_health
|
||
|
||
devices = usb_health.read_devices(reader=usb_health.SysfsReader())
|
||
|
||
found = [d for d in devices if getattr(d, "model", "") == want_model]
|
||
realsense = [d for d in devices if getattr(d, "is_realsense", False)]
|
||
lines = [
|
||
f"本臂腕部相机：{cam_name}（期望 {want_model}"
|
||
+ (f"，SN {want_serial}" if want_serial else "")
|
||
+ "）",
|
||
"USB 上现在有的 RealSense："
|
||
+ (
|
||
"、".join(f"{d.model or d.product}（{d.path}）" for d in realsense)
|
||
or "一台都没有"
|
||
),
|
||
]
|
||
if not found:
|
||
return Check(
|
||
"cameras",
|
||
"腕部相机（USB）",
|
||
LEVEL_FAIL,
|
||
lines,
|
||
actions=[
|
||
f"本臂腕部相机 {want_model}（{cam_name}）没在 USB 上 —— 装配全程的位置"
|
||
f"都来自它，没有它后面每一步都会以别的样子报错。查：相机线/腕部延长线"
|
||
f"插紧（它常挂在腕部/第三轴的活动线上，反复弯折最先断），"
|
||
f"USB Hub 是否还在（lsusb），换一个口再 lsusb 确认。"
|
||
],
|
||
)
|
||
|
||
level = LEVEL_OK
|
||
# 型号对上了，再看序列号对不对：两台臂各有一只同型号，插反了要能看出来。
|
||
serials = [d.serial for d in found if d.serial]
|
||
if want_serial:
|
||
if serials and want_serial not in serials:
|
||
level = LEVEL_WARN
|
||
lines.append(
|
||
f" [warn] 型号对得上，但序列号是 {serials}，配置里写的是 {want_serial}"
|
||
" —— 可能把另一台臂的腕部相机插到了本臂上"
|
||
)
|
||
elif not serials:
|
||
lines.append(" （这只相机 sysfs 里没透出序列号，无法核对是不是本臂那只）")
|
||
return Check("cameras", "腕部相机（USB）", level, lines)
|
||
|
||
|
||
def check_geometry(site_path: Path, sn: str) -> Check:
|
||
"""手眼矩阵自洽 + 悬停高度会不会把工具捅进工件。"""
|
||
import real_geometry_check
|
||
|
||
try:
|
||
rep = real_geometry_check.geometry_report(site_path, sn)
|
||
except Exception as exc: # noqa: BLE001 —— 单项失败不能连坐
|
||
return Check(
|
||
"geometry",
|
||
"几何（手眼 / 悬停）",
|
||
LEVEL_WARN,
|
||
[f"跑不了几何检查：{type(exc).__name__}: {exc}"],
|
||
actions=["确认工作区已构建（install/ 里有 arm_application），再重跑体检"],
|
||
)
|
||
lines: List[str] = []
|
||
problems: List[str] = []
|
||
he = rep["handeye"]
|
||
lines.append(f"手眼：{he.get('file')}")
|
||
if he.get("translation_m"):
|
||
t = he["translation_m"]
|
||
lines.append(f" 平移 [{t[0]:+.4f}, {t[1]:+.4f}, {t[2]:+.4f}] m")
|
||
for p in he.get("problems") or []:
|
||
problems.append(p)
|
||
lines.append(f" [fail] {p}")
|
||
if not (he.get("problems") or []):
|
||
lines.append(" [ok ] 矩阵自洽（正交、det=+1、量级合理）")
|
||
|
||
hv = rep["hover"]
|
||
lines.append(
|
||
f"悬停：法兰 {hv['hole_hover_z_m']:.4f} − 工具长 "
|
||
f"{hv['tool_tip_below_flange_m'] * 1000:.1f}mm，工件面 "
|
||
f"{hv['workpiece_surface_z_m']:.4f}"
|
||
+ ("（未标定）" if hv.get("uncalibrated_surface") else "")
|
||
)
|
||
for p in hv.get("problems") or []:
|
||
problems.append(p)
|
||
lines.append(f" [fail] {p}")
|
||
actions = []
|
||
if hv.get("uncalibrated_surface"):
|
||
actions.append(
|
||
"实测工件上表面 z 并写回 site.yaml 的 robots[].gauge_assembly."
|
||
"workpiece_surface_z_m（跑 scripts/run_stage2_locate.sh 会把它打出来）"
|
||
)
|
||
return Check(
|
||
"geometry",
|
||
"几何（手眼 / 悬停）",
|
||
LEVEL_FAIL if problems else LEVEL_OK,
|
||
lines,
|
||
actions=actions,
|
||
)
|
||
|
||
|
||
def check_environment(site_path: Path, sn: str) -> Check:
|
||
"""把这一轮到底在读哪份配置、认哪台设备打出来（换臂/换口最容易错）。"""
|
||
import export_site_env as ex
|
||
|
||
try:
|
||
data = ex.load_site(site_path)
|
||
_merged, robot = ex.apply_active_arm(data, sn)
|
||
except Exception as exc: # noqa: BLE001
|
||
return Check(
|
||
"env",
|
||
"现场配置",
|
||
LEVEL_FAIL,
|
||
[f"读不了 {site_path} 或找不到 SN={sn}：{exc}"],
|
||
actions=[f"确认 site.yaml 与 --arm/--sn 一致（当前 {sn}）"],
|
||
)
|
||
lines = [
|
||
f"site={site_path}",
|
||
f"SN={ex.resolve_robot_sn(data, sn)[0]} "
|
||
f"arm={robot.get('arm')} {robot.get('label') or ''}".rstrip(),
|
||
f"IP={robot.get('ip')} iface={robot.get('iface') or (data.get('net') or {}).get('iface')}",
|
||
]
|
||
cams = data.get("cameras") or {}
|
||
for key, cam in cams.items():
|
||
if not isinstance(cam, dict):
|
||
continue
|
||
res = f"{cam.get('width')}x{cam.get('height')}"
|
||
if cam.get("fps"):
|
||
res += f"@{cam['fps']}"
|
||
role = str(cam.get("role") or "")
|
||
tag = "（仿真用，真机不读）" if "sim" in role or str(key).startswith("sim_") else ""
|
||
lines.append(f"cameras.{key}: serial={cam.get('serial')} {res} {role}{tag}".rstrip())
|
||
grip = robot.get("gripper") or {}
|
||
if grip:
|
||
lines.append(
|
||
f"gripper: {grip.get('model')} port={grip.get('port')} "
|
||
f"auto_init={grip.get('auto_init')}"
|
||
)
|
||
if grip.get("auto_init"):
|
||
lines.append(
|
||
" 注意 auto_init=true：上电会先把手指张到满行程，"
|
||
"爪里已经夹着工件时会把工件放掉"
|
||
)
|
||
return Check("env", "现场配置", LEVEL_OK, lines)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 编排
|
||
# ---------------------------------------------------------------------------
|
||
|
||
|
||
def _ps_output() -> str:
|
||
exe = shutil.which("ps") or "/bin/ps"
|
||
code, out, _err = _run([exe, "-eo", "pid,cmd"], 5.0)
|
||
return out if code == 0 else ""
|
||
|
||
|
||
def run_all(
|
||
site_path: Path,
|
||
sn: str,
|
||
rdk_python: str = DEFAULT_RDK_PYTHON,
|
||
iface: str = "",
|
||
) -> Report:
|
||
"""跑完整套体检。顺序 = 输出的顺序 = 现场该处理的顺序。"""
|
||
rep = Report()
|
||
|
||
# 0) 现场配置：先把"在读哪份配置"钉住，后面所有结论都依赖它。
|
||
env_check = check_environment(site_path, sn)
|
||
rep.checks.append(env_check)
|
||
|
||
# 1) 遗留进程：它会让后面每一项的现象都指向别处，所以先查。
|
||
rep.checks.append(
|
||
check_leftovers(_ps_output(), running_cmds=(sys.argv[0] if sys.argv else "",))
|
||
)
|
||
|
||
# 2) 网络：RDK 连不上的前提条件。
|
||
ip = ""
|
||
try:
|
||
import export_site_env as ex
|
||
|
||
data = ex.load_site(site_path)
|
||
_merged, robot = ex.apply_active_arm(data, sn)
|
||
ip = str(robot.get("ip") or "")
|
||
iface = iface or str(robot.get("iface") or (data.get("net") or {}).get("iface") or "")
|
||
except Exception: # noqa: BLE001
|
||
pass
|
||
if ip:
|
||
code, out, _err = _run(["ping", "-c", "2", "-W", "1", ip], 8.0)
|
||
_arp_code, arp, _arp_err = _run(["ip", "neigh", "show"], 5.0)
|
||
rep.checks.append(check_network(code, out, arp.splitlines() if arp else [], ip))
|
||
|
||
# 3) 机械臂：能不能使能。
|
||
status_py = SCRIPTS_LIB / "rdk_status.py"
|
||
if status_py.is_file():
|
||
cmd = [rdk_python, str(status_py), "--sn", sn, "--json"]
|
||
if iface:
|
||
cmd += ["--iface", iface]
|
||
# 磁盘上没有的 SN 会走完整搜索超时；给足但绝不无限等。
|
||
code, out, err = _run(cmd, 90.0)
|
||
payload = _json_tail(out) or _json_tail(err) or {}
|
||
if not payload:
|
||
payload = {
|
||
"level": LEVEL_FAIL,
|
||
"error": (out + err).strip()[-400:] or "读不到 RDK 状态",
|
||
}
|
||
rep.checks.append(check_arm_rdk(payload, code))
|
||
|
||
# 4) USB / 触觉 / 几何：都不需要使能机械臂，随时可查。
|
||
# 腕部相机排在 USB 之前：USB 那条只说"速率对不对"，这条说"在不在"，
|
||
# 而"不在"是更硬的阻断，必须先报。
|
||
for fn in (
|
||
lambda: check_cameras(site_path, sn),
|
||
lambda: check_usb(),
|
||
lambda: check_v4l(_tactile_pairs(site_path, sn)),
|
||
lambda: check_geometry(site_path, sn),
|
||
):
|
||
try:
|
||
rep.checks.append(fn())
|
||
except Exception as exc: # noqa: BLE001 —— 单项失败不连坐
|
||
rep.checks.append(
|
||
Check(
|
||
"unknown",
|
||
"检查项异常",
|
||
LEVEL_WARN,
|
||
[f"{type(exc).__name__}: {exc}"],
|
||
)
|
||
)
|
||
|
||
rep.checks.sort(key=lambda c: _LEVEL_RANK.get(c.level, 3))
|
||
return rep
|
||
|
||
|
||
def _tactile_pairs(site_path: Path, sn: str) -> List[Tuple[str, str]]:
|
||
"""从 site.yaml 取本臂触觉的 by-path（按口认设备，换口就会失配）。
|
||
|
||
注意 ``robots[].tactile`` 的写法有两种：布尔开关（本臂装没装）和
|
||
参数覆盖字典。以前只按字典处理，遇到 ``tactile: true`` 就
|
||
``dict.update(True)`` 抛 TypeError —— 现象是"整个触觉检查项没跑"，
|
||
而不是报错，很容易被当成"这台没触觉"。
|
||
"""
|
||
import export_site_env as ex
|
||
|
||
try:
|
||
data = ex.load_site(site_path)
|
||
_merged, robot = ex.apply_active_arm(data, sn)
|
||
except Exception: # noqa: BLE001
|
||
return []
|
||
tac = dict(data.get("tactile") or {})
|
||
override = robot.get("tactile")
|
||
if isinstance(override, dict):
|
||
tac.update(override)
|
||
elif override is not None and not override:
|
||
return [] # 显式关掉
|
||
if not tac:
|
||
return []
|
||
pairs: List[Tuple[str, str]] = []
|
||
for side, key in (("左指", "left_video"), ("右指", "right_video")):
|
||
path = str(tac.get(key) or "")
|
||
if path:
|
||
pairs.append((side, path))
|
||
return pairs
|
||
|
||
|
||
def render(rep: Report) -> str:
|
||
marks = {LEVEL_OK: "OK ", LEVEL_WARN: "WARN", LEVEL_FAIL: "FAIL"}
|
||
out: List[str] = []
|
||
for c in rep.checks:
|
||
out.append(f"[{marks.get(c.level, '? ')}] {c.title}")
|
||
for line in c.lines:
|
||
if line:
|
||
out.append(f" {line}")
|
||
out.append("")
|
||
out.append("=" * 68)
|
||
if rep.level == LEVEL_OK:
|
||
out.append("结论：GO —— 所有检查通过，可以起栈跑流程。")
|
||
else:
|
||
out.append(
|
||
f"结论：NO-GO（{'有硬阻断' if rep.level == LEVEL_FAIL else '有告警'}）"
|
||
)
|
||
acts = rep.manual_actions()
|
||
if acts:
|
||
out.append("")
|
||
out.append("要人工做的事（按顺序，软件做不了的部分）：")
|
||
for i, a in enumerate(acts, 1):
|
||
out.append(f" {i}. {a}")
|
||
else:
|
||
out.append(" （没有需要人工处理的项，见上面 WARN 的具体内容）")
|
||
return "\n".join(out)
|
||
|
||
|
||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||
ap = argparse.ArgumentParser(description="真机开工前的一次性体检")
|
||
ap.add_argument("--sn", default="", help="机械臂 SN 或别名（89/91）")
|
||
ap.add_argument("--site", default=str(ROOT / "config" / "site.yaml"))
|
||
ap.add_argument("--iface", default="")
|
||
ap.add_argument("--rdk-python", default=DEFAULT_RDK_PYTHON)
|
||
ap.add_argument("--json", action="store_true")
|
||
ap.add_argument("--out", default="", help="把结论写到这个文件（JSON）")
|
||
args = ap.parse_args(argv)
|
||
|
||
if not args.sn:
|
||
ap.error("--sn 必填（89 / 91 / Rizon4-XXXXXX）")
|
||
|
||
rep = run_all(Path(args.site), args.sn, args.rdk_python, args.iface)
|
||
if args.json:
|
||
print(json.dumps(rep.as_dict(), ensure_ascii=False, indent=2))
|
||
else:
|
||
print(render(rep))
|
||
if args.out:
|
||
Path(args.out).write_text(
|
||
json.dumps(rep.as_dict(), ensure_ascii=False, indent=2) + "\n",
|
||
encoding="utf-8",
|
||
)
|
||
return 0 if rep.level == LEVEL_OK else 1
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|