arm_dev
36a59c81bf
三处都是"看着像硬件问题、其实是判据问题"的假警报：
1. S00 自检假阴性（gauge_boot.py）
探测等待时用 time.sleep 不 spin → 订阅回调根本没机会执行，
于是**健康的硬件也会被判"没有数据"**。改成等待期间持续 spin。
2. S07 触觉假断流（stage3_search_node.py）
判据原本是 |fnormal| > 1.0，但力本来就可能是零或很小（真机轻触、
仿真刚接触）→ 明明在发数据却判"触觉断流"。
改成"数据够新（TACTILE_MAX_AGE_S 内）+ connected" —— 判活看的是
**有没有在发**，不是**数值大不大**。
3. S07 下探静默卡死（gauge_fsm.py / gauge_assembly_node.py）
_goto 的到位容差 ≥ 步长时，每一步都被判成"已在容差内"，臂一步都不走，
而外层只看到"没进度"→ 死循环到超时，日志里看不出原因。
新增 descend_tol_m / descend_iter_budget / descend_progress：
容差从步长推导（不该手工调大），并显式判"连续 N 步零位移"。
容差 ≥ 步长的反例已进 site 回归测试，防止再犯。
4. site 分段化（export_site_env.py）：89/91 两臂的 servo/handeye/点位
各一段，避免"改 91 影响 89"。
另：run_unit_tests.sh 作为统一测试入口（932→933 项），
devices/sim/gazebo/README.md 同步。
Co-authored-by: Cursor <cursoragent@cursor.com>
698 lines
31 KiB
Python
698 lines
31 KiB
Python
#!/usr/bin/env python3
|
||
"""读取 ``~/ros2/config/site.yaml``，打印 bash 的 export 行或 JSON。
|
||
|
||
用法:
|
||
eval "$(python3 ~/ros2/scripts/lib/export_site_env.py)"
|
||
python3 ~/ros2/scripts/lib/export_site_env.py --json
|
||
python3 ~/ros2/scripts/lib/export_site_env.py --print
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import json
|
||
import os
|
||
import sys
|
||
from pathlib import Path
|
||
|
||
try:
|
||
import yaml
|
||
except ImportError:
|
||
yaml = None # type: ignore
|
||
|
||
|
||
def _root() -> Path:
|
||
return Path(__file__).resolve().parents[2]
|
||
|
||
|
||
def site_path() -> Path:
|
||
env = os.environ.get("SITE_CONFIG", "").strip()
|
||
if env:
|
||
return Path(env).expanduser()
|
||
return _root() / "config" / "site.yaml"
|
||
|
||
|
||
def load_site(path: Path | None = None) -> dict:
|
||
path = path or site_path()
|
||
if not path.is_file():
|
||
raise FileNotFoundError(f"找不到 site 配置: {path}")
|
||
text = path.read_text(encoding="utf-8")
|
||
if yaml is None:
|
||
raise RuntimeError("需要 PyYAML：pip/conda install pyyaml")
|
||
data = yaml.safe_load(text) or {}
|
||
if not isinstance(data, dict):
|
||
raise ValueError(f"无效的 site.yaml: {path}")
|
||
return data
|
||
|
||
|
||
def list_robots(data: dict) -> list[dict]:
|
||
"""从 robot.robots 取清单条目（列表或按 SN 索引的映射）。"""
|
||
raw = (data.get("robot") or {}).get("robots") or []
|
||
out: list[dict] = []
|
||
if isinstance(raw, dict):
|
||
for sn, item in raw.items():
|
||
row = dict(item or {}) if isinstance(item, dict) else {}
|
||
row.setdefault("sn", str(sn))
|
||
out.append(row)
|
||
return out
|
||
if isinstance(raw, list):
|
||
for item in raw:
|
||
if isinstance(item, dict) and str(item.get("sn") or "").strip():
|
||
out.append(dict(item))
|
||
return out
|
||
|
||
|
||
# 侧别词 —— 这是**语言别名**，不是站点数据：任何站点都希望
|
||
# `--arm left` / `--arm 左` / `--arm 左臂` 能选中 `robots[]` 里 `arm: left` 的那台。
|
||
#
|
||
# 这里**绝不允许**出现具体 SN（`Rizon4-063989` 之类）。历史上这里是一张
|
||
# `89 -> Rizon4-063989` 的写死映射表：换一台机器人后 `--arm 89` 仍然会选中
|
||
# 上一台，而且不报错（R1.1）。现在 `89` 这个写法的来源是
|
||
# **这台机器 SN 的末两位**，由 `_arm_index()` 从 `robots[]` 现算出来。
|
||
_SIDE_WORDS = {
|
||
"left": "left",
|
||
"l": "left",
|
||
"左": "left",
|
||
"左臂": "left",
|
||
"right": "right",
|
||
"r": "right",
|
||
"右": "right",
|
||
"右臂": "right",
|
||
}
|
||
|
||
_ARM_SECTION_KEYS = (
|
||
"camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo", "perception",
|
||
)
|
||
|
||
|
||
def arm_short_id(sn: str) -> str:
|
||
"""Flexiv 序列号的"臂号"= 末两位数字（``Rizon4-063989`` → ``89``）。
|
||
|
||
手眼文件名、结果文件都用这个编号。它是从 SN 派生出来的，不是常量。
|
||
"""
|
||
tail = str(sn or "").strip().rsplit("-", 1)[-1]
|
||
return tail[-2:] if tail.isdigit() and len(tail) >= 2 else ""
|
||
|
||
|
||
def _arm_index(data: dict) -> dict[str, str]:
|
||
"""从 ``robots[]`` 派生 ``别名 -> SN`` 索引。
|
||
|
||
别名包括：完整 SN、``label``（如 ``左臂89``）、侧别词（``left``/``左``）、
|
||
SN 末两位（``89``）。全部来自 site.yaml —— 这是"换一台机器人只改配置"的前提。
|
||
"""
|
||
index: dict[str, str] = {}
|
||
for item in list_robots(data):
|
||
sn = str(item.get("sn") or "").strip()
|
||
if not sn:
|
||
continue
|
||
names = {sn.lower()}
|
||
label = str(item.get("label") or "").strip()
|
||
if label:
|
||
names.add(label.lower())
|
||
arm = str(item.get("arm") or "").strip().lower()
|
||
if arm:
|
||
names.add(arm)
|
||
names.update(word for word, side in _SIDE_WORDS.items() if side == arm)
|
||
short = arm_short_id(sn)
|
||
if short:
|
||
names.add(short)
|
||
if arm:
|
||
names.add(f"{arm}{short}")
|
||
for name in names:
|
||
if name:
|
||
index.setdefault(name, sn)
|
||
return index
|
||
|
||
|
||
def _looks_like_full_sn(token: str) -> bool:
|
||
"""区分"完整 SN"与"臂别名"。
|
||
|
||
``Rizon4-063989`` 这种带连字符的写法允许不在 ``robots[]`` 里
|
||
（单臂站点可以只写 ``robot.sn`` 不登记清单）；而 ``89`` / ``left`` 这类
|
||
**别名**查不到就必须报错，不能当成 SN 硬用。
|
||
"""
|
||
return "-" in token
|
||
|
||
|
||
def normalize_robot_sn(data: dict, want: str) -> str:
|
||
"""把 89 / left / 左臂 / label / 完整 SN 归一成完整 SN（查不到则原样返回）。"""
|
||
return resolve_robot_sn(data, want)[0]
|
||
|
||
|
||
def resolve_robot_sn(data: dict, want: str) -> tuple[str, bool]:
|
||
"""归一化并报告是否命中。
|
||
|
||
返回 ``(sn, matched)``。``matched=False`` 表示这个写法在 ``robots[]`` 里
|
||
查不到 —— 调用方必须**报错**（见 ``resolve_robot``），不能静默拿它当 SN，
|
||
否则会去连一台不存在的机器人。
|
||
"""
|
||
w = (want or "").strip()
|
||
if not w:
|
||
return "", False
|
||
index = _arm_index(data)
|
||
hit = index.get(w.lower())
|
||
if hit:
|
||
return hit, True
|
||
if _looks_like_full_sn(w):
|
||
return w, True
|
||
return w, False
|
||
|
||
|
||
def describe_arms(data: dict) -> str:
|
||
"""给报错信息用：列出本配置里可选的全部机械臂。"""
|
||
lines = []
|
||
for item in list_robots(data):
|
||
sn = str(item.get("sn") or "").strip()
|
||
if not sn:
|
||
continue
|
||
bits = [f" - {sn}"]
|
||
arm = str(item.get("arm") or "").strip()
|
||
label = str(item.get("label") or "").strip()
|
||
if arm or label:
|
||
bits.append(f"arm={arm or '?'}")
|
||
if label:
|
||
bits.append(f"label={label}")
|
||
short = arm_short_id(sn)
|
||
if short:
|
||
bits.append(f"--arm {short}")
|
||
lines.append(" ".join(bits))
|
||
return "\n".join(lines) or " （robot.robots[] 是空的）"
|
||
|
||
|
||
def _deep_merge(base: dict, overlay: dict) -> dict:
|
||
out = dict(base)
|
||
for k, v in overlay.items():
|
||
if isinstance(out.get(k), dict) and isinstance(v, dict):
|
||
out[k] = _deep_merge(out[k], v)
|
||
else:
|
||
out[k] = v
|
||
return out
|
||
|
||
|
||
def resolve_robot(data: dict, sn: str | None = None) -> dict:
|
||
"""当前生效的机械臂：环境变量 ``ROBOT_SN`` 覆盖 robot.sn；清单补齐 ip/iface/frames。
|
||
|
||
选不出来时**抛 ``ValueError``**，绝不猜：
|
||
|
||
- 给了别名（``89`` / ``left``）但 ``robots[]`` 里没有对应的臂；
|
||
- ``robot.sn`` 没写、也没给 ``--arm``。
|
||
|
||
以前这两种情况会静默落回一个写死的 SN 或一个空的 ``sn``，结果就是
|
||
"配置漏了一键，程序照样启动，连的是哪台看运气"。
|
||
"""
|
||
robot = dict(data.get("robot") or {})
|
||
catalog = list_robots(data)
|
||
raw = (
|
||
sn
|
||
or os.environ.get("ROBOT_SN")
|
||
or os.environ.get("ROBOT_ARM")
|
||
or robot.get("sn")
|
||
or ""
|
||
)
|
||
want, matched = resolve_robot_sn(data, raw)
|
||
if str(raw).strip() and not matched:
|
||
raise ValueError(
|
||
f"选不出机械臂：{str(raw).strip()!r} 既不是 robot.robots[] 里的别名，"
|
||
f"也不像完整 SN（完整 SN 形如 Rizon4-063989）。\n"
|
||
f"config/site.yaml 里现有：\n{describe_arms(data)}\n"
|
||
f"用法：所有终端加同一个 --arm，或直接写完整 SN。"
|
||
)
|
||
if not want:
|
||
raise ValueError(
|
||
"config/site.yaml 没有指定机械臂：robot.sn 为空，也没给 --arm。\n"
|
||
f"可选：\n{describe_arms(data)}"
|
||
)
|
||
match: dict = {}
|
||
for item in catalog:
|
||
if str(item.get("sn") or "").strip() == want:
|
||
match = dict(item)
|
||
break
|
||
out = {k: v for k, v in robot.items() if k != "robots"}
|
||
out.update({k: v for k, v in match.items() if k != "robots"})
|
||
out["sn"] = want
|
||
# 侧别只认 robots[].arm。**不要**从 SN 里猜数字（"063989 就是左臂"）——
|
||
# 那是站点数据写进了代码，换个站点就会把左右搞反。
|
||
if not str(out.get("joint_prefix") or "").strip():
|
||
out["joint_prefix"] = f"{want}_"
|
||
if not str(out.get("base_frame") or "").strip():
|
||
out["base_frame"] = f"{want}_base_link"
|
||
if not str(out.get("ee_frame") or "").strip():
|
||
out["ee_frame"] = f"{want}_flange"
|
||
net = data.get("network") or {}
|
||
if not str(out.get("iface") or "").strip():
|
||
out["iface"] = str(net.get("prefer_iface") or "")
|
||
if not str(out.get("ip") or "").strip():
|
||
out["ip"] = "192.168.100.1"
|
||
if not str(out.get("pc_ip") or "").strip():
|
||
out["pc_ip"] = "192.168.100.10"
|
||
return out
|
||
|
||
|
||
def apply_active_arm(data: dict, sn: str | None = None) -> tuple[dict, dict]:
|
||
"""把 robots[] 里的 camera/gripper/paths/servo 合并到当前 SN 对应的 site 默认值上。"""
|
||
robot = resolve_robot(data, sn)
|
||
merged = dict(data)
|
||
for key in _ARM_SECTION_KEYS:
|
||
base = merged.get(key) if isinstance(merged.get(key), dict) else {}
|
||
ov = robot.get(key) if isinstance(robot.get(key), dict) else {}
|
||
merged[key] = _deep_merge(dict(base or {}), dict(ov or {}))
|
||
return merged, robot
|
||
|
||
|
||
def camera_ns(data: dict) -> str:
|
||
"""当前腕部相机（d405_left / d405_right）的话题命名空间。"""
|
||
cam = data.get("camera") or {}
|
||
name = str(cam.get("name") or "d405").strip().strip("/")
|
||
return name or "d405"
|
||
|
||
|
||
def camera_frames(data: dict) -> tuple[str, str, str]:
|
||
"""当前机械臂的（命名空间, 光学坐标系, 深度话题）。"""
|
||
ns = camera_ns(data)
|
||
yolo = data.get("yolo") or {}
|
||
frame = str(yolo.get("camera_frame") or "").strip()
|
||
depth = str(yolo.get("depth_topic") or "").strip()
|
||
if frame in ("", "d405_color_optical_frame"):
|
||
frame = f"{ns}_color_optical_frame"
|
||
if depth in ("", "/d405/aligned_depth_to_color/image_raw"):
|
||
depth = f"/{ns}/aligned_depth_to_color/image_raw"
|
||
return ns, frame, depth
|
||
|
||
|
||
def flatten(data: dict) -> dict:
|
||
"""供 bash 脚本使用的扁平 ``SITE_*`` 环境变量映射。"""
|
||
data, robot = apply_active_arm(data)
|
||
paths = data.get("paths") or {}
|
||
cam = data.get("camera") or {}
|
||
yolo = data.get("yolo") or {}
|
||
servo = data.get("servo") or {}
|
||
home = data.get("go_home") or {}
|
||
net = data.get("network") or {}
|
||
he = data.get("handeye") or {}
|
||
grip = data.get("gripper") or {}
|
||
named_poses = str(
|
||
robot.get("named_poses_file")
|
||
or paths.get("named_poses_file")
|
||
or str(_root() / "src" / "arm_control" / "config" / "named_poses.yaml")
|
||
)
|
||
|
||
weights = str(paths.get("yolo_weights") or "")
|
||
# 解析符号链接，方便日志里看到真实文件（历史上有人用 active.pt 做切换，已废弃）
|
||
try:
|
||
weights_resolved = str(Path(weights).expanduser().resolve())
|
||
except Exception:
|
||
weights_resolved = weights
|
||
|
||
# 派生文件名的口径：臂号 = 当前 SN 末两位（见 arm_short_id）。
|
||
# 用当前臂现算，**不要**钉死 "89" —— 否则换成 91 或另一台臂时，
|
||
# 两边会共用同一个手眼文件，表现是"标定做了但不生效"。
|
||
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
|
||
|
||
out = {
|
||
"SITE_CONFIG": str(site_path()),
|
||
"SITE_ROS2_ROOT": str(_root()),
|
||
# 必须来自配置。缺了就留空让调用方报错 —— 兜底成某个真实 SN
|
||
# 等于"配置漏了照样启动，连的是哪台看运气"。
|
||
"SITE_ROBOT_SN": str(robot.get("sn") or ""),
|
||
"SITE_ROBOT_IP": str(robot.get("ip") or "192.168.100.1"),
|
||
"SITE_PC_IP": str(robot.get("pc_ip") or "192.168.100.10"),
|
||
"SITE_JOINT_PREFIX": str(robot.get("joint_prefix") or ""),
|
||
"SITE_BASE_FRAME": str(robot.get("base_frame") or ""),
|
||
"SITE_EE_FRAME": str(robot.get("ee_frame") or ""),
|
||
"SITE_ROBOT_TOOL_NAME": str(robot.get("tool_name") or ""),
|
||
"SITE_ROBOT_LABEL": str(robot.get("label") or ""),
|
||
"SITE_ROBOT_ARM": str(robot.get("arm") or ""),
|
||
"SITE_ROBOT_SHORT_ID": short,
|
||
# 本臂有没有装 Orisys 触觉。以前脚本靠 `SN == Rizon4-063989` 来判断，
|
||
# 于是"哪台臂有传感器"变成了代码里的常量。现在由 site.yaml 说了算。
|
||
"SITE_ROBOT_HAS_TACTILE": "1" if robot.get("tactile") else "0",
|
||
"SITE_NAMED_POSES_FILE": named_poses,
|
||
"SITE_ROBOT_CATALOG": ",".join(
|
||
str(r.get("sn") or "") for r in list_robots(data) if r.get("sn")
|
||
),
|
||
"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_CALIBRATE": str(
|
||
paths.get("handeye_calibrate")
|
||
or str(_root() / "handeye" / "calibrate.yaml")
|
||
),
|
||
"SITE_HANDEYE_TOUR": str(
|
||
paths.get("handeye_tour")
|
||
or str(_root() / "handeye" / "waypoints.yaml")
|
||
),
|
||
"SITE_HANDEYE_WORKDIR": str(
|
||
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")
|
||
),
|
||
"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)),
|
||
"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)),
|
||
"SITE_HANDEYE_SETTLE_QD": str(float(he.get("settle_qd_max") or 0.015)),
|
||
"SITE_HANDEYE_SETTLE_STABLE": str(float(he.get("settle_stable_sec") or 0.6)),
|
||
"SITE_HANDEYE_SETTLE_TO": str(float(he.get("settle_timeout_sec") or 8.0)),
|
||
"SITE_HANDEYE_PIN_HOLDS": str(int(he.get("pin_holds") if he.get("pin_holds") is not None else 2)),
|
||
"SITE_HANDEYE_PIN_SEC": str(float(he.get("pin_hold_sec") or 0.45)),
|
||
"SITE_HANDEYE_SKIP_UNSTABLE": (
|
||
"true" if he.get("skip_unstable_poses", True) else "false"
|
||
),
|
||
"SITE_FASTDDS_PROFILE": str(paths.get("fastdds_profile") or ""),
|
||
"SITE_CAM_NAME": camera_ns(data),
|
||
"SITE_CAM_SERIAL": str(cam.get("serial") or ""),
|
||
"SITE_CAM_WIDTH": str(int(cam.get("width") or 848)),
|
||
"SITE_CAM_HEIGHT": str(int(cam.get("height") or 480)),
|
||
"SITE_CAM_FPS": str(int(cam.get("fps") or 30)),
|
||
"SITE_CAM_ENABLE_DEPTH": "1" if cam.get("enable_depth", True) else "0",
|
||
"SITE_CAM_ENABLE_INFRA": "1" if cam.get("enable_infra", True) else "0",
|
||
"SITE_CAM_PREVIEW": "1" if cam.get("preview", True) else "0",
|
||
"SITE_CAM_PREVIEW_STEREO": "1" if cam.get("preview_stereo", True) else "0",
|
||
"SITE_CAM_OVERLAY_HOLD": str(float(cam.get("overlay_hold_sec") or 1.5)),
|
||
"SITE_CAM_USE_SITE_RES": "1" if cam.get("use_site_resolution", True) else "0",
|
||
"SITE_CAM_AUTO_EXPOSURE": "1" if cam.get("auto_exposure", True) else "0",
|
||
"SITE_CAM_AE_LIMIT_US": str(float(cam.get("ae_limit_us") if cam.get("ae_limit_us") is not None else 4000)),
|
||
"SITE_CAM_EMITTER": "1" if cam.get("emitter_enabled", False) else "0",
|
||
"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)),
|
||
"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)),
|
||
"SITE_D435_FPS": str(int(((data.get("cameras") or {}).get("d435") or {}).get("fps") or 30)),
|
||
# 序列号**不给默认值**：兜底成某个真实序列号 = 换站点后悄悄打开别人的相机。
|
||
# 缺了就让相机节点以"序列号为空"报错，比连错设备好得多（M1）。
|
||
"SITE_D405_LEFT_SERIAL": str(((data.get("cameras") or {}).get("d405_left") or {}).get("serial") or ""),
|
||
"SITE_D405_RIGHT_SERIAL": str(((data.get("cameras") or {}).get("d405_right") or {}).get("serial") or ""),
|
||
"SITE_YOLO_CONF": str(yolo.get("conf") or 0.10),
|
||
"SITE_YOLO_IMGSZ": str(int(yolo.get("imgsz") or 640)),
|
||
"SITE_YOLO_DEVICE": str(yolo.get("device") or "0"),
|
||
"SITE_YOLO_MIN_CONF": str(yolo.get("min_conf") or 0.12),
|
||
"SITE_GO_HOME_POSE": str(home.get("pose") or "home"),
|
||
"SITE_GO_HOME_SPEED": str(home.get("speed") or 0.25),
|
||
"SITE_NET_IFACE": str(robot.get("iface") or net.get("prefer_iface") or ""),
|
||
"SITE_SERVO_Z_REF": str(servo.get("z_ref_m") or 0.40),
|
||
"SITE_EE_X_M": str(float(servo.get("ee_x_m") or 0.25)),
|
||
"SITE_EE_Y_M": str(float(servo.get("ee_y_m") or 0.0)),
|
||
"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_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 ""),
|
||
"SITE_GRIP_BACKEND": str(grip.get("backend") or "usb"),
|
||
"SITE_GRIP_DEVICE": str(grip.get("device_name") or "GripperDahuanModbus"),
|
||
"SITE_GRIP_FORCE_N": str(float(grip.get("force_n") or 50.0)),
|
||
"SITE_GRIP_VEL_MPS": str(float(grip.get("vel_mps") or 0.05)),
|
||
"SITE_GRIP_PORT": str(grip.get("port") or "/dev/ttyUSB0"),
|
||
"SITE_GRIP_BAUD": str(int(grip.get("baud") or 115200)),
|
||
"SITE_GRIP_SLAVE": str(int(grip.get("slave_id") or 1)),
|
||
"SITE_GRIP_STROKE_MM": str(float(grip.get("stroke_mm") or 20.0)),
|
||
"SITE_GRIP_FORCE": str(int(grip.get("force_pct") or 50)),
|
||
"SITE_GRIP_SPEED": str(int(grip.get("speed_pct") or 50)),
|
||
"SITE_GRIP_AUTO_INIT": "true" if grip.get("auto_init", True) else "false",
|
||
"SITE_GRIP_RS485_RTS": str(grip.get("rs485_rts") or ""),
|
||
"SITE_HOLE_BACKEND": str((data.get("hole_detect") or {}).get("backend") or "two_stage"),
|
||
"SITE_HOLE_WEIGHTS": str(
|
||
(data.get("hole_detect") or {}).get("weights")
|
||
or paths.get("hole_weights")
|
||
or str(_root() / "models" / "hole_insert" / "detect_best_1.onnx")
|
||
),
|
||
"SITE_HOLE_WORKPIECE_WEIGHTS": str(
|
||
(data.get("hole_detect") or {}).get("workpiece_weights")
|
||
or paths.get("hole_workpiece_weights")
|
||
or weights
|
||
),
|
||
"SITE_HOLE_CONFIG": str(
|
||
(data.get("hole_detect") or {}).get("config")
|
||
or (
|
||
str(_root() / "src" / "arm_application" / "config" / "hole_detect.yaml")
|
||
if str((data.get("hole_detect") or {}).get("backend") or "two_stage")
|
||
in ("two_stage", "hough_roi")
|
||
else str(_root() / "src" / "arm_application" / "config" / "hole_insert.yaml")
|
||
)
|
||
),
|
||
"SITE_TACTILE_BACKEND": str((data.get("tactile") or {}).get("backend") or "orisys"),
|
||
"SITE_TACTILE_CONFIG": str((data.get("tactile") or {}).get("config") or "dd03"),
|
||
"SITE_TACTILE_CONFIG_FILE": str(
|
||
(data.get("tactile") or {}).get("config_file")
|
||
or str(_root() / "src" / "arm_application" / "config" / "orisys" / "dd03.json")
|
||
),
|
||
"SITE_TACTILE_LEFT": str((data.get("tactile") or {}).get("left_video", 0)),
|
||
"SITE_TACTILE_RIGHT": str((data.get("tactile") or {}).get("right_video", 2)),
|
||
}
|
||
return out
|
||
|
||
|
||
def gripper_overrides(data: dict) -> dict:
|
||
"""供 dh_pgea_node 使用的 ROS 参数字典（DH Modbus，不是 Flexiv 夹爪）。"""
|
||
data, _robot = apply_active_arm(data)
|
||
g = data.get("gripper") or {}
|
||
return {
|
||
"port": str(g.get("port") or "/dev/ttyUSB0"),
|
||
"baud": int(g.get("baud") or 115200),
|
||
"slave_id": int(g.get("slave_id") or 1),
|
||
"stroke_mm": float(g.get("stroke_mm") or 20.0),
|
||
"force_pct": int(g.get("force_pct") or 50),
|
||
"speed_pct": int(g.get("speed_pct") or 50),
|
||
"auto_init": bool(g.get("auto_init", True)),
|
||
"state_hz": float(g.get("state_hz") or 20.0),
|
||
"model": str(g.get("model") or "PGEA-100-20-W"),
|
||
"serial_number": str(g.get("sn") or ""),
|
||
"open_position": int(g.get("open_position") if g.get("open_position") is not None else 1000),
|
||
"close_position": int(g.get("close_position") if g.get("close_position") is not None else 0),
|
||
"rs485_rts": str(g.get("rs485_rts") or ""),
|
||
}
|
||
|
||
|
||
def perception_overrides(data: dict) -> dict:
|
||
"""供 perception_node 使用的 ROS 参数字典。"""
|
||
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)
|
||
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 {
|
||
"weights": str(paths.get("yolo_weights") or ""),
|
||
"conf": float(yolo.get("conf") or 0.10),
|
||
"iou": float(yolo.get("iou") or 0.45),
|
||
"imgsz": int(yolo.get("imgsz") or 640),
|
||
"device": str(yolo.get("device") or "0"),
|
||
"half_precision": bool(yolo.get("half_precision", True)),
|
||
"backend": str(yolo.get("backend") or "auto"),
|
||
"mp_infer": bool(yolo.get("mp_infer", True)),
|
||
"min_conf": float(yolo.get("min_conf") or 0.12),
|
||
"vision_hz": float(yolo.get("vision_hz") or 30.0),
|
||
"infer_max_width": int(yolo.get("infer_max_width") if yolo.get("infer_max_width") is not None else 0),
|
||
"box_ema_alpha": float(yolo.get("box_ema_alpha") or 0.85),
|
||
"miss_clear_frames": int(yolo.get("miss_clear_frames") or 12),
|
||
"overlay_hold_on_miss": bool(yolo.get("overlay_hold_on_miss", True)),
|
||
"jump_px": float(yolo.get("jump_px") or 280.0),
|
||
"jump_z_m": float(yolo.get("jump_z_m") or 0.20),
|
||
"z_ref_m": float(yolo.get("z_ref_m") or 0.22),
|
||
"min_depth_m": float(yolo.get("min_depth_m") or 0.04),
|
||
"max_depth_m": float(yolo.get("max_depth_m") or 0.55),
|
||
"work_z_max_m": float(yolo.get("work_z_max_m") or 0.50),
|
||
"hold_point_on_miss": bool(yolo.get("hold_point_on_miss", True)),
|
||
"miss_hold_point_frames": int(yolo.get("miss_hold_point_frames") or 2),
|
||
"meas_blend": float(yolo.get("meas_blend") or 0.65),
|
||
"z_alpha_min": float(yolo.get("z_alpha_min") or 0.10),
|
||
"z_alpha_max": float(yolo.get("z_alpha_max") or 0.60),
|
||
"z_alpha_gain": float(yolo.get("z_alpha_gain") or 2.0),
|
||
"camera_frame": cam_frame,
|
||
"object_frame": obj,
|
||
"depth_topic": depth_topic,
|
||
"camera_source": cam_ns,
|
||
"feature_mode": str(yolo.get("feature_mode") or "plane"),
|
||
"plane_inlier_m": float(yolo.get("plane_inlier_m") or 0.008),
|
||
"plane_band_m": float(yolo.get("plane_band_m") or 0.06),
|
||
}
|
||
|
||
|
||
def servo_overrides(data: dict) -> dict:
|
||
"""供 servo_node 使用的 ROS 参数字典。"""
|
||
data, robot = apply_active_arm(data)
|
||
paths = data.get("paths") or {}
|
||
yolo = data.get("yolo") or {}
|
||
servo = data.get("servo") or {}
|
||
_ns, cam_frame, _depth = camera_frames(data)
|
||
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 ""),
|
||
"camera_frame": cam_frame,
|
||
"handeye_child_frame": cam_frame,
|
||
"arm_label": str(robot.get("label") or robot.get("arm") or ""),
|
||
"z_ref_m": float(servo.get("z_ref_m") or yolo.get("z_ref_m") or 0.40),
|
||
}
|
||
# 透传其余与节点参数同名的 servo 键
|
||
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",
|
||
"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",
|
||
"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",
|
||
"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",
|
||
"use_static_handeye_tf",
|
||
"workspace_xyz_min", "workspace_xyz_max",
|
||
]
|
||
for k in keys:
|
||
if k in servo:
|
||
out[k] = servo[k]
|
||
return out
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser()
|
||
ap.add_argument("--json", action="store_true")
|
||
ap.add_argument("--print", action="store_true", dest="do_print")
|
||
ap.add_argument("--config", default="")
|
||
ap.add_argument("--sn", "--arm", dest="sn", default="", help="89|91|left|right|完整 SN")
|
||
args = ap.parse_args()
|
||
if args.sn:
|
||
os.environ["ROBOT_SN"] = str(args.sn)
|
||
path = Path(args.config).expanduser() if args.config else site_path()
|
||
data = load_site(path)
|
||
try:
|
||
flat = flatten(data)
|
||
except ValueError as exc:
|
||
# 调用方是 `eval "$(python3 export_site_env.py)"`，抛栈对使用者毫无意义，
|
||
# 而且 eval 拿到空字符串后 SITE_* 全是空的，真正失败的地方会离这里很远。
|
||
# 所以这里必须打印一句人能看懂的原因并明确非零退出。
|
||
print(f"错误：{exc}", file=sys.stderr)
|
||
return 2
|
||
if args.json:
|
||
print(json.dumps({"site": data, "env": flat}, ensure_ascii=False, indent=2))
|
||
return 0
|
||
if args.do_print:
|
||
print(f"site 配置: {path}")
|
||
print(
|
||
f" 机械臂 SN : {flat['SITE_ROBOT_SN']}"
|
||
f"{(' (' + flat['SITE_ROBOT_LABEL'] + ')') if flat.get('SITE_ROBOT_LABEL') else ''}"
|
||
f" arm={flat.get('SITE_ROBOT_ARM') or '?'}"
|
||
)
|
||
print(f" 清单 : {flat.get('SITE_ROBOT_CATALOG') or '（无）'}")
|
||
print(f" 机械臂 IP : {flat['SITE_ROBOT_IP']} pc={flat['SITE_PC_IP']} iface={flat.get('SITE_NET_IFACE') or '?'}")
|
||
print(f" 坐标系 : prefix={flat['SITE_JOINT_PREFIX']} ee={flat['SITE_EE_FRAME']}")
|
||
print(f" 工具名 : {flat.get('SITE_ROBOT_TOOL_NAME') or '（保持当前）'}")
|
||
catalog_sns = [str(r.get("sn") or "") for r in list_robots(data)]
|
||
if catalog_sns and flat["SITE_ROBOT_SN"] not in catalog_sns:
|
||
print(
|
||
f" 警告：robot.sn {flat['SITE_ROBOT_SN']!r} 不在 robots[] 里",
|
||
file=sys.stderr,
|
||
)
|
||
print(f" YOLO 权重 : {flat['SITE_YOLO_WEIGHTS']}")
|
||
print(f" 解析后 : {flat['SITE_YOLO_WEIGHTS_RESOLVED']}")
|
||
print(f" 手眼 : {flat['SITE_HANDEYE_FILE']}")
|
||
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_BOARD_COLS']}x{flat['SITE_HANDEYE_BOARD_ROWS']} "
|
||
f"内角点, square={flat['SITE_HANDEYE_SQUARE_M']} m"
|
||
)
|
||
print(
|
||
f" 相机 : serial={flat['SITE_CAM_SERIAL'] or '（空）'} "
|
||
f"{flat['SITE_CAM_WIDTH']}x{flat['SITE_CAM_HEIGHT']}@{flat['SITE_CAM_FPS']} "
|
||
f"ae={flat.get('SITE_CAM_AUTO_EXPOSURE')} "
|
||
f"exp={flat.get('SITE_CAM_EXPOSURE_US')}us "
|
||
f"gain={flat.get('SITE_CAM_GAIN')} "
|
||
f"ae_limit={flat.get('SITE_CAM_AE_LIMIT_US')}us emitter={flat.get('SITE_CAM_EMITTER')}"
|
||
)
|
||
print(
|
||
f" 清单相机 : leftD405={flat.get('SITE_D405_LEFT_SERIAL')} "
|
||
f"rightD405={flat.get('SITE_D405_RIGHT_SERIAL')} "
|
||
f"D435={flat.get('SITE_D435_SERIAL')}"
|
||
)
|
||
print(
|
||
f" 夹爪 : {flat.get('SITE_GRIP_MODEL')} port={flat.get('SITE_GRIP_PORT')}"
|
||
)
|
||
print(
|
||
f" hole_detect : backend={flat.get('SITE_HOLE_BACKEND')} "
|
||
f"weights={flat.get('SITE_HOLE_WEIGHTS')}"
|
||
)
|
||
print(
|
||
f" 触觉 : backend={flat.get('SITE_TACTILE_BACKEND')} "
|
||
f"L={flat.get('SITE_TACTILE_LEFT')} R={flat.get('SITE_TACTILE_RIGHT')} "
|
||
f"cfg={flat.get('SITE_TACTILE_CONFIG')}"
|
||
)
|
||
print(f" go_home : pose={flat['SITE_GO_HOME_POSE']} speed={flat['SITE_GO_HOME_SPEED']}")
|
||
he_path = Path(flat["SITE_HANDEYE_FILE"]).expanduser()
|
||
if not he_path.is_file():
|
||
print(f" 警告：手眼文件缺失: {he_path}", file=sys.stderr)
|
||
else:
|
||
try:
|
||
he_data = yaml.safe_load(he_path.read_text(encoding="utf-8")) or {}
|
||
except Exception:
|
||
he_data = {}
|
||
if isinstance(he_data, dict) and (
|
||
he_data.get("uncalibrated") or "T_4x4" not in he_data
|
||
):
|
||
arm_hint = flat.get("SITE_ROBOT_SHORT_ID") or flat.get("SITE_ROBOT_ARM") or "?"
|
||
print(
|
||
f" 警告：{he_path.name} 没有针对该机械臂标定 "
|
||
f"(run_handeye.sh teach/calibrate --arm {arm_hint})",
|
||
file=sys.stderr,
|
||
)
|
||
# 右臂不该用左腕 D405 的序列号 —— 判据是**侧别**，不是 SN 里的数字。
|
||
# （两台臂的相机装反了不会报错，只会"跟踪一直差几厘米"。）
|
||
if (
|
||
flat.get("SITE_ROBOT_ARM") == "right"
|
||
and flat.get("SITE_CAM_SERIAL")
|
||
and flat.get("SITE_CAM_SERIAL") == flat.get("SITE_D405_LEFT_SERIAL")
|
||
):
|
||
print(
|
||
" 警告：右臂用的是左腕 D405 的序列号 — 请检查 robots[].camera.serial",
|
||
file=sys.stderr,
|
||
)
|
||
if "RGI" in str(flat.get("SITE_GRIP_MODEL") or ""):
|
||
arm_hint = flat.get("SITE_ROBOT_SHORT_ID") or flat.get("SITE_ROBOT_ARM") or "?"
|
||
print(
|
||
f" 提示：本臂是 RGI。CLI: bash ~/ros2/scripts/run_gripper.sh --arm {arm_hint} --cli open|rotate",
|
||
file=sys.stderr,
|
||
)
|
||
w = Path(flat["SITE_YOLO_WEIGHTS"]).expanduser()
|
||
if not w.is_file():
|
||
print(f" 警告：权重文件缺失: {w}", file=sys.stderr)
|
||
return 2
|
||
return 0
|
||
# bash 导出
|
||
for k, v in flat.items():
|
||
# 为 eval 转义
|
||
vv = str(v).replace("'", "'\"'\"'")
|
||
print(f"export {k}='{vv}'")
|
||
return 0
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|