arm_dev
b29339e522
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
yolo / truth / external —— truth 就走这条路。
为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
既准又稳，还能当真实检测算法的对照基线。
2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
visual_servo/filtered_point 用，绕开 ONNX。
3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。
4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
宁可用退化模式跑，也不要整个适配器起不来。
5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
与 use_sim_time 向子 launch 的传递。
另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。
Co-authored-by: Cursor <cursoragent@cursor.com>
293 lines
11 KiB
Python
293 lines
11 KiB
Python
#!/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 未显式配就用 <sn>_ 前缀（见 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())
|