diff --git a/devices/sim/gazebo/config/cameras.yaml b/devices/sim/gazebo/config/cameras.yaml index 4d81fba..9caea06 100644 --- a/devices/sim/gazebo/config/cameras.yaml +++ b/devices/sim/gazebo/config/cameras.yaml @@ -44,8 +44,13 @@ cameras: # --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------ - gz_ns: /d405 # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 ） - ros_ns: d405 # ROS 侧命名空间。应用按 camera_name 参数用这个名字取话题， - # 所以它必须等于启动应用时给的 camera_name（默认 d405） + # ROS 侧命名空间。应用按 site.yaml → robots[].camera.name 取话题，所以这里 + # 必须**等于那一项**：仿真那台在 site.yaml 里登记为 robots[] 的 `sn: sim`， + # 它的 camera.name 就是这里写的 sim_d405。 + # 曾经这里写的是 d405，而 site 写的是 d405_left —— 后果是通止规装配在仿真里 + # S00 报"腕部相机无图像"（图像 13Hz 好好地流着），人只会去怀疑相机。 + # 改这里的名字，site.yaml 里那个 sn: sim 条目的 camera.name 必须同步改。 + ros_ns: sim_d405 link_frame: sim_d405_link # 图像上盖的坐标系必须是**光学系**，不能是本体系。 # gz 与 URDF 里的相机本体遵循"X 朝前 / Y 朝左 / Z 朝上"（相机机身约定）， @@ -78,9 +83,14 @@ cameras: # world → d435_link。位姿从 world 文件里**解析**得到（不在这里重复配置）， # 避免"改了一处忘了另一处"。 - gz_ns: /d435 - ros_ns: d435 - link_frame: d435_link - optical_frame: d435_color_optical_frame + # ROS 侧命名空间 = sim_d435（与真机 d435 分开）。为什么不能沿用 d435： + # eye-to-hand 标定文件是按**相机名**命名的（handeye/results/<相机名>_eye_to_hand.yaml）， + # 仿真自动生成的这份若叫 d435_...，会和真机的手外标定**抢同一个文件名** —— + # 后果是"跑一次仿真把真机的标定覆盖了"，而标定文件平时没人看。 + # frame 名也必须跟着改（默认 _color_optical_frame，应用按这个查 TF）。 + ros_ns: sim_d435 + link_frame: sim_d435_link + optical_frame: sim_d435_color_optical_frame depth: true horizontal_fov: 1.5184 width: 640 diff --git a/devices/sim/gazebo/config/scene.yaml b/devices/sim/gazebo/config/scene.yaml index bae44fc..5d81a67 100644 --- a/devices/sim/gazebo/config/scene.yaml +++ b/devices/sim/gazebo/config/scene.yaml @@ -50,7 +50,7 @@ table_bounds: # 详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。 operating: # 用哪台相机做孔检测（决定用哪组内参算像素尺度） - camera: d405 + camera: sim_d405 # 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。 # 这里写的是物体的 name（不是模型名）。 part_object: workpiece diff --git a/devices/sim/gazebo/config/tactile.yaml b/devices/sim/gazebo/config/tactile.yaml index d112d52..945bac7 100644 --- a/devices/sim/gazebo/config/tactile.yaml +++ b/devices/sim/gazebo/config/tactile.yaml @@ -94,11 +94,19 @@ sides: # 力传感器所在的坐标系（即 *_inner_finger_joint 的 child link）。 # 接触点会转到这个系里，用来算 centroid_u/v，也是轴的参照系。 frame: sim_left_finger_mount + # 指腹内侧的"夹持点"（= tool center point）。**上料脚本要用它**： + # 两指 TCP 的中点就是工具轴，规尖垂在这条轴线上。 + # 与 frame 的区别：frame 是力传感器的参照系（指尖关节的 child link）， + # TCP 在它的子链再往下 0.036m（见 flexiv_description 的 finger_tcp_joint）。 + # 名字不能靠 frame 加后缀推 —— 实测这两条链分别是 + # sim_left_finger_mount / sim_left_finger_tcp（父链与子链并列命名）。 + tcp_frame: sim_left_finger_tcp - side: right wrench_topic: /tactile/right/wrench contact_topic: /tactile/right/raw frame: sim_right_finger_mount + tcp_frame: sim_right_finger_tcp # --- 轴向映射（已实测，不是猜的）---------------------------------------- # 力传感器挂在指尖关节上，给出的是该关节 child link 坐标系里的三向力。 diff --git a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro index c32278c..458c855 100644 --- a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro +++ b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro @@ -45,7 +45,7 @@ d405_y:=-0.06 d405_z:=0.05 d405_roll:=0.0 - d405_pitch:=0.0 + d405_pitch:=-1.5708 d405_yaw:=0.0 image_width:=848 image_height:=480 @@ -93,13 +93,32 @@ diff --git a/handeye/results/sim_d435_eye_to_hand.yaml b/handeye/results/sim_d435_eye_to_hand.yaml new file mode 100644 index 0000000..55af98d --- /dev/null +++ b/handeye/results/sim_d435_eye_to_hand.yaml @@ -0,0 +1,11 @@ +# 仿真手眼真值 —— **自动生成，不要手改** +# mode: eye_to_hand +# 来源: Gazebo/URDF 的 TF 真值 `sim_base_link` ← `sim_d435_color_optical_frame`（仿真不需要标定） +# 生成: 2026-09-24T13:10:21+0800 生成器: scripts/gen_sim_handeye.py +# 仿真台：手外相机固定在工位/世界上（不随臂动） +translation_m: [-0.45000000, 0.00000000, 1.35000000] +T_4x4: + - [-0.00000236, -0.76604704, 0.64278451, -0.45000000] + - [-1.00000000, 0.00000000, -0.00000367, 0.00000000] + - [0.00000281, -0.64278451, -0.76604704, 1.35000000] + - [0.00000000, 0.00000000, 0.00000000, 1.00000000] diff --git a/handeye/results/sim_eye_in_hand.yaml b/handeye/results/sim_eye_in_hand.yaml new file mode 100644 index 0000000..65ee22e --- /dev/null +++ b/handeye/results/sim_eye_in_hand.yaml @@ -0,0 +1,11 @@ +# 仿真手眼真值 —— **自动生成，不要手改** +# mode: eye_in_hand +# 来源: Gazebo/URDF 的 TF 真值 `sim_flange` ← `sim_d405_color_optical_frame`（仿真不需要标定） +# 生成: 2026-09-24T13:10:21+0800 生成器: scripts/gen_sim_handeye.py +# 仿真台：腕部相机装在法兰上，位姿来自 URDF +translation_m: [0.00000000, -0.06000000, 0.05000000] +T_4x4: + - [0.00000000, 1.00000000, -0.00000000, 0.00000000] + - [-1.00000000, 0.00000000, -0.00000367, -0.06000000] + - [-0.00000367, 0.00000000, 1.00000000, 0.05000000] + - [0.00000000, 0.00000000, 0.00000000, 1.00000000] diff --git a/scripts/check_sim_cameras.py b/scripts/check_sim_cameras.py index 1f5b5a7..a4e6e35 100755 --- a/scripts/check_sim_cameras.py +++ b/scripts/check_sim_cameras.py @@ -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 里该 的 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, diff --git a/scripts/check_sim_scene.py b/scripts/check_sim_scene.py index d32ecdf..4d4f46b 100644 --- a/scripts/check_sim_scene.py +++ b/scripts/check_sim_scene.py @@ -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 diff --git a/scripts/gen_sim_handeye.py b/scripts/gen_sim_handeye.py new file mode 100644 index 0000000..cdc51e1 --- /dev/null +++ b/scripts/gen_sim_handeye.py @@ -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 未显式配就用 _ 前缀（见 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()) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py index b25f557..92000ca 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_cameras.py @@ -441,6 +441,78 @@ def depth_m_to_mm16(depth_m: np.ndarray, max_mm: int = MAX_MM) -> np.ndarray: return out +#: 腕部相机的视轴与法兰轴线的允许夹角 [度]。 +#: 真机装支架时也不会完全正装，但"看得到工件"要求它基本沿法兰轴； +#: 20° 是给手工装配留的余量，同时足以把"少转 90°"这类错误拦下来。 +MOUNT_TOL_DEG = 20.0 + +#: 手外相机的视轴在世界系里的 z 分量上限（-1 = 正朝下）。松于 -0.5 就算没在朝下看。 +WORLD_LOOK_DOWN_COS = -0.5 + + +def flange_frame_for(spec: CameraSpec) -> Optional[str]: + """从相机的 link_frame 反推它所在臂的法兰 frame：sim_d405_link → sim_flange。 + + 只对**挂在臂上**的相机有意义（``world_model`` 非空的是世界相机，返回 None）。 + 为什么要反推而不是读配置：``site.yaml`` 的 ``sim.joint_prefix`` 自己都写着 + "✘ 未接线"，拿一个没接线的值当路径判据会静默失效。 + link_frame 里的前缀才是真正生效的那个（由 xacro 的 arm_prefix 生成）。 + """ + if spec.world_model: + return None + model = spec.ros_ns.rsplit("_", 1)[-1] # sim_d405 → d405 + suffix = f"{model}_link" + if not spec.link_frame.endswith(suffix): + return None + return spec.link_frame[: -len(suffix)] + "flange" + + +def mount_axis_deg(R_flange_optical: np.ndarray) -> float: + """光学系视轴（+Z）与**法兰 +Z**的夹角 [度]。0 = 正装在法兰轴线上。 + + 这是个可以**脱离姿态**核对的量，所以特别有用：不管机械臂当时摆成什么样， + "相机有没有沿法兰轴线装"都只在安装处决定。 + + 为什么它必须接近 0（而不是"调多少都行"）：Gz 的相机视轴是模型/连杆的 +X， + 而 OPTICAL_RPY(-90°,0,-90°) 把"机身系（X 前/Y 左/Z 上）"转成光学系。 + 两者串起来 ⇒ 相机沿法兰轴线装 ⇔ 光学 +Z ∥ 法兰 +Z。 + 少转那 90° 时：话题有数据、预览有图像、内参也对，只是整个视野搬到了侧面， + 应用侧表现成"目标永远不在相机前方"（曾真实踩过，见 sensors.xacro 的说明）。 + """ + v = np.asarray(R_flange_optical, dtype=float) @ np.array([0.0, 0.0, 1.0]) + return float(np.degrees(np.arccos(float(np.clip(v[2], -1.0, 1.0))))) + + +def world_look_down_cos(R_world_optical: np.ndarray) -> float: + """光学系视轴在世界系里的 z 分量（-1 = 正朝下看，0 = 平视）。""" + return float((np.asarray(R_world_optical, dtype=float) @ np.array([0.0, 0.0, 1.0]))[2]) + + +def rpy_to_R(rpy: Sequence[float]) -> np.ndarray: + """URDF/SDF 的 (roll, pitch, yaw) → 3x3 旋转矩阵。 + + 复合顺序是 ``R = Rz(yaw)·Ry(pitch)·Rx(roll)``（URDF 规范）—— 顺序不能想当然， + 反了在"只有一个角非零"时看不出来，两个角同时非零就会错得莫名其妙。 + """ + r, p, y = (float(v) for v in rpy) + cr, sr = math.cos(r), math.sin(r) + cp, sp = math.cos(p), math.sin(p) + cy, sy = math.cos(y), math.sin(y) + Rx = np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]]) + Ry = np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]]) + Rz = np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]]) + return Rz @ Ry @ Rx + + +def mount_rpy_axis_deg(rpy: Sequence[float]) -> float: + """腕相机的**安装 rpy**（URDF 里那个 fixed joint 的 rpy）→ 视轴与法兰 +Z 的夹角。 + + 与运行期查 TF 的 :func:`mount_axis_deg` 是同一件事，只是输入不同： + 没接 TF 时（比如核对 xacro 里的默认值）用这个算。 + """ + return mount_axis_deg(rpy_to_R(rpy) @ rpy_to_R(OPTICAL_RPY)) + + def optical_rpy() -> Tuple[float, float, float]: """本体 → 光学系的固定旋转，供静态 TF 使用。""" return OPTICAL_RPY diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_home_service.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_home_service.py new file mode 100644 index 0000000..9678f7e --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_home_service.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""仿真里的 ``/home`` —— 把真机 Flexiv 驱动的**同名服务**补齐，契约一致。 + +为什么需要它 +------------ +真机上 ``/home``（``std_srvs/Trigger``）由 ``arm_driver_flexiv`` 提供， +它的实现就是"切工具系（可选）+ 转发给 ``/set_named_pose``"。 +仿真里没有 Flexiv 驱动，于是通止规装配（S01 回 home、S11 复位）在 Gazebo 里 +第一步就过不了自检 —— 而 S01 本身在两种环境下**没有区别**。 + +所以这里的做法不是"让上层代码判断自己在不在仿真"，而是**补一份同样的契约**： +``/home`` → ``/set_named_pose``（``arm_msgs/srv/SetNamedPose``）。 + +有意保留的差异（写在这里，免得有人以为两边完全一样）： + +* 真机的 ``/home`` 会顺带切工具系（``switch_tool_on_home``）； + 仿真里没有工具坐标系的概念，**不切**（S00 也把 ``/switch_tool`` 列为选检）。 +* 真机 ``/home`` 转发时用的 ``home_pose`` / ``home_speed`` 来自 ``site.yaml`` + （``go_home.pose`` / ``go_home.speed``）；这里用同名参数，默认值与 site 一致， + launch 不必再传。 + +用法（由 ``gazebo_sim.launch.py`` 拉起）:: + + ros2 service call /home std_srvs/srv/Trigger '{}' +""" + +from __future__ import annotations + +import time + +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from std_srvs.srv import Trigger + +from arm_msgs.srv import SetNamedPose + + +class SimHomeService(Node): + """``/home`` → ``/set_named_pose``。""" + + def __init__(self) -> None: + super().__init__("sim_home_service") + # 与 site.yaml 的 go_home 段保持一致：仿真与真机用**同一份默认值**， + # 免得"仿真里回零是 4 秒、真机是 1 秒"这种口径差异。 + self.declare_parameter("home_pose", "home") + self.declare_parameter("home_speed", 0.25) + self.declare_parameter("set_named_pose_service", "/set_named_pose") + #: 等 ``/set_named_pose`` 出现的超时 [s]。仿真栈刚起来时它可能还没就绪， + #: 直接失败会让人误以为"服务没接线"。 + self.declare_parameter("wait_service_s", 5.0) + #: 等 ` deadline: + response.success = False + response.message = f"回位姿[{self._pose}]跟丢：{self._timeout:.0f}s 内没结果" + self.get_logger().error(response.message) + return response + # 这里是**回调线程**（MultiThreadedExecutor），可以 sleep； + # 不能 spin（会与执行器抢同一节点的回调）。 + time.sleep(0.02) + res = fut.result() + response.success = bool(res.success) + response.message = res.message or f"已回位姿[{self._pose}]" + (self.get_logger().info if response.success else self.get_logger().error)( + f"/home → {response.message}" + ) + return response + + +def main(args=None) -> None: + rclpy.init(args=args) + node = SimHomeService() + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + executor.shutdown() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py index 2412898..fd2d20b 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py @@ -112,8 +112,23 @@ class SceneSpec: # 读配置 # --------------------------------------------------------------------------- def repo_root() -> Path: - """仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。""" - return Path(__file__).resolve().parents[3] + """仓库根。**实现只有一份**：``robot_description.repo_root()``。 + + 以前这里是 ``Path(__file__).resolve().parents[3]`` —— 那在**源码树**里对， + 在**安装后**是错的：``install_sim/`` 底下是**拷贝**（不是软链）， + 所以装出来的模块算出来的"仓库根"是 ``install_sim/arm_sim_gazebo/lib``， + 而 ``devices/`` 根本不在那儿。现象是节点一启动就报 + "找不到场景配置 .../install_sim/arm_sim_gazebo/lib/devices/sim/gazebo/config/scene.yaml"。 + + 而**节点永远是从安装路径启动的** —— 也就是说 parents[N] 这种写法在 + "脚本里手工跑"时看着没问题，一到正式启动就坏，属于最容易被漏掉的一类。 + + ``robot_description.repo_root()`` 处理了三种情形（``ROS2_WS`` 环境变量、 + 标记文件向上查找、cwd 向上查找），并且找不到时会说清怎么修。 + """ + from arm_sim_gazebo.robot_description import repo_root as _robust_root + + return _robust_root() def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> SceneSpec: @@ -170,7 +185,7 @@ def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> Scen op_raw = doc.get("operating") or {} operating = Operating( - camera=str(op_raw.get("camera") or "d405").strip().strip("/"), + camera=str(op_raw.get("camera") or "sim_d405").strip().strip("/"), camera_to_part_m=float(op_raw.get("camera_to_part_m") or 0.20), camera_offset_below_flange_m=float(op_raw.get("camera_offset_below_flange_m") or 0.0), part_object=str(op_raw.get("part_object") or "workpiece"), @@ -758,6 +773,81 @@ def pose_mismatches( return out +@dataclass(frozen=True) +class SceneRing: + """工件上的一个嵌件亮环（模型里 ```` 的几何）。 + + 坐标是**物体局部系、单位米**（SDF 原生的量纲）：``z`` 是环心高度， + ``radius`` 是亮环外半径。用途有两个——投影成像素（要 x/y/z）和算像素半径 + （要 radius）。 + + 为什么单独建一个类型而不是返回元组：下游要按下标取值，写错不会报错， + 只会安静地算偏 —— 而"孔位总偏一点"是最难归因的一类症状。 + """ + + name: str + x: float + y: float + z: float + radius: float + + +def _visual_geometry(tree: ET.ElementTree) -> Tuple[List[SceneRing], Dict[str, float]]: + """一次遍历解出（亮环列表, 孔半径表）。名字约定见 :func:`features_from_workpiece`。""" + rings: List[SceneRing] = [] + bores: Dict[str, float] = {} + for vis in tree.iter("visual"): + name = vis.get("name") or "" + cyl = vis.find("geometry/cylinder") + if cyl is None: + continue # 板面是 box，不是"环/孔" —— 不看 + r_el = cyl.find("radius") + if r_el is None or not r_el.text: + continue + try: + r = float(r_el.text) + except ValueError: + continue + if name.endswith("_ring"): + pose_el = vis.find("pose") + if pose_el is None or not pose_el.text: + # 没有 pose 就没有圆心。**宁可不收**，也不要拿 (0,0,0) 当圆心： + # 那会让"两孔间距"算成 0，看起来像"检测不到孔"，白查半天。 + continue + parts = pose_el.text.split() + if len(parts) < 3: + continue + try: + x, y, z = float(parts[0]), float(parts[1]), float(parts[2]) + except ValueError: + continue + rings.append(SceneRing(name=name, x=x, y=y, z=z, radius=r)) + elif name == "center_bore": + # ⚠️ 必须先判 center_bore 再判 _bore 后缀：名字 "center_bore" 也以 + # "_bore" 结尾，先判后缀会让中心大孔的半径覆盖掉小孔的。 + # （踩过：insert_bore_radius 报出 0.02，看起来"孔比环大"，很费解。） + bores["center_bore"] = r + elif name.endswith("_bore"): + bores["insert_bore"] = r + return rings, bores + + +def rings_local( + spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None +) -> List[SceneRing]: + """工件模型里所有嵌件亮环（物体局部系，单位米）。 + + 与 :func:`features_from_workpiece` **共用同一份解析**（``_visual_geometry``）， + 因为两处必须永远一致：前者算"该看到多大"，后者算"该看到在哪" —— + 漂移了就会出现"像素门槛说能看到、真值却投影不到那儿"的怪现象。 + """ + try: + tree = ET.parse(model_sdf_path(spec, obj, root)) + except (OSError, ET.ParseError): + return [] + return _visual_geometry(tree)[0] + + def features_from_workpiece( spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None ) -> Dict[str, float]: @@ -780,47 +870,18 @@ def features_from_workpiece( except (OSError, ET.ParseError): return {} + rings, bores = _visual_geometry(tree) out: Dict[str, float] = {} - rings: List[float] = [] - for vis in tree.iter("visual"): - name = vis.get("name") or "" - cyl = vis.find("geometry/cylinder") - if cyl is None: - continue - r_el = cyl.find("radius") - if r_el is None or not r_el.text: - continue - try: - r = float(r_el.text) - except ValueError: - continue - if name.endswith("_ring"): - out["insert_ring_radius"] = r - rings.append(r) - elif name == "center_bore": - # ⚠️ 必须先判 center_bore 再判 _bore 后缀：名字 "center_bore" 也以 - # "_bore" 结尾，先判后缀会让中心大孔的半径覆盖掉小孔的。 - # （踩过：insert_bore_radius 报出 0.02，看起来"孔比环大"，很费解。） - out["center_bore_radius"] = r - elif name.endswith("_bore"): - out["insert_bore_radius"] = r - - # 两孔间距 = 两个环心之间的距离（从 visual 的 pose 里取） - centers: List[Tuple[float, float]] = [] - for vis in tree.iter("visual"): - if not (vis.get("name") or "").endswith("_ring"): - continue - pose_el = vis.find("pose") - if pose_el is not None and pose_el.text: - parts = pose_el.text.split() - if len(parts) >= 3: - try: - centers.append((float(parts[0]), float(parts[1]))) - except ValueError: - pass - if len(centers) == 2: + if rings: + out["insert_ring_radius"] = rings[-1].radius + if "center_bore" in bores: + out["center_bore_radius"] = bores["center_bore"] + if "insert_bore" in bores: + out["insert_bore_radius"] = bores["insert_bore"] + # 两孔间距 = 两个环心之间的距离 + if len(rings) == 2: out["hole_spacing"] = math.hypot( - centers[0][0] - centers[1][0], centers[0][1] - centers[1][1] + rings[0].x - rings[1].x, rings[0].y - rings[1].y ) return out diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py index 9cfca18..358ef63 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_site.py @@ -101,8 +101,14 @@ def load_sim_site(root: Optional[Path] = None) -> SimSite: def repo_root() -> Path: - """仓库根。本模块在 src/arm_sim_gazebo/arm_sim_gazebo/ 下，向上走四层。""" - return Path(__file__).resolve().parents[3] + """仓库根。**实现只有一份**：``robot_description.repo_root()``。 + + 这里曾经也是 ``parents[3]`` —— 源码树里对、安装后错（install_sim 是拷贝）。 + 两处各写一份还会各自漂移，所以都改成委托（见 sim_scene.repo_root 的注释）。 + """ + from arm_sim_gazebo.robot_description import repo_root as _robust_root + + return _robust_root() def resolve_world( diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py index 28006b0..c33e546 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_tactile_adapter.py @@ -50,10 +50,20 @@ from rclpy.time import Time from arm_msgs.msg import TactileStatus from geometry_msgs.msg import Wrench -from ros_gz_interfaces.msg import Contacts from sensor_msgs.msg import Image from tf2_ros import Buffer, TransformListener +# ros_gz_interfaces 只在仿真里用得到，而且是 conda 装的（不在本工作区）。 +# 万一它的 typesupport 加载不了（见下），我们只丢掉"接触点"这一路信息 —— +# 力本身走 geometry_msgs/Wrench，不受影响。所以这里**容错导入**： +# 宁可用退化模式跑，也不要整个适配器起不来。 +_contacts_err: Optional[str] = None +try: + from ros_gz_interfaces.msg import Contacts +except Exception as _exc: # pragma: no cover - 取决于运行环境 + Contacts = None # type: ignore[assignment] + _contacts_err = f"{type(_exc).__name__}: {_exc}" + from .sim_tactile import ( DEFAULT_TACTILE_REL, BaselineTracker, @@ -128,6 +138,7 @@ class SimTactileAdapter(Node): self._pub_status = {} self._pub_image: Dict[str, Tuple[object, object, float, float]] = {} + self._contact_unavailable = False for s in self.spec.sides: self._got_wrench[s.side] = False self._got_contact[s.side] = False @@ -140,12 +151,15 @@ class SimTactileAdapter(Node): self._make_wrench_cb(s.side), 10, ) - self.create_subscription( - Contacts, - s.contact_topic, - self._make_contact_cb(s.side), - 10, - ) + if Contacts is not None: + self.create_subscription( + Contacts, + s.contact_topic, + self._make_contact_cb(s.side), + 10, + ) + else: + self._contact_unavailable = True if self.spec.image_hz > 0.0: self._pub_image[s.side] = ( self.create_publisher(Image, s.image_topic, 3), @@ -165,6 +179,20 @@ class SimTactileAdapter(Node): self._check_at = time.monotonic() + TOPIC_CHECK_DELAY_S self._checked = False + if self._contact_unavailable: + detail = f"（导入失败：{_contacts_err}）" if _contacts_err else "" + self.get_logger().warn( + f"ros_gz_interfaces/msg/Contacts 用不了{detail}，" + f"本节点进入退化模式：\n" + f" 仍会发 TactileStatus（fnormal/fshearx/fsheary、connected、" + f"接触去基线都正常），\n" + f" 但 contact 恒为 False、centroid_u/v 恒为 0" + f"（这两项依赖接触点消息）。\n" + f" 三力判据（本项目 S07 用的那个）不受影响。\n" + f" 要修：在**干净 shell** 里 source scripts/sim_env.sh（它会清掉真机" + f"环境残留的 PYTHONPATH/LD_LIBRARY_PATH 并补上 $CONDA_PREFIX/lib）。" + ) + # --- 订阅回调 --------------------------------------------------------- def _make_wrench_cb(self, side: str): def cb(msg: Wrench) -> None: diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py new file mode 100644 index 0000000..25002b2 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_truth_perception.py @@ -0,0 +1,328 @@ +#!/usr/bin/env python3 +"""仿真"真值感知"：把 Gazebo 里**已知的真实位姿**当成视觉测量发给伺服。 + +为什么要这个节点 +---------------- +真机的视觉链是 ``perception_node``（YOLO/ONNX）→ 话题 +``visual_servo/filtered_point``。仿真环境（ros_gz_sim conda）里没有 +torch/ultralytics，而且这台机器**离线**装不上，于是 perception_node 一启动就死。 + +死法还很误导人：S00 只看"权重文件在不在"（在），S04 一阶段则一直等 +``/visual_servo/locked``，等满 150s 报"没锁定（视野里没工件？）"—— +现场看到的是"算法不收敛/标定不准"，真因是"仿真里根本没有推理后端"。 + +仿真里其实**不需要推理**：工件摆在哪、长什么样都是已知的 +（``scene.yaml`` 的绝对位姿 + 模型自己的几何）。伺服要的那个"相机系下的 +目标点"可以直接算出来： + + 世界系目标点 = 物体世界 XY 轮廓中心 + 高度（模型顶面） ＋ 可选局部偏移 + 相机系目标点 = T_cam_world · 世界系目标点 （T 由 TF 查，不自己算） + + 发布 → ``PointStamped(visual_servo/filtered_point)`` + 与 perception_node **同话题、同类型**，所以下游（servo_node、 + 整条装配流程）一个字都不用改。 + +⚠️ 它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排）， + 不是感知那一半。真值箱体没有像素噪声、没有遮挡、没有光照变化，也没有 + "算法把孔认成别的"这类错。所以： + + 能跑通 = "控制侧与接口是对的" + 不能跑通 ≠ "真机上也能跑通" ← 规律写在 + devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节 + +绝不做的事 +---------- +* 目标在相机背后 / 相机查不到 TF 时**不发假点**（和 perception_node 的 + "没有可用深度就不发点"同一条规矩）。宁可让伺服看门狗超时，也不给它一个 + 幻影目标 —— 后者会让机械臂朝错误方向走，而且看起来像"标定坏了"。 +* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。 + 这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是 + "总能到附近、永远差一点"。数学实现只能有一份。 +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from pathlib import Path +from typing import Optional, Sequence, Tuple + +import numpy as np +import rclpy +from geometry_msgs.msg import PointStamped +from rclpy.node import Node +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from rclpy.time import Time +from tf2_ros import Buffer, TransformListener + +from arm_sim_gazebo import sim_scene + +#: 与 perception_node / servo_node 一致：点话题用 BEST_EFFORT + KEEP_LAST(5)。 +#: 匹配不上 QoS 的后果是"话题在、消息发了、订阅方一条也收不到"， +#: 而两边单独看都正常 —— 这类问题排查成本极高，所以直接抄同一份配置。 +POINT_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=5, + reliability=ReliabilityPolicy.BEST_EFFORT, +) + +#: 目标点离相机光心太近（或跑到背后）就不发：z<=0 在相机系里是"背后"。 +MIN_CAM_Z_M = 0.01 + + +class TruthError(RuntimeError): + """真值目标解不出来（场景 / 模型 / 参数有问题）。 + + 单独一个异常类型是为了让"配置错了"和"TF 还没到"分开处理： + 前者要直接起不来（早报），后者只该刷限流告警（等机器人动起来就有了）。 + """ + + +@dataclass(frozen=True) +class TruthTarget: + """真值目标：一个**世界系**点，以及它是怎么算出来的（给人看）。""" + + object_name: str + world_xyz: Tuple[float, float, float] + note: str + + +def target_point_world( + spec: sim_scene.SceneSpec, + obj: sim_scene.SceneObject, + offset_m: Sequence[float] = (0.0, 0.0, 0.0), + root: Optional[Path] = None, +) -> TruthTarget: + """物体顶面中心（世界系）＋ 物体局部系偏移。 + + 为什么取"顶面中心"：真机 perception_node 默认 ``feature_mode: plane``， + 发的是检测框在**工件上表面**上的几何中心。真值要想替得住它，就得发同一个 + 物理点；差一个板厚（12mm）会让对比结论完全反过来。 + + 为什么用 ``sim_scene.footprint`` 而不是自己按 yaw 算： + 轮廓换算里已经含了 yaw（``world_boxes`` 里有且只有一份实现）， + 自己再写一遍迟早不一致，而"哪个才是对的"很难判断。 + + ``offset_m`` 是**物体局部系**里的偏移（会跟着 yaw 转），用途例如： + 把目标从"板中心"挪到 k1 孔 ``[0, -0.029, 0]``，用来单独验证二阶段对孔。 + """ + fp = sim_scene.footprint(spec, obj, root) + height = sim_scene.model_height(spec, obj, root) + if fp is None or height is None: + raise TruthError( + f"读不出 {obj.name!r} 的几何（模型 {obj.model}）。\n" + f" 期望：{sim_scene.model_sdf_path(spec, obj, root)} 里有 \n" + f" 修法：确认 models_dir 指对了、模型目录里有 model.sdf 且至少一个碰撞体。" + ) + vals = [float(v) for v in offset_m] + if len(vals) != 3: + # 长度不对必须报错，不能"缺的当 0"：把 [0.03] 当成 (0.03,0,0) 还是 + # (0,0.03,0) 全凭猜，而猜错的表现是"偏了一点"，很难归因。 + raise TruthError( + f"offset_m 要给 3 个数（物体局部系 xyz，单位米），现在给的是 {list(offset_m)!r}。" + ) + ox, oy, oz = vals + cx = 0.5 * (fp[0] + fp[1]) + cy = 0.5 * (fp[2] + fp[3]) + c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw)) + x = cx + c * ox - s * oy + y = cy + s * ox + c * oy + z = float(obj.z) + float(height) + oz + note = ( + f"{obj.name}（模型 {obj.model}）" + f"轮廓中心 ({cx:.3f},{cy:.3f}) + 高 {height * 1000:.1f}mm" + f" + 局部偏移 ({ox * 1000:+.0f},{oy * 1000:+.0f},{oz * 1000:+.0f})mm" + f" yaw={math.degrees(float(obj.yaw)):.1f}°" + ) + return TruthTarget(object_name=obj.name, world_xyz=(x, y, z), note=note) + + +def pick_object(spec: sim_scene.SceneSpec, name: str) -> sim_scene.SceneObject: + """按名字找物体；找不到时把**现有名字**列出来（改配置时不用再翻文件）。""" + want = (name or "").strip() or spec.operating.part_object + for obj in spec.objects: + if obj.name == want: + return obj + raise TruthError( + f"场景 {spec.source} 里没有名为 {want!r} 的物体。\n" + f" 现有物体：{', '.join(o.name for o in spec.objects) or '（空）'}\n" + f" 修法：改 site.yaml 的 perception.truth.object，" + f"或确认 scene.yaml 的 objects 里确实有它。" + ) + + +def load_truth_target( + scene_file: Optional[Path], + object_name: str, + offset_m: Sequence[float] = (0.0, 0.0, 0.0), + root: Optional[Path] = None, +) -> Tuple[sim_scene.SceneSpec, TruthTarget]: + """读场景 + 解出真值目标。返回 ``(场景, 目标)``，场景用于启动日志。 + + ``object_name`` 空 = 用场景里的 ``operating.part_object``：**"哪个是工件" + 是场景的知识，不是流程的知识**，所以默认跟着场景走，换场景不用改流程参数。 + """ + spec = sim_scene.load_scene(scene_file, root) + return spec, target_point_world(spec, pick_object(spec, object_name), offset_m, root) + + +def apply_transform(T: np.ndarray, p: Sequence[float]) -> Tuple[float, float, float]: + """4×4 齐次变换作用于点。""" + q = np.asarray(T, dtype=float) @ np.array([p[0], p[1], p[2], 1.0], dtype=float) + return (float(q[0]), float(q[1]), float(q[2])) + + +def matrix_from_transform(tr) -> np.ndarray: + """``TransformStamped`` → 4×4 齐次矩阵。 + + 四元数→旋转矩阵**不在这里实现**：复用 ``handeye_lib.quat_xyzw_to_R`` + （那是全仓库唯一一份，且有回环测试钉着）。重复实现的下场见模块 docstring。 + """ + from arm_application.handeye_lib import quat_xyzw_to_R + + t = tr.transform.translation + q = tr.transform.rotation + T = np.eye(4, dtype=float) + T[:3, :3] = quat_xyzw_to_R([q.x, q.y, q.z, q.w]) + T[0, 3] = float(t.x) + T[1, 3] = float(t.y) + T[2, 3] = float(t.z) + return T + + +class SimTruthPerception(Node): + """把真值目标点按固定频率发到 ``visual_servo/filtered_point``。""" + + def __init__(self) -> None: + super().__init__("sim_truth_perception") + self.declare_parameter("scene_file", "") + """场景文件（空 = 仓库根下 devices/sim/gazebo/config/scene.yaml）。""" + + self.declare_parameter("target_object", "") + """目标物体名（空 = 场景里的 operating.part_object）。""" + + self.declare_parameter("offset_m", [0.0, 0.0, 0.0]) + """物体局部系偏移 [m]，例如挪到某个孔：``[0.0, -0.029, 0.0]``。""" + + self.declare_parameter("camera_frame", "") + """相机光学系（由 launch 按当前臂/相机传进来，不写死）。""" + + self.declare_parameter("world_frame", "world") + """场景的世界系。Gazebo 里模型位姿就是相对它，TF 树上也有。""" + + self.declare_parameter("point_topic", "visual_servo/filtered_point") + self.declare_parameter("publish_hz", 20.0) + """发布频率：不必跟着相机帧率，伺服 100Hz 控制会自己插值/外推。""" + + scene_file = str(self.get_parameter("scene_file").value or "").strip() + self._scene_file = Path(scene_file) if scene_file else None + offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0] + # 长度的校验在 target_point_world 里（纯函数，有测试钉着），这里不重复。 + self._camera_frame = str(self.get_parameter("camera_frame").value or "").strip() + if not self._camera_frame: + raise TruthError( + "camera_frame 没给。它由 visual_servo.launch.py 按当前臂的相机传进来" + "（site.yaml → cameras/ perception.camera_frame）。" + "手工跑这个节点时必须显式指定，例如 " + "--ros-args -p camera_frame:=sim_d405_color_optical_frame" + ) + self._world_frame = str(self.get_parameter("world_frame").value or "world").strip() + + # 目标点在启动时算一次就够：场景是静态的（工件被推动后位置会变， + # 但那种情况下"真值"本身也就失效了 —— 此时应该重跑 gen_sim_handeye + # 或重启，而不是悄悄跟着一个被撞歪的位姿走）。 + self._spec, self._target = load_truth_target( + self._scene_file, + str(self.get_parameter("target_object").value or ""), + offset, + ) + + self._pub = self.create_publisher( + PointStamped, str(self.get_parameter("point_topic").value), POINT_QOS + ) + self._tf = Buffer() + TransformListener(self._tf, self, spin_thread=True) + self._sent = 0 + self._warned_tf = 0 + + hz = max(1.0, min(100.0, float(self.get_parameter("publish_hz").value))) + self.create_timer(1.0 / hz, self._tick) + self.get_logger().warn( + "真值感知已启动（仿真专用）：不跑任何推理，直接把已知位姿当测量发出去。\n" + f" 场景 {self._spec.source}\n" + f" 目标 {self._target.note}\n" + f" 世界系点 ({self._target.world_xyz[0]:.3f}, " + f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f}) " + f"→ 相机系 {self._camera_frame}（TF: {self._world_frame} → " + f"{self._camera_frame}） 频率 {hz:.0f}Hz\n" + f" ⚠️ 它验证的是控制与接口，不是检测算法 —— 见 " + f"devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」" + ) + + # ------------------------------------------------------------------ + def _tick(self) -> None: + try: + tr = self._tf.lookup_transform(self._camera_frame, self._world_frame, Time()) + except Exception as exc: # noqa: BLE001 —— tf2 的异常类型很杂，统一当"还没有" + # 前几拍必然查不到（TF 要等 robot_state_publisher 起来）， + # 所以只刷限流告警，不报错。 + self._warned_tf += 1 + if self._warned_tf in (1, 20, 100) or self._warned_tf % 200 == 0: + self.get_logger().warn( + f"还查不到 TF {self._world_frame} → {self._camera_frame}" + f"（已试 {self._warned_tf} 次）：{exc}", + throttle_duration_sec=5.0, + ) + return + p = apply_transform(matrix_from_transform(tr), self._target.world_xyz) + if p[2] < MIN_CAM_Z_M: + # 目标在相机背后或贴到镜头上：不发！发出去伺服会朝反方向走。 + self.get_logger().warn( + f"真值目标不在相机前方（相机系 z={p[2] * 1000:.0f}mm < " + f"{MIN_CAM_Z_M * 1000:.0f}mm）—— 不发点。" + f"世界系点 ({self._target.world_xyz[0]:.3f}, " + f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})" + f" 不在 {self._camera_frame} 视野里；先让臂摆到能看到它的位置。", + throttle_duration_sec=5.0, + ) + return + + msg = PointStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = self._camera_frame + msg.point.x, msg.point.y, msg.point.z = p + self._pub.publish(msg) + self._sent += 1 + if self._sent == 1: + self.get_logger().info( + f"首个真值目标点已发出：相机系 ({p[0]:.4f}, {p[1]:.4f}, {p[2]:.4f}) m" + ) + elif self._sent % 200 == 0: + self.get_logger().debug(f"已发 {self._sent} 个真值点") + + +def main(argv=None) -> int: + import traceback + + rclpy.init(args=argv) + node = None + try: + node = SimTruthPerception() + rclpy.spin(node) + return 0 + except TruthError as exc: + # 配置错：把原因原样打出来（它是给人看的），别让 ROS 的异常栈盖住。 + print(f"真值感知起不来：{exc}", flush=True) + return 2 + except Exception: # noqa: BLE001 + traceback.print_exc() + return 1 + finally: + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py b/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py new file mode 100644 index 0000000..80a422a --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/truth_holes.py @@ -0,0 +1,214 @@ +#!/usr/bin/env python3 +"""真值孔位：把场景里**已知的孔几何**投影成二阶段要的那两个像素点。 + +为什么需要它 +------------ +二阶段（S05 精拍 → 对准）原本的输入是 ``hole_detect_hough`` 在图上找**亮环**。 +那条路要求工件检测模型先给出工件框（ROI），否则连"找哪儿"都不知道。 + +仿真的处境是：合成图过不了真机训练出来的工件模型（实测最高分 0.017）， +于是 S05 只能退回"整幅画面当 ROI"—— 而整幅画面里有**夹爪、通止规、台面**， +HoughCircles 会在那儿找到"看起来像一对孔"的东西（实测两孔间距 108mm， +真值是 58mm），随后被间距门槛丢掉，每拍都丢，最后超时。 + +真值路径直接把"孔在图上应该在哪"算出来： + + 孔世界点 = 物体世界位姿（scene.yaml）+ 环在物体局部系的位置（模型 ） + 孔像素 = K · T_cam_world · 孔世界点 + +它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排）， +不是感知那一半 —— 与 ``sim_truth_perception`` 同一条设计（见 +``devices/sim/gazebo/README.md`` 的「仿真里哪些模块用真值替代」）。 + +绝不做的事 +---------- +* 孔在相机背后（z<=0）时**不返回**结果。给伺服一个幻影目标会让机械臂朝错误 + 方向走，而且看起来像"标定坏了" —— 和 ``sim_truth_perception`` 同一条规矩。 +* 不自己算四元数→旋转矩阵：T 由调用方（节点）用 TF 查好传进来。 + 本模块只做**纯几何**，因此在没有 ROS 的环境里也能测。 +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, field +from pathlib import Path +from typing import List, Optional, Sequence, Tuple + +import numpy as np + +from arm_sim_gazebo import sim_scene + +#: 孔（环）离相机光心太近或跑到背后就不返回：z 太小除法会放大误差， +#: z<=0 在相机系里就是"背后"。取 2cm，与真值感知节点的取值同量级。 +MIN_CAM_Z_M = 0.02 + +#: 两种孔排序的判据：图像上 |dx| 大于这个比例倍数就按 x 排，否则按 y 排。 +#: 与 ``hole_detect_hough._detect_insert_pair`` 的规则一致（k1=左/上， +#: k2=右/下）—— 换后端不该换语义，否则"哪边是 k1"会随环境变。 +_PAIR_HORIZONTAL_RATIO = 1.0 + + +@dataclass(frozen=True) +class HoleTruth: + """一次真值定位的结果（``ok=False`` 时只有 ``why`` 有意义）。""" + + ok: bool + why: str = "" + hole_points: List[Tuple[float, float]] = field(default_factory=list) + #: 两孔的像素半径（环的物理半径按透视缩放；Hough 路径给的是它自己的估计， + #: 真值路径给它一个**可信的**值 —— 上下游只把它当"尺度参考"用） + hole_radii: List[float] = field(default_factory=list) + #: 工件在画面里占的框（孔检测的 ROI 用；也是"工件中心"的来源） + bbox: Tuple[int, int, int, int] = (0, 0, 0, 0) + #: 给人看的说明（进了日志才知道这个数是哪来的） + note: str = "" + + +def _object_world_xy(obj: sim_scene.SceneObject, lx: float, ly: float) -> Tuple[float, float]: + """物体局部 (x,y) → 世界 (x,y)（含物体自身 yaw）。""" + c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw)) + return (float(obj.x) + c * lx - s * ly, float(obj.y) + s * lx + c * ly) + + +def project_point( + T_cam_world: np.ndarray, intr: Sequence[float], xyz: Sequence[float] +) -> Optional[Tuple[float, float, float]]: + """世界点 → ``(u, v, z_cam)``；在相机背后/太近时返回 None。""" + T = np.asarray(T_cam_world, dtype=float) + p = T @ np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float) + z = float(p[2]) + if not math.isfinite(z) or z <= MIN_CAM_Z_M: + return None + fx, fy, cx, cy = (float(intr[0]), float(intr[1]), float(intr[2]), float(intr[3])) + return (fx * float(p[0]) / z + cx, fy * float(p[1]) / z + cy, z) + + +def object_bbox_px( + T_cam_world: np.ndarray, + intr: Sequence[float], + boxes: Sequence[Sequence[float]], + image_shape: Tuple[int, int], +) -> Optional[Tuple[int, int, int, int]]: + """物体各碰撞体世界 AABB 的角点投影 → 像素包围框（裁剪到画面内）。 + + 返回 None = 一个角点都投不出来（跑到背后去了），调用方应当放弃这一拍。 + """ + h, w = int(image_shape[0]), int(image_shape[1]) + us: List[float] = [] + vs: List[float] = [] + for b in boxes: + for x in (b[0], b[1]): + for y in (b[2], b[3]): + for z in (b[4], b[5]): + got = project_point(T_cam_world, intr, (x, y, z)) + if got is None: + continue + us.append(got[0]) + vs.append(got[1]) + if not us: + return None + x1 = max(0, min(w - 1, int(math.floor(min(us))))) + y1 = max(0, min(h - 1, int(math.floor(min(vs))))) + x2 = max(0, min(w - 1, int(math.ceil(max(us))))) + y2 = max(0, min(h - 1, int(math.ceil(max(vs))))) + if x2 <= x1 or y2 <= y1: + return None + return (x1, y1, x2, y2) + + +def locate_holes( + T_cam_world: Optional[np.ndarray], + intr: Optional[Sequence[float]], + image_shape: Tuple[int, int], + *, + spec: Optional[sim_scene.SceneSpec] = None, + object_name: str = "", + root: Optional[Path] = None, +) -> HoleTruth: + """算这一拍的两个孔（像素 + ROI 框）。 + + ``T_cam_world`` 为 None（TF 还没到）时返回 ``ok=False``，由调用方决定是 + 继续用检测器还是报错 —— 本函数不猜、不兜底。 + """ + if T_cam_world is None or intr is None: + return HoleTruth(ok=False, why="还没有 T_cam_world / 相机内参（TF 或 camera_info 未到）") + try: + spec = spec if spec is not None else sim_scene.load_scene(None, root) + except Exception as exc: # noqa: BLE001 —— 场景读不出来要说清是场景的问题 + return HoleTruth(ok=False, why=f"读不到场景配置：{exc}") + want = (object_name or "").strip() or spec.operating.part_object + obj = next((o for o in spec.objects if o.name == want), None) + if obj is None: + names = ", ".join(o.name for o in spec.objects) or "（空）" + return HoleTruth(ok=False, why=f"场景里没有物体 {want!r}（现有：{names}）") + rings = sim_scene.rings_local(spec, obj, root) + if len(rings) != 2: + return HoleTruth( + ok=False, + why=( + f"工件 {obj.name!r} 的模型里有 {len(rings)} 个嵌件亮环，" + f"真值孔位要求正好 2 个（模型 的名字约定）" + ), + ) + + pts: List[Tuple[float, float]] = [] + rad: List[float] = [] + for ring in rings: + wx, wy = _object_world_xy(obj, ring.x, ring.y) + got = project_point(T_cam_world, intr, (wx, wy, float(obj.z) + ring.z)) + if got is None: + return HoleTruth( + ok=False, + why=f"孔 {ring.name} 投影到相机背后（z<=0）—— 相机没对着工件时不该出结果", + ) + u, v, z = got + pts.append((u, v)) + rad.append(float(intr[0]) * float(ring.radius) / z) + + # k1/k2 的左右（上下）规则与 Hough 路径一致：水平对按 x，竖直对按 y。 + (p, q) = pts + horizontal = abs(p[0] - q[0]) >= _PAIR_HORIZONTAL_RATIO * abs(p[1] - q[1]) + key = 0 if horizontal else 1 + if pts[0][key] > pts[1][key]: + pts = [pts[1], pts[0]] + rad = [rad[1], rad[0]] + + bbox = object_bbox_px(T_cam_world, intr, sim_scene.world_boxes(spec, obj, root), image_shape) + if bbox is None: + return HoleTruth(ok=False, why="工件整体投影不出画面框（角点都在相机背后）") + + dist_px = math.hypot(pts[0][0] - pts[1][0], pts[0][1] - pts[1][1]) + note = ( + f"{obj.name}（模型 {obj.model}） 环心距 {dist_px:.1f}px " + f"环半径 {rad[0]:.1f}/{rad[1]:.1f}px 框 {bbox}" + ) + return HoleTruth( + ok=True, + why="", + hole_points=[(float(pts[0][0]), float(pts[0][1])), (float(pts[1][0]), float(pts[1][1]))], + hole_radii=[float(rad[0]), float(rad[1])], + bbox=bbox, + note=note, + ) + + +def draw(bgr: np.ndarray, res: HoleTruth) -> np.ndarray: + """把真值结果画在图上（叠图/日志用）。失败时只写一行原因。""" + import cv2 + + img = bgr.copy() + h, w = img.shape[:2] + if not res.ok: + cv2.putText(img, "TRUTH: n/a", (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2) + return img + x1, y1, x2, y2 = res.bbox + cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 255), 2) + for i, (u, v) in enumerate(res.hole_points): + r = int(round(res.hole_radii[i])) if i < len(res.hole_radii) else 8 + cv2.circle(img, (int(round(u)), int(round(v))), max(3, r), (0, 0, 255), 2) + cv2.putText( + img, f"T{i + 1}", (int(u) + 8, int(v)), cv2.FONT_HERSHEY_SIMPLEX, 0.60, (0, 0, 255), 2 + ) + cv2.putText(img, "TRUTH", (12, min(h - 8, 28)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2) + return img diff --git a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py index cbc4746..d814490 100644 --- a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py +++ b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py @@ -541,6 +541,58 @@ def _prepare(context, *args, **kwargs): ) ) + # --- 6c) 机械臂的"应用接口"对齐真机 --------------------------------------- + # 仿真里唯一的控制通路是 ros2_control 的 JTC（control_msgs）。但应用层在 + # 真机上用的是另一层接口 —— 这里把它们**按真机的名字与类型**补齐， + # 于是通止规装配（以及 stage2/stage3）在仿真里能不改一行代码就跑： + # + # /follow_joint_trajectory arm_msgs/FollowJointTrajectory → JTC + # 真机由 real_trajectory_adapter 提供（名字带 real，但实现只做 action + # 桥接、与硬件无关；这里**复用同一个节点**而不是再写一份 —— + # 两份实现迟早会不一致）。 + # /set_named_pose arm_msgs/SetNamedPose（命名位姿）→ 上面那个 + # 真机由 arm_bringup/stack.launch.py 起，仿真起同一份，只是把 + # joint_prefix 换成 sim_（关节名不同，点位数值是同一台机器人）。 + # /home std_srvs/Trigger + # 真机由 flexiv_arm_driver 提供（= 切工具系 + 转 /set_named_pose）。 + # 仿真没有 Flexiv 驱动，用 sim_home_service 补同一份契约（不切工具系）。 + actions.append( + Node( + package="arm_control", + executable="real_trajectory_adapter", + name="real_trajectory_adapter", + output="screen", + parameters=[{"use_sim_time": True}], + ) + ) + actions.append( + Node( + package="arm_control", + executable="named_pose_server", + name="named_pose_server", + output="screen", + parameters=[ + # 命名位姿用**包内默认那份**（src/arm_control/config/named_poses.yaml）： + # 它只有 home/ready/inspect 这几个点，与"仿真里是哪台臂"无关； + # 真机走 site.yaml 的 robots[].paths.named_poses_file（那份带该臂的点位）。 + str(root / "src" / "arm_control" / "config" / "named_poses.yaml"), + # 关节名：仿真里是 sim_joint1..7（前缀来自 urdf 的 arm_prefix， + # site.yaml 的 sim.joint_prefix 目前是"声明了但没接线"，见那一段注释）。 + {"joint_prefix": "sim_"}, + {"use_sim_time": True}, + ], + ) + ) + actions.append( + Node( + package="arm_sim_gazebo", + executable="sim_home_service", + name="sim_home_service", + output="screen", + parameters=[{"use_sim_time": True}], + ) + ) + actions.append( LogInfo( msg=( diff --git a/src/arm_sim_gazebo/setup.py b/src/arm_sim_gazebo/setup.py index 28e282b..d848703 100644 --- a/src/arm_sim_gazebo/setup.py +++ b/src/arm_sim_gazebo/setup.py @@ -50,6 +50,16 @@ setup( # 7 个夹爪关节的目标位置（含 mimic 耦合换算），让仿真能走 # 真机那一套夹爪接口。由 gazebo_sim.launch.py 拉起。 'sim_gripper_adapter = arm_sim_gazebo.sim_gripper_adapter:main', + # 仿真 /home：真机上这个服务由 Flexiv 驱动提供（包装 + # /set_named_pose），仿真里没有驱动，用它补同一份契约， + # 让装配流程（S01 回 home / S11 复位）在两处都不改代码。 + 'sim_home_service = arm_sim_gazebo.sim_home_service:main', + # 仿真"真值感知"：不跑推理，直接把场景里已知的物体位姿算成 + # 相机系目标点，发到 visual_servo/filtered_point（与 + # perception_node 同话题同类型）。仿真环境没有 torch/ultralytics + # 时用它顶替感知，让整条装配流程能在仿真里跑通。 + # 由 visual_servo.launch.py 按 site.yaml 的 perception.backend 选择。 + 'sim_truth_perception = arm_sim_gazebo.sim_truth_perception:main', # 独立小工具：只看归一化做了什么，不启动 Gazebo。 # 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。 'urdf_normalize = arm_sim_gazebo.urdf_normalize:main', diff --git a/src/arm_sim_gazebo/test/test_check_sim_scene.py b/src/arm_sim_gazebo/test/test_check_sim_scene.py index b356d01..acd585c 100644 --- a/src/arm_sim_gazebo/test/test_check_sim_scene.py +++ b/src/arm_sim_gazebo/test/test_check_sim_scene.py @@ -363,6 +363,60 @@ def test_workspace_z_limits_degrades_to_none(chk, tmp_path): assert chk.workspace_z_limits(root2) is None +def test_workspace_limits_use_the_sim_robots_override(chk, tmp_path): + """工作盒要取**仿真那台臂**的覆盖值，不能只读顶层。 + + 顶层那套是真机的（台面与基座同高）；仿真里一切 Z 大 0.412。 + 只读顶层 → 拿真机的界核对仿真的悬停高，看起来过了，而生效配置可能不对。 + """ + root = _write_tree( + tmp_path, + pkg={}, + site={ + "servo": { + "workspace_xyz_min": [0.2, -0.5, 0.18], + "workspace_xyz_max": [0.8, 0.5, 0.65], + }, + "robot": { + "robots": [ + {"sn": "Rizon4-063989", "arm": "left"}, + { + "sn": "sim", + "arm": "sim", + "servo": { + "workspace_xyz_min": [0.08, -0.55, 0.55], + "workspace_xyz_max": [0.85, 0.55, 0.75], + }, + }, + ] + }, + }, + ) + assert chk.workspace_z_limits(root) == (0.55, 0.75) + + +def test_tracking_height_is_checked_against_the_workbox(chk, tmp_path, monkeypatch): + """一阶段跟踪高（悬停 + 50mm）超上界必须报错。 + + 否则会出现：二阶段位姿检查能过、S04 一跟踪就被「超出工作盒」截断 —— + 现象是"越跟踪越偏"，很难往配置上想（真机 0.65 上界对仿真 0.632 悬停 + 只差 18mm，一抬 5cm 就超了）。 + """ + root = _write_tree( + tmp_path, + pkg={}, + site={"servo": {"workspace_xyz_min": [0.0, 0.0, 0.0], "workspace_xyz_max": [1.0, 1.0, 0.65]}}, + ) + import arm_sim_gazebo.sim_scene as sim_scene + + monkeypatch.setattr(sim_scene, "operating_flange_z", lambda spec, root=None: 0.632) + probs = chk.check_hover_height(_fake_scene_with_part(), root) + assert len(probs) == 1 + assert "0.682" in probs[0] # 悬停 + 0.05 + assert "workspace_xyz_max.z" in probs[0] + assert "camera_to_part_m" in probs[0] + + def test_check_hover_height_flags_above_workspace(chk, tmp_path, monkeypatch): """悬停高度超出工作空间上界必须报错，并指明两个可调的旋钮。 @@ -380,9 +434,12 @@ def test_check_hover_height_flags_above_workspace(chk, tmp_path, monkeypatch): monkeypatch.setattr(sim_scene, "operating_flange_z", lambda spec, root=None: 0.632) probs = chk.check_hover_height(_fake_scene_with_part(), root) - assert len(probs) == 1 + # 悬停本身就超界时，跟踪高必然也超 —— 两条都要报（都是不同修法： + # 一条要抬界/拉近相机，另一条只要抬 workspace_xyz_max.z）。 + assert len(probs) == 2 assert "0.632" in probs[0] and "0.2" in probs[0] assert "camera_to_part_m" in probs[0] + assert "0.682" in probs[1] def test_check_hover_height_silent_when_inside_workspace(chk, tmp_path, monkeypatch): diff --git a/src/arm_sim_gazebo/test/test_repo_root.py b/src/arm_sim_gazebo/test/test_repo_root.py new file mode 100644 index 0000000..3ea1fb7 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_repo_root.py @@ -0,0 +1,98 @@ +"""仓库根定位：装好的包也**必须**找得到仓库根。 + +为什么单独一套测试 +------------------ +``parents[N]`` 这类写法在"从源码树里手工跑"时看着完全正常，一到正式启动就坏： +``install_sim/`` 底下是**拷贝**（不是软链），于是装出来的模块算出来的 +"仓库根"是 ``install_sim/arm_sim_gazebo/lib``，而 ``devices/`` 不在那儿。 +现场表现是节点一启动就报 + + 找不到场景配置 .../install_sim/arm_sim_gazebo/lib/devices/sim/gazebo/config/scene.yaml + +—— 而这跟"场景文件不存在"完全是两件事，很容易查错方向。 + +本文件钉三件事： + 1. 三个模块的 ``repo_root()`` 都委托给同一份实现（不许再冒出 parents[N]）； + 2. ``ROS2_WS`` 环境变量优先； + 3. 标记文件查找真的能找到（而不是"永远返回某个路径"）。 +""" + +from __future__ import annotations + +import inspect +from pathlib import Path + +import pytest + +from arm_sim_gazebo import robot_description, sim_scene, sim_site + +MARKER = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro" + + +def test_repo_root_implementations_all_delegate(): + """三个入口都必须委托给 robot_description（路径实现只能有一份）。 + + 只看**函数体**（去掉 docstring）：docstring 里正当地写着"以前是 + parents[3]"这段历史，把它算进判据会误报。 + """ + import ast + + for mod in (sim_scene, sim_site): + tree = ast.parse(inspect.getsource(mod.repo_root)) + func = tree.body[0] + assert isinstance(func, ast.FunctionDef) + body = [ + n for n in func.body + if not (isinstance(n, ast.Expr) and isinstance(n.value, ast.Constant)) + ] + src = ast.dump(ast.Module(body=body, type_ignores=[])) + assert "robot_description" in src, f"{mod.__name__}.repo_root 必须委托，不能自己算" + assert "parents" not in src, f"{mod.__name__}.repo_root 里不许再出现 parents[N]" + + +def test_repo_root_finds_the_real_repo(): + root = sim_scene.repo_root() + assert (root / MARKER).is_file(), f"{root} 里没有 {MARKER} —— 这不是仓库根" + # 顺带确认框架自带的场景文件确实在（它在 git 里，不属于站点数据） + assert (root / "devices/sim/gazebo/config/scene.yaml").is_file() + + +def test_ros2_ws_env_wins(tmp_path, monkeypatch): + """ROS2_WS 指向一个像仓库的目录时必须直接采信它。""" + (tmp_path / MARKER).parent.mkdir(parents=True, exist_ok=True) + (tmp_path / MARKER).write_text("", encoding="utf-8") + monkeypatch.setenv("ROS2_WS", str(tmp_path)) + assert robot_description.repo_root() == tmp_path + assert sim_scene.repo_root() == tmp_path + + +def test_ros2_ws_is_ignored_when_it_does_not_look_like_a_repo(tmp_path, monkeypatch): + """环境变量指向一个不存在的仓库时要**继续找**，而不是返回一个错路径。 + + 返回错路径是最坏的一种：后续报"文件不存在"，而人会去查文件， + 不会想到是环境变量指错了。 + """ + monkeypatch.setenv("ROS2_WS", str(tmp_path / "nope")) + root = sim_scene.repo_root() + assert (root / MARKER).is_file() + + +def test_marker_search_from_a_nested_cwd(tmp_path, monkeypatch): + """从任意深度的子目录启动（cwd 在仓库内）也能找到根。""" + monkeypatch.delenv("ROS2_WS", raising=False) + root = sim_scene.repo_root() + nested = root / "src" / "arm_sim_gazebo" + monkeypatch.chdir(nested) + assert sim_scene.repo_root() == root + + +def test_missing_repo_root_error_tells_how_to_fix(tmp_path, monkeypatch): + """找不到时必须说清怎么修（导出 ROS2_WS / 在仓库根下执行）。""" + monkeypatch.delenv("ROS2_WS", raising=False) + monkeypatch.chdir(tmp_path) + monkeypatch.setattr(robot_description, "__file__", str(tmp_path / "x" / "y.py")) + with pytest.raises(robot_description.SimDescriptionError) as e: + robot_description.repo_root(start=tmp_path) + msg = str(e.value) + assert "ROS2_WS" in msg + assert "仓库根" in msg diff --git a/src/arm_sim_gazebo/test/test_sim_cameras.py b/src/arm_sim_gazebo/test/test_sim_cameras.py index 3c8317a..5a2a9f2 100644 --- a/src/arm_sim_gazebo/test/test_sim_cameras.py +++ b/src/arm_sim_gazebo/test/test_sim_cameras.py @@ -12,12 +12,17 @@ import pytest from arm_sim_gazebo.sim_cameras import ( MAX_MM, + MOUNT_TOL_DEG, CameraConfigError, CameraSpec, bridge_args, depth_m_to_mm16, + flange_frame_for, load_cameras, + mount_axis_deg, + mount_rpy_axis_deg, rpy_to_quat, + world_look_down_cos, ) @@ -507,3 +512,65 @@ def test_fov_check_against_real_repo_files(): sensors_xacro=root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro", ) assert issues == [], "视场与 SDF 不一致：\n" + "\n".join(issues) + + +# ------------------------------------------------------------------ 安装朝向 +def test_mount_axis_deg_zero_when_optical_axis_is_the_flange_axis(): + assert mount_axis_deg(np.eye(3)) == pytest.approx(0.0) + + +def test_mount_axis_deg_is_ninety_when_the_camera_is_short_one_quarter_turn(): + """少转 90° 正是踩过的那个坑：数值全正常，只是相机看着侧面。""" + R = np.array([[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]) # Ry(-90°) + assert mount_axis_deg(R) == pytest.approx(90.0) + + +def test_world_look_down_cos_sign_and_value(): + assert world_look_down_cos(np.eye(3)) == pytest.approx(1.0) + flip = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) # Rx(180°) + assert world_look_down_cos(flip) == pytest.approx(-1.0) + + +def test_flange_frame_for_wrist_camera_but_not_world_camera(): + wrist = CameraSpec( + gz_ns="/d405", + ros_ns="sim_d405", + link_frame="sim_d405_link", + optical_frame="sim_d405_color_optical_frame", + ) + assert flange_frame_for(wrist) == "sim_flange" + world = CameraSpec( + gz_ns="/d435", + ros_ns="sim_d435", + link_frame="sim_d435_link", + optical_frame="sim_d435_color_optical_frame", + world_model="d435", + ) + assert flange_frame_for(world) is None + + +def test_wrist_camera_mount_angle_in_the_shipped_xacro_is_zero(): + """仓库里那份 URDF 的腕相机安装角必须是"正装"。 + + Gazebo 的相机视轴是连杆的 +X，所以"正装在法兰上"= d405_pitch 为 -90°。 + 以前它是 0（视轴指向法兰 +X，也就是**水平**）：话题、图像、内参、深度 + 反投影全都正常，只有应用侧永远"目标不在相机前方"，极难归因。 + 这条测试把"默认值"钉住，改它必须是有意的。 + """ + import re + from pathlib import Path + + root = Path(__file__).resolve().parents[3] + xacro = (root / "devices/sim/gazebo/urdf/arm.sim.sensors.xacro").read_text( + encoding="utf-8" + ) + m = re.search(r"d405_pitch:=([-\d.]+)", xacro) + assert m, "xacro 里找不到 d405_pitch 的默认值" + pitch = float(m.group(1)) + ang = mount_rpy_axis_deg((0.0, pitch, 0.0)) + assert ang <= MOUNT_TOL_DEG, ( + f"腕相机安装角 {ang:.1f}° 超过 {MOUNT_TOL_DEG}° —— " + f"法兰垂直朝下时它看不到工件（见 sensors.xacro 的说明）" + ) + # 对照：老默认值（pitch=0）必须被判不合格，否则这条测试等于没测 + assert mount_rpy_axis_deg((0.0, 0.0, 0.0)) > MOUNT_TOL_DEG diff --git a/src/arm_sim_gazebo/test/test_sim_scene.py b/src/arm_sim_gazebo/test/test_sim_scene.py index c1ccc0f..99f05bb 100644 --- a/src/arm_sim_gazebo/test/test_sim_scene.py +++ b/src/arm_sim_gazebo/test/test_sim_scene.py @@ -265,7 +265,9 @@ def test_load_scene_pose_not_number_raises(tmp_path): def test_load_scene_operating_defaults(tmp_path): _write_scene(tmp_path, [], operating={}) spec = load_scene(root=tmp_path) - assert spec.operating.camera == "d405" + # 默认值 = 仿真腕部相机名（sim_ 前缀）。改名时这里必须一起改 —— + # 它同时钉住"默认值就是仿真那台"，避免有人以为默认是 d405_left（真机名）。 + assert spec.operating.camera == "sim_d405" assert spec.operating.part_object == "workpiece" assert spec.operating.camera_to_part_m == 0.20 diff --git a/src/arm_sim_gazebo/test/test_sim_truth_perception.py b/src/arm_sim_gazebo/test/test_sim_truth_perception.py new file mode 100644 index 0000000..d873875 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_sim_truth_perception.py @@ -0,0 +1,255 @@ +"""``sim_truth_perception`` 的测试：真值目标点怎么算、什么时候**不发**。 + +为什么值得单独一套测试 +---------------------- +这个节点替掉的是 perception_node —— 也就是说，它一旦算错，错的不是一个 +显示值，而是**伺服的目标**： + +* 目标点算错 12mm（比如把"物体原点"当成"上表面"）→ 全程贴着工件下不去， + 现象是"定位偏差"、像手眼标定不准； +* 目标点算在相机背后还照发 → 伺服朝反方向走，现象是"越追越远"； +* ``offset_m`` 不跟着 yaw 转 → 换了工件摆放角度后偏到工件外面。 + +三类都是**静默错**：不报错、日志漂亮、结果错。所以这里逐条钉住，而不是 +只测"能不能算出个数"。 + +顶部那条 golden（``test_shipped_scene_*``）会随 scene.yaml / 模型板厚变化 +而失败 —— 这是**故意**的：工件上表面高度是流程参数（悬停高、跟踪高）的 +推导依据，它改了就该有人来看一眼，而不是悄悄漂过去。 +""" + +from __future__ import annotations + +import inspect +import math +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from arm_sim_gazebo import sim_scene +from arm_sim_gazebo.sim_truth_perception import ( + MIN_CAM_Z_M, + TruthError, + apply_transform, + load_truth_target, + matrix_from_transform, + pick_object, + target_point_world, +) + +MODELS_REL = "devices/sim/gazebo/models" + + +# --------------------------------------------------------------------------- +# 迷你仓库（不依赖真实模型文件，数值可以先手算出来） +# --------------------------------------------------------------------------- +def _model(root: Path, name: str, size=(0.14, 0.10, 0.012)) -> Path: + """一个"原点在底面中心"的板子（本框架的建模约定）。""" + d = root / MODELS_REL / name + d.mkdir(parents=True, exist_ok=True) + (d / "model.sdf").write_text( + f""" + + + + + 0 0 {size[2] / 2.0} 0 0 0 + {size[0]} {size[1]} {size[2]} + + + + +""", + encoding="utf-8", + ) + return d + + +def _scene(root: Path, objects, operating=None) -> Path: + import yaml + + cfg = root / "devices/sim/gazebo/config" + cfg.mkdir(parents=True, exist_ok=True) + doc = { + "models_dir": MODELS_REL, + "table_top_z": 0.40, + "table_bounds": {"x": [0.15, 0.95], "y": [-0.60, 0.60]}, + "objects": objects, + } + if operating is not None: + doc["operating"] = operating + p = cfg / "scene.yaml" + p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + return p + + +def _obj(name="workpiece", x=0.5, y=0.0, z=0.40, yaw=0.0) -> dict: + return {"name": name, "model": name, "pose": {"x": x, "y": y, "z": z, "yaw": yaw}} + + +# --------------------------------------------------------------------------- +# 目标点 = 顶面中心 +# --------------------------------------------------------------------------- +def test_target_is_top_center_not_origin(tmp_path): + """原点在**底面**，真值点要在**顶面** —— 差一个板厚，正是最容易漏的地方。""" + _model(tmp_path, "workpiece") # 板厚 12mm + p = _scene(tmp_path, [_obj()]) + _spec, tgt = load_truth_target(p, "", (0.0, 0.0, 0.0), root=tmp_path) + assert tgt.world_xyz[0] == pytest.approx(0.5) + assert tgt.world_xyz[1] == pytest.approx(0.0) + # 0.40（原点/台面）+ 0.012（板厚）= 0.412；写成 0.40 就会低一个板厚 + assert tgt.world_xyz[2] == pytest.approx(0.412) + assert "12.0mm" in tgt.note + + +def test_default_object_follows_scene_part_object(tmp_path): + """``target_object`` 空 = 场景里的 operating.part_object（不是写死 'workpiece'）。""" + _model(tmp_path, "plate") + p = _scene( + tmp_path, + [_obj("plate", y=0.2)], + operating={"camera": "sim_d405", "part_object": "plate"}, + ) + _spec, tgt = load_truth_target(p, "", (0.0, 0.0, 0.0), root=tmp_path) + assert tgt.object_name == "plate" + assert tgt.world_xyz[1] == pytest.approx(0.2) + + +def test_offset_is_in_object_frame_so_yaw_rotates_it(tmp_path): + """偏移必须跟着 yaw 转：否则换了摆放角度就偏到工件外面。 + + 局部 +x 偏 30mm，yaw=90° ⇒ 世界系里应落在 +y 方向。 + """ + _model(tmp_path, "workpiece") + p = _scene(tmp_path, [_obj(yaw=math.pi / 2.0)]) + _spec, tgt = load_truth_target(p, "", (0.03, 0.0, 0.0), root=tmp_path) + assert tgt.world_xyz[0] == pytest.approx(0.5, abs=1e-9) + assert tgt.world_xyz[1] == pytest.approx(0.03, abs=1e-9) + + +def test_offset_can_target_a_hole(tmp_path): + """把目标挪到孔上（验证二阶段对孔用），z 偏移是相对顶面的。""" + _model(tmp_path, "workpiece") + p = _scene(tmp_path, [_obj()]) + _spec, tgt = load_truth_target(p, "", (0.0, -0.029, 0.0), root=tmp_path) + assert tgt.world_xyz[2] == pytest.approx(0.412) + assert tgt.world_xyz[1] == pytest.approx(-0.029) + + +# --------------------------------------------------------------------------- +# 出错时要给出可执行的修法 +# --------------------------------------------------------------------------- +def test_unknown_object_lists_what_is_available(tmp_path): + _model(tmp_path, "workpiece") + p = _scene(tmp_path, [_obj()]) + with pytest.raises(TruthError) as e: + load_truth_target(p, "not_there", (0.0, 0.0, 0.0), root=tmp_path) + msg = str(e.value) + assert "not_there" in msg + assert "workpiece" in msg # 现有物体列出来，改配置不用翻文件 + assert "perception.truth.object" in msg + + +def test_model_without_collision_is_an_error_not_a_guess(tmp_path): + """没有碰撞体就**没有**几何 → 报错，不要拿别的数凑一个点出来。""" + d = tmp_path / MODELS_REL / "empty" + d.mkdir(parents=True) + (d / "model.sdf").write_text( + '', + encoding="utf-8", + ) + p = _scene(tmp_path, [_obj("empty")]) + with pytest.raises(TruthError) as e: + load_truth_target(p, "empty", (), root=tmp_path) + assert "model.sdf" in str(e.value) + + +def test_pick_object_reports_scene_path(tmp_path): + _model(tmp_path, "workpiece") + p = _scene(tmp_path, [_obj()]) + spec = sim_scene.load_scene(p, root=tmp_path) + with pytest.raises(TruthError) as e: + pick_object(spec, "nope") + assert str(p) in str(e.value) + + +# --------------------------------------------------------------------------- +# 变换数学 +# --------------------------------------------------------------------------- +def test_apply_transform_identity_translation_rotation(): + import numpy as np + + I = np.eye(4) + assert apply_transform(I, (1.0, 2.0, 3.0)) == pytest.approx((1.0, 2.0, 3.0)) + + T = np.eye(4) + T[:3, 3] = [0.1, -0.2, 0.3] + assert apply_transform(T, (1.0, 2.0, 3.0)) == pytest.approx((1.1, 1.8, 3.3)) + + R = np.eye(4) + R[:3, :3] = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] # 绕 z 转 +90° + assert apply_transform(R, (1.0, 0.0, 0.0)) == pytest.approx((0.0, 1.0, 0.0)) + + +def test_matrix_from_transform_uses_the_shared_quaternion_math(): + """守住"数学只有一份"：四元数→旋转矩阵必须走 handeye_lib，不许在这里再抄一遍。 + + 以前这段在 4 个 launch 文件里各写一遍、错了 3 处，症状是"总能到附近、 + 永远差一点"，被当成手眼标定不准查了很久。 + """ + src = inspect.getsource(matrix_from_transform) + assert "quat_xyzw_to_R" in src, "四元数换算必须复用 handeye_lib.quat_xyzw_to_R" + assert "cos(" not in src and "sin(" not in src, "这里不该出现手写的三角换算" + + +def test_matrix_from_transform_matches_rotation_about_z(): + pytest.importorskip("arm_application.handeye_lib") + import numpy as np + + s = math.sin(math.pi / 4.0) # 90° → 四元数 (0,0,sin45,cos45) + tr = SimpleNamespace( + transform=SimpleNamespace( + translation=SimpleNamespace(x=1.0, y=2.0, z=3.0), + rotation=SimpleNamespace(x=0.0, y=0.0, z=s, w=s), + ) + ) + T = matrix_from_transform(tr) + assert T[:3, 3] == pytest.approx((1.0, 2.0, 3.0)) + assert (T[:3, :3] @ np.array([1.0, 0.0, 0.0])) == pytest.approx((0.0, 1.0, 0.0), abs=1e-9) + + +# --------------------------------------------------------------------------- +# 发行版自带的场景：钉住"工件上表面到底在哪" +# --------------------------------------------------------------------------- +def test_shipped_scene_workpiece_top_is_12mm_above_the_table(): + """官方场景里真值点必须落在**工件上表面**（台面 + 板厚 12mm）。 + + 这条会随 scene.yaml / 模型板厚变化而失败 —— 故意的： + 上表面高度是悬停高、跟踪高的推导依据，动了就该有人看一眼再更新别处。 + """ + spec, tgt = load_truth_target(None, "workpiece", (0.0, 0.0, 0.0)) + obj = pick_object(spec, "workpiece") + assert tgt.world_xyz[2] == pytest.approx(obj.z + 0.012, abs=2e-3), ( + f"真值点 z={tgt.world_xyz[2]:.4f} 与 scene.yaml 里工件位姿 " + f"z={obj.z:.4f} + 板厚 12mm 对不上：模型的板厚或摆放改了？" + ) + # 台面之上、工作空间之内（真值是"看得见、够得着"的点） + assert spec.table_top_z < tgt.world_xyz[2] < 0.65 + b = spec.table_bounds + assert b.x_min < tgt.world_xyz[0] < b.x_max + assert b.y_min < tgt.world_xyz[1] < b.y_max + + +def test_offset_length_is_checked_not_guessed(tmp_path): + """长度不对必须报错，不能"缺的当 0" —— 猜错的表现是"偏了一点"，很难归因。""" + _model(tmp_path, "workpiece") + p = _scene(tmp_path, [_obj()]) + with pytest.raises(TruthError) as e: + load_truth_target(p, "", (0.03,), root=tmp_path) + assert "3 个数" in str(e.value) + + +def test_camera_backside_guard_is_positive(): + """背后判据用 z<=0，且阈值是个正数（写成 0 会把贴镜头的点也发出去）。""" + assert MIN_CAM_Z_M > 0.0 diff --git a/src/arm_sim_gazebo/test/test_truth_holes.py b/src/arm_sim_gazebo/test/test_truth_holes.py new file mode 100644 index 0000000..892f599 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_truth_holes.py @@ -0,0 +1,295 @@ +"""``truth_holes`` 的测试：真值孔位怎么投影、什么时候**不给**结果。 + +为什么这套测试重要：真值孔位是仿真里二阶段唯一的"测量来源"。它算错**不会 +报错**，只会让 S05 对准一个偏掉的点，然后现象出现在很远的地方（拧不进去 / +掉件重放 / "标定好像不准"）。所以这里用一支手搓相机把几何关系钉死： +相机正对工件时，孔必须落在哪、间距必须等于多少像素，全都算得出来。 + +另外两类必须钉住： + * **不给幻影**：孔在相机背后时返回 ok=False（给伺服一个背后的目标会让 + 机械臂往错误方向走）。 + * **环数不是 2 就不猜**：模型改了（多一个环/少一个环）必须报"几个环"， + 而不是默默取前两个。 +""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest +import yaml + +from arm_sim_gazebo import sim_scene +from arm_sim_gazebo.sim_scene import SceneObject +from arm_sim_gazebo.truth_holes import locate_holes, object_bbox_px, project_point + +MODELS_REL = "devices/sim/gazebo/models" + + +# ---------------------------------------------------------------- 迷你仓库 +def _box_on_floor(name: str, size, xy=(0.0, 0.0)) -> str: + x, y = xy + return ( + f' \n' + f' {x} {y} {size[2] / 2.0} 0 0 0\n' + f" {size[0]} {size[1]} {size[2]}\n" + f" " + ) + + +def _cyl_visual(name: str, radius: float, pose="0 0 0.001 0 0 0") -> str: + return ( + f' \n' + f" {pose}\n" + f" {radius}0.001" + f"\n" + f" " + ) + + +def _write_model(root, name: str, collisions, visuals) -> None: + d = root / MODELS_REL / name + d.mkdir(parents=True, exist_ok=True) + (d / "model.sdf").write_text( + f""" + + + true + +{chr(10).join(collisions)} +{chr(10).join(visuals)} + + + +""", + encoding="utf-8", + ) + (d / "model.config").write_text( + f""" + + {name} + 1.0 + model.sdf + +""", + encoding="utf-8", + ) + + +def _write_scene(root, objects) -> None: + cfg = root / "devices/sim/gazebo/config" + cfg.mkdir(parents=True, exist_ok=True) + (cfg / "scene.yaml").write_text( + yaml.safe_dump( + { + "models_dir": MODELS_REL, + "table_top_z": 0.40, + "table_bounds": {"x": [0.15, 0.95], "y": [-0.60, 0.60]}, + "operating": {"camera": "d405", "part_object": "workpiece", + "camera_to_part_m": 0.17, "camera_offset_below_flange_m": 0.05}, + "objects": objects, + }, + allow_unicode=True, + ), + encoding="utf-8", + ) + + +# 工件：140×100×12 的板，两个嵌件环在局部 x=∓29mm、半径 7mm（与真机一致） +PART_SIZE = (0.140, 0.100, 0.012) +RING_R = 0.007 +RING_X = 0.029 + + +def _build_repo(root, rings=((-RING_X, 0.0), (RING_X, 0.0)), model="workpiece"): + """建一个迷你仓库：模型 + 场景。``rings`` 是各环的局部 (x, y)。""" + visuals = [_cyl_visual(f"insert_{i}_ring", RING_R, f"{x} {y} 0.010 0 0 0") + for i, (x, y) in enumerate(rings)] + visuals.append(_cyl_visual("center_bore", 0.020, "0 0 0.007 0 0 0")) + _write_model(root, model, [_box_on_floor("body", PART_SIZE)], visuals) + _write_scene(root, [{"name": "workpiece", "model": model, + "pose": {"x": 0.50, "y": 0.0, "z": 0.40, "yaw": 0.0}}]) + return sim_scene.load_scene(root=root) + + +# ---------------------------------------------------------------- 手搓相机 +#: D405 的内参与图像尺寸（与仿真里一致），这样像素数可以直接对比现场日志。 +FX = FY = 446.819 +CX, CY = 424.0, 240.0 +IMAGE = (480, 848) +INTR = (FX, FY, CX, CY) + + +def _cam_looking_down(pos_world) -> np.ndarray: + """一支正朝下看的相机（光轴 = 世界 -z，像面 x = 世界 x）→ T_cam_world。 + + 为什么用这支"理想相机"而不是从 TF 查：这是**纯几何测试**，要的是 + "算出来的像素是否符合解析预期"，TF 正确性由现场脚本 + （``scripts/check_sim_cameras.py``）负责。 + """ + R = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = -R @ np.array([float(pos_world[0]), float(pos_world[1]), float(pos_world[2])]) + return T + + +# ---------------------------------------------------------------- 环的解析 +def test_rings_local_reads_only_cylindrical_ring_visuals(tmp_path): + _build_repo(tmp_path) + spec = sim_scene.load_scene(root=tmp_path) + obj = spec.objects[0] + rings = sim_scene.rings_local(spec, obj, tmp_path) + assert [r.name for r in rings] == ["insert_0_ring", "insert_1_ring"] + assert rings[0].x == pytest.approx(-RING_X) + assert rings[1].x == pytest.approx(RING_X) + assert rings[0].z == pytest.approx(0.010) + assert rings[0].radius == pytest.approx(RING_R) + + +def test_rings_local_skips_ring_without_pose(tmp_path): + """没有 pose 的环**不收**：拿 (0,0,0) 当圆心会让"两孔间距"变成 0。""" + _write_model( + tmp_path, + "workpiece", + [_box_on_floor("body", PART_SIZE)], + [ + '' + "0.0070.001" + "", + _cyl_visual("insert_right_ring", RING_R, f"{RING_X} 0 0.010 0 0 0"), + ], + ) + _write_scene(tmp_path, [{"name": "workpiece", "model": "workpiece", + "pose": {"x": 0.5, "y": 0.0, "z": 0.4, "yaw": 0.0}}]) + spec = sim_scene.load_scene(root=tmp_path) + rings = sim_scene.rings_local(spec, spec.objects[0], tmp_path) + assert [r.name for r in rings] == ["insert_right_ring"] + + +def test_features_and_rings_agree_on_spacing(tmp_path): + """两处解析必须一致：``features_from_workpiece`` 说"该看到 58mm"， + ``rings_local`` 就得摆出 58mm —— 漂移了会出现"门槛说看得到、却投影不到那儿"。""" + _build_repo(tmp_path) + spec = sim_scene.load_scene(root=tmp_path) + obj = spec.objects[0] + feats = sim_scene.features_from_workpiece(spec, obj, tmp_path) + rings = sim_scene.rings_local(spec, obj, tmp_path) + assert feats["hole_spacing"] == pytest.approx( + math.hypot(rings[0].x - rings[1].x, rings[0].y - rings[1].y) + ) + assert feats["insert_ring_radius"] == pytest.approx(rings[-1].radius) + + +# ---------------------------------------------------------------- 投影 +def test_project_point_refuses_behind_camera(): + T = _cam_looking_down((0.0, 0.0, 1.0)) + # 相机朝下看：世界 z=0.5 的点在相机下方 → 正常；z=1.5 的点在相机**背后** + assert project_point(T, INTR, (0.0, 0.0, 0.5)) is not None + assert project_point(T, INTR, (0.0, 0.0, 1.5)) is None + + +def test_locate_holes_projects_the_expected_pixels(tmp_path): + spec = _build_repo(tmp_path) + # 相机在工件上方 0.222m（= 环 z 0.410 与相机 0.632 的差，仿真里的作业距离） + T = _cam_looking_down((0.50, 0.0, 0.632)) + res = locate_holes(T, INTR, IMAGE, spec=spec, root=tmp_path) + assert res.ok, res.why + + # 间距：58mm 在 0.222m 处 = fx * 0.058 / 0.222 + want = FX * (2 * RING_X) / 0.222 + got = math.hypot( + res.hole_points[0][0] - res.hole_points[1][0], + res.hole_points[0][1] - res.hole_points[1][1], + ) + assert got == pytest.approx(want, rel=0.01) + # 环半径：7mm 在 0.222m 处 + assert res.hole_radii[0] == pytest.approx(FX * RING_R / 0.222, rel=0.01) + # 水平对 → 按图像 x 排序（k1=左） + assert res.hole_points[0][0] < res.hole_points[1][0] + assert res.hole_points[0][1] == pytest.approx(res.hole_points[1][1], abs=1.0) + # 两个孔都在画框里，而且近画面中心（相机正对工件中心） + for (u, v) in res.hole_points: + assert 0 <= u < IMAGE[1] and 0 <= v < IMAGE[0] + assert abs(u - CX) < 0.35 * IMAGE[1] + + +def test_locate_holes_bbox_covers_the_part(tmp_path): + spec = _build_repo(tmp_path) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert res.ok, res.why + x1, y1, x2, y2 = res.bbox + # 140mm × 100mm 的板在 0.22m 处 ≈ 284px × 203px + assert (x2 - x1) == pytest.approx(FX * PART_SIZE[0] / 0.222, rel=0.06) + assert (y2 - y1) == pytest.approx(FY * PART_SIZE[1] / 0.222, rel=0.06) + # 孔必须落在工件的框里（否则 ROI 会把孔切掉） + for (u, v) in res.hole_points: + assert x1 < u < x2 and y1 < v < y2 + + +def test_locate_holes_orders_vertical_pair_by_y(tmp_path): + """工件转 90° 后两孔在图上变成竖排 → 按 y 排序（k1=上），与 Hough 路径同规则。""" + _build_repo(tmp_path) + cfg = tmp_path / "devices/sim/gazebo/config/scene.yaml" + doc = yaml.safe_load(cfg.read_text(encoding="utf-8")) + doc["objects"][0]["pose"]["yaw"] = math.pi / 2 + cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8") + spec = sim_scene.load_scene(root=tmp_path) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert res.ok, res.why + assert res.hole_points[0][1] < res.hole_points[1][1] # 上者在前 + assert res.hole_points[0][0] == pytest.approx(res.hole_points[1][0], abs=1.0) + + +def test_locate_holes_says_why_when_camera_is_below_the_part(tmp_path): + spec = _build_repo(tmp_path) + # 相机位置比工件上表面还低 → 环在相机背后 + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.30)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert not res.ok + assert "背后" in res.why + + +def test_locate_holes_requires_exactly_two_rings(tmp_path): + spec = _build_repo(tmp_path, rings=((0.0, 0.0),)) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, root=tmp_path) + assert not res.ok + assert "1 个" in res.why + + +def test_locate_holes_without_tf_says_so(tmp_path): + _build_repo(tmp_path) + res = locate_holes(None, INTR, IMAGE, root=tmp_path) + assert not res.ok + assert "TF" in res.why + + +def test_locate_holes_unknown_object_lists_existing(tmp_path): + spec = _build_repo(tmp_path) + res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE, + spec=spec, object_name="没有这个", root=tmp_path) + assert not res.ok + assert "workpiece" in res.why # 现有名字要列出来，省得再去翻场景文件 + + +def test_object_bbox_px_returns_none_when_everything_is_behind(tmp_path): + spec = _build_repo(tmp_path) + obj = spec.objects[0] + boxes = sim_scene.world_boxes(spec, obj, tmp_path) + T = _cam_looking_down((0.50, 0.0, -1.0)) # 相机在台面下、朝下 → 工件在背后 + assert object_bbox_px(T, INTR, boxes, IMAGE) is None + + +def test_object_bbox_px_clips_to_image(): + """框必须裁到画面内：给 Hough 一个超出画面的 ROI，裁剪会静默改变 ROI 尺寸。""" + spec_box = [(0.0, 10.0, 0.0, 10.0, 0.0, 0.01)] + T = _cam_looking_down((0.0, 0.0, 1.0)) + bbox = object_bbox_px(T, INTR, spec_box, IMAGE) + assert bbox is not None + x1, y1, x2, y2 = bbox + assert 0 <= x1 <= x2 <= IMAGE[1] - 1 + assert 0 <= y1 <= y2 <= IMAGE[0] - 1