orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

把当前控制栈收成一份可克隆的源码：腕部看不见时用顶视一次定到基座，二阶段对孔和手外标定跟着进同一份树。

Browse Source 
现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-09 14:51:03 +08:00
parent 16d2912a9a
commit 7d22f777c8
67 changed files with 8597 additions and 409 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/_verify_hole_lost.py
+63
View File
@@ -0,0 +1,63 @@
#!/usr/bin/env python3
"""确定性复现「工件被推走 ⇒ 规没进孔」：在 k1 之后把工件挪 25mm。
为什么需要它：这个故障在自然运行里是**随机**的（实测三跑坏一跑），
靠碰运气验证判据不可靠。这里改成主动制造：等流程走到 S10（离开 k1、去 k2），
就把工件沿 +Y 推 25mm —— 而 k2 的孔位是 S05 一次性定下的，于是必然失配。
判据（要看的就是这个）：S07 必须报「规没有进孔」（HOLE_LOST），
**不是** 之前那句把人引去查工具系的「转轴偏差」。
"""
from __future__ import annotations
import pathlib
import subprocess
import sys
import time
ROOT = pathlib.Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "src" / "arm_sim_gazebo"))
LOG = pathlib.Path("/tmp/sim_guard.log")
WORLD = "assembly"
SHIFT_M = -0.020 # 与 2026-09-28 自然失效同向同量级（工件 y: 0.000 → -0.0206）
def _shift() -> None:
from arm_sim_gazebo.sim_pose_service import set_model_pose
# 工件当前在 (0.520, 0.000, 0.400)，沿 +Y 挪 25mm（远超孔半径，必然错过）
ok, out = set_model_pose(WORLD, "workpiece", (0.520, SHIFT_M, 0.400), (0.0, 0.0, 0.0, 1.0))
print(f"[driver] 推工件 {SHIFT_M * 1000:.0f}mm -> {'成功' if ok else '失败'}: {out[:160]}")
def main() -> int:
LOG.write_text("")
proc = subprocess.Popen(
["bash", "scripts/run_gauge_assembly.sh", "--arm", "sim", "--sim"],
cwd=str(ROOT), stdout=LOG.open("w"), stderr=subprocess.STDOUT,
)
print(f"[driver] 流程已起 pid={proc.pid}，等它走到 S10…")
shifted = False
t0 = time.monotonic()
while proc.poll() is None and time.monotonic() - t0 < 1500:
txt = LOG.read_text(errors="replace")
if not shifted and "S10_to_k2" in txt:
print("[driver] 检测到 S10_to_k2 —— 现在推工件")
_shift()
shifted = True
time.sleep(1.0)
proc.wait(timeout=30)
txt = LOG.read_text(errors="replace")
print(f"[driver] 流程退出码 = {proc.returncode}, 是否推过工件 = {shifted}")
for line in txt.splitlines():
if "规没有进孔" in line or "孔位失效" in line or "转轴偏差" in line:
print(" |", line.strip()[:150])
return 0
if __name__ == "__main__":
raise SystemExit(main())
scripts/gen_site_example.py
+13 -2
View File
@@ -67,9 +67,11 @@ LITERAL_REPLACEMENTS: list[tuple[str, str]] = [
# 用户名匿名化。放在其它规则之前，避免路径被二次处理。
("/home/cc", "/home/USER"),
# 注释里裸写的机械臂编号（无引号，正则抓不到）。
# ⚠️ 这行是**逐字匹配**：site.yaml 里改一个字它就静默失效，SN 就漏进模板了
#（test_real_devices.test_example_contains_no_live_values 会挡下来）。
(
"# User1=enp4s0 本机 192.168.2.10 ↔ 063989=.101 / 063991=.100",
"# User1=enp4s0 本机 <PC_IP> ↔ <左臂SN>=.101 / <右臂SN>=.100",
"# 本机 192.168.2.10 ↔ 063989(左89)=.101 / 063991(右91)=.100",
"# 本机 <PC_IP> ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100",
),
]
@@ -77,6 +79,15 @@ LITERAL_REPLACEMENTS: list[tuple[str, str]] = [
REGEX_REPLACEMENTS: list[tuple[str, str]] = [
# 机械臂序列号：Rizon4-063989 -> Rizon4-XXXXXX
(r'"(Rizon4)-\d+"', r'"\1-XXXXXX"'),
# 同上，但**不带引号**（注释里的裸写）。留一条兜底，免得上面的逐字规则
# 一旦和 site.yaml 不同步就把 SN 漏出去。
(r"(Rizon4)-\d+", r"\1-XXXXXX"),
# sn 里的连字符在下划线形式下（话题名不许含 '-'，broadcaster 用 '_'）：
# 例 "/Rizon4_063989/external_wrench_in_tcp" -> "/Rizon4_XXXXXX/external_wrench_in_tcp"
(r"(Rizon4)_\d+", r"\1_XXXXXX"),
# USB 网卡的 iface 名是 MAC 派生的（enx + 12 位十六进制）= 现场值。
# enp4s0 这类板载名是通用的，不动。
(r"enx[0-9a-fA-F]{12}", "enxXXXXXXXXXXXX"),
# IP 地址（仅带引号的配置值；注释里的裸 IP 由上面的字面替换处理）
(r'"192\.168\.\d{1,3}\.\d{1,3}"', '"192.168.2.XXX"'),
# 8 位及以上的纯数字字符串 = 设备序列号（相机 12 位 / 夹爪 8 位）。
scripts/lib/export_site_env.py
+402 -25
View File
@@ -272,7 +272,12 @@ def camera_ns(data: dict) -> str:
def camera_frames(data: dict) -> tuple[str, str, str]:
"""当前机械臂的（命名空间, 光学坐标系, 深度话题）。"""
"""当前机械臂**腕部**相机的（命名空间, 光学坐标系, 深度话题）。
一阶段跟踪用哪台相机见 ``track_camera_frames`` —— 不许改本函数去跟
``perception.track_camera``：``SITE_CAM_NAME`` / 二阶段 / 入孔检测都走这里，
切跟踪相机不该把精拍那路也切走。
"""
ns = camera_ns(data)
yolo = data.get("yolo") or {}
frame = str(yolo.get("camera_frame") or "").strip()
@@ -284,6 +289,218 @@ def camera_frames(data: dict) -> tuple[str, str, str]:
return ns, frame, depth
#: 顶视跟踪必须自己写的深度量程。缺了就借腕部 0.55m 上限 = D435 的点全丢。
_OVERHEAD_RANGE_REQUIRED = ("min_depth_m", "max_depth_m", "feature_min_range_m")
#: D405 主跟踪切 D435 一次性时必须显式写的键。缺了就静默不切 = 臂停在半空。
_ONESHOT_REQUIRED = (
"oneshot_conf",
"cooldown_s",
"frame_max_age_s",
"oneshot_hold_s",
)
def track_camera_frames(data: dict) -> tuple[str, str, str, str]:
"""一阶段跟踪用的（命名空间, 光学坐标系, 深度话题, 手眼模式）。
``perception.track_camera``：
* ``wrist`` → 腕部相机 + ``eye_in_hand``
* ``overhead`` → 手外相机 + ``eye_to_hand``
"""
data, robot = apply_active_arm(data)
sio = _load_site_io()
role = sio.track_camera_role(data)
mode = sio.track_handeye_mode(role)
if role == "overhead":
over = overhead_camera(data, robot)
ns = str(over.get("name") or "").strip("/")
if not ns:
raise RuntimeError(
"perception.track_camera=overhead，但 cameras 里没有 "
"role 以 overhead 开头的相机"
)
return ns, f"{ns}_color_optical_frame", f"/{ns}/aligned_depth_to_color/image_raw", mode
ns, frame, depth = camera_frames(data)
return ns, frame, depth, mode
def _track_handeye_file(data: dict, robot: dict, mode: str) -> str:
"""跟踪用的手眼文件。overhead 不许去读 ``paths.handeye_file``（那是腕部的）。"""
if mode != "eye_to_hand":
return str((data.get("paths") or {}).get("handeye_file") or "")
over = overhead_camera(data, robot)
cam = str(over.get("name") or "").strip("/")
short = arm_short_id(str(robot.get("sn") or ""))
name = _load_site_io().eye_to_hand_result_name(short, cam)
return str(_root() / "handeye" / "results" / name)
def _apply_overhead_range(data: dict, params: dict) -> None:
"""把 ``perception.overhead`` 的深度量程盖到感知参数上。缺键就红。"""
oh = (data.get("perception") or {}).get("overhead")
if not isinstance(oh, dict):
raise RuntimeError(
"perception.track_camera=overhead 必须在 perception.overhead 写顶视"
"深度量程（min_depth_m / max_depth_m / feature_min_range_m）。"
"腕部那组 0.55m 上限会把 D435 的点全丢掉，看起来像检测失败。"
)
missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh]
if missing:
raise RuntimeError(
f"perception.overhead 缺 {missing} —— 不许借腕部量程。"
)
params["min_depth_m"] = float(oh["min_depth_m"])
params["max_depth_m"] = float(oh["max_depth_m"])
params["feature_min_range_m"] = float(oh["feature_min_range_m"])
if "work_z_max_m" in oh:
params["work_z_max_m"] = float(oh["work_z_max_m"])
if "z_ref_m" in oh:
params["z_ref_m"] = float(oh["z_ref_m"])
if "feature_mode" in oh:
params["feature_mode"] = str(oh["feature_mode"])
if "hold_point_on_miss" in oh:
params["hold_point_on_miss"] = bool(oh["hold_point_on_miss"])
def _apply_wrist_hybrid(data: dict, robot: dict, params: dict) -> None:
"""腕部主跟踪 + D435 一次性：参数接到感知节点，**不许盖掉腕部量程**。
顶视量程写进 ``overhead_*`` 键。若直接复用 ``min_depth_m``，D405 会按
0.25–1.80 m 收点，近处工件全丢，看起来像检测失败。
"""
perc = data.get("perception") or {}
if "wrist_handoff_conf" not in perc:
raise RuntimeError(
"perception.track_camera=wrist 的混合跟踪必须写 "
"perception.wrist_handoff_conf（画面内最高 conf 低于此才切 D435）。"
"不许在代码里写死 0.5 再抄一份到 yaml。"
)
params["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"])
oh = perc.get("overhead")
if not isinstance(oh, dict):
raise RuntimeError(
"wrist 混合跟踪必须在 perception.overhead 写顶视量程和一次性参数"
"（min_depth_m / max_depth_m / feature_min_range_m / oneshot_conf / "
"cooldown_s / frame_max_age_s / oneshot_hold_s）。"
)
missing = [k for k in _OVERHEAD_RANGE_REQUIRED if k not in oh]
if missing:
raise RuntimeError(
f"perception.overhead 缺 {missing} —— 不许借腕部量程做 D435 一次性。"
)
miss2 = [k for k in _ONESHOT_REQUIRED if k not in oh]
if miss2:
raise RuntimeError(f"perception.overhead 缺一次性键 {miss2} —— 不许用代码默认值。")
over = overhead_camera(data, robot)
ns = str(over.get("name") or "").strip("/")
if not ns:
raise RuntimeError(
"wrist 混合跟踪需要 cameras 里 role 以 overhead 开头的相机"
)
e2h = _track_handeye_file(data, robot, "eye_to_hand")
params["overhead_oneshot_enable"] = True
params["overhead_min_depth_m"] = float(oh["min_depth_m"])
params["overhead_max_depth_m"] = float(oh["max_depth_m"])
params["overhead_feature_min_range_m"] = float(oh["feature_min_range_m"])
params["overhead_oneshot_conf"] = float(oh["oneshot_conf"])
params["overhead_cooldown_s"] = float(oh["cooldown_s"])
params["overhead_frame_max_age_s"] = float(oh["frame_max_age_s"])
params["overhead_hold_s"] = float(oh["oneshot_hold_s"])
params["overhead_handeye_file"] = e2h
params["overhead_camera_source"] = ns
params["overhead_camera_frame"] = f"{ns}_color_optical_frame"
params["overhead_image_topic"] = f"/{ns}/color/image_raw"
params["overhead_info_topic"] = f"/{ns}/color/camera_info"
params["overhead_depth_topic"] = f"/{ns}/aligned_depth_to_color/image_raw"
#: 手眼标定的两种装法。写在这里当"合法值清单"，避免各处各写一遍字符串。
HANDEYE_MODES = ("eye_in_hand", "eye_to_hand")
def handeye_board(he: dict, mode: str) -> tuple[dict, str]:
"""取某个模式用的标定板尺寸 → ``({cols, rows, square_size_m}, 来源)``。
权威只有一处：``handeye.boards.<mode>``。为什么按模式分开——两种装法用的是
**两块不同的物理板**（本项目：眼在手上 A5 / 眼在手外 A4），方格边长不一样。
合成一组键的后果不是报错，而是"跑另一种模式时静默用错边长"，结果只是系统性偏，
现场几乎联想不到板尺寸。
兼容：没有 ``boards`` 段时，回落到旧的顶层 ``board_cols/rows/square_size_m``。
返回的第二个值是来源（``boards`` / ``legacy`` / ``default``），调用方据此决定
要不要拦下——``default`` 意味着**没人配过这块板**，此时开标定等于赌一个尺寸。
"""
boards = he.get("boards") if isinstance(he.get("boards"), dict) else {}
node = boards.get(mode) if isinstance(boards, dict) else None
src = "boards"
if not isinstance(node, dict):
node = he
src = "legacy"
if mode not in HANDEYE_MODES:
# 不认识的模式：不猜板子，交给调用方报错（猜错就是静默用错尺寸）。
node, src = {}, "default"
def pick(key: str, cast, default):
v = node.get(key) if isinstance(node, dict) else None
if v is None:
return default, False
try:
return cast(v), True
except (TypeError, ValueError):
return default, False
# boards 段用 cols/rows；旧的顶层键是 board_cols/board_rows。两种都认，
# 免得"兼容旧配置"其实读不到旧键，悄悄退回默认 8×11。
cols, ok_c = pick("cols", int, 8)
if not ok_c:
cols, ok_c = pick("board_cols", int, 8)
rows, ok_r = pick("rows", int, 11)
if not ok_r:
rows, ok_r = pick("board_rows", int, 11)
sq, ok_s = pick("square_size_m", float, 0.015)
if src == "legacy" and not (ok_c or ok_r or ok_s):
# 顶层键一个都没有 ⇒ 真的没人配过，不要把"内置默认值"伪装成"配置"。
src = "default"
return {"cols": cols, "rows": rows, "square_size_m": sq}, src
def overhead_camera(data: dict, robot: dict | None = None) -> dict:
"""手外（固定）相机：``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台。
**规则只有一份**，在 ``arm_application.site_io.overhead_camera``——S03 粗定位
取命名空间走的也是它。这里若自己再写一遍"按 role 找 + 按 arm 收敛"，两份判据
必然漂移，症状是"标定用的相机"和"推理用的相机"不是同一台，且错而不报。
所以这里只做一层薄封装（从源码树 import），拿到配置字典。
"""
fn = _load_overhead_camera_fn()
cam, _others = fn(data, robot)
return cam
def _load_site_io():
"""从 ``src/arm_application`` 取 ``site_io`` 模块。
为什么从源码树而不是已安装包：``export_site_env.py`` 在 ``colcon build``
之前就被调用（``load_site_config.sh`` 的第一件事），那时 install 里可能还没有
这个包。源码树在本仓库里恒定存在（``scripts/`` 与 ``src/`` 同根）。
"""
src = _root() / "src" / "arm_application"
if not (src / "arm_application" / "site_io.py").is_file():
raise RuntimeError(
f"找不到 {src}/arm_application/site_io.py —— 手外相机 / 结果文件名的规则"
"只有那一处，不接受本文件自己再实现一份（两份判据必然漂移）。"
)
if str(src) not in sys.path:
sys.path.insert(0, str(src))
from arm_application import site_io as _mod # noqa: PLC0415
return _mod
def _load_overhead_camera_fn():
return _load_site_io().overhead_camera
def flatten(data: dict) -> dict:
"""供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。"""
data, robot = apply_active_arm(data)
@@ -313,7 +530,59 @@ def flatten(data: dict) -> dict:
# 两边会共用同一个手眼文件，表现是"标定做了但不生效"。
short = arm_short_id(str(robot.get("sn") or ""))
he_mode = str(he.get("mode") or "eye_in_hand")
he_stem = f"{short}_{he_mode}" if short else he_mode
over = overhead_camera(data, robot)
over_name = str(over.get("name") or "").strip("/")
root = _root()
# 手眼标定的"按模式"派生物：结果文件 / 示教点 / 标定板。
#
# 为什么一切都要按模式分开（eye_in_hand / eye_to_hand）：
# 1) 结果文件：眼在手外的产物是"**这台相机的光心 → 这条臂的基座**"，换条臂
# （基座不同）或换台相机都不通用 ⇒ 名字要同时带臂号与相机名
# （<臂号>_<相机>_eye_to_hand）。曾经只有 <臂号>_<模式>，于是一个双臂共用
# 的手外相机只能存一份外参，第二条臂拿到的必然错；
# 2) 标定板：两种装法用两块不同的物理板（A5 / A4），方格边长不同 ⇒ 见
# handeye_board() 的说明；
# 3) 示教点：两种模式采的关节角完全不同，共用一份会把上一模式的点位当成本
# 模式的（merge 之后才发现在动错的姿态）。
he_modes_out: dict[str, str] = {}
_e2h_name = _load_site_io().eye_to_hand_result_name
for _m in HANDEYE_MODES:
_tag = _m.upper()
_pose_stem = f"{short}_{_m}" if short else _m
# 结果文件名：眼在手外走 site_io.eye_to_hand_result_name（唯一口径）；
# 眼在手上仍是 <臂号>_eye_in_hand.yaml。
if _m == "eye_to_hand":
_result_name = _e2h_name(short, over_name)
else:
_result_name = f"{_pose_stem}.yaml"
# eye_to_hand 允许用显式路径覆盖（与 eye_in_hand 的 paths.handeye_file 对称）；
# 不写就用派生的名字。
if _m == "eye_in_hand":
_file = str(
paths.get("handeye_file")
or root / "handeye" / "results" / _result_name
)
_wps = str(
paths.get("handeye_waypoints")
or root / "handeye" / "poses" / f"{_pose_stem}.yaml"
)
else:
_file = str(
paths.get(f"handeye_file_{_m}")
or root / "handeye" / "results" / _result_name
)
_wps = str(
paths.get(f"handeye_waypoints_{_m}")
or root / "handeye" / "poses" / f"{_pose_stem}.yaml"
)
_board, _src = handeye_board(he, _m)
he_modes_out[f"SITE_HANDEYE_FILE_{_tag}"] = _file
he_modes_out[f"SITE_HANDEYE_WAYPOINTS_{_tag}"] = _wps
he_modes_out[f"SITE_HANDEYE_BOARD_COLS_{_tag}"] = str(_board["cols"])
he_modes_out[f"SITE_HANDEYE_BOARD_ROWS_{_tag}"] = str(_board["rows"])
he_modes_out[f"SITE_HANDEYE_SQUARE_{_tag}"] = str(_board["square_size_m"])
he_modes_out[f"SITE_HANDEYE_BOARD_SRC_{_tag}"] = _src
out = {
"SITE_CONFIG": str(site_path()),
@@ -339,10 +608,7 @@ def flatten(data: dict) -> dict:
),
"SITE_YOLO_WEIGHTS": weights,
"SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
"SITE_HANDEYE_WAYPOINTS": str(
paths.get("handeye_waypoints")
or str(_root() / "handeye" / "poses" / f"{he_stem}.yaml")
),
"SITE_HANDEYE_WAYPOINTS": he_modes_out["SITE_HANDEYE_WAYPOINTS_EYE_IN_HAND"],
"SITE_HANDEYE_CALIBRATE": str(
paths.get("handeye_calibrate")
or str(_root() / "handeye" / "calibrate.yaml")
@@ -355,18 +621,24 @@ def flatten(data: dict) -> dict:
paths.get("handeye_workdir")
or str(_root() / "handeye" / "tmp" / short)
),
"SITE_HANDEYE_FILE": str(
paths.get("handeye_file")
or str(_root() / "handeye" / "results" / f"{he_stem}.yaml")
),
# 不带后缀的两个名字保持旧语义（= 腕部 eye-in-hand 的产物），因为
# launch / 其它脚本一直按这两个名字读；换成"当前模式"会让 site.yaml 里
# 一改 handeye.mode 就把 S05 精拍定位的 TF 换掉，那是另一个功能的静默失效。
# 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。
"SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"],
"SITE_HANDEYE_MODE": he_mode,
# 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
# 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的
# 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸，
# 表现为"标定能跑完但结果明显不对"。
"SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 8)),
"SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 11)),
"SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.015)),
**he_modes_out,
# 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方；
# 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。
"SITE_HANDEYE_BOARD_COLS": he_modes_out[
f"SITE_HANDEYE_BOARD_COLS_{he_mode.upper()}"
],
"SITE_HANDEYE_BOARD_ROWS": he_modes_out[
f"SITE_HANDEYE_BOARD_ROWS_{he_mode.upper()}"
],
"SITE_HANDEYE_SQUARE_M": he_modes_out[
f"SITE_HANDEYE_SQUARE_{he_mode.upper()}"
],
"SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)),
"SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)),
"SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)),
@@ -396,6 +668,21 @@ def flatten(data: dict) -> dict:
"SITE_CAM_LASER_POWER": str(float(cam.get("laser_power") if cam.get("laser_power") is not None else 0)),
"SITE_CAM_EXPOSURE_US": str(float(cam.get("exposure_us") or 0)),
"SITE_CAM_GAIN": str(float(cam.get("gain") or 0)),
# 手外（固定）相机的**按角色**导出。eye-to-hand 标定要用它：标定时得订阅
# 这台固定相机，而不是腕部相机（run_handeye.sh 原来恒用 SITE_CAM_NAME，
# 于是 `--mode eye_to_hand` 会去开腕部相机 —— 标出一个"看着成功"的错结果）。
"SITE_OVERHEAD_CAM_NAME": over_name,
"SITE_OVERHEAD_CAM_SERIAL": str(over.get("serial") or ""),
"SITE_OVERHEAD_CAM_WIDTH": str(int(over.get("width") or 1280)),
"SITE_OVERHEAD_CAM_HEIGHT": str(int(over.get("height") or 720)),
"SITE_OVERHEAD_CAM_FPS": str(int(over.get("fps") or 30)),
"SITE_OVERHEAD_CAM_AUTO_EXPOSURE": "1" if over.get("auto_exposure", True) else "0",
"SITE_OVERHEAD_CAM_AE_LIMIT_US": str(
float(over.get("ae_limit_us") if over.get("ae_limit_us") is not None else 8000)
),
"SITE_OVERHEAD_CAM_EMITTER": "1" if over.get("emitter_enabled", True) else "0",
"SITE_OVERHEAD_CAM_EXPOSURE_US": str(float(over.get("exposure_us") or 0)),
"SITE_OVERHEAD_CAM_GAIN": str(float(over.get("gain") or 0)),
"SITE_D435_SERIAL": str(((data.get("cameras") or {}).get("d435") or {}).get("serial") or ""),
"SITE_D435_WIDTH": str(int(((data.get("cameras") or {}).get("d435") or {}).get("width") or 1280)),
"SITE_D435_HEIGHT": str(int(((data.get("cameras") or {}).get("d435") or {}).get("height") or 720)),
@@ -417,6 +704,14 @@ def flatten(data: dict) -> dict:
"SITE_EE_Z_M": str(float(servo.get("ee_z_m") or 0.24)),
"SITE_SERVO_WATCHDOG": str(servo.get("watchdog_timeout_sec") or 0.3),
"SITE_CAMERA_FRAME": camera_frames(data)[1],
# 一阶段跟踪用的相机/手眼（可与腕部精拍不同）。SITE_CAM_NAME 仍是腕部。
"SITE_TRACK_CAMERA": _load_site_io().track_camera_role(data),
"SITE_TRACK_CAM_NAME": track_camera_frames(data)[0],
"SITE_TRACK_CAMERA_FRAME": track_camera_frames(data)[1],
"SITE_TRACK_HANDEYE_MODE": track_camera_frames(data)[3],
"SITE_TRACK_HANDEYE_FILE": _track_handeye_file(
data, robot, track_camera_frames(data)[3]
),
"SITE_GRIP_VENDOR": str(grip.get("vendor") or "dh"),
"SITE_GRIP_MODEL": str(grip.get("model") or "PGEA-100-20-W"),
"SITE_GRIP_SN": str(grip.get("sn") or ""),
@@ -490,14 +785,14 @@ def perception_overrides(data: dict) -> dict:
data, _robot = apply_active_arm(data)
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
cam_ns, cam_frame, depth_topic = camera_frames(data)
cam_ns, cam_frame, depth_topic, _he_mode = track_camera_frames(data)
obj = str(yolo.get("object_frame") or "").strip()
if not obj or obj == "tracked_object":
# 跟踪目标的坐标系按**臂号**派生（SN 末两位），两侧互不干扰。
# 以前是从 SN 里找 "063989"/"063991" 子串，换个站点就失去意义了。
short = arm_short_id(str((_robot or {}).get("sn") or ""))
obj = f"tracked_object_{short}" if short else "tracked_object"
return {
out = {
"weights": str(paths.get("yolo_weights") or ""),
"conf": float(yolo.get("conf") or 0.10),
"iou": float(yolo.get("iou") or 0.45),
@@ -528,10 +823,36 @@ def perception_overrides(data: dict) -> dict:
"object_frame": obj,
"depth_topic": depth_topic,
"camera_source": cam_ns,
"base_frame": str((_robot or {}).get("base_frame") or ""),
"feature_mode": str(yolo.get("feature_mode") or "plane"),
# feature_mode=keypoint 时用：哪个关键点当跟踪点（k0）、可信度门槛。
"keypoint_index": int(yolo.get("keypoint_index") if yolo.get("keypoint_index") is not None else 0),
"keypoint_min_conf": float(yolo.get("keypoint_min_conf") or 0.30),
"keypoint_win_px": int(yolo.get("keypoint_win_px") if yolo.get("keypoint_win_px") is not None else 6),
# 三条关键点量纲守卫（2026-10-08 真机一阶段被假 k0 带走，见
# feature_guard.py 的注释）。**必须在这里透传**：节点里声明的默认值
# 恰好等于 site.yaml 当前值，漏了不会报错，只会让以后改配置毫无反应
# （R1.1 的"声明了却没接线"）。
"keypoint_border_px": float(yolo.get("keypoint_border_px") if yolo.get("keypoint_border_px") is not None else 6.0),
"track_max_step_m": float(yolo.get("track_max_step_m") if yolo.get("track_max_step_m") is not None else 0.04),
"reacquire_far_px": float(yolo.get("reacquire_far_px") if yolo.get("reacquire_far_px") is not None else 120.0),
"reacquire_confirm_frames": int(yolo.get("reacquire_confirm_frames") if yolo.get("reacquire_confirm_frames") is not None else 3),
"plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
"plane_band_m": float(yolo.get("plane_band_m") or 0.06),
}
sio = _load_site_io()
role = sio.track_camera_role(data)
perc = data.get("perception") or {}
if role == "overhead":
_apply_overhead_range(data, out)
out["overhead_oneshot_enable"] = False
elif perc.get("track_camera") or "wrist_handoff_conf" in perc:
# 显式 wrist / 写了混合键 → 必须接全套。缺键就红，不许用代码默认。
# 没写 perception 段的探针测试保持旧行为（不加一次性）。
_apply_wrist_hybrid(data, _robot, out)
else:
out["overhead_oneshot_enable"] = False
return out
def servo_overrides(data: dict) -> dict:
@@ -540,12 +861,20 @@ def servo_overrides(data: dict) -> dict:
paths = data.get("paths") or {}
yolo = data.get("yolo") or {}
servo = data.get("servo") or {}
_ns, cam_frame, _depth = camera_frames(data)
_ns, cam_frame, _depth, he_mode = track_camera_frames(data)
he_file = _track_handeye_file(data, robot, he_mode)
xy_track = str(servo.get("xy_track") or "hover").strip().lower()
if he_mode == "eye_to_hand" and xy_track == "lookat":
raise RuntimeError(
"perception.track_camera=overhead 时相机固定，lookat（把目标对准画面中心）"
"没有意义。site.yaml 的 servo.xy_track 必须是 hover。"
)
out = {
"joint_prefix": str(robot.get("joint_prefix") or ""),
"base_frame": str(robot.get("base_frame") or ""),
"ee_frame": str(robot.get("ee_frame") or ""),
"handeye_file": str(paths.get("handeye_file") or ""),
"handeye_file": he_file,
"handeye_mode": he_mode,
"camera_frame": cam_frame,
"handeye_child_frame": cam_frame,
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
@@ -555,14 +884,19 @@ def servo_overrides(data: dict) -> dict:
keys = [
"control_hz", "watchdog_timeout_sec", "lost_timeout_sec", "stale_hold_sec",
"max_extrapolate_sec", "predict_enable", "predict_sec",
"predict_v_max", "predict_jump_m",
"predict_v_max", "predict_jump_m", "jump_confirm_frames", "feat_ws_margin_m",
"kp_xy", "ki_xy", "kd_xy", "kp_z", "ki_z", "kd_z", "i_sep_pos_m",
"deadzone_m", "deadzone_z_m", "v_max_xy", "v_max_z", "v_approach_z",
# 远处限速（见 _slow_xy_limit）：漏了这两个键，site.yaml 改了也不生效，
# 节点会静默退回声明默认值（R1.1）。
"slow_far_m", "v_slow_xy",
"z_min_m", "approach_xy_gate_m", "min_ee_z_m", "e_xy_clip_m",
"ee_x_m", "ee_y_m", "ee_z_m", "ee_hold_xy", "ee_vertical_down",
"xy_track", "hover_offset_ee_xy_m", "arm_label",
"ee_hold_kp", "ee_w_max",
"lock_hold_sec",
"lock_still_sec",
"lock_still_move_m",
"a_max", "max_qdot", "damping",
"vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
"min_cmd_delta_rad", "point_topic", "traj_topic",
@@ -572,6 +906,38 @@ def servo_overrides(data: dict) -> dict:
for k in keys:
if k in servo:
out[k] = servo[k]
sio = _load_site_io()
perc = data.get("perception") or {}
if sio.track_camera_role(data) == "wrist" and (
perc.get("track_camera") or "wrist_handoff_conf" in perc
):
if "wrist_handoff_conf" not in perc:
raise RuntimeError(
"servo 混合跟踪必须读 perception.wrist_handoff_conf "
"（与感知同一门槛，不许两边各写一个数）。"
)
out["wrist_handoff_conf"] = float(perc["wrist_handoff_conf"])
oh = perc.get("overhead") if isinstance(perc.get("overhead"), dict) else {}
if "oneshot_hold_s" not in oh:
raise RuntimeError(
"servo 混合跟踪必须写 perception.overhead.oneshot_hold_s "
"（D435 一次性设定保持多久）。"
)
out["overhead_hold_s"] = float(oh["oneshot_hold_s"])
if "lock_still_sec" not in servo:
raise RuntimeError(
"servo.lock_still_sec 必须显式写"
"（D405 跟住后法兰静止多久才进二阶段）。不许用代码默认 3。"
)
if "lock_still_move_m" not in servo:
raise RuntimeError(
"servo.lock_still_move_m 必须显式写"
"（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
)
out["lock_still_sec"] = float(servo["lock_still_sec"])
out["lock_still_move_m"] = float(servo["lock_still_move_m"])
out["overhead_k0_topic"] = "visual_servo/overhead_k0"
out["wrist_conf_topic"] = "visual_servo/wrist_conf"
return out
@@ -620,10 +986,21 @@ def main() -> int:
print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}")
print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}")
print(f" 眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}")
print(f" 眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}")
for _m in ("eye_in_hand", "eye_to_hand"):
_t = _m.upper()
_src = flat.get(f"SITE_HANDEYE_BOARD_SRC_{_t}")
print(
f" 标定板[{_m:12s}]: "
f"{flat.get(f'SITE_HANDEYE_BOARD_COLS_{_t}')}x"
f"{flat.get(f'SITE_HANDEYE_BOARD_ROWS_{_t}')} 内角点, "
f"square={flat.get(f'SITE_HANDEYE_SQUARE_{_t}')} m"
+ (" ← 没配过（标定会被拦下）" if _src == "default" else "")
)
print(
" 标定板 : "
f"{flat['SITE_HANDEYE_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m"
f" 手外相机 : {flat.get('SITE_OVERHEAD_CAM_NAME') or '（未登记）'} "
f"serial={flat.get('SITE_OVERHEAD_CAM_SERIAL') or '（空）'}"
)
print(
f" 相机 : serial={flat['SITE_CAM_SERIAL'] or '（空）'} "
scripts/run_all.sh
+34 -1
View File
@@ -20,6 +20,7 @@
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 只对准不轻降（验证精对准）
#
# 前置（另开一个终端，先跑起来）：
# bash ~/ros2/scripts/run_real.sh --arm 89
@@ -41,6 +42,7 @@ RESNAP=false
WITH_CAMERA=true
WITH_TACTILE=true
CAMERA_PREVIEW=false
NO_DESCENT=false
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
# shellcheck disable=SC1091
@@ -57,6 +59,9 @@ while [[ $# -gt 0 ]]; do
--depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
--pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
--resnap) RESNAP=true; shift ;;
# 只验证**精对准**、不轻降。对准逻辑改完先这么跑，确认规真被对到孔心再放开下降
# （对不准就下压会把规顶在孔沿上：2026-10-08 真机顶到 63N）。
--no-descent) NO_DESCENT=true; shift ;;
--no-preview) PIPELINE_PREVIEW=false; shift ;;
--camera-preview) CAMERA_PREVIEW=true; shift ;;
--no-camera) WITH_CAMERA=false; shift ;;
@@ -138,6 +143,33 @@ if [[ "${WITH_CAMERA}" == true ]]; then
fi
echo " 相机 ${CAM_NS} 出图 OK"
fi
# 一阶段混合跟踪：D405 看不见时要马上拍 D435。顶视没起来就只能干等。
D435_TOPIC="/d435/color/image_raw"
_d435_ok() {
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
}
if _d435_ok; then
echo "=== 顶视 ${D435_TOPIC} 已在发图，跳过拉起 ==="
else
echo "=== 拉起顶视 D435（一阶段一次性定位）==="
D435_ARGS=()
[[ "${CAMERA_PREVIEW}" == true ]] && D435_ARGS+=(--preview) || D435_ARGS+=(--no-preview)
nohup bash "${ROOT}/scripts/run_camera_d435.sh" \
"${D435_ARGS[@]}" > /tmp/run_all_d435.log 2>&1 &
echo " d435 pid=$! log=/tmp/run_all_d435.log"
d435_ok=false
for _i in $(seq 1 24); do
if _d435_ok; then d435_ok=true; break; fi
sleep 0.5
done
if [[ "${d435_ok}" != true ]]; then
echo "ERROR: ${D435_TOPIC} 仍无图像。看日志：/tmp/run_all_d435.log" >&2
echo " 混合跟踪没有 D435 就切不出去。USB 独占或没插好都会这样。" >&2
exit 1
fi
echo " 顶视 D435 出图 OK"
fi
else
echo "=== --no-camera：跳过相机检查 ==="
fi
@@ -206,10 +238,11 @@ ARGS=(
"skip_stage3:=${SKIP_STAGE3}"
"preview:=${PIPELINE_PREVIEW}"
"resnap:=${RESNAP}"
"no_descent:=${NO_DESCENT}"
)
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} ==="
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
scripts/run_camera_d405.sh
+8 -4
View File
@@ -34,7 +34,11 @@ source "${ROOT}/scripts/env.sh"
# shellcheck disable=SC1091
source "${ROOT}/scripts/lib/load_site_config.sh"
pkill -f 'lib/arm_application/realsense_color_pub' 2>/dev/null || true
CAM_NAME="${SITE_CAM_NAME:-d405}"
# 只杀本臂这台腕部相机。以前 pkill 全部 realsense_color_pub / realsense_view，
# 会把已经开着的顶视 D435 一起干掉（或反过来：后开 D435 把 D405 预览窗杀掉），
# 现场看起来就像"只剩眼外画面、绿框也不见了"。
pkill -f "camera_name:=${CAM_NAME}( |$)" 2>/dev/null || true
sleep 2
USB_INFO="$(python3 - <<PY 2>/dev/null || true
@@ -88,7 +92,6 @@ else
echo "=== camera 1280x720@15 ==="
fi
CAM_NAME="${SITE_CAM_NAME:-d405}"
CAM_ARGS=(
--ros-args
-p camera_name:="${CAM_NAME}"
@@ -140,12 +143,13 @@ if [[ "${DO_PREVIEW}" == "1" ]]; then
echo "=== close window = hide preview only; camera keeps publishing ==="
fi
echo "=== next: bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} [--control] ==="
echo "=== 本脚本只出图，不跑 YOLO。绿框来自 perception_node，请另开终端： ==="
echo "=== bash ~/ros2/scripts/run_visual_servo_rt.sh --arm ${SITE_ROBOT_ARM:-89} ==="
if [[ "${DO_PREVIEW}" == "1" ]]; then
# 相机进程只负责发布；Qt 查看器跑在自己的进程里，所以界面绘制
# 永远不会把采集循环饿死（以前共用 GIL 时每帧要花约 70 ms）。
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
# 前面已经按 camera_name 杀过本臂进程，这里不要再杀全部 realsense_view。
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
CAM_PID=$!
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
scripts/run_camera_d435.sh
+9 -4
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env bash
# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。不会打开任何一个 D405。
# 两臂之间的顶置 D435（`eye-to-hand`，相机固定在外）。
# 可以和 run_camera_d405.sh 同时开：只杀 camera_name:=d435，不动腕部那台。
#
# bash ~/ros2/scripts/run_camera_d435.sh --preview
#
@@ -24,7 +25,9 @@ if [[ -z "${SN}" ]]; then
exit 2
fi
echo "=== D435 overhead serial=${SN} ${SITE_D435_WIDTH:-1280}x${SITE_D435_HEIGHT:-720}@${SITE_D435_FPS:-30} ==="
pkill -f 'camera_name:=d435' 2>/dev/null || true
# 只杀顶视这一台。不要 pkill 全部 realsense_view —— 那会把正在看的 D405 预览窗干掉，
# 绿框叠在 D405 窗口上，窗没了就像"伺服没画面、也没工件框"。
pkill -f "camera_name:=d435( |$)" 2>/dev/null || true
CAM_ARGS=(
--ros-args
-p camera_name:=d435
@@ -40,11 +43,13 @@ CAM_ARGS=(
-p emitter_enabled:=true
)
if [[ "${PREVIEW}" == "1" ]]; then
pkill -f 'lib/arm_application/realsense_view' 2>/dev/null || true
ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
CAM_PID=$!
trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
ros2 run arm_application realsense_view --ros-args -p camera_name:=d435
# 工件框是 D405 像素，叠到 1280×720 的顶视图上位置全错，看起来像没检出。
ros2 run arm_application realsense_view --ros-args \
-p camera_name:=d435 \
-p overlay_detection:=false
else
exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
fi
scripts/run_handeye.sh
+95 -32
View File
@@ -68,25 +68,21 @@ if [[ ! -d "${APP_CFG}" ]]; then
fi
# 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。
# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号**
# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算）。
# 这些值全部由 site.yaml → export_site_env.py 提供，**包括文件名里的臂号与相机名**
# （臂号 = 该臂 SN 末两位，由 arm_short_id() 现算；相机名来自 cameras 注册表）。
#
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id` 和 `HANDEYE_MODE`
# 这里刻意**不**自己拼 `<臂号>_<模式>.yaml`：`_arm_id`、`HANDEYE_MODE` 与相机名
# 在下面几行之后才算得出来，在这里拼会拼出 `results/_.yaml` 这种空文件名，
# 而症状是"标定写完了、用的时候读不到"。
HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
WP="${SITE_HANDEYE_WAYPOINTS:?site.yaml 没有给出该臂的手眼示教点文件}"
CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
RES="${SITE_HANDEYE_FILE:?site.yaml 没有给出该臂的手眼结果文件}"
WORKDIR="${SITE_HANDEYE_WORKDIR:-${HE_DIR}/tmp}"
TAUGHT="${WORKDIR}/taught_poses.txt"
PREFIX="${SITE_JOINT_PREFIX:?SITE_JOINT_PREFIX 为空 —— site.yaml 里需要该臂的 robot.sn}"
CAM="${SITE_CAM_NAME:-d405}"
# ---- --mode：切换 eye-in-hand / eye-to-hand ----
# 必须在 WP/RES 定下来之前处理，因为它会改变读写的文件名。
# 其余参数原样留给子命令（teach 自己有 --rdk/--ros 等）。
# 必须在 WP/RES/板/相机定下来之前处理：模式决定了读哪个结果文件、用哪块板、
# 开哪台相机、采点落到哪个文件。其余参数原样留给子命令（teach 自己有 --rdk 等）。
HANDEYE_MODE="${SITE_HANDEYE_MODE:-eye_in_hand}"
_rest=()
while [[ $# -gt 0 ]]; do
@@ -109,15 +105,61 @@ set -- "${_rest[@]}"
# 臂号取 Flexiv 序列号后两位：Rizon4-063989 → 89
_arm_id="${SITE_ROBOT_SN: -2}"
if [[ -n "${_arm_id}" && "${_arm_id}" != "${SITE_ROBOT_SN}" ]]; then
if [[ "${HANDEYE_MODE}" != "${SITE_HANDEYE_MODE:-eye_in_hand}" ]]; then
# 显式指定了非默认模式 → 按 <臂号>_<模式>.yaml 重新定位
WP="${HE_DIR}/poses/${_arm_id}_${HANDEYE_MODE}.yaml"
RES="${HE_DIR}/results/${_arm_id}_${HANDEYE_MODE}.yaml"
echo "[handeye] --mode ${HANDEYE_MODE} → poses: ${WP}"
echo "[handeye] result: ${RES}"
# 取"当前模式"的那一份值。名字由 export_site_env 按模式导出成
# SITE_HANDEYE_<字段>_EYE_IN_HAND / _EYE_TO_HAND
# 这样"模式 → 结果文件 / 示教点 / 标定板"的对应关系只有 site.yaml 一处定义，
# 脚本不再自己拼文件名（拼错/漏拼都是**静默**用错文件，最难查）。
_HE_TAG="$(echo "${HANDEYE_MODE}" | tr '[:lower:]' '[:upper:]')"
_he() {
local name="SITE_HANDEYE_$1_${_HE_TAG}"
printf '%s' "${!name-}"
}
WP="$(_he WAYPOINTS)"
WP="${WP:-${HE_DIR}/poses/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
RES="$(_he FILE)"
RES="${RES:-${HE_DIR}/results/${_arm_id:-?}_${HANDEYE_MODE}.yaml}"
# 示教点临时文件也按模式分开：两个模式共用一份时，后一个 teach 会清掉前一个的采点，
# 而症状只是"merge 出来的点位不对"，很难联想到是文件串了。
TAUGHT="${WORKDIR}/taught_poses_${HANDEYE_MODE}.txt"
# ---- 相机：两种模式用的是**两台不同的相机** ----
# 眼在手上 = 腕部相机（跟着臂动）；眼在手外 = 固定在工位外的相机（不随臂动）。
# 原来这里恒用 SITE_CAM_NAME（腕部），于是 `--mode eye_to_hand` 会去开腕部相机 ——
# 板跟着臂动、相机也跟着臂动，"标定"能跑完，但解出的是个毫无意义的错外参。
if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
CAM="${SITE_OVERHEAD_CAM_NAME:-}"
if [[ -z "${CAM}" ]]; then
echo "ERROR: eye_to_hand 需要一台固定相机，但 site.yaml 的 cameras 段里没有登记的。" >&2
echo " 加一条 role: overhead_eye_to_hand（眼在手外、不随臂动）的相机即可。" >&2
exit 2
fi
CAM_SERIAL="${SITE_OVERHEAD_CAM_SERIAL:-}"
CAM_WIDTH="${SITE_OVERHEAD_CAM_WIDTH:-}"
CAM_HEIGHT="${SITE_OVERHEAD_CAM_HEIGHT:-}"
CAM_FPS="${SITE_OVERHEAD_CAM_FPS:-30}"
CAM_AE="${SITE_OVERHEAD_CAM_AUTO_EXPOSURE:-1}"
CAM_AE_LIMIT="${SITE_OVERHEAD_CAM_AE_LIMIT_US:-8000}"
CAM_EMITTER="${SITE_OVERHEAD_CAM_EMITTER:-1}"
CAM_EXPOSURE="${SITE_OVERHEAD_CAM_EXPOSURE_US:-0}"
CAM_GAIN="${SITE_OVERHEAD_CAM_GAIN:-0}"
else
CAM="${SITE_CAM_NAME:-d405}"
CAM_SERIAL="${SITE_CAM_SERIAL:-}"
CAM_WIDTH="${SITE_CAM_WIDTH:-}"
CAM_HEIGHT="${SITE_CAM_HEIGHT:-}"
CAM_FPS="${SITE_CAM_FPS:-30}"
CAM_AE="${SITE_CAM_AUTO_EXPOSURE:-1}"
CAM_AE_LIMIT="${SITE_CAM_AE_LIMIT_US:-1800}"
CAM_EMITTER="${SITE_CAM_EMITTER:-0}"
CAM_EXPOSURE="${SITE_CAM_EXPOSURE_US:-0}"
CAM_GAIN="${SITE_CAM_GAIN:-0}"
fi
echo "[handeye] mode=${HANDEYE_MODE} poses=${WP}"
echo "[handeye] result=${RES}"
echo "[handeye] camera=${CAM} serial=${CAM_SERIAL:-（空）}"
_kill_servo() {
pkill -f 'visual_servo.launch' 2>/dev/null || true
@@ -159,7 +201,9 @@ cmd_show() {
echo " tour : ${TOUR}"
echo " result : ${RES}"
echo " workdir : ${WORKDIR}"
echo " board : ${SITE_HANDEYE_BOARD_COLS:-?}x${SITE_HANDEYE_BOARD_ROWS:-?} inner, square=${SITE_HANDEYE_SQUARE_M:-?} m"
echo " camera : ${CAM} serial=${CAM_SERIAL:-（空）}"
echo " board : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m (来源=$(_he BOARD_SRC))"
echo " （另一种模式的板见下面 export 的 标定板[...] 两行）"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
if [[ -f "${WP}" ]]; then
python3 - <<PY
@@ -181,15 +225,26 @@ cmd_merge() {
}
cmd_calibrate() {
# 先查配置（板尺寸）再看硬件：配置没配好的话，连硬件都不该去动。
local cols="$(_he BOARD_COLS)"
local rows="$(_he BOARD_ROWS)"
local sq="$(_he SQUARE)"
local src="$(_he BOARD_SRC)"
# 板尺寸必须**真的配过**。没配就开标定 = 赌一个方格边长，而错了不会报错，
# 只会让结果系统性偏（见 handeye/README.md 第 3 节）。所以这里直接拦下。
if [[ -z "${src}" || "${src}" == "default" ]]; then
echo "ERROR: 没有配置 [${HANDEYE_MODE}] 用的标定板尺寸。" >&2
echo " 在 config/site.yaml 的 handeye.boards.${HANDEYE_MODE} 里写：" >&2
echo " cols / rows / square_size_m" >&2
echo " （内角点 = 方格数 − 1；测一下实际方格边长，别照抄另一块板。）" >&2
exit 2
fi
_need_files "${CAL}" "${WP}"
_need_robot
_need_camera
echo "=== kill visual_servo (avoid traj fight) ==="
_kill_servo
mkdir -p "${WORKDIR}"
local cols="${SITE_HANDEYE_BOARD_COLS:-8}"
local rows="${SITE_HANDEYE_BOARD_ROWS:-11}"
local sq="${SITE_HANDEYE_SQUARE_M:-0.015}"
local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"
@@ -200,7 +255,8 @@ cmd_calibrate() {
local ps="${SITE_HANDEYE_PIN_SEC:-0.45}"
local skip="${SITE_HANDEYE_SKIP_UNSTABLE:-true}"
echo "=== calibrate mode=${HANDEYE_MODE} ==="
echo " board ${cols}x${rows} inner, square=${sq} m (from site.yaml)"
echo " board ${cols}x${rows} inner, square=${sq} m (site.yaml handeye.boards.${HANDEYE_MODE}，来源=${src})"
echo " camera ${CAM} serial=${CAM_SERIAL:-（空）}"
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
echo " poses=${WP}"
echo " result → ${RES}"
@@ -209,6 +265,7 @@ cmd_calibrate() {
--params-file "${WP}" \
-p joint_prefix:="${PREFIX}" \
-p camera_source:="${CAM}" \
${CAM_SERIAL:+-p camera_serial:="${CAM_SERIAL}"} \
-p output_dir:="${WORKDIR}" \
-p result_file:="${RES}" \
-p mode:="${HANDEYE_MODE}" \
@@ -304,18 +361,18 @@ cmd_teach() {
-p output_dir:="${WORKDIR}"
-p target_points:="${points}"
)
if [[ -n "${SITE_CAM_SERIAL:-}" ]]; then
TEACH_ARGS+=(-p camera_serial:="${SITE_CAM_SERIAL}")
if [[ -n "${CAM_SERIAL}" ]]; then
TEACH_ARGS+=(-p camera_serial:="${CAM_SERIAL}")
fi
if [[ -n "${SITE_CAM_WIDTH:-}" ]]; then
TEACH_ARGS+=(-p width:="${SITE_CAM_WIDTH}" -p height:="${SITE_CAM_HEIGHT:-480}" -p fps:="${SITE_CAM_FPS:-30}")
if [[ -n "${CAM_WIDTH}" ]]; then
TEACH_ARGS+=(-p width:="${CAM_WIDTH}" -p height:="${CAM_HEIGHT:-480}" -p fps:="${CAM_FPS:-30}")
fi
TEACH_ARGS+=(
-p auto_exposure:="$([ "${SITE_CAM_AUTO_EXPOSURE:-1}" = "1" ] && echo true || echo false)"
-p ae_limit_us:="${SITE_CAM_AE_LIMIT_US:-1800}"
-p emitter_enabled:="$([ "${SITE_CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
-p exposure_us:="${SITE_CAM_EXPOSURE_US:-0}"
-p gain:="${SITE_CAM_GAIN:-0}"
-p auto_exposure:="$([ "${CAM_AE:-1}" = "1" ] && echo true || echo false)"
-p ae_limit_us:="${CAM_AE_LIMIT:-1800}"
-p emitter_enabled:="$([ "${CAM_EMITTER:-0}" = "1" ] && echo true || echo false)"
-p exposure_us:="${CAM_EXPOSURE:-0}"
-p gain:="${CAM_GAIN:-0}"
)
ros2 run arm_application handeye_teach --ros-args "${TEACH_ARGS[@]}" || true
@@ -362,8 +419,14 @@ cmd_teach() {
# 但 teach 阶段把相机杀掉了（USB 独占），标定拍照要用，得先拉回来。
echo
echo "=== 重启相机（teach 阶段杀掉了它，标定拍照需要）==="
nohup bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" \
--no-preview > /tmp/run_handeye_camera.log 2>&1 &
if [[ "${HANDEYE_MODE}" == "eye_to_hand" ]]; then
# 眼在手外用的是固定相机（D435），不是腕部 D405 —— 拉错相机的话，
# 标定会去订一个没人发布的图，表现是"相机没数据"。
_cam_cmd=(bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview)
else
_cam_cmd=(bash "${ROOT}/scripts/run_camera_d405.sh" --arm "${SITE_ROBOT_ARM:-89}" --no-preview)
fi
nohup "${_cam_cmd[@]}" > /tmp/run_handeye_camera.log 2>&1 &
echo " pid=$! log=/tmp/run_handeye_camera.log"
# _need_camera 内部失败时会自己打印原因并 exit，不需要在这里再判一次。
_need_camera
scripts/run_visual_servo_rt.sh
+33 -4
View File
@@ -8,6 +8,11 @@
# --control 从当前位姿直接开始跟踪（不抬升，也不回 home）。
# 89/91 手眼与相机各自独立：必须与 run_real.sh 同一 --arm。
#
# 跟踪相机由 site.yaml 的 perception.track_camera 决定：
# wrist 腕部 D405 主跟踪；画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0
# —— 需要同时开 run_camera_d405.sh 与 run_camera_d435.sh
# overhead 整段只用顶置 D435（实验）—— 另开 run_camera_d435.sh
#
# 切换 YOLO 权重：改 site.yaml 里的 paths.yolo_weights
set -eo pipefail
@@ -71,13 +76,37 @@ else
echo "=== launch perception + servo (observe only, no arm motion) ==="
fi
EE="${SITE_EE_FRAME:?SITE_EE_FRAME 为空 —— site.yaml 的 robot.robots[] 里需要该臂的 SN}"
HANDEYE="${SITE_HANDEYE_FILE:-${ROOT}/src/arm_application/config/handeye_result.yaml}"
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} handeye=${HANDEYE}"
echo " weights=${SITE_YOLO_WEIGHTS} camera=${SITE_CAM_NAME:-} control=${CONTROL}"
TRACK="${SITE_TRACK_CAMERA:-wrist}"
HANDEYE="${SITE_TRACK_HANDEYE_FILE:-${SITE_HANDEYE_FILE:-}}"
CAM_FRAME="${SITE_TRACK_CAMERA_FRAME:-${SITE_CAMERA_FRAME:-}}"
CAM_NAME="${SITE_TRACK_CAM_NAME:-${SITE_CAM_NAME:-}}"
if [[ -z "${HANDEYE}" ]]; then
echo "ERROR: 跟踪用手眼文件为空（SITE_TRACK_HANDEYE_FILE / SITE_HANDEYE_FILE）" >&2
exit 2
fi
if [[ ! -f "${HANDEYE}" ]]; then
echo "ERROR: 跟踪用手眼文件不存在：${HANDEYE}" >&2
if [[ "${TRACK}" == "overhead" ]]; then
echo " 先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate --mode eye_to_hand" >&2
else
echo " 先：bash ~/ros2/scripts/run_handeye.sh --arm ${SITE_ROBOT_ARM:-89} calibrate" >&2
fi
exit 2
fi
echo " arm=${SITE_ROBOT_SN} ${SITE_ROBOT_LABEL:-} track=${TRACK} camera=${CAM_NAME}"
echo " handeye=${HANDEYE} frame=${CAM_FRAME} control=${CONTROL}"
if [[ "${TRACK}" == "overhead" ]]; then
echo "=== 整段顶视：D435 追踪，D405 不参与一阶段 ==="
echo " 相机请开：bash ~/ros2/scripts/run_camera_d435.sh --preview"
else
echo "=== 本脚本不弹预览窗。工件框画在 D405 那个查看器上 ==="
echo " bash ~/ros2/scripts/run_camera_d405.sh --arm ${SITE_ROBOT_ARM:-89} --preview"
echo " bash ~/ros2/scripts/run_camera_d435.sh --preview # 顶视可开，绿框仍在 D405 窗"
fi
LAUNCH_ARGS=(
enable_control:="${CONTROL}"
handeye_file:="${HANDEYE}"
ee_frame:="${EE}"
camera_frame:="${SITE_CAMERA_FRAME:-d405_color_optical_frame}"
camera_frame:="${CAM_FRAME}"
)
exec ros2 launch arm_application visual_servo.launch.py "${LAUNCH_ARGS[@]}"
scripts/sense_tool_length.py
+309
View File
@@ -0,0 +1,309 @@
#!/usr/bin/env python3
"""真机标定 `tool_tip_below_flange_m`：**规尖**在法兰下方多远 —— 视觉感知。
为什么需要单独标定
------------------
规是**夹在爪里**的，露出多少随装夹变化（仿真上料脚本实测见过 243 / 254mm 两种）。
而真机上**没有"规尖"的 TF**：规只是被夹住的一个零件，TF 树里查不到它。所以只能
**感知**：腕部相机看到规，用 `T_base = T_base_flange · T_flange_cam` 把手眼链
反投影回基座，量「法兰 z − 规尖 z」。
这个值一旦偏小，下游几何就会把规按进工件（site.yaml 记着 2026-09-29 的事故：
悬停这一步就把规插进工件，S05 横向对准变成拿规尖拨工件，工件被推走 19mm）。
口径（必须显式）
----------------
* 量的是**竖直**距离（基座 z 之差），不是相机光轴方向的深度 —— 两者因相机
29.7° 的倾角不同，混用会差 20mm 量级。
* 只在法兰近似**竖直朝下**时成立（悬停/跟踪都是该姿态）。
自证（不许"看起来差不多"）
--------------------------
先把**当场**反投影出来的**工件上表面** z 与 site.yaml 记录的
`workpiece_surface_z_m` 比：差 > `--tol-mm`（默认 5mm）就判链路/手眼/深度有
系统性问题并**拒绝输出**。这一步用的是独立已知量，能真的失败。
用法
----
bash ~/ros2/scripts/run_real.sh --arm 89 # 另开终端：起控制栈
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 # 起相机
# 把臂开到**悬停高**（stage2 的 hole_hover_z_m，竖直朝下、规在视野里）
python3 ~/ros2/scripts/sense_tool_length.py --sn Rizon4-063989
输出一行 `**tool_tip_below_flange_m: 0.xxx**` 与它是否通过自证。
"""
from __future__ import annotations
import argparse
import sys
import time
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
def _load_frame(sn: str, handeye_file: Path):
"""读手眼 T_flange_cam（eye-in-hand）。缺文件直接失败。"""
import yaml
doc = yaml.safe_load(handeye_file.read_text(encoding="utf-8")) or {}
if "T_4x4" not in doc:
raise SystemExit(f"手眼文件 {handeye_file} 里没有 T_4x4（键：{sorted(doc)}）")
return np.array(doc["T_4x4"], dtype=float)
class _Grabber:
"""抓彩色/对齐深度/内参 + 查 TF。放在类里是为了让订阅和 TF 监听一起 spin。"""
def __init__(self, cam_ns: str):
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from sensor_msgs.msg import CameraInfo, Image
import tf2_ros
self.rclpy = rclpy
self.node = Node("sense_tool_length")
self.color = self.depth = self.info = None
qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.BEST_EFFORT)
ns = cam_ns.strip("/")
self.node.create_subscription(Image, f"/{ns}/color/image_raw",
lambda m: setattr(self, "color", m), qos)
self.node.create_subscription(Image, f"/{ns}/aligned_depth_to_color/image_raw",
lambda m: setattr(self, "depth", m), qos)
self.node.create_subscription(CameraInfo, f"/{ns}/color/camera_info",
lambda m: setattr(self, "info", m), qos)
self.buf = tf2_ros.Buffer()
self.lis = tf2_ros.TransformListener(self.buf, self.node)
def spin(self, dt: float = 0.05) -> None:
self.rclpy.spin_once(self.node, timeout_sec=dt)
def wait_frames(self, timeout_s: float = 8.0) -> bool:
t0 = time.monotonic()
while time.monotonic() - t0 < timeout_s:
self.spin()
if self.color is not None and self.depth is not None and self.info is not None:
return True
return False
def need_fresh(self, timeout_s: float = 4.0) -> bool:
t0 = time.monotonic()
while time.monotonic() - t0 < timeout_s:
self.spin()
if self.color is None or self.depth is None:
continue
now = self.node.get_clock().now().nanoseconds * 1e-9
def age(m):
return now - m.header.stamp.sec - m.header.stamp.nanosec * 1e-9
if age(self.color) < 1.0 and age(self.depth) < 1.0:
return True
return False
def T(self, parent: str, child: str, timeout_s: float = 3.0):
t0 = time.monotonic()
while time.monotonic() - t0 < timeout_s:
self.spin()
try:
tr = self.buf.lookup_transform(parent, child, self.rclpy.time.Time())
except Exception:
continue
t, r = tr.transform.translation, tr.transform.rotation
x, y, z, w = r.x, r.y, r.z, r.w
M = np.eye(4)
M[:3, :3] = [
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
]
M[:3, 3] = [t.x, t.y, t.z]
return M
return None
def _depth_m(msg) -> np.ndarray:
if (msg.encoding or "").lower() != "16uc1":
raise SystemExit(f"深度编码应为 16UC1(mm)，实际 {msg.encoding!r}")
return (np.frombuffer(msg.data, dtype=np.uint16)
.reshape(int(msg.height), int(msg.width)).astype(np.float32) * 1e-3)
def _near_tip(depth: np.ndarray, near_m: float, min_m: float, max_m: float):
"""近端连通域里"最深"的像素 = 规尖候选。
为什么用连通域而不是全图最深处：工件面/台面比规更远，全图最深处会落到工件上；
规是**唯一**贴着相机（< near_m）的物体，所以先在近端找块，再取块内最深点。
"""
import cv2
valid = (depth > min_m) & (depth < max_m)
near = (valid & (depth < near_m)).astype(np.uint8)
if int(near.sum()) < 150:
return None
n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8)
if n <= 1:
return None
big = max(range(1, n), key=lambda i: stats[i, 4])
ys, xs = np.where(lab == big)
dv = depth[ys, xs]
k = np.percentile(dv, 97) # 块内最深 3% ≈ 规尖
sel = dv >= k - 1e-6
ty = int(round(float(np.median(ys[sel]))))
tx = int(round(float(np.median(xs[sel]))))
return tx, ty
def main(argv=None) -> int:
ap = argparse.ArgumentParser(description="真机视觉标定 tool_tip_below_flange_m")
ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml robot.sn）")
ap.add_argument("--site", default=str(ROOT / "config" / "site.yaml"))
ap.add_argument("--frames", type=int, default=8, help="取多少帧求中值")
ap.add_argument("--near-m", type=float, default=0.15, help="近端阈值：规比这更近 [m]")
ap.add_argument("--min-m", type=float, default=0.04)
ap.add_argument("--max-m", type=float, default=0.60)
ap.add_argument("--tol-mm", type=float, default=5.0,
help="反投影出的工件面 z 与 site.yaml 记录值的最大允许差 [mm]")
ap.add_argument("--save", default="", help="把带标记的彩图写到这个路径（可选）")
args = ap.parse_args(argv)
import export_site_env as ex
data = ex.load_site(Path(args.site))
sn = (args.sn or "").strip() or str((data.get("robot") or {}).get("sn") or "").strip()
if not sn:
raise SystemExit("没给 --sn，且 site.yaml 的 robot.sn 为空")
merged, robot = ex.apply_active_arm(data, sn)
cam_ns = ex.camera_ns(merged)
base = str(merged.get("servo", {}).get("base_frame") or f"{sn}_base_link")
flange = f"{sn}_flange"
he_file = Path(str((merged.get("paths") or {}).get("handeye_file") or ""))
if not he_file.is_file():
raise SystemExit(f"手眼文件不存在：{he_file}")
gp = (merged.get("gauge_assembly") or {})
surface_ref = float(gp.get("workpiece_surface_z_m") or 0.0)
cur = float(gp.get("tool_tip_below_flange_m") or 0.0)
print(f"=== 真机工具长标定 SN={sn} cam=/{cam_ns.strip('/')} ===")
print(f" 基座帧={base} 法兰帧={flange}")
print(f" 手眼 ={he_file}")
print(f" site 现值 tool_tip_below_flange_m={cur:.4f} 工件面={surface_ref:.4f}")
import rclpy
rclpy.init()
try:
g = _Grabber(cam_ns)
except Exception as exc: # noqa: BLE001
print(f"节点初始化失败：{exc}", file=sys.stderr)
return 2
try:
if not g.wait_frames(10.0):
print("错误：没收到相机帧。先起相机：bash scripts/run_camera_d405.sh --arm <臂>",
file=sys.stderr)
return 1
T_flange_cam = _load_frame(sn, he_file)
tips, flange_zs, surf = [], [], []
for i in range(max(1, args.frames)):
if not g.need_fresh():
continue
dep = _depth_m(g.depth)
k = g.info.k
fx, fy, cx, cy = float(k[0]), float(k[4]), float(k[2]), float(k[5])
T_bf = g.T(base, flange)
if T_bf is None:
print("错误：TF 查不到 base→flange（控制栈起了吗？）", file=sys.stderr)
return 1
T_bc = T_bf @ T_flange_cam
got = _near_tip(dep, args.near_m, args.min_m, args.max_m)
if got is None:
continue
ux, vy = got
# 尖端深度取 5x5 中值抑噪；再用整条链反投到基座
y0, y1 = max(0, vy - 2), min(dep.shape[0], vy + 3)
x0, x1 = max(0, ux - 2), min(dep.shape[1], ux + 3)
patch = dep[y0:y1, x0:x1]
pv = patch[(patch > args.min_m) & (patch < args.max_m)]
if pv.size == 0:
continue
zt = float(np.median(pv))
p_cam = np.array([(ux - cx) * zt / fx, (vy - cy) * zt / fy, zt, 1.0])
tip_base = (T_bc @ p_cam)[:3]
tips.append(float(T_bf[2, 3]) - float(tip_base[2]))
flange_zs.append(float(T_bf[2, 3]))
# 工件面：取画面下部中央一块（工件/台面），用同一链反投影
h, w = dep.shape
band = dep[int(h * 0.80):int(h * 0.95), int(w * 0.30):int(w * 0.70)]
bv = band[(band > args.min_m) & (band < args.max_m)]
if bv.size > 50:
zb = float(np.median(bv))
bu, bvv = int(w * 0.50), int(h * 0.88)
pb = (T_bc @ np.array([(bu - cx) * zb / fx, (bvv - cy) * zb / fy, zb, 1.0]))[:3]
surf.append(float(pb[2]))
if args.save and args.frames == 1:
try:
import cv2
bgr = np.frombuffer(g.color.data, np.uint8).reshape(
int(g.color.height), int(g.color.width), 3).copy()
cv2.circle(bgr, (ux, vy), 8, (0, 0, 255), 2)
cv2.imwrite(args.save, bgr)
except Exception:
pass
g.spin(0.05)
if not tips:
print("错误：没找到规（近端 < %.0fmm 的物体）。确认规夹在爪里、且臂在竖直朝下姿态。"
% (args.near_m * 1000), file=sys.stderr)
return 1
tl = float(np.median(tips))
spread = (max(tips) - min(tips)) * 1000.0
fz = float(np.median(flange_zs))
print(f"\n 帧数={len(tips)} 法兰 z={fz:.4f}")
print(f" 感知工具长 = **{tl:.4f} m** (逐帧散布 {spread:.1f}mm)")
ok = True
tip_z = fz - tl
if surf:
zs = float(np.median(surf))
# 画面下部那块可能是**工件面**（site 记录 surface_ref）也可能是**台面**
# （真机基座与台面同高 ⇒ ≈0）。两者都是**独立已知量**，用它们卡链路：
# 只要反投影出的 z 落在允许窗内，就说明手眼/深度/姿态没有系统性问题。
lo = min(0.0, surface_ref) - 0.03
hi = max(0.0, surface_ref) + 0.04
tag = "OK" if lo <= zs <= hi else "FAIL"
print(f" 自证 反投影支撑面 z={zs:.4f}"
f"（site 工件面 {surface_ref:.4f}、台面≈0）允许 [{lo:+.3f},{hi:+.3f}] → [{tag}]")
if not (lo <= zs <= hi):
ok = False
if not (tip_z > zs + 0.003):
print(f" 自证：规尖 z={tip_z:.4f} 不在支撑面 z={zs:.4f} 之上 → FAIL")
ok = False
else:
print(" 自证：画面下部没采到有效深度，跳过（不敢保证链路）")
if not (0.10 <= tl <= 0.35):
print(f" 自证：工具长 {tl:.3f} 不在合理区间 [0.10, 0.35] → FAIL")
ok = False
if not ok:
print("\n ✗ 自证未过 —— **不要**把这个值写进 site.yaml，先查手眼/深度/姿态。")
return 4
print(f"\n ✓ 可写：gauge_assembly.tool_tip_below_flange_m: {tl:.3f}"
f"（现值 {cur:.4f}，差 {(tl - cur) * 1000:+.1f}mm）")
return 0
finally:
try:
g.rclpy.shutdown()
except Exception:
pass
if __name__ == "__main__":
raise SystemExit(main())
scripts/test_rdk_python.sh
+13
View File
@@ -28,6 +28,19 @@ if iface:
break
except TypeError:
continue
except RuntimeError as exc:
# 这个 iface 用不了（实测：enp4s0 上有 5 个 IPv4 时，RDK 的
# UDPv4Transport 会把整张白名单都过滤掉，报 "All whitelist
# interfaces were filtered out"），别再拿它试了，交给下面的
# 「不指定网卡」兜底。
#
# **这一条以前漏了**：老代码只捕获 TypeError，而真实失败是
# RuntimeError —— 于是异常直接抛出去，下面那行兜底永远走不到。
# 后果是预检报「机械臂 RDK 连不上」这个**假阻断**，
# 而真机栈其实连得上（flexiv_hardware_interface.cpp 构造
# flexiv::rdk::Robot 时只传 robot_sn、不传网卡）。
print(f" iface={iface} 不可用（{str(exc)[:70]}），回退到不指定网卡")
break
if robot is None:
robot = flexivrdk.Robot(sn)
print("Connected. Clearing fault if any...")