#!/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", # gauge_assembly：几何类参数是**按台子**的 —— 同一个数在仿真与真机上不同。 # 例：workpiece_surface_z_m 仿真 0.412（臂站地面、台面 0.40m），真机 0.012。 # 不登记就会**静默失效**：site.yaml 里写了 robots[sim].gauge_assembly， # 合出来还是全局值，S07 的几何自检算错 0.4m 却不报错。 "gauge_assembly", ) 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]: """当前机械臂**腕部**相机的（命名空间, 光学坐标系, 深度话题）。 一阶段跟踪用哪台相机见 ``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() 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 #: 顶视跟踪必须自己写的深度量程。缺了就借腕部 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.``。为什么按模式分开——两种装法用的是 **两块不同的物理板**（本项目：眼在手上 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) 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") 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()), "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": he_modes_out["SITE_HANDEYE_WAYPOINTS_EYE_IN_HAND"], "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) ), # 不带后缀的两个名字保持旧语义（= 腕部 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_HANDEYE_GRIPPER_TOOL": str(he.get("gripper_tool") or ""), "SITE_HANDEYE_GRIPPER_TCP_XYZ": ",".join( f"{float(v):.6f}" for v in list(he.get("gripper_tcp_xyz") or []) ), "SITE_HANDEYE_GRIPPER_TCP_QWXYZ": ",".join( f"{float(v):.6f}" for v in list(he.get("gripper_tcp_qwxyz") or []) ), **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)), "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)), # 手外（固定）相机的**按角色**导出。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)), "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_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 ""), "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, _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" out = { "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, "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: """供 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, 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": 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 ""), "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", "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", "d435_handback_sec", "wrist_align_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", "use_static_handeye_tf", "workspace_xyz_min", "workspace_xyz_max", ] 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 必须显式写。" "不许用代码默认秒数。" ) if "d435_handback_sec" not in servo: raise RuntimeError( "servo.d435_handback_sec 必须显式写" "（厂家 MoveL 结束后，法兰停稳多久才打开 D405 框心）。不许用代码默认秒数。" ) if "wrist_align_sec" not in servo: raise RuntimeError( "servo.wrist_align_sec 必须显式写" "（D405 框心对准多久才进对孔）。不许用代码默认 1。" ) 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["d435_handback_sec"] = float(servo["d435_handback_sec"]) out["wrist_align_sec"] = float(servo["wrist_align_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 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_HANDEYE_GRIPPER_TOOL') or '（无，ee_pose 原样）'} " f"tcp=[{flat.get('SITE_HANDEYE_GRIPPER_TCP_XYZ') or ''}]" ) 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.get('SITE_OVERHEAD_CAM_NAME') or '（未登记）'} " f"serial={flat.get('SITE_OVERHEAD_CAM_SERIAL') or '（空）'}" ) 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())