仿真感知：真值孔位 + 真值相机 + 补 /home，让流程不改代码就能跑仿真
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
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>
This commit is contained in:
arm_dev
@@ -0,0 +1,292 @@
|
||||
#!/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())
|
||||
Reference in New Issue
Block a user