仿真感知：真值孔位 + 真值相机 + 补 /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
@@ -54,11 +54,11 @@ except ImportError as exc: # pragma: no cover - 环境问题，不是逻辑问
|
||||
|
||||
|
||||
# 默认期望：哪台相机"必须"看见哪个平面。
|
||||
# d435 是手外相机，它的职责就是看见台面（P6 的粗定位要靠它），
|
||||
# sim_d435 是手外相机，它的职责就是看见台面（P6 的粗定位要靠它），
|
||||
# 所以要求 ≥10% 的像素落在台面上。
|
||||
# d405 是腕上相机，视野窄且会拍到机械臂自己，台面占比要求放宽到 5%。
|
||||
# sim_d405 是腕上相机（仿真那台 = 左臂），视野窄且会拍到机械臂自己，台面占比放宽到 5%。
|
||||
# 地面(z=0) 不作为要求：它被拍到多少取决于姿态，不构成"对不对"的判据。
|
||||
DEFAULT_EXPECT = ("d435:0.40:10", "d405:0.40:5")
|
||||
DEFAULT_EXPECT = ("sim_d435:0.40:10", "sim_d405:0.40:5")
|
||||
|
||||
|
||||
def quat_to_R(q) -> np.ndarray:
|
||||
@@ -115,6 +115,95 @@ def spin_until(node: Node, pred, timeout: float) -> bool:
|
||||
return False
|
||||
|
||||
|
||||
def _tf_rot(node: "Grab", parent: str, child: str) -> np.ndarray:
|
||||
"""TF parent ← child 的旋转部分（child 的三根轴在 parent 系里的表示）。"""
|
||||
tf = node.buf.lookup_transform(parent, child, rclpy.time.Time())
|
||||
r = tf.transform.rotation
|
||||
return quat_to_R((r.x, r.y, r.z, r.w))
|
||||
|
||||
|
||||
def mount_problems(node: "Grab", ns: str, frame: str, problems: list[str]) -> None:
|
||||
"""核对**安装朝向**：腕相机视轴要沿法兰轴线，手外相机要朝下看台面。
|
||||
|
||||
为什么单独查这一条（光看"命中平面"不够）：
|
||||
腕相机要是朝侧面看，它照样能反投影出 z≈0 的地面 —— 几何自洽、数值"正常"，
|
||||
只是**看不见工件**。表现全在应用侧："目标不在相机前方"、伺服一直找工件。
|
||||
所以这里查的不是"看见了什么"，而是"朝哪看"：前者会被姿态带偏，后者不会。
|
||||
"""
|
||||
try:
|
||||
from arm_sim_gazebo.sim_cameras import (
|
||||
MOUNT_TOL_DEG,
|
||||
WORLD_LOOK_DOWN_COS,
|
||||
flange_frame_for,
|
||||
load_cameras,
|
||||
mount_axis_deg,
|
||||
world_look_down_cos,
|
||||
)
|
||||
except ImportError as exc:
|
||||
# 不静默跳过：安装朝向是本工具**最重要**的一条判据（它拦的是"相机装歪
|
||||
# 但数值全正常"那类问题），悄悄不查等于把这条守门人撤了。
|
||||
problems.append(
|
||||
f"拿不到 sim_cameras 的安装朝向工具（{exc}）—— install_sim 是旧的，"
|
||||
f"先跑 bash scripts/build_sim.sh"
|
||||
)
|
||||
return
|
||||
spec = next((s for s in load_cameras() if s.ros_ns == ns), None)
|
||||
if spec is None:
|
||||
problems.append(f"cameras.yaml 里找不到 ros_ns={ns} 的相机声明，无法核对安装朝向")
|
||||
return
|
||||
now = rclpy.time.Time()
|
||||
flange = flange_frame_for(spec)
|
||||
if flange is not None and node.buf.can_transform(flange, frame, now):
|
||||
R = _tf_rot(node, flange, frame)
|
||||
ang = mount_axis_deg(R)
|
||||
print(f" 相对法兰 {flange} 的视轴夹角: {ang:.1f}° （0° = 正装在法兰轴线上）")
|
||||
if ang > MOUNT_TOL_DEG:
|
||||
problems.append(
|
||||
f"腕部相机视轴与法兰 +Z 差 {ang:.1f}°（要求 ≤{MOUNT_TOL_DEG:.0f}°）—— "
|
||||
f"相机装歪了，法兰垂直朝下时它看不到下面的工件。\n"
|
||||
f" 90° 左右就是「少转一个直角」：话题/图像/内参全都正常，\n"
|
||||
f" 而且反投影仍会命中地面（看的是远处），所以只查平面发现不了。\n"
|
||||
f" 修法：devices/sim/gazebo/urdf/arm.sim.sensors.xacro 里"
|
||||
f" d405_pitch（正装 = -1.5708）。"
|
||||
)
|
||||
else:
|
||||
cos = world_look_down_cos(_tf_rot(node, "world", frame))
|
||||
print(f" 视轴在世界系里的 z 分量: {cos:+.2f} （-1 = 正朝下看）")
|
||||
if cos > WORLD_LOOK_DOWN_COS:
|
||||
problems.append(
|
||||
f"手外相机没在朝下看台面（视轴 z 分量 {cos:+.2f}，要求 ≤{WORLD_LOOK_DOWN_COS:+.1f}）—— "
|
||||
f"位姿 rpy 写错时几何自洽、不报错，只是拍不到工件。\n"
|
||||
f" 修法：devices/sim/gazebo/worlds/assembly.world 里该 <model> 的 <pose> pitch。"
|
||||
)
|
||||
|
||||
|
||||
def looking_at_table(node: "Grab", frame: str) -> "bool | None":
|
||||
"""相机此刻是不是**朝下看着台面**（光心在台面 xy 内 + 视轴朝下）。
|
||||
|
||||
为什么要问这一句：腕相机的职责是"悬在工件上方时看清螺纹孔"。机械臂停在初始位
|
||||
或别处时它当然只看得见地面（初始位那副姿态里法兰甚至是横着的）—— 这时候拿
|
||||
"命中台面 ≥5%"判它不合格是**假警报**，而假警报多了真问题就没人看了。
|
||||
拿不到场景配置时返回 None（按"不知道"处理，照常判）。
|
||||
"""
|
||||
try:
|
||||
from arm_sim_gazebo.sim_cameras import WORLD_LOOK_DOWN_COS, world_look_down_cos
|
||||
from arm_sim_gazebo import sim_scene
|
||||
|
||||
scene = sim_scene.load_scene()
|
||||
b = scene.table_bounds
|
||||
tf = node.buf.lookup_transform("world", frame, rclpy.time.Time())
|
||||
x = tf.transform.translation.x
|
||||
y = tf.transform.translation.y
|
||||
margin = 0.05
|
||||
inside = (b.x_min - margin <= x <= b.x_max + margin) and (
|
||||
b.y_min - margin <= y <= b.y_max + margin
|
||||
)
|
||||
downward = world_look_down_cos(_tf_rot(node, "world", frame)) <= WORLD_LOOK_DOWN_COS
|
||||
return inside and downward
|
||||
except Exception: # noqa: BLE001 —— 拿不到就不拦人
|
||||
return None
|
||||
|
||||
|
||||
def expected_intrinsics(ns: str):
|
||||
"""从 cameras.yaml 算出该相机的应有内参。拿不到就返回 None（不阻断检查）。"""
|
||||
try:
|
||||
@@ -210,6 +299,9 @@ def check_one(ns: str, args) -> tuple[bool, list[str]]:
|
||||
]
|
||||
)
|
||||
zw = (pts @ R.T + t)[:, 2]
|
||||
# 安装朝向要在 destroy_node 之前查 —— 它还要用 TF（腕相机的视轴相对法兰）
|
||||
mount_problems(node, ns, frame, problems)
|
||||
above_table = looking_at_table(node, frame)
|
||||
node.destroy_node()
|
||||
|
||||
print(f"\n[{ns}] 光学系={frame} fx={fx:.1f} fy={fy:.1f} cx={cx:.1f} cy={cy:.1f}")
|
||||
@@ -223,15 +315,26 @@ def check_one(ns: str, args) -> tuple[bool, list[str]]:
|
||||
print(f" 命中 {name}(z={pz:+.2f}) : {rate:5.1f}%")
|
||||
|
||||
# 期望判据
|
||||
table_z = 0.40
|
||||
for want_z, want_min in args.expect.get(ns, []):
|
||||
name = dict(args.planes).get(want_z, f"z={want_z}")
|
||||
got = rates.get(want_z, 0.0)
|
||||
if got < want_min:
|
||||
problems.append(
|
||||
f"只看见 {name} 的 {got:.1f}%，要求 ≥{want_min:.0f}%。\n"
|
||||
f" 相机很可能是**背对**目标（位姿 rpy 写错）—— "
|
||||
f"这种错几何自洽、不报错，只会让相机拍不到该拍的东西。"
|
||||
if got >= want_min:
|
||||
continue
|
||||
if abs(want_z - table_z) < 1e-9 and above_table is False:
|
||||
# 腕相机不在台面上方时，"没看见台面"是正常的（它看见的是地面）。
|
||||
print(
|
||||
f" ⚠ 跳过「{name}」判据：相机此刻没朝下看着台面"
|
||||
f"（光心不在 scene.yaml 的 table_bounds 内，或视轴没朝下）——\n"
|
||||
f" 此刻只看得见地面是正常的。\n"
|
||||
f" 想让这条生效：把机械臂摆到工件上方（例如 S04 跟踪时）再跑本脚本。"
|
||||
)
|
||||
continue
|
||||
problems.append(
|
||||
f"只看见 {name} 的 {got:.1f}%，要求 ≥{want_min:.0f}%。\n"
|
||||
f" 相机很可能是**背对**目标（位姿 rpy 写错）—— "
|
||||
f"这种错几何自洽、不报错，只会让相机拍不到该拍的东西。"
|
||||
)
|
||||
|
||||
total = sum(rates.values())
|
||||
if total < 1.0:
|
||||
@@ -279,7 +382,7 @@ def main() -> int:
|
||||
description="仿真相机几何验收：反投影深度，检查是否命中预期平面。",
|
||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||
)
|
||||
ap.add_argument("--cameras", default="d405,d435", help="要检查的相机命名空间，逗号分隔")
|
||||
ap.add_argument("--cameras", default="sim_d405,sim_d435", help="要检查的相机命名空间，逗号分隔")
|
||||
ap.add_argument(
|
||||
"--planes",
|
||||
type=parse_planes,
|
||||
|
||||
@@ -125,8 +125,23 @@ def effective_gates(root: Optional[Path] = None) -> dict:
|
||||
}
|
||||
|
||||
|
||||
def sim_robot_servo(doc: dict) -> dict:
|
||||
"""仿真那台臂的 servo 段（顶层 servo 被 robots[] 里 sim 那台覆盖后的结果）。
|
||||
|
||||
⚠️ 必须合并，不能只读顶层：顶层那套是**真机**的（台面与基座同高，所以
|
||||
工作盒 z 是 0.18~0.65），而仿真里机械臂站在地面、台面 0.40 高，一切 Z 都
|
||||
大 0.412。只读顶层会拿真机的界去核对仿真的悬停高 —— 看起来"过了"，
|
||||
而真正生效的配置可能根本不对（或者反过来，明明配对了却报越界）。
|
||||
"""
|
||||
servo = dict(doc.get("servo") or {})
|
||||
for robot in (doc.get("robot") or {}).get("robots") or []:
|
||||
if str(robot.get("arm") or "") == "sim" or str(robot.get("sn") or "") == "sim":
|
||||
servo.update(robot.get("servo") or {})
|
||||
return servo
|
||||
|
||||
|
||||
def workspace_z_limits(root: Optional[Path] = None):
|
||||
"""从 site.yaml 读机械臂法兰的工作空间 Z 上下界（用于核对悬停高度）。"""
|
||||
"""机械臂法兰的工作空间 Z 上下界（按**仿真那台臂**的生效值）。"""
|
||||
import yaml
|
||||
|
||||
base_root = Path(root) if root is not None else REPO_ROOT
|
||||
@@ -136,7 +151,7 @@ def workspace_z_limits(root: Optional[Path] = None):
|
||||
if not site.is_file():
|
||||
return None
|
||||
doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||||
servo = doc.get("servo") or {}
|
||||
servo = sim_robot_servo(doc)
|
||||
try:
|
||||
return float(servo["workspace_xyz_min"][2]), float(servo["workspace_xyz_max"][2])
|
||||
except (KeyError, IndexError, TypeError, ValueError):
|
||||
@@ -217,6 +232,25 @@ def check_hover_height(scene, root) -> list[str]:
|
||||
)
|
||||
elif fz < lo:
|
||||
out.append(f"算出的悬停法兰高度 z≈{fz:.3f}m 低于工作空间下界 {lo}m。")
|
||||
|
||||
# 一阶段跟踪高还要再抬 TRACK_CLEARANCE_M（见 pipeline_node 的常量）。
|
||||
# 单独查这一条的原因：二阶段能过、S04 一跟踪就被"超出工作盒"截断，
|
||||
# 现象是"跟踪位置总差一截/越跟踪越偏"，而配置看着完全合理。
|
||||
try:
|
||||
from arm_application.pipeline_node import TRACK_CLEARANCE_M
|
||||
except Exception: # noqa: BLE001 —— 拿不到常量就不查，不因此拦人
|
||||
return out
|
||||
track = fz + float(TRACK_CLEARANCE_M)
|
||||
if track > hi:
|
||||
out.append(
|
||||
f"一阶段跟踪高 = 悬停 {fz:.3f} + {TRACK_CLEARANCE_M:.2f} = {track:.3f}m "
|
||||
f"**超出工作空间上界 {hi}m**。\n"
|
||||
f" 二阶段的位姿检查能过、S04 一跟踪却会被「超出工作盒」截断 —— "
|
||||
f"现象是「越跟踪越偏」，很难往配置上想。\n"
|
||||
f" 修法（仿真里都自由）：把 site.yaml 里仿真那台臂的 "
|
||||
f"servo.workspace_xyz_max.z 抬到 ≥ {track:.3f}，"
|
||||
f"或调小 scene.yaml 的 camera_to_part_m（法兰随之降低）。"
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
|
||||
@@ -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