#!/usr/bin/env python3 """把仿真里的**手眼真值**导出成和真机同格式的手眼文件。 为什么要有它 ------------ 真机的手眼标定是"标出来"的（``run_handeye.sh``）。仿真里不用标：腕部相机在 URDF 里就固定在法兰上，位姿**本来就知道**，而且就是真值。但应用读的是 ``handeye/results/*.yaml`` 这个接口（``servo_node`` 需要 ``T_flange_cam`` 矩阵，``visual_servo.launch.py`` 缺文件会直接拒绝启动）。 所以这里的做法不是"让应用判断自己在不在仿真"，而是**把真值导出成同一格式的 文件**，路径在 ``site.yaml`` 的仿真台条目里指定。应用一行都不用改。 同时导出两个（真机上对应两次标定）:: sim_eye_in_hand.yaml flange ← 腕部相机光学系（eye-in-hand，跟踪/精定位用） sim_d435_eye_to_hand.yaml base ← 手外相机光学系（eye-to-hand，S03 粗定位用） 用法:: python3 scripts/gen_sim_handeye.py # 生成/刷新（仿真栈要在跑） python3 scripts/gen_sim_handeye.py --check # 只校验：现有文件是否仍是当前真值 source scripts/sim_env.sh && python3 scripts/gen_sim_handeye.py 为什么要 ``--check``：URDF 一改（相机装到别处、加了垫片），那份文件就过期了， 而过期的表现是"定位总能到附近、永远差一点"—— 看起来像算法不准。 """ from __future__ import annotations import argparse import math import os import sys import tempfile import time from pathlib import Path from typing import Optional, Sequence import numpy as np _HERE = Path(__file__).resolve() sys.path.insert(0, str(_HERE.parent)) # scripts/ sys.path.insert(0, str(_HERE.parent / "lib")) # scripts/lib/ import export_site_env as site_env # noqa: E402 from arm_application.handeye_lib import R_to_quat_xyzw # noqa: E402 try: import rclpy from rclpy.node import Node from rclpy.time import Time from tf2_ros import Buffer, TransformListener except ImportError as exc: # pragma: no cover - 只在没 source 环境时触发 sys.exit( f"找不到 {exc.name}：先 source 仿真环境再跑本脚本\n" " source scripts/sim_env.sh\n" "若仍失败，说明 install_sim/ 还没构建：bash scripts/build_sim.sh" ) #: 认为"文件与真值一致"的容差。1e-9 m 太苛刻（YAML 打印会截断）， #: 1e-6 m = 1 微米、1e-6 rad 级别足够：相机装配误差远大于它。 TOL_M = 1e-6 TOL_RAD = 1e-6 class HandeyeError(RuntimeError): """面向使用者的可预期失败。""" def _matrix_to_yaml(T: np.ndarray) -> str: rows = [] for i in range(4): vals = ", ".join(f"{float(v):.8f}" for v in T[i]) rows.append(f" - [{vals}]") return "\n".join(rows) def _render( *, mode: str, parent: str, child: str, T: np.ndarray, note: str ) -> str: xyz = T[:3, 3] return ( f"# 仿真手眼真值 —— **自动生成，不要手改**\n" f"# mode: {mode}\n" f"# 来源: Gazebo/URDF 的 TF 真值 `{parent}` ← `{child}`（仿真不需要标定）\n" f"# 生成: {time.strftime('%Y-%m-%dT%H:%M:%S%z')} 生成器: scripts/gen_sim_handeye.py\n" f"# {note}\n" f"translation_m: [{float(xyz[0]):.8f}, {float(xyz[1]):.8f}, {float(xyz[2]):.8f}]\n" f"T_4x4:\n{_matrix_to_yaml(T)}\n" ) class _TfGrab(Node): """只做一件事：等到能查到某对坐标系，然后把 4x4 拿出来。""" def __init__(self, timeout_s: float) -> None: super().__init__("gen_sim_handeye") self._buf = Buffer() self._listener = TransformListener(self._buf, self) self._timeout = timeout_s def look(self, parent: str, child: str) -> np.ndarray: deadline = time.monotonic() + self._timeout last = "" while time.monotonic() < deadline: self._spin(0.1) try: # 用 Time()（=最新可用）而不是 Time(0)：0 在 ROS 里也表示最新， # 但语义上容易被读成"仿真时刻 0"，这里写明意图。 tf = self._buf.lookup_transform(parent, child, Time()) except Exception as exc: # noqa: BLE001 last = f"{type(exc).__name__}: {exc}" continue return _tf_to_matrix(tf) raise HandeyeError( f"{self._timeout:.0f}s 内查不到 TF `{parent}` ← `{child}`：{last}\n" " 仿真栈没起来？先 bash scripts/run_sim_gz.sh\n" " 名字对不上？site.yaml 的仿真台条目里 ee_frame / camera.name 要与 URDF 一致" ) def _spin(self, dt: float) -> None: rclpy.spin_once(self, timeout_sec=dt) def _tf_to_matrix(tf) -> np.ndarray: from arm_application.handeye_lib import quat_xyzw_to_R t = tf.transform.translation q = tf.transform.rotation T = np.eye(4) T[:3, :3] = quat_xyzw_to_R([q.x, q.y, q.z, q.w]) T[:3, 3] = [t.x, t.y, t.z] return T def _read_matrix(path: Path) -> Optional[np.ndarray]: """读回文件里的 T_4x4（与 ``handeye_lib.read_transform_yaml`` 同一份解析）。""" from arm_application.handeye_lib import read_transform_yaml if not path.is_file(): return None xyz, quat = read_transform_yaml(str(path)) from arm_application.handeye_lib import quat_xyzw_to_R T = np.eye(4) T[:3, :3] = quat_xyzw_to_R(list(quat)) T[:3, 3] = list(xyz) return T def _same(a: np.ndarray, b: np.ndarray) -> bool: if not np.allclose(a[:3, 3], b[:3, 3], atol=TOL_M): return False # 旋转差用"作用到单位向量上的最大偏差"衡量，比逐元素比更直观 R = a[:3, :3].T @ b[:3, :3] ang = math.acos(max(-1.0, min(1.0, (float(np.trace(R)) - 1.0) / 2.0))) return ang <= TOL_RAD def _write_atomic(path: Path, text: str) -> None: """原子落盘：脚本/节点可能正读这个文件，写一半会被读到半截。""" path.parent.mkdir(parents=True, exist_ok=True) fd, tmp = tempfile.mkstemp(dir=str(path.parent), prefix=path.name, suffix=".tmp") try: with os.fdopen(fd, "w", encoding="utf-8") as f: f.write(text) f.flush() os.fsync(f.fileno()) os.replace(tmp, path) except BaseException: Path(tmp).unlink(missing_ok=True) raise def _targets(arm: str) -> Sequence[tuple[str, str, str, str, Path]]: """返回 (mode, parent, child, note, 输出路径)。 parent/child 全部从 site.yaml 现取 —— 名字写在配置里，脚本不猜。 """ data = site_env.load_site() # 注意第二个返回值才是"这台臂"自己的字典（含 sn / base_frame / ee_frame / camera）； # 第一个是整份 site，顶层没有 sn（顶层的是 robot.sn = 默认臂）。 try: _merged, robot = site_env.apply_active_arm(data, arm) except ValueError as exc: raise HandeyeError(str(exc)) from exc sn = str(robot.get("sn") or "") if not sn: raise HandeyeError(f"site.yaml 里选不出机械臂（--arm {arm}）") prefix = f"{sn}_" # 关节/坐标系命名沿用应用的口径：base/ee 未显式配就用 _ 前缀（见 stage2） base = str(robot.get("base_frame") or f"{prefix}base_link") ee = str(robot.get("ee_frame") or f"{prefix}flange") cam = str((robot.get("camera") or {}).get("name") or "").strip().strip("/") if not cam: raise HandeyeError(f"`--arm {arm}` 的 camera.name 是空的，没法确定腕部相机") # 手外相机：与 gauge_assembly_node._d435_ns 同一套判据（role 前缀 + 同臂） coarse = "" arm_name = str(robot.get("arm") or "").strip() for cfg in (data.get("cameras") or {}).values(): if not isinstance(cfg, dict): continue if not str(cfg.get("role") or "").startswith("overhead"): continue if arm_name and str(cfg.get("arm") or "").strip() != arm_name: continue coarse = str(cfg.get("name") or "").strip().strip("/") break root = site_env._root() out = [ ( "eye_in_hand", ee, f"{cam}_color_optical_frame", f"{note_who(robot)}：腕部相机装在法兰上，位姿来自 URDF", root / "handeye" / "results" / f"{sn}_eye_in_hand.yaml", ) ] if coarse: out.append( ( "eye_to_hand", base, f"{coarse}_color_optical_frame", f"{note_who(robot)}：手外相机固定在工位/世界上（不随臂动）", root / "handeye" / "results" / f"{coarse}_eye_to_hand.yaml", ) ) return out def note_who(merged: dict) -> str: return str(merged.get("label") or merged.get("sn") or "仿真台") def main(argv: Optional[Sequence[str]] = None) -> int: ap = argparse.ArgumentParser(description="把仿真手眼真值导出成手眼文件") ap.add_argument("--arm", default="sim", help="site.yaml 里仿真台的别名（默认 sim）") ap.add_argument("--check", action="store_true", help="只校验，不写文件") ap.add_argument("--timeout", type=float, default=20.0, help="等 TF 出现的秒数") args = ap.parse_args(argv) try: targets = _targets(args.arm) except HandeyeError as exc: print(f"ERROR: {exc}", file=sys.stderr) return 2 rclpy.init() node = _TfGrab(args.timeout) rc = 0 try: for mode, parent, child, note, path in targets: try: T = node.look(parent, child) except HandeyeError as exc: print(f"FAIL: [{mode}] {exc}", file=sys.stderr) rc = 1 continue old = _read_matrix(path) if args.check: if old is None: print(f"FAIL: [{mode}] 文件不存在：{path}") rc = 1 elif _same(old, T): print(f"OK: [{mode}] {path.name} 与当前真值一致") else: d = float(np.linalg.norm(old[:3, 3] - T[:3, 3])) print( f"FAIL: [{mode}] {path.name} 已过期：平移差 {d * 1000:.3f}mm" f"（URDF 改过？重跑本脚本刷新）" ) rc = 1 continue if old is not None and _same(old, T): print(f"OK: [{mode}] {path} 已是最新（无需重写）") continue _write_atomic(path, _render(mode=mode, parent=parent, child=child, T=T, note=note)) print(f"写入: [{mode}] {path} ({parent} ← {child})") finally: node.destroy_node() if rclpy.ok(): rclpy.shutdown() return rc if __name__ == "__main__": sys.exit(main())