克隆后还缺仿真安装树和两边的编译目录。把 build、build_sim、install_sim 一并放进仓库；仿真入口不再写死本机 conda，build 里指向源码的链接改成相对路径。

Co-authored-by: Cursor <cursoragent@cursor.com>

build_sim/arm_application/arm_application.egg-info/PKG-INFO

@@ -0,0 +1,13 @@
+Metadata-Version: 2.4
+Name: arm_application
+Version: 0.1.0
+Summary: High-level arm application nodes.
+Maintainer: arm_dev
+Maintainer-email: dev@example.com
+License: Apache-2.0
+Requires-Dist: setuptools
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary

build_sim/arm_application/arm_application.egg-info/SOURCES.txt

@@ -0,0 +1,105 @@
+README.md
+package.xml
+setup.cfg
+setup.py
+../../build_sim/arm_application/arm_application.egg-info/PKG-INFO
+../../build_sim/arm_application/arm_application.egg-info/SOURCES.txt
+../../build_sim/arm_application/arm_application.egg-info/dependency_links.txt
+../../build_sim/arm_application/arm_application.egg-info/entry_points.txt
+../../build_sim/arm_application/arm_application.egg-info/requires.txt
+../../build_sim/arm_application/arm_application.egg-info/top_level.txt
+../../build_sim/arm_application/arm_application.egg-info/zip-safe
+arm_application/__init__.py
+arm_application/box_tracker.py
+arm_application/demo_named_poses.py
+arm_application/dh_official.py
+arm_application/dh_pgea.py
+arm_application/dh_pgea_node.py
+arm_application/dual_gripper.py
+arm_application/dual_gripper_cli.py
+arm_application/dual_gripper_node.py
+arm_application/gauge_assembly_node.py
+arm_application/gauge_boot.py
+arm_application/gauge_config.py
+arm_application/gauge_fsm.py
+arm_application/gauge_gripper.py
+arm_application/gauge_kin.py
+arm_application/geom_ik.py
+arm_application/goto_ee_plane.py
+arm_application/handeye_calibrate.py
+arm_application/handeye_lib.py
+arm_application/handeye_realsense.py
+arm_application/handeye_teach.py
+arm_application/handeye_waypoints.py
+arm_application/hole_detect_hough.py
+arm_application/hole_detect_node.py
+arm_application/hole_detector.py
+arm_application/image_utils.py
+arm_application/kalman_filter.py
+arm_application/model_labels.py
+arm_application/move_to_pose_client.py
+arm_application/onnx_workpiece.py
+arm_application/orisys_tactile.py
+arm_application/perception_node.py
+arm_application/pid_controller.py
+arm_application/pipeline_node.py
+arm_application/plane_feature.py
+arm_application/realsense_color_pub.py
+arm_application/realsense_sdk_view.py
+arm_application/realsense_view.py
+arm_application/ros_node.py
+arm_application/run_joint_trajectory.py
+arm_application/servo_node.py
+arm_application/site_io.py
+arm_application/stage2_locate_node.py
+arm_application/stage3_search_node.py
+arm_application/tactile_cli.py
+arm_application/tactile_interface.py
+arm_application/tactile_node.py
+arm_application/weights_layout.py
+arm_application/yolo_detector.py
+config/demo.yaml
+config/dual_grippers.yaml
+config/hole_detect.yaml
+config/hole_insert.yaml
+config/perception.yaml
+config/servo.yaml
+config/xunhuan_trajectory.yaml
+launch/demo.launch.py
+launch/dh_pgea.launch.py
+launch/dual_gripper.launch.py
+launch/gauge_assembly.launch.py
+launch/hole_detect.launch.py
+launch/pipeline.launch.py
+launch/stage2_locate.launch.py
+launch/stage3_search.launch.py
+launch/tactile.launch.py
+launch/visual_servo.launch.py
+resource/arm_application
+test/test_gauge_boot.py
+test/test_gauge_config.py
+test/test_gauge_exit_lift.py
+test/test_gauge_fsm.py
+test/test_gauge_gripper.py
+test/test_gauge_gripper_client.py
+test/test_gauge_kin.py
+test/test_gauge_result_gate.py
+test/test_gauge_step_events.py
+test/test_handeye_lib.py
+test/test_handeye_lib_roundtrip.py
+test/test_hole_detector.py
+test/test_image_utils.py
+test/test_model_labels.py
+test/test_node_handler_collisions.py
+test/test_orisys_vector.py
+test/test_pipeline_stage1_diag.py
+test/test_real_geometry_check.py
+test/test_real_preflight.py
+test/test_ros_domain.py
+test/test_ros_node.py
+test/test_site_arm_sections.py
+test/test_stage3_tactile_live.py
+test/test_usb_health.py
+test/test_v4l_health.py
+test/test_weights_layout.py
+trajectories/xunhuan.traj

build_sim/arm_application/arm_application.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

build_sim/arm_application/arm_application.egg-info/entry_points.txt

@@ -0,0 +1,23 @@
+[console_scripts]
+assembly_pipeline = arm_application.pipeline_node:main
+demo_named_poses = arm_application.demo_named_poses:main
+dh_pgea = arm_application.dh_pgea_node:main
+dh_pgea_cli = arm_application.dh_pgea:main
+dual_gripper = arm_application.dual_gripper_node:main
+dual_gripper_cli = arm_application.dual_gripper_cli:main
+gauge_assembly = arm_application.gauge_assembly_node:main
+goto_ee_plane = arm_application.goto_ee_plane:main
+handeye_calibrate = arm_application.handeye_calibrate:main
+handeye_teach = arm_application.handeye_teach:main
+handeye_waypoints = arm_application.handeye_waypoints:main
+hole_detect = arm_application.hole_detect_node:main
+move_to_pose_client = arm_application.move_to_pose_client:main
+perception_node = arm_application.perception_node:main
+realsense_color_pub = arm_application.realsense_color_pub:main
+realsense_view = arm_application.realsense_view:main
+run_joint_trajectory = arm_application.run_joint_trajectory:main
+servo_node = arm_application.servo_node:main
+stage2_locate = arm_application.stage2_locate_node:main
+stage3_search = arm_application.stage3_search_node:main
+tactile = arm_application.tactile_node:main
+tactile_cli = arm_application.tactile_cli:main

build_sim/arm_application/arm_application.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

build_sim/arm_application/arm_application.egg-info/top_level.txt

@@ -0,0 +1 @@
+arm_application

build_sim/arm_application/arm_application.egg-info/zip-safe

@@ -0,0 +1 @@
+

build_sim/arm_application/build/lib/arm_application/__init__.py

@@ -0,0 +1,32 @@
+"""arm_application —— Flexiv Rizon + D405 + DH 夹爪的现场节点包。
+
+对外可直接运行的 ROS 程序（见 ``setup.py`` 的 ``console_scripts``）
+------------------------------------------------------------------
+相机：      realsense_color_pub
+感知：      perception_node
+伺服：      servo_node
+手眼：      handeye_teach、handeye_waypoints、handeye_calibrate
+夹爪：      dh_pgea、dh_pgea_cli、dual_gripper、dual_gripper_cli
+入孔检测：  hole_detect
+触觉：      tactile、tactile_cli
+辅助：      goto_ee_plane、demo_named_poses、run_joint_trajectory
+
+可复用的 Python 模块（可以在你自己的节点里 import）
+------------------------------------------------------------------
+``yolo_detector``     YOLO / TensorRT 推理 + ``pick_best`` 挑最优框
+``box_tracker``       像素平面内的匀速模型框预测（丢帧时"滑行"用）
+``plane_feature``     在 YOLO 框内取立体平面的质心
+``dh_pgea``           大寰 PGEA 的官方 Modbus RTU 客户端
+``dh_official``       ``dual_gripper`` 用的 DH Modbus 客户端
+``dual_gripper``      左 PGEA + 右 RGI
+``hole_detector``     yolo / hough / two_stage 三种孔心检测
+``orisys_tactile``    Orisys 视触觉适配器
+``geom_ik``           Rizon 几何雅可比 / DLS / 手眼加载
+``handeye_lib``       棋盘格 + OpenCV ``calibrateHandEye``
+``handeye_realsense`` D405 的话题 / 序列号辅助
+``pid_controller``    带积分分离的 PID
+``kalman_filter``     三维匀速模型卡尔曼滤波
+
+现场操作请走 ``scripts/*.sh`` 和 ``config/site.yaml``，不要直接 import 这些模块。
+接口契约见 ``docs/INTERFACES.md``。
+"""

build_sim/arm_application/build/lib/arm_application/box_tracker.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+"""针对 YOLO ``xyxy`` 框（像素坐标系）的匀速跟踪器。
+
+用于避免工件快速运动时丢失锁定：下一帧的闸门使用*预测*框，
+短暂的 YOLO 漏检则由惯性滑行（coast）维持叠加显示。
+"""
+
+from __future__ import annotations
+
+from typing import Optional, Tuple
+
+import numpy as np
+
+
+def _center(xyxy: np.ndarray) -> np.ndarray:
+    return np.array(
+        [0.5 * (float(xyxy[0]) + float(xyxy[2])), 0.5 * (float(xyxy[1]) + float(xyxy[3]))],
+        dtype=float,
+    )
+
+
+def _wh(xyxy: np.ndarray) -> Tuple[float, float]:
+    return (
+        max(8.0, float(xyxy[2]) - float(xyxy[0])),
+        max(8.0, float(xyxy[3]) - float(xyxy[1])),
+    )
+
+
+class BoxTracker:
+    def __init__(
+        self,
+        *,
+        vmax_px_s: float = 1800.0,
+        v_lpf: float = 0.70,
+        max_dt: float = 0.25,
+    ) -> None:
+        self.vmax_px_s = float(vmax_px_s)
+        self.v_lpf = float(np.clip(v_lpf, 0.05, 1.0))
+        self.max_dt = float(max_dt)
+        self.xyxy: Optional[np.ndarray] = None
+        self.v = np.zeros(2, dtype=float)  # 中心点 px/s
+        self.t: Optional[float] = None
+
+    def reset(self) -> None:
+        self.xyxy = None
+        self.v[:] = 0.0
+        self.t = None
+
+    def predict(self, t: float, img_wh: Optional[Tuple[int, int]] = None) -> Optional[np.ndarray]:
+        if self.xyxy is None:
+            return None
+        out = self.xyxy.copy()
+        if self.t is None:
+            return out
+        dt = float(np.clip(t - self.t, 0.0, self.max_dt))
+        if dt <= 1e-4:
+            return out
+        c = _center(out) + self.v * dt
+        w, h = _wh(out)
+        x1, y1 = c[0] - 0.5 * w, c[1] - 0.5 * h
+        x2, y2 = c[0] + 0.5 * w, c[1] + 0.5 * h
+        if img_wh is not None:
+            iw, ih = float(img_wh[0]), float(img_wh[1])
+            x1 = float(np.clip(x1, 0.0, iw - 2.0))
+            y1 = float(np.clip(y1, 0.0, ih - 2.0))
+            x2 = float(np.clip(x2, x1 + 4.0, iw))
+            y2 = float(np.clip(y2, y1 + 4.0, ih))
+        return np.array([x1, y1, x2, y2], dtype=float)
+
+    def update(self, xyxy: np.ndarray, t: float) -> None:
+        xyxy = np.asarray(xyxy, dtype=float).reshape(4)
+        c = _center(xyxy)
+        if self.xyxy is not None and self.t is not None:
+            dt = float(t - self.t)
+            if dt > 0.28:
+                self.v[:] = 0.0
+            elif dt > 1e-3:
+                inst = (c - _center(self.xyxy)) / dt
+                n = float(np.hypot(inst[0], inst[1]))
+                if n > self.vmax_px_s > 0.0:
+                    inst = inst * (self.vmax_px_s / n)
+                a = self.v_lpf
+                self.v = a * inst + (1.0 - a) * self.v
+        self.xyxy = xyxy.copy()
+        self.t = float(t)

build_sim/arm_application/build/lib/arm_application/demo_named_poses.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+"""演示：通过 ``/set_named_pose`` 循环切换命名位姿（对任何后端都适用）。"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from rclpy.node import Node
+
+from arm_msgs.srv import SetNamedPose
+
+from arm_application.ros_node import TolerantNode
+
+
+class DemoNamedPoses(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('demo_named_poses')
+        self.declare_parameter(
+            'poses', ['home', 'ready', 'inspect', 'pos_left', 'home']
+        )
+        self.declare_parameter('velocity_scale', 0.5)
+        self.declare_parameter('dwell_sec', 0.3)
+        self.declare_parameter('loops', 1)
+        self.poses = list(
+            self.get_parameter('poses').get_parameter_value().string_array_value
+            or ['home', 'ready', 'inspect', 'home']
+        )
+        self.velocity_scale = float(self.get_parameter('velocity_scale').value)
+        self.dwell_sec = float(self.get_parameter('dwell_sec').value)
+        self.loops = int(self.get_parameter('loops').value)
+        self._cli = self.create_client(SetNamedPose, 'set_named_pose')
+
+    def run(self) -> int:
+        if not self._cli.wait_for_service(timeout_sec=15.0):
+            self.get_logger().error('``/set_named_pose`` 服务不可用')
+            return 1
+        for loop in range(max(self.loops, 1)):
+            self.get_logger().info(f'第 {loop + 1}/{self.loops} 轮')
+            for pose in self.poses:
+                req = SetNamedPose.Request()
+                req.pose_name = pose
+                req.velocity_scale = self.velocity_scale
+                req.acceleration_scale = self.velocity_scale
+                fut = self._cli.call_async(req)
+                deadline = time.monotonic() + 60.0
+                while rclpy.ok() and not fut.done():
+                    rclpy.spin_once(self, timeout_sec=0.05)
+                    if time.monotonic() > deadline:
+                        self.get_logger().error(f'{pose} 调用超时')
+                        return 2
+                res = fut.result()
+                if res is None or not res.success:
+                    self.get_logger().error(
+                        f'{pose} 执行失败：{res.message if res else "无响应"}'
+                    )
+                    return 3
+                self.get_logger().info(f'{pose}：{res.message}')
+                time.sleep(self.dwell_sec)
+        self.get_logger().info('演示结束')
+        return 0
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = DemoNamedPoses()
+    code = 1
+    try:
+        code = node.run()
+    except KeyboardInterrupt:
+        code = 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    raise SystemExit(code)
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/dh_official.py

@@ -0,0 +1,271 @@
+"""DH-Robotics 官方 Modbus RTU 控制（非 Flexiv RDK）。
+
+覆盖两类设备：
+- PGE/PGEA 平行夹爪：初始化 0x0100，力 0x0101，位置 0x0103（0~1000‰），速度 0x0104。
+- RGI 旋转夹爪：在平行夹爪寄存器基础上，增加旋转角度 0x0105、旋转速度 0x0107、
+  旋转力 0x0108；反馈增加旋转角度 0x0208、旋转初始化状态 0x020A、旋转状态 0x020B。
+
+说明：
+- RGI 手册示例中负角度使用“反码/one's complement”：+360 -> 0x0168，-360 -> 0xFE97。
+  因此这里默认按 one's complement 编解码；若后续厂家确认为 two's complement，只改
+  `_encode_signed` / `_decode_signed` 两处即可。
+- 写位置/角度寄存器后会立即运动，没有 ChangingTek SDK 的 0x0108 触发寄存器。
+"""
+
+from __future__ import annotations
+
+import threading
+import time
+from typing import Iterable
+
+
+# =============================================================================
+# DH-Robotics Modbus 寄存器映射
+# =============================================================================
+
+# 基础控制寄存器（保持寄存器，功能码 03/06/10）
+REG_INIT = 0x0100                 # 0x01 初始化，0xA5 完全初始化
+REG_FORCE = 0x0101                # 夹爪闭合力，20~100 %
+REG_POSITION = 0x0103             # 手指参考位置，0~1000 千分比
+REG_SPEED = 0x0104                # 手指开合速度，1~100 %
+
+# RGI 旋转控制寄存器
+REG_ROTATION_ANGLE = 0x0105       # 目标旋转角度，有符号角度值
+REG_ROTATION_SPEED = 0x0107       # 旋转速度，1~100 %
+REG_ROTATION_FORCE = 0x0108       # 旋转力/力矩，20~100 %
+
+# 反馈寄存器
+REG_INIT_STATE = 0x0200           # 0 未初始化，1 已初始化，2 初始化中
+REG_GRIPPER_STATE = 0x0201        # 0 运动中，1 已到位，2 已夹到物体，3 物体掉落
+REG_CURRENT_POSITION = 0x0202     # 当前手指位置，千分比
+REG_ROTATION_ANGLE_FB = 0x0208    # 当前旋转角度，有符号角度值
+REG_ROTATION_INIT_STATE = 0x020A  # 0 未初始化，1 已初始化，2 初始化中
+REG_ROTATION_STATE = 0x020B       # 0 运动中，1 已到位，2 受阻，3 曾经受阻
+
+# 配置寄存器
+REG_SAVE_PARAMS = 0x0300          # 写入 1 可把所有参数保存到 flash
+
+
+class DHModbusError(RuntimeError):
+    """DH Modbus 通信或协议错误。"""
+
+
+def _clamp_int(value: float | int, lo: int, hi: int) -> int:
+    return int(max(lo, min(hi, int(round(float(value))))))
+
+
+def _encode_signed(value: float | int) -> int:
+    """把有符号角度值编码为 DH 的反码（one's complement）寄存器值。
+
+    RGI 手册示例：+360 -> 0x0168，-360 -> 0xFE97。
+    """
+    value = _clamp_int(value, -32767, 32767)
+    if value >= 0:
+        return value & 0xFFFF
+    return (~abs(value)) & 0xFFFF
+
+
+def _decode_signed(raw: int) -> int:
+    """把 DH 的反码（one's complement）寄存器值解码为有符号整数。"""
+    raw = int(raw) & 0xFFFF
+    if raw & 0x8000:
+        mag = (~raw) & 0x7FFF
+        return -mag
+    return raw & 0x7FFF
+
+
+class DHModbusClient:
+    """面向 DH-Robotics 夹爪的最小线程安全 Modbus RTU 客户端。"""
+
+    def __init__(
+        self,
+        port: str,
+        slave_id: int = 1,
+        baudrate: int = 115200,
+        timeout: float = 0.2,
+        *,
+        bytesize: int = 8,
+        parity: str = "N",
+        stopbits: int = 1,
+    ) -> None:
+        try:
+            import minimalmodbus
+            import serial
+        except ImportError as exc:  # pragma: no cover - 取决于运行时环境
+            raise DHModbusError(
+                "缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial"
+            ) from exc
+
+        parity_map = {
+            "N": serial.PARITY_NONE,
+            "E": serial.PARITY_EVEN,
+            "O": serial.PARITY_ODD,
+        }
+        self.instrument = minimalmodbus.Instrument(port, int(slave_id))
+        self.instrument.serial.baudrate = int(baudrate)
+        self.instrument.serial.bytesize = int(bytesize)
+        self.instrument.serial.parity = parity_map.get(str(parity).upper(), serial.PARITY_NONE)
+        self.instrument.serial.stopbits = int(stopbits)
+        self.instrument.serial.timeout = float(timeout)
+        self.instrument.mode = minimalmodbus.MODE_RTU
+        self.instrument.clear_buffers_before_each_transaction = True
+        try:
+            self.instrument.serial.exclusive = True
+        except Exception:
+            pass
+        self._lock = threading.Lock()
+        self.port = port
+
+    def close(self) -> None:
+        with self._lock:
+            try:
+                self.instrument.serial.close()
+            except Exception:
+                pass
+
+    # ------------------------------------------------------------------
+    # 底层寄存器读写
+    # ------------------------------------------------------------------
+    def write_register(self, address: int, value: int) -> None:
+        value = _clamp_int(value, 0, 0xFFFF)
+        last_exc: Exception | None = None
+        for attempt in range(3):
+            try:
+                with self._lock:
+                    self.instrument.write_register(address, value, functioncode=6)
+                return
+            except Exception as exc:
+                last_exc = exc
+                time.sleep(0.05 * (attempt + 1))
+        raise DHModbusError(f"写寄存器 0x{address:04X}={value} 失败: {last_exc}") from last_exc
+
+    def read_register(self, address: int) -> int:
+        last_exc: Exception | None = None
+        for attempt in range(3):
+            try:
+                with self._lock:
+                    return int(self.instrument.read_register(address, functioncode=3))
+            except Exception as exc:
+                last_exc = exc
+                time.sleep(0.05 * (attempt + 1))
+        hint = ""
+        text = str(last_exc).lower()
+        if "multiple access" in text or "no data" in text:
+            hint = f"。串口 {getattr(self, 'port', '')} 可能被其它进程占用，先执行: fuser -k {getattr(self, 'port', '/dev/ttyUSB*')}"
+        raise DHModbusError(f"读寄存器 0x{address:04X} 失败: {last_exc}{hint}") from last_exc
+
+    def write_registers(self, address: int, values: Iterable[int]) -> None:
+        vals = [_clamp_int(v, 0, 0xFFFF) for v in values]
+        with self._lock:
+            self.instrument.write_registers(address, vals)
+
+    def read_registers(self, address: int, count: int) -> list[int]:
+        with self._lock:
+            return [int(v) for v in self.instrument.read_registers(address, count, functioncode=3)]
+
+    def write_signed_register(self, address: int, value: float | int) -> None:
+        self.write_register(address, _encode_signed(value))
+
+    def read_signed_register(self, address: int) -> int:
+        return _decode_signed(self.read_register(address))
+
+    # ------------------------------------------------------------------
+    # PGE/PGEA/RGI 通用控制
+    # ------------------------------------------------------------------
+    def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
+        """初始化/回零夹爪，并等待其就绪。"""
+        self.write_register(REG_INIT, 0xA5 if full else 0x01)
+        start = time.monotonic()
+        while time.monotonic() - start < timeout:
+            init_state = self.read_register(REG_INIT_STATE)
+            rot_ok = True
+            if rotation:
+                rot_ok = self.read_register(REG_ROTATION_INIT_STATE) == 1
+            if init_state == 1 and rot_ok:
+                return
+            time.sleep(0.1)
+        raise DHModbusError(f"夹爪初始化超时（>{timeout:.1f}s）")
+
+    def save_parameters(self) -> None:
+        self.write_register(REG_SAVE_PARAMS, 1)
+
+    def set_force_pct(self, force_pct: float | int) -> int:
+        value = _clamp_int(force_pct, 20, 100)
+        self.write_register(REG_FORCE, value)
+        return value
+
+    def set_speed_pct(self, speed_pct: float | int) -> int:
+        value = _clamp_int(speed_pct, 1, 100)
+        self.write_register(REG_SPEED, value)
+        return value
+
+    def set_position_per_mille(self, position: float | int) -> int:
+        value = _clamp_int(position, 0, 1000)
+        try:
+            cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
+        except Exception:
+            cmd = value
+        if cmd == value:
+            try:
+                cur = self.get_position_per_mille()
+            except Exception:
+                cur = value
+            nudge = cur + (40 if value >= cur else -40)
+            nudge = _clamp_int(nudge, 0, 1000)
+            if nudge == value:
+                nudge = 0 if value != 0 else 40
+            self.write_register(REG_POSITION, nudge)
+            time.sleep(0.06)
+        self.write_register(REG_POSITION, value)
+        return value
+
+    def get_position_per_mille(self) -> int:
+        return _clamp_int(self.read_register(REG_CURRENT_POSITION), 0, 1000)
+
+    def get_gripper_state(self) -> int:
+        return self.read_register(REG_GRIPPER_STATE)
+
+    def get_init_state(self) -> int:
+        return self.read_register(REG_INIT_STATE)
+
+    def hold_current_position(self) -> None:
+        """在没有专用停止寄存器的 DH 夹爪上尽力停止/保持。"""
+        self.set_position_per_mille(self.get_position_per_mille())
+
+    # ------------------------------------------------------------------
+    # RGI 旋转控制
+    # ------------------------------------------------------------------
+    def set_rotation_speed_pct(self, speed_pct: float | int) -> int:
+        value = _clamp_int(speed_pct, 1, 100)
+        self.write_register(REG_ROTATION_SPEED, value)
+        return value
+
+    def set_rotation_force_pct(self, force_pct: float | int) -> int:
+        value = _clamp_int(force_pct, 20, 100)
+        self.write_register(REG_ROTATION_FORCE, value)
+        return value
+
+    def set_rotation_angle_deg(self, angle_deg: float | int) -> int:
+        angle = _clamp_int(angle_deg, -32767, 32767)
+        self.write_signed_register(REG_ROTATION_ANGLE, angle)
+        return angle
+
+    def get_rotation_angle_raw(self) -> int:
+        return self.read_register(REG_ROTATION_ANGLE_FB) & 0xFFFF
+
+    def get_rotation_angle_deg(self) -> int:
+        return self.read_signed_register(REG_ROTATION_ANGLE_FB)
+
+    def get_rotation_init_state(self) -> int:
+        return self.read_register(REG_ROTATION_INIT_STATE)
+
+    def get_rotation_state(self) -> int:
+        return self.read_register(REG_ROTATION_STATE)
+
+    def hold_current_rotation(self) -> None:
+        """重新下发当前反馈角度来停止旋转。"""
+        self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw())
+
+    def hold_current_all(self) -> None:
+        self.hold_current_position()
+        self.hold_current_rotation()

build_sim/arm_application/build/lib/arm_application/dh_pgea.py

@@ -0,0 +1,584 @@
+#!/usr/bin/env python3
+"""DH Robotics PGE / PGEA Modbus RTU 客户端。
+
+移植自官方 C++ 驱动：
+  DH-Robotics/dh_gripper_ros  →  DH_Modbus_Gripper
+  （工厂会按型号名里是否包含 ``PGE`` / ``PGEA`` 映射到本类）
+
+Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。
+改用 RS485（USB–UART）通信，寄存器与 DH 的 ROS 驱动完全一致。
+
+寄存器（保持寄存器，FC03 读 / FC06 写，CRC-16/Modbus 小端）：
+  0x0100  初始化，写 0xA5
+  0x0101  力 20–100 %
+  0x0103  位置 0–1000 ‰
+  0x0104  速度 1–100 %
+  0x0200  初始化状态  0=需要  1=完成  2=进行中
+  0x0201  夹持状态  0=运动中  1=已到位  2=已夹住  3=已掉落
+  0x0202  当前位置
+"""
+
+from __future__ import annotations
+
+import argparse
+import sys
+import threading
+import time
+from dataclasses import dataclass
+from typing import Optional
+
+try:
+    import serial
+except ImportError as exc:  # pragma: no cover
+    raise SystemExit(
+        "缺少 pyserial。在 conda 环境 ros_flexiv 里执行：pip install pyserial"
+    ) from exc
+
+
+# 官方 CRC-16/Modbus（与 dh_modbus_gripper.cpp 使用同一张表）。
+_CRC_TABLE = (
+    0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
+    0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
+    0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
+    0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
+    0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
+    0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
+    0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
+    0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
+    0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
+    0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
+    0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
+    0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
+    0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
+    0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
+    0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
+    0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
+    0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
+    0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
+    0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
+    0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
+    0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
+    0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
+    0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
+    0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
+    0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
+    0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
+    0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
+    0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
+    0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
+    0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
+    0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
+    0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040,
+)
+
+REG_INIT = 0x0100
+REG_FORCE = 0x0101
+REG_POSITION = 0x0103
+REG_SPEED = 0x0104
+REG_INIT_STATE = 0x0200
+REG_GRIP_STATE = 0x0201
+REG_CUR_POSITION = 0x0202
+
+INIT_CMD = 0xA5
+
+S_INIT_NOT = 0
+S_INIT_FINISHED = 1
+S_INIT_DOING = 2
+
+S_GRIP_MOVING = 0
+S_GRIP_ARRIVED = 1
+S_GRIP_CAUGHT = 2
+S_GRIP_DROPPED = 3
+
+GRIP_STATE_NAME = {
+    S_GRIP_MOVING: "moving",
+    S_GRIP_ARRIVED: "arrived",
+    S_GRIP_CAUGHT: "caught",
+    S_GRIP_DROPPED: "dropped",
+}
+
+INIT_STATE_NAME = {
+    S_INIT_NOT: "need_init",
+    S_INIT_FINISHED: "initialized",
+    S_INIT_DOING: "initializing",
+}
+
+
+def crc16_modbus(data: bytes) -> int:
+    """官方 DH CRC16（Modbus，线路上按小端发送）。"""
+    crc = 0xFFFF
+    for byte in data:
+        crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF]
+    return crc
+
+
+def _frame(slave: int, func: int, index: int, value: int) -> bytes:
+    body = bytes(
+        [
+            slave & 0xFF,
+            func & 0xFF,
+            (index >> 8) & 0xFF,
+            index & 0xFF,
+            (value >> 8) & 0xFF,
+            value & 0xFF,
+        ]
+    )
+    crc = crc16_modbus(body)
+    return body + bytes([crc & 0xFF, (crc >> 8) & 0xFF])
+
+
+@dataclass
+class GripperState:
+    init_state: int = S_INIT_NOT
+    grip_state: int = S_GRIP_MOVING
+    position: int = 0
+    target_position: int = 0
+    target_force: int = 0
+    stroke_mm: float = 20.0
+
+    @property
+    def initialized(self) -> bool:
+        return self.init_state == S_INIT_FINISHED
+
+    @property
+    def width_mm(self) -> float:
+        return float(self.position) / 1000.0 * float(self.stroke_mm)
+
+    def as_tuple(self) -> tuple[float, float, float, float, float]:
+        return (
+            1.0 if self.initialized else 0.0,
+            float(self.grip_state),
+            float(self.position),
+            float(self.target_position),
+            float(self.target_force),
+        )
+
+    def summary(self) -> str:
+        return (
+            f"init={INIT_STATE_NAME.get(self.init_state, self.init_state)} "
+            f"grip={GRIP_STATE_NAME.get(self.grip_state, self.grip_state)} "
+            f"pos={self.position}/1000 ({self.width_mm:.2f} mm) "
+            f"target={self.target_position} force={self.target_force}%"
+        )
+
+
+class DhPgeaError(RuntimeError):
+    pass
+
+
+WIRING_HINT = (
+    "PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n"
+    "  1) 夹爪 24 V DC 已上电（指示灯亮）\n"
+    "  2) USB 转接头必须是 RS485（A/B），不能是 TTL 的 TX/RX\n"
+    "  3) A↔A、B↔B；不通就对调 A/B\n"
+    "  4) 先 Ctrl+C 停节点，再：bash ~/ros2/scripts/run_gripper.sh --cli scan"
+)
+
+
+class DhPgeaGripper:
+    """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
+
+    def __init__(
+        self,
+        port: str = "/dev/ttyUSB0",
+        baudrate: int = 115200,
+        slave_id: int = 1,
+        timeout_s: float = 0.25,
+        stroke_mm: float = 20.0,
+        rs485_rts: str = "",
+    ) -> None:
+        self.port = port
+        self.baudrate = int(baudrate)
+        self.slave_id = int(slave_id)
+        self.timeout_s = float(timeout_s)
+        self.stroke_mm = float(stroke_mm)
+        mode = str(rs485_rts or "").strip().lower()
+        if mode in ("", "off", "none", "false", "0"):
+            mode = ""
+        if mode not in ("", "tx", "tx_inv"):
+            raise DhPgeaError(f"rs485_rts 只能是 ''、tx 或 tx_inv，实际为 {rs485_rts!r}")
+        self.rs485_rts = mode
+        self._ser: Optional[serial.Serial] = None
+        self._lock = threading.Lock()
+
+    def open(self) -> None:
+        if self._ser is not None and self._ser.is_open:
+            return
+        try:
+            self._ser = serial.Serial(
+                port=self.port,
+                baudrate=self.baudrate,
+                bytesize=serial.EIGHTBITS,
+                parity=serial.PARITY_NONE,
+                stopbits=serial.STOPBITS_ONE,
+                timeout=self.timeout_s,
+                write_timeout=self.timeout_s,
+                xonxoff=False,
+                rtscts=False,
+                dsrdtr=False,
+                exclusive=True,
+            )
+        except Exception as exc:
+            raise DhPgeaError(
+                f"无法打开 {self.port}：{exc}。"
+                "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
+            ) from exc
+        try:
+            self._ser.dtr = True
+        except Exception:
+            pass
+        self._set_tx(False)
+        self._ser.reset_input_buffer()
+        self._ser.reset_output_buffer()
+        time.sleep(0.05)
+
+    def close(self) -> None:
+        if self._ser is not None:
+            try:
+                self._ser.close()
+            except Exception:
+                pass
+            self._ser = None
+
+    def __enter__(self) -> "DhPgeaGripper":
+        self.open()
+        return self
+
+    def __exit__(self, *args) -> None:
+        self.close()
+
+    def write_register(self, index: int, value: int, retries: int = 3) -> bool:
+        """FC06 写，等待回显（对应官方 WriteRegisterFunc）。"""
+        frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF)
+        with self._lock:
+            for _ in range(max(1, retries)):
+                if self._transact(frame, 8) == frame:
+                    return True
+                time.sleep(0.02)
+        return False
+
+    def read_register(self, index: int, retries: int = 8) -> int:
+        """FC03 读一个保持寄存器（对应官方 ReadRegisterFunc）。"""
+        req = _frame(self.slave_id, 0x03, index, 1)
+        last: Optional[bytes] = None
+        with self._lock:
+            for _ in range(max(1, retries)):
+                rsp = self._transact(req, 7)
+                last = rsp
+                if rsp is None or len(rsp) != 7:
+                    time.sleep(0.02)
+                    continue
+                if crc16_modbus(rsp[:-2]) != (rsp[5] | (rsp[6] << 8)):
+                    continue
+                if rsp[0] != (self.slave_id & 0xFF) or rsp[1] != 0x03:
+                    continue
+                return (rsp[3] << 8) | (rsp[4] & 0xFF)
+        hint = f" last={last.hex() if last else '(empty)'}"
+        raise DhPgeaError(
+            f"在 {self.port} 上 Modbus 读 0x{index:04X} 失败 "
+            f"（无有效应答{hint}）。baud={self.baudrate} slave={self.slave_id} "
+            f"rts={self.rs485_rts or 'off'}"
+        )
+
+    def probe(self) -> int:
+        """短读一次初始化状态。夹爪无应答时会抛异常。"""
+        return self.read_register(REG_INIT_STATE, retries=2)
+
+    def _set_tx(self, tx: bool) -> None:
+        if not self.rs485_rts or self._ser is None:
+            return
+        try:
+            if self.rs485_rts == "tx":
+                self._ser.rts = bool(tx)
+            else:
+                self._ser.rts = not bool(tx)
+        except Exception:
+            pass
+
+    def _transact(self, frame: bytes, expect: int) -> Optional[bytes]:
+        if self._ser is None or not self._ser.is_open:
+            raise DhPgeaError("串口未打开")
+        bit_s = 10.0 / max(1, self.baudrate)
+        self._set_tx(True)
+        if self.rs485_rts:
+            time.sleep(2.0 * bit_s)
+        self._ser.reset_input_buffer()
+        self._ser.write(frame)
+        self._ser.flush()
+        if self.rs485_rts:
+            time.sleep(len(frame) * bit_s + 0.0008)
+            self._set_tx(False)
+        deadline = time.monotonic() + self.timeout_s
+        buf = bytearray()
+        while time.monotonic() < deadline and len(buf) < expect:
+            chunk = self._ser.read(expect - len(buf))
+            if chunk:
+                buf.extend(chunk)
+        return bytes(buf) if buf else None
+
+    def initialization(self) -> bool:
+        return self.write_register(REG_INIT, INIT_CMD)
+
+    def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
+        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
+        state = self.get_init_state()
+        if state == S_INIT_FINISHED:
+            return True
+        if not self.initialization():
+            raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
+        deadline = time.monotonic() + float(timeout_s)
+        while time.monotonic() < deadline:
+            state = self.get_init_state()
+            if state == S_INIT_FINISHED:
+                return True
+            time.sleep(poll_s)
+        raise DhPgeaError(
+            f"初始化在 {timeout_s:.0f}s 后超时（state={state}）。"
+            "首次上电时手指本应先回零一次。"
+        )
+
+    def set_target_force(self, force_pct: int) -> bool:
+        force_pct = int(max(20, min(100, force_pct)))
+        return self.write_register(REG_FORCE, force_pct)
+
+    def set_target_speed(self, speed_pct: int) -> bool:
+        speed_pct = int(max(1, min(100, speed_pct)))
+        return self.write_register(REG_SPEED, speed_pct)
+
+    def set_target_position(self, pos: int) -> bool:
+        pos = int(max(0, min(1000, pos)))
+        # 大寰固件：0x0103 写入与当前命令相同的值不会启动新运动。
+        # 开/关卡在 caught 时最常见——命令已是 1000，手指却停在夹持位。
+        try:
+            cmd = self.get_target_position(retries=2)
+        except Exception:
+            cmd = pos
+        if cmd == pos:
+            try:
+                cur = self.get_current_position(retries=2)
+            except Exception:
+                cur = pos
+            nudge = cur + (40 if pos >= cur else -40)
+            nudge = int(max(0, min(1000, nudge)))
+            if nudge == pos:
+                nudge = 0 if pos != 0 else 40
+            self.write_register(REG_POSITION, nudge)
+            time.sleep(0.06)
+        return self.write_register(REG_POSITION, pos)
+
+    def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
+        """等到到位/夹住/松开，或超时返回最后一帧。"""
+        t0 = time.monotonic()
+        last = None
+        st = self.get_run_states()
+        while time.monotonic() - t0 < timeout_s:
+            st = self.get_run_states()
+            moved = last is not None and st.position != last
+            last = st.position
+            if st.grip_state == S_GRIP_MOVING or moved:
+                time.sleep(0.08)
+                continue
+            if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
+                return st
+            if st.grip_state == S_GRIP_CAUGHT:
+                return st
+            time.sleep(0.08)
+        return st
+
+    def get_init_state(self, retries: int = 8) -> int:
+        return self.read_register(REG_INIT_STATE, retries=retries)
+
+    def get_grip_state(self, retries: int = 8) -> int:
+        return self.read_register(REG_GRIP_STATE, retries=retries)
+
+    def get_current_position(self, retries: int = 8) -> int:
+        return self.read_register(REG_CUR_POSITION, retries=retries)
+
+    def get_target_position(self, retries: int = 8) -> int:
+        return self.read_register(REG_POSITION, retries=retries)
+
+    def get_target_force(self, retries: int = 8) -> int:
+        return self.read_register(REG_FORCE, retries=retries)
+
+    def get_run_states(self, retries: int = 8) -> GripperState:
+        return GripperState(
+            init_state=self.get_init_state(retries=retries),
+            grip_state=self.get_grip_state(retries=retries),
+            position=self.get_current_position(retries=retries),
+            target_position=self.get_target_position(retries=retries),
+            target_force=self.get_target_force(retries=retries),
+            stroke_mm=self.stroke_mm,
+        )
+
+    def open_jaw(self, force: int = 50, speed: int = 50) -> bool:
+        ok = self.set_target_force(force)
+        ok = self.set_target_speed(speed) and ok
+        return self.set_target_position(1000) and ok
+
+    def close_jaw(self, force: int = 50, speed: int = 50) -> bool:
+        ok = self.set_target_force(force)
+        ok = self.set_target_speed(speed) and ok
+        return self.set_target_position(0) and ok
+
+    def move(self, position: int, force: int = 50, speed: int = 50) -> bool:
+        ok = self.set_target_force(force)
+        ok = self.set_target_speed(speed) and ok
+        return self.set_target_position(position) and ok
+
+
+def _selfcheck_crc() -> None:
+    """官方简短手册示例：设置位置 500。"""
+    frame = _frame(1, 0x06, 0x0103, 500)
+    expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
+    if frame != expect:
+        raise AssertionError(f"CRC 不匹配：{frame.hex()} != {expect.hex()}")
+
+
+def _site_defaults() -> dict:
+    try:
+        from pathlib import Path
+
+        root = Path(__file__).resolve().parents[3]
+        sys.path.insert(0, str(root / "scripts" / "lib"))
+        from export_site_env import load_site  # type: ignore
+
+        g = (load_site().get("gripper") or {})
+        return {
+            "port": str(g.get("port") or "/dev/ttyUSB0"),
+            "baudrate": int(g.get("baud") or 115200),
+            "slave_id": int(g.get("slave_id") or 1),
+            "stroke_mm": float(g.get("stroke_mm") or 20.0),
+            "force": int(g.get("force_pct") or 50),
+            "speed": int(g.get("speed_pct") or 50),
+            "rs485_rts": str(g.get("rs485_rts") or ""),
+        }
+    except Exception:
+        return {
+            "port": "/dev/ttyUSB0",
+            "baudrate": 115200,
+            "slave_id": 1,
+            "stroke_mm": 20.0,
+            "force": 50,
+            "speed": 50,
+            "rs485_rts": "",
+        }
+
+
+def _scan_bus(port: str) -> int:
+    """遍历波特率 × 从站号 × RTS，直到某个组合有应答。"""
+    bauds = (115200, 57600, 38400, 19200, 9600)
+    slaves = (1, 2, 3)
+    rts_modes = ("", "tx", "tx_inv")
+    print(f"扫描 {port}（请先停掉 ROS 节点 —— 该串口是独占的）")
+    for baud in bauds:
+        for slave in slaves:
+            for rts in rts_modes:
+                label = f"baud={baud} slave={slave} rts={rts or 'off'}"
+                g = DhPgeaGripper(
+                    port=port,
+                    baudrate=baud,
+                    slave_id=slave,
+                    timeout_s=0.08,
+                    rs485_rts=rts,
+                )
+                try:
+                    g.open()
+                    state = g.probe()
+                    print(f"OK  {label}  init_state={state}")
+                    print(f"  → site.yaml: baud: {baud}  slave_id: {slave}  rs485_rts: \"{rts}\"")
+                    g.close()
+                    return 0
+                except DhPgeaError:
+                    print(f"  miss {label}")
+                finally:
+                    g.close()
+    print("所有组合都无应答。", file=sys.stderr)
+    print(WIRING_HINT, file=sys.stderr)
+    return 1
+
+
+def _cli(argv: Optional[list[str]] = None) -> int:
+    site = _site_defaults()
+    ap = argparse.ArgumentParser(
+        description="DH PGEA Modbus 命令行工具（官方寄存器映射，不用 Flexiv RDK）"
+    )
+    ap.add_argument("--port", default=site["port"])
+    ap.add_argument("--baud", type=int, default=site["baudrate"])
+    ap.add_argument("--slave", type=int, default=site["slave_id"])
+    ap.add_argument("--force", type=int, default=site["force"])
+    ap.add_argument("--speed", type=int, default=site["speed"])
+    ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE：空 | tx | tx_inv")
+    ap.add_argument(
+        "cmd",
+        choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"],
+    )
+    ap.add_argument("value", nargs="?", default="", help="'pos' 命令的位置，0–1000")
+    args = ap.parse_args(argv)
+
+    if args.cmd == "selfcheck":
+        _selfcheck_crc()
+        print("CRC 自检通过（01 06 01 03 01 F4 78 21）")
+        return 0
+    if args.cmd == "scan":
+        return _scan_bus(args.port)
+
+    grip = DhPgeaGripper(
+        port=args.port,
+        baudrate=args.baud,
+        slave_id=args.slave,
+        stroke_mm=float(site["stroke_mm"]),
+        rs485_rts=args.rts,
+    )
+    try:
+        grip.open()
+    except DhPgeaError as exc:
+        print(f"错误：{exc}", file=sys.stderr)
+        return 1
+
+    try:
+        if args.cmd == "probe":
+            st = grip.get_run_states(retries=2)
+            print(st.summary())
+            return 0
+        if args.cmd == "init":
+            print("正在初始化（手指回零）…")
+            grip.initialize()
+            print(grip.get_run_states().summary())
+            return 0
+        if args.cmd == "status":
+            print(grip.get_run_states().summary())
+            return 0
+        if args.cmd == "open":
+            if not grip.open_jaw(args.force, args.speed):
+                raise DhPgeaError("张开写入失败")
+            print(grip.wait_settle(1000).summary())
+            return 0
+        if args.cmd == "close":
+            if not grip.close_jaw(args.force, args.speed):
+                raise DhPgeaError("闭合写入失败")
+            print(grip.wait_settle(0).summary())
+            return 0
+        if args.cmd == "pos":
+            if args.value == "":
+                print("用法：pos <0-1000>", file=sys.stderr)
+                return 2
+            pos = int(float(args.value))
+            if not grip.move(pos, args.force, args.speed):
+                raise DhPgeaError("位置写入失败")
+            print(grip.wait_settle(pos).summary())
+            return 0
+    except DhPgeaError as exc:
+        print(f"错误：{exc}", file=sys.stderr)
+        print(WIRING_HINT, file=sys.stderr)
+        return 1
+    finally:
+        grip.close()
+    return 0
+
+
+def main(argv: Optional[list[str]] = None) -> int:
+    return _cli(argv)
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

build_sim/arm_application/build/lib/arm_application/dh_pgea_node.py

@@ -0,0 +1,246 @@
+#!/usr/bin/env python3
+"""DH PGEA 的 ROS 2 节点（官方 Modbus RTU，不用 Flexiv 夹爪）。
+
+话题（布局与 DH-Robotics/dh_gripper_ros 一致）：
+  /gripper/ctrl     std_msgs/Float32MultiArray
+                    data: [initialize, position, force, speed]
+                    initialize>0.5 → 写 0xA5；否则设置力/速度/位置
+  /gripper/states   std_msgs/Float32MultiArray
+                    data: [initialized, grip_state, position, target_pos, target_force]
+  /gripper/joint_states  sensor_msgs/JointState
+
+服务（std_srvs/Trigger）：
+  /gripper/initialize  /gripper/open  /gripper/close
+
+不要设置 Flexiv bringup 的 load_gripper:=true —— 那只是 Flexiv-GN01 用的。
+"""
+
+from __future__ import annotations
+
+import threading
+from typing import Optional
+
+import rclpy
+from rcl_interfaces.msg import ParameterDescriptor
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiArrayLayout
+from std_srvs.srv import Trigger
+
+from arm_application.dh_pgea import (
+    DhPgeaError,
+    DhPgeaGripper,
+    GripperState,
+    WIRING_HINT,
+)
+from arm_application.ros_node import TolerantNode, param_as_str
+
+
+def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
+    data = list(msg.data)
+    while len(data) < 4:
+        data.append(0.0)
+    return float(data[0]), float(data[1]), float(data[2]), float(data[3])
+
+
+class DhPgeaNode(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__("dh_pgea")
+        self.declare_parameter("port", "/dev/ttyUSB0")
+        self.declare_parameter("baud", 115200)
+        self.declare_parameter("slave_id", 1)
+        self.declare_parameter("stroke_mm", 20.0)
+        self.declare_parameter("force_pct", 50)
+        self.declare_parameter("speed_pct", 50)
+        self.declare_parameter("auto_init", True)
+        self.declare_parameter("state_hz", 20.0)
+        self.declare_parameter("open_position", 1000)
+        self.declare_parameter("close_position", 0)
+        self._model = param_as_str(self, "model", "PGEA-100-20-W")
+        self._sn = param_as_str(self, "serial_number", "")
+        self._rts = param_as_str(self, "rs485_rts", "")
+
+        self._port = str(self.get_parameter("port").value)
+        self._baud = int(self.get_parameter("baud").value)
+        self._slave = int(self.get_parameter("slave_id").value)
+        self._stroke = float(self.get_parameter("stroke_mm").value)
+        self._force = int(self.get_parameter("force_pct").value)
+        self._speed = int(self.get_parameter("speed_pct").value)
+        _auto = self.get_parameter("auto_init").value
+        if isinstance(_auto, str):
+            self._auto_init = _auto.strip().lower() in ("1", "true", "yes")
+        else:
+            self._auto_init = bool(_auto)
+        self._open_pos = int(self.get_parameter("open_position").value)
+        self._close_pos = int(self.get_parameter("close_position").value)
+        hz = float(self.get_parameter("state_hz").value)
+
+        self._grip = DhPgeaGripper(
+            port=self._port,
+            baudrate=self._baud,
+            slave_id=self._slave,
+            stroke_mm=self._stroke,
+            rs485_rts=self._rts,
+        )
+        self._state = GripperState(stroke_mm=self._stroke)
+        self._cmd_lock = threading.Lock()
+
+        self._pub_state = self.create_publisher(Float32MultiArray, "/gripper/states", 10)
+        self._pub_js = self.create_publisher(JointState, "/gripper/joint_states", 10)
+        self.create_subscription(Float32MultiArray, "/gripper/ctrl", self._on_ctrl, 10)
+        self.create_subscription(Float32, "/gripper/set_position", self._on_set_pos, 10)
+
+        self.create_service(Trigger, "/gripper/initialize", self._srv_init)
+        self.create_service(Trigger, "/gripper/open", self._srv_open)
+        self.create_service(Trigger, "/gripper/close", self._srv_close)
+
+        self.get_logger().info(
+            f"DH {self._model} SN={self._sn or '?'}  "
+            f"{self._port} @{self._baud} slave={self._slave} rts={self._rts or 'off'}  "
+            f"（官方 Modbus RTU；未使用 Flexiv RDK 夹爪 API）"
+        )
+        try:
+            self._grip.open()
+        except DhPgeaError as exc:
+            self.get_logger().error(str(exc))
+            raise
+
+        self._online = False
+        try:
+            self._grip.probe()
+            self._online = True
+        except DhPgeaError as exc:
+            self.get_logger().error(str(exc))
+            for line in WIRING_HINT.splitlines():
+                self.get_logger().error(line)
+            self.get_logger().error("会持续重试；在总线恢复应答前 open/close 都会失败")
+
+        if self._auto_init and self._online:
+            try:
+                self._grip.initialize()
+                self.get_logger().info("夹爪已初始化")
+            except DhPgeaError as exc:
+                self.get_logger().error(f"auto_init 失败：{exc}")
+                self._online = False
+
+        period = 1.0 / max(1.0, hz)
+        self._timer = self.create_timer(period, self._on_timer)
+
+    def destroy_node(self) -> bool:
+        try:
+            self._grip.close()
+        except Exception:
+            pass
+        return super().destroy_node()
+
+    def _apply_move(self, position: int, force: Optional[int] = None, speed: Optional[int] = None) -> str:
+        with self._cmd_lock:
+            f = self._force if force is None else int(force)
+            s = self._speed if speed is None else int(speed)
+            if force is not None:
+                self._force = f
+            if speed is not None:
+                self._speed = s
+            if not self._grip.move(int(position), f, s):
+                raise DhPgeaError("移动写入失败")
+        return f"pos={int(position)} force={f}% speed={s}%"
+
+    def _on_ctrl(self, msg: Float32MultiArray) -> None:
+        initialize, position, force, speed = _as_ctrl_array(msg)
+        try:
+            if initialize > 0.5:
+                self._grip.initialize()
+                self.get_logger().info("通过 /gripper/ctrl 触发初始化")
+                return
+            f = int(force) if force > 0 else self._force
+            s = int(speed) if speed > 0 else self._speed
+            info = self._apply_move(int(position), f, s)
+            self.get_logger().info(f"ctrl {info}")
+        except DhPgeaError as exc:
+            self.get_logger().error(str(exc))
+
+    def _on_set_pos(self, msg: Float32) -> None:
+        try:
+            info = self._apply_move(int(msg.data))
+            self.get_logger().info(f"set_position {info}")
+        except DhPgeaError as exc:
+            self.get_logger().error(str(exc))
+
+    def _srv_init(self, _req, resp):
+        try:
+            self._grip.initialize()
+            resp.success = True
+            resp.message = self._grip.get_run_states().summary()
+        except DhPgeaError as exc:
+            resp.success = False
+            resp.message = str(exc)
+        return resp
+
+    def _srv_open(self, _req, resp):
+        try:
+            info = self._apply_move(self._open_pos)
+            resp.success = True
+            resp.message = info
+        except DhPgeaError as exc:
+            resp.success = False
+            resp.message = str(exc)
+        return resp
+
+    def _srv_close(self, _req, resp):
+        try:
+            info = self._apply_move(self._close_pos)
+            resp.success = True
+            resp.message = info
+        except DhPgeaError as exc:
+            resp.success = False
+            resp.message = str(exc)
+        return resp
+
+    def _on_timer(self) -> None:
+        try:
+            st = self._grip.get_run_states(retries=2 if self._online else 1)
+            self._state = st
+            if not self._online:
+                self._online = True
+                self.get_logger().info("夹爪总线已恢复")
+        except DhPgeaError as exc:
+            self._online = False
+            self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
+            return
+
+        arr = Float32MultiArray()
+        arr.layout = MultiArrayLayout(
+            dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)],
+            data_offset=0,
+        )
+        arr.data = list(st.as_tuple())
+        self._pub_state.publish(arr)
+
+        js = JointState()
+        js.header.stamp = self.get_clock().now().to_msg()
+        js.name = ["gripper_finger1_joint"]
+        # 官方 ROS 驱动把 0–1000 映射到约 0–0.637 rad；我们这里也发布 mm。
+        js.position = [float(st.width_mm) * 0.001]
+        js.effort = [float(st.target_force)]
+        self._pub_js.publish(js)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = DhPgeaNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    except DhPgeaError:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

build_sim/arm_application/build/lib/arm_application/dual_gripper.py

@@ -0,0 +1,803 @@
+"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。
+
+设计目标：
+- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。
+- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。
+- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。
+
+注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。
+"""
+
+from __future__ import annotations
+
+import time
+from abc import ABC, abstractmethod
+from dataclasses import dataclass, field
+from enum import Enum, auto
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from arm_application.dh_official import DHModbusClient, DHModbusError
+
+
+class GripperType(Enum):
+    PARALLEL = auto()      # 平行夹爪
+    ROTARY = auto()        # 旋转夹爪
+    VACUUM = auto()        # 真空吸盘（预留）
+
+
+class GripperState(Enum):
+    IDLE = auto()
+    MOVING = auto()
+    GRIPPING = auto()
+    RELEASING = auto()
+    ERROR = auto()
+
+
+@dataclass
+class GripperStatus:
+    """夹爪状态反馈。"""
+
+    state: GripperState = GripperState.IDLE
+    position: float = 0.0           # 平行：mm；旋转：角度 deg
+    force: float = 0.0              # 最近一次命令力/力矩 %
+    velocity: float = 0.0           # 最近一次命令速度 %
+    is_moving: bool = False
+    is_gripped: bool = False        # 是否夹持到物体
+    connected: bool = False
+    initialized: bool = False
+    finger_position_per_mille: float = 0.0
+    rotation_deg: float = 0.0
+    gripper_state: int = -1         # DH 0x0201：0 moving,1 reached,2 caught,3 dropped
+    rotation_state: int = -1        # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked
+    error_msg: str = ""
+    timestamp: float = field(default_factory=time.time)
+
+
+@dataclass
+class GripperCommand:
+    """夹爪控制指令。"""
+
+    target_position: float | None = None    # 平行：mm；旋转：deg
+    target_force: float | None = None       # 目标力/力矩 %
+    target_velocity: float | None = None    # 目标速度 %
+    wait_complete: bool = True              # 是否等待完成
+    timeout: float = 10.0                   # 超时时间
+
+
+class BaseGripper(ABC):
+    """夹爪基类，定义统一接口。"""
+
+    name: str = "base_gripper"
+    gripper_type: GripperType = GripperType.PARALLEL
+
+    def __init__(self, config: dict[str, Any] | None = None) -> None:
+        self.cfg = config or {}
+        self._connected = False
+        self._status = GripperStatus()
+
+    @abstractmethod
+    def connect(self) -> None: ...
+
+    @abstractmethod
+    def disconnect(self) -> None: ...
+
+    @abstractmethod
+    def execute(self, cmd: GripperCommand) -> GripperStatus: ...
+
+    @abstractmethod
+    def get_status(self) -> GripperStatus: ...
+
+    def is_connected(self) -> bool:
+        return self._connected
+
+    def open(self, **kwargs) -> GripperStatus:
+        raise NotImplementedError
+
+    def close(self, **kwargs) -> GripperStatus:
+        raise NotImplementedError
+
+    def stop(self) -> None:
+        raise NotImplementedError
+
+
+# =============================================================================
+# 大寰平行夹爪（PGE/PGEA 系列）
+# =============================================================================
+
+@dataclass
+class ParallelGripperConfig:
+    """大寰平行夹爪配置。"""
+
+    port: str = "/dev/ttyUSB0"
+    slave_id: int = 1
+    baudrate: int = 115200
+    timeout: float = 0.2
+
+    # PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。
+    stroke_min: float = 0.0
+    stroke_max: float = 40.0
+
+    # 默认参数（DH 百分比）
+    default_speed: int = 50
+    default_force: int = 50
+    force_min: int = 20
+    force_max: int = 100
+
+    # 控制行为
+    auto_initialize: bool = True
+    init_timeout: float = 10.0
+    position_tolerance_per_mille: int = 15
+    status_poll_interval: float = 0.05
+
+
+class ParallelGripper(BaseGripper):
+    """大寰 PGE/PGEA 平行夹爪。"""
+
+    name = "dh_pgea_parallel"
+    gripper_type = GripperType.PARALLEL
+
+    def __init__(self, config: ParallelGripperConfig | None = None) -> None:
+        super().__init__()
+        self.cfg = config or ParallelGripperConfig()
+        self._client: DHModbusClient | None = None
+        self._last_position_mm = float(self.cfg.stroke_min)
+        self._last_force = int(self.cfg.default_force)
+        self._last_speed = int(self.cfg.default_speed)
+
+    def connect(self) -> None:
+        if self._client is not None:
+            self.disconnect()
+        self._client = DHModbusClient(
+            self.cfg.port,
+            slave_id=self.cfg.slave_id,
+            baudrate=self.cfg.baudrate,
+            timeout=self.cfg.timeout,
+        )
+        self._connected = True
+        self._status.connected = True
+        self._status.state = GripperState.IDLE
+        self._status.error_msg = ""
+        if self.cfg.auto_initialize:
+            self.initialize()
+
+    def disconnect(self) -> None:
+        if self._client is not None:
+            self._client.close()
+        self._client = None
+        self._connected = False
+        self._status.connected = False
+        self._status.state = GripperState.IDLE
+
+    def initialize(self, full: bool = False) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout)
+        self._status.initialized = True
+
+    def _require_client(self) -> None:
+        if not self._connected or self._client is None:
+            self._status.state = GripperState.ERROR
+            self._status.error_msg = "平行夹爪未连接"
+            raise DHModbusError(self._status.error_msg)
+
+    def _mm_to_per_mille(self, position_mm: float) -> int:
+        lo = float(self.cfg.stroke_min)
+        hi = float(self.cfg.stroke_max)
+        if hi <= lo:
+            hi = lo + 1.0
+        ratio = (float(position_mm) - lo) / (hi - lo)
+        return int(np.clip(round(ratio * 1000.0), 0, 1000))
+
+    def _per_mille_to_mm(self, position: float | int) -> float:
+        lo = float(self.cfg.stroke_min)
+        hi = float(self.cfg.stroke_max)
+        return lo + (float(position) / 1000.0) * (hi - lo)
+
+    def execute(self, cmd: GripperCommand) -> GripperStatus:
+        try:
+            self._require_client()
+        except DHModbusError:
+            return self._status
+        assert self._client is not None
+
+        position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position)
+        position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max))
+        speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100))
+        force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max))
+
+        self._client.set_speed_pct(speed)
+        self._client.set_force_pct(force)
+        target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm))
+
+        self._last_position_mm = position_mm
+        self._last_speed = speed
+        self._last_force = force
+        self._status.state = GripperState.MOVING
+        self._status.error_msg = ""
+
+        if cmd.wait_complete:
+            self._wait_complete(target_per_mille, cmd.timeout)
+        return self.get_status()
+
+    def _wait_complete(self, target_per_mille: int, timeout: float) -> None:
+        assert self._client is not None
+        start = time.monotonic()
+        time.sleep(0.05)
+        while time.monotonic() - start < timeout:
+            state = self._client.get_gripper_state()
+            pos = self._client.get_position_per_mille()
+            self._status.gripper_state = state
+            self._status.finger_position_per_mille = float(pos)
+            self._status.position = self._per_mille_to_mm(pos)
+            self._status.is_moving = state == 0
+            self._status.is_gripped = state == 2
+            if state in (1, 2):
+                self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE
+                return
+            if state == 3:
+                self._status.state = GripperState.ERROR
+                self._status.error_msg = "夹持后物体掉落"
+                return
+            if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0:
+                self._status.state = GripperState.IDLE
+                return
+            time.sleep(self.cfg.status_poll_interval)
+        self._status.state = GripperState.ERROR
+        self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）"
+
+    def get_status(self) -> GripperStatus:
+        if not self._connected or self._client is None:
+            self._status.connected = False
+            return self._status
+        try:
+            pos = self._client.get_position_per_mille()
+            state = self._client.get_gripper_state()
+            init_state = self._client.get_init_state()
+            self._status.connected = True
+            self._status.initialized = init_state == 1
+            self._status.finger_position_per_mille = float(pos)
+            self._status.position = self._per_mille_to_mm(pos)
+            self._status.velocity = float(self._last_speed)
+            self._status.force = float(self._last_force)
+            self._status.gripper_state = state
+            self._status.is_moving = state == 0
+            self._status.is_gripped = state == 2
+            if self._status.state != GripperState.ERROR:
+                self._status.state = GripperState.MOVING if state == 0 else (
+                    GripperState.GRIPPING if state == 2 else GripperState.IDLE
+                )
+            self._status.timestamp = time.time()
+        except Exception as exc:
+            self._status.state = GripperState.ERROR
+            self._status.error_msg = str(exc)
+            self._status.connected = False
+        return self._status
+
+    def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
+        # DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。
+        return self.execute(GripperCommand(
+            target_position=float(self.cfg.stroke_max),
+            target_force=force or self.cfg.default_force,
+            target_velocity=speed or self.cfg.default_speed,
+            **kwargs,
+        ))
+
+    def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
+        return self.execute(GripperCommand(
+            target_position=float(self.cfg.stroke_min),
+            target_force=force or self.cfg.default_force,
+            target_velocity=speed or self.cfg.default_speed,
+            **kwargs,
+        ))
+
+    def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None,
+                **kwargs) -> GripperStatus:
+        return self.execute(GripperCommand(
+            target_position=position_mm,
+            target_force=force or self.cfg.default_force,
+            target_velocity=speed or self.cfg.default_speed,
+            **kwargs,
+        ))
+
+    def set_force(self, force_pct: int) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._last_force = self._client.set_force_pct(force_pct)
+
+    def set_speed(self, speed_pct: int) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._last_speed = self._client.set_speed_pct(speed_pct)
+
+    def stop(self) -> None:
+        if self._client is not None:
+            self._client.hold_current_position()
+        self._status.state = GripperState.IDLE
+
+
+# =============================================================================
+# 大寰旋转夹爪（RGI-100-30-O-S-M1）
+# =============================================================================
+
+@dataclass
+class RotaryGripperConfig:
+    """大寰 RGI 旋转夹爪配置。"""
+
+    port: str = "/dev/ttyUSB1"
+    slave_id: int = 1
+    baudrate: int = 115200
+    timeout: float = 0.2
+
+    # RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。
+    angle_min: float = -32767.0
+    angle_max: float = 32767.0
+    max_step_deg: float = 30000.0      # rotate_revolutions 分段旋转的最大单步角度
+
+    # DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。
+    finger_open_per_mille: int = 1000
+    finger_close_per_mille: int = 0
+
+    # 默认参数（DH 百分比）
+    default_speed: int = 50            # 旋转速度 %
+    default_torque: int = 50           # 旋转力/力矩 %
+    default_finger_speed: int = 50     # 手指速度 %
+    default_finger_force: int = 50     # 手指夹持力 %
+    torque_min: int = 20
+    torque_max: int = 100
+
+    # 控制行为
+    auto_initialize: bool = True
+    init_timeout: float = 15.0         # RGI 初始化包含旋转找零，时间更长
+    angle_tolerance_deg: float = 1.0
+    status_poll_interval: float = 0.05
+
+
+class RotaryGripper(BaseGripper):
+    """大寰 RGI-100-30-O-S-M1 旋转夹爪。"""
+
+    name = "dh_rgi_rotary"
+    gripper_type = GripperType.ROTARY
+
+    def __init__(self, config: RotaryGripperConfig | None = None) -> None:
+        super().__init__()
+        self.cfg = config or RotaryGripperConfig()
+        self._client: DHModbusClient | None = None
+        self._last_angle = 0.0
+        self._last_torque = int(self.cfg.default_torque)
+        self._last_speed = int(self.cfg.default_speed)
+        self._last_finger_force = int(self.cfg.default_finger_force)
+        self._last_finger_speed = int(self.cfg.default_finger_speed)
+        self._last_finger_per_mille = int(self.cfg.finger_open_per_mille)
+
+    def connect(self) -> None:
+        if self._client is not None:
+            self.disconnect()
+        self._client = DHModbusClient(
+            self.cfg.port,
+            slave_id=self.cfg.slave_id,
+            baudrate=self.cfg.baudrate,
+            timeout=self.cfg.timeout,
+        )
+        self._connected = True
+        self._status.connected = True
+        self._status.state = GripperState.IDLE
+        self._status.error_msg = ""
+        if self.cfg.auto_initialize:
+            self.initialize()
+
+    def disconnect(self) -> None:
+        if self._client is not None:
+            self._client.close()
+        self._client = None
+        self._connected = False
+        self._status.connected = False
+        self._status.state = GripperState.IDLE
+
+    def initialize(self, full: bool = False) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout)
+        self._status.initialized = True
+
+    def _require_client(self) -> None:
+        if not self._connected or self._client is None:
+            self._status.state = GripperState.ERROR
+            self._status.error_msg = "旋转夹爪未连接"
+            raise DHModbusError(self._status.error_msg)
+
+    def execute(self, cmd: GripperCommand) -> GripperStatus:
+        """执行旋转指令；cmd.target_position 单位为度。"""
+        return self.rotate_to(
+            self._last_angle if cmd.target_position is None else float(cmd.target_position),
+            speed=cmd.target_velocity,
+            torque=cmd.target_force,
+            wait_complete=cmd.wait_complete,
+            timeout=cmd.timeout,
+        )
+
+    def _wait_complete(self, target_angle: float, timeout: float) -> None:
+        assert self._client is not None
+        start = time.monotonic()
+        start_angle = float(self._client.get_rotation_angle_deg())
+        expected = abs(float(target_angle) - start_angle) + 30.0
+        settled = 0
+        last_angle = start_angle
+        time.sleep(0.05)
+        while time.monotonic() - start < timeout:
+            rot_state = self._client.get_rotation_state()
+            angle = float(self._client.get_rotation_angle_deg())
+            self._status.rotation_state = rot_state
+            self._status.rotation_deg = angle
+            self._status.position = angle
+            self._status.is_moving = rot_state == 0
+            if abs(angle - start_angle) > expected:
+                self._client.hold_current_rotation()
+                self._status.state = GripperState.ERROR
+                self._status.error_msg = (
+                    f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止"
+                )
+                return
+            if rot_state in (1, 2):
+                self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING
+                if rot_state == 2:
+                    self._status.error_msg = "旋转受阻（blocking）"
+                return
+            if rot_state == 3:
+                self._status.state = GripperState.ERROR
+                self._status.error_msg = "旋转曾受阻且未到目标位"
+                return
+            if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg:
+                settled += 1
+                if settled >= 3:
+                    self._status.state = GripperState.IDLE
+                    self._status.is_moving = False
+                    return
+            elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg:
+                settled += 1
+                if settled >= 8 and abs(angle - float(target_angle)) <= 15.0:
+                    self._status.state = GripperState.IDLE
+                    self._status.is_moving = False
+                    return
+            else:
+                settled = 0
+            last_angle = angle
+            time.sleep(self.cfg.status_poll_interval)
+        self._client.hold_current_rotation()
+        self._status.state = GripperState.ERROR
+        self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止"
+
+    def get_status(self) -> GripperStatus:
+        if not self._connected or self._client is None:
+            self._status.connected = False
+            return self._status
+        try:
+            finger_pos = self._client.get_position_per_mille()
+            finger_state = self._client.get_gripper_state()
+            init_state = self._client.get_init_state()
+            rot_angle = self._client.get_rotation_angle_deg()
+            rot_state = self._client.get_rotation_state()
+            rot_init = self._client.get_rotation_init_state()
+
+            self._status.connected = True
+            self._status.initialized = init_state == 1 and rot_init == 1
+            self._status.finger_position_per_mille = float(finger_pos)
+            self._status.rotation_deg = float(rot_angle)
+            self._status.position = float(rot_angle)
+            self._status.velocity = float(self._last_speed)
+            self._status.force = float(self._last_torque)
+            self._status.gripper_state = finger_state
+            self._status.rotation_state = rot_state
+            self._status.is_moving = finger_state == 0 or rot_state == 0
+            self._status.is_gripped = finger_state == 2
+            if self._status.state != GripperState.ERROR:
+                if finger_state == 2 or rot_state == 2:
+                    self._status.state = GripperState.GRIPPING
+                elif finger_state == 0 or rot_state == 0:
+                    self._status.state = GripperState.MOVING
+                else:
+                    self._status.state = GripperState.IDLE
+            self._status.timestamp = time.time()
+        except Exception as exc:
+            self._status.state = GripperState.ERROR
+            self._status.error_msg = str(exc)
+            self._status.connected = False
+        return self._status
+
+    # -----------------------------
+    # 手指夹持
+    # -----------------------------
+    def move_fingers(self, position_per_mille: int, force: int | None = None,
+                     speed: int | None = None, wait_complete: bool = True,
+                     timeout: float = 10.0) -> GripperStatus:
+        try:
+            self._require_client()
+        except DHModbusError:
+            return self._status
+        assert self._client is not None
+        target = int(np.clip(position_per_mille, 0, 1000))
+        f = int(np.clip(force or self.cfg.default_finger_force, 20, 100))
+        s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100))
+        self._client.set_speed_pct(s)
+        self._client.set_force_pct(f)
+        self._client.set_position_per_mille(target)
+        self._last_finger_per_mille = target
+        self._last_finger_force = f
+        self._last_finger_speed = s
+        if wait_complete:
+            start = time.monotonic()
+            time.sleep(0.05)
+            while time.monotonic() - start < timeout:
+                state = self._client.get_gripper_state()
+                pos = self._client.get_position_per_mille()
+                self._status.gripper_state = state
+                self._status.finger_position_per_mille = float(pos)
+                if state in (1, 2):
+                    break
+                if state == 3:
+                    self._status.state = GripperState.ERROR
+                    self._status.error_msg = "夹持后物体掉落"
+                    break
+                time.sleep(self.cfg.status_poll_interval)
+        return self.get_status()
+
+    def open_fingers(self, **kwargs) -> GripperStatus:
+        return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs)
+
+    def close_fingers(self, **kwargs) -> GripperStatus:
+        return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs)
+
+    # -----------------------------
+    # 旋转
+    # -----------------------------
+    def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None,
+                  wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus:
+        try:
+            self._require_client()
+        except DHModbusError:
+            return self._status
+        assert self._client is not None
+
+        angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max))
+        s = int(np.clip(speed or self.cfg.default_speed, 1, 100))
+        t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max))
+
+        self._client.set_rotation_speed_pct(s)
+        self._client.set_rotation_force_pct(t)
+        target = self._client.set_rotation_angle_deg(angle)
+
+        self._last_angle = float(target)
+        self._last_speed = s
+        self._last_torque = t
+        self._status.state = GripperState.MOVING
+        self._status.error_msg = ""
+
+        if wait_complete:
+            self._wait_complete(float(target), timeout)
+        return self.get_status()
+
+    def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus:
+        current = self.get_status().rotation_deg
+        return self.rotate_to(current + float(delta_deg), **kwargs)
+
+    def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus:
+        """旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。"""
+        remaining = float(revolutions) * 360.0
+        status = self.get_status()
+        max_step = abs(float(self.cfg.max_step_deg)) or 30000.0
+        while abs(remaining) > 1e-6:
+            before = float(self.get_status().rotation_deg)
+            step = float(np.clip(remaining, -max_step, max_step))
+            status = self.rotate_to(before + step, **kwargs)
+            if status.state == GripperState.ERROR:
+                return status
+            after = float(status.rotation_deg)
+            achieved = after - before
+            if abs(achieved) < max(1.0, abs(step) * 0.2):
+                status.state = GripperState.ERROR
+                status.error_msg = (
+                    "旋转未达到预期步进，可能超出角度寄存器范围或反馈异常："
+                    f"step={step:.3f}, before={before:.3f}, after={after:.3f}"
+                )
+                return status
+            remaining -= achieved
+        return status
+
+    def home_rotation(self, **kwargs) -> GripperStatus:
+        return self.rotate_to(0.0, **kwargs)
+
+    def set_rotation_speed(self, speed_pct: int) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._last_speed = self._client.set_rotation_speed_pct(speed_pct)
+
+    def set_rotation_torque(self, torque_pct: int) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._last_torque = self._client.set_rotation_force_pct(torque_pct)
+
+    def set_finger_speed(self, speed_pct: int) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._last_finger_speed = self._client.set_speed_pct(speed_pct)
+
+    def set_finger_force(self, force_pct: int) -> None:
+        self._require_client()
+        assert self._client is not None
+        self._last_finger_force = self._client.set_force_pct(force_pct)
+
+    def open(self, **kwargs) -> GripperStatus:
+        """对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。"""
+        return self.open_fingers(**kwargs)
+
+    def close(self, **kwargs) -> GripperStatus:
+        """对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。"""
+        return self.close_fingers(**kwargs)
+
+    def stop(self) -> None:
+        if self._client is not None:
+            self._client.hold_current_all()
+        self._status.state = GripperState.IDLE
+
+
+# =============================================================================
+# 双臂夹爪管理器
+# =============================================================================
+
+@dataclass
+class DualArmGripperConfig:
+    """双臂夹爪配置。
+
+    这里**没有** ``left_arm_sn`` / ``right_arm_sn``：它们曾经存在但从来没有被读过
+    （只写不读的死字段，R5.5），而且默认值是本机的 SN —— 换台臂就会误导读者。
+    哪个侧别是哪台臂，由 ``load_from_site()`` 从 site.yaml 的 ``robots[].arm`` 决定。
+    """
+
+    left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
+    right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
+
+
+class DualArmGripperManager:
+    """双臂夹爪管理器：统一控制左右臂夹爪。"""
+
+    def __init__(self, config: DualArmGripperConfig | None = None) -> None:
+        self.cfg = config or DualArmGripperConfig()
+        self._left: ParallelGripper | None = None
+        self._right: RotaryGripper | None = None
+
+    @property
+    def left(self) -> ParallelGripper:
+        if self._left is None:
+            self._left = ParallelGripper(self.cfg.left_gripper)
+        return self._left
+
+    @property
+    def right(self) -> RotaryGripper:
+        if self._right is None:
+            self._right = RotaryGripper(self.cfg.right_gripper)
+        return self._right
+
+    def connect_all(self) -> None:
+        self.left.connect()
+        self.right.connect()
+
+    def disconnect_all(self) -> None:
+        if self._left:
+            self._left.disconnect()
+        if self._right:
+            self._right.disconnect()
+
+    def initialize_all(self, full: bool = False) -> None:
+        self.left.initialize(full=full)
+        self.right.initialize(full=full)
+
+    def get_all_status(self) -> dict[str, GripperStatus]:
+        return {
+            "left": self.left.get_status(),
+            "right": self.right.get_status(),
+        }
+
+    def emergency_stop(self) -> None:
+        self.left.stop()
+        self.right.stop()
+
+
+def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
+    """从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。"""
+    import yaml
+
+    data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {}
+    left = data.get("left_arm", {}) or {}
+    right = data.get("right_arm", {}) or {}
+    grip = data.get("gripper", {}) or {}
+    parallel = grip.get("parallel", {}) or {}
+    rotary = grip.get("rotary", {}) or {}
+
+    left_cfg = ParallelGripperConfig(
+        port=left.get("gripper_port", "/dev/ttyUSB0"),
+        slave_id=int(left.get("gripper_slave_id", 1)),
+        baudrate=int(left.get("gripper_baudrate", 115200)),
+        stroke_min=float(parallel.get("stroke_min", 0.0)),
+        stroke_max=float(parallel.get("stroke_max", 40.0)),
+        default_speed=int(parallel.get("default_speed", 50)),
+        default_force=int(parallel.get("default_force", 50)),
+        force_min=int(parallel.get("force_min", 20)),
+        force_max=int(parallel.get("force_max", 100)),
+        auto_initialize=bool(parallel.get("auto_initialize", True)),
+    )
+    right_cfg = RotaryGripperConfig(
+        port=right.get("gripper_port", "/dev/ttyUSB1"),
+        slave_id=int(right.get("gripper_slave_id", 1)),
+        baudrate=int(right.get("gripper_baudrate", 115200)),
+        angle_min=float(rotary.get("angle_min", -32767.0)),
+        angle_max=float(rotary.get("angle_max", 32767.0)),
+        max_step_deg=float(rotary.get("max_step_deg", 30000.0)),
+        finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)),
+        finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)),
+        default_speed=int(rotary.get("default_speed", 50)),
+        default_torque=int(rotary.get("default_torque", 50)),
+        default_finger_speed=int(rotary.get("default_finger_speed", 50)),
+        default_finger_force=int(rotary.get("default_finger_force", 50)),
+        torque_min=int(rotary.get("torque_min", 20)),
+        torque_max=int(rotary.get("torque_max", 100)),
+        auto_initialize=bool(rotary.get("auto_initialize", True)),
+    )
+    return DualArmGripperConfig(
+        left_gripper=left_cfg,
+        right_gripper=right_cfg,
+    )
+
+
+def load_from_site(data: dict) -> DualArmGripperConfig:
+    """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。
+
+    左右靠 ``robots[].arm`` 认，**不靠 SN 里的数字**：``"063989 就是左臂"``
+    是站点数据，写进代码后换个站点就会把左右夹爪搞反
+    （症状是"右臂的旋转爪去开左臂的平行爪"，串口对不上才报错）。
+    """
+    robots = list((data.get("robot") or {}).get("robots") or [])
+    left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
+    right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
+    g = data.get("gripper") or {}
+    parallel = g.get("parallel") or {}
+    rotary = g.get("rotary") or {}
+    lg = left.get("gripper") or {}
+    rg = right.get("gripper") or {}
+    stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0)
+    left_cfg = ParallelGripperConfig(
+        port=str(lg.get("port") or "/dev/ttyUSB0"),
+        slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1),
+        baudrate=int(lg.get("baud") or g.get("baud") or 115200),
+        stroke_min=float(parallel.get("stroke_min") or 0.0),
+        stroke_max=stroke,
+        default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50),
+        default_force=int(parallel.get("default_force") or g.get("force_pct") or 50),
+        force_min=int(parallel.get("force_min") or 20),
+        force_max=int(parallel.get("force_max") or 100),
+        auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))),
+        init_timeout=float(parallel.get("init_timeout") or 10.0),
+    )
+    right_cfg = RotaryGripperConfig(
+        port=str(rg.get("port") or "/dev/ttyUSB1"),
+        slave_id=int(rg.get("slave_id") or 1),
+        baudrate=int(rg.get("baud") or g.get("baud") or 115200),
+        angle_min=float(rotary.get("angle_min") or -32767.0),
+        angle_max=float(rotary.get("angle_max") or 32767.0),
+        max_step_deg=float(rotary.get("max_step_deg") or 30000.0),
+        finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000),
+        finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0),
+        default_speed=int(rotary.get("default_speed") or 50),
+        default_torque=int(rotary.get("default_torque") or 50),
+        default_finger_speed=int(rotary.get("default_finger_speed") or 50),
+        default_finger_force=int(rotary.get("default_finger_force") or 50),
+        torque_min=int(rotary.get("torque_min") or 20),
+        torque_max=int(rotary.get("torque_max") or 100),
+        auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))),
+        init_timeout=float(rotary.get("init_timeout") or 15.0),
+    )
+    return DualArmGripperConfig(
+        left_gripper=left_cfg,
+        right_gripper=right_cfg,
+    )

build_sim/arm_application/build/lib/arm_application/dual_gripper_cli.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import sys
+import time
+from pathlib import Path
+
+import yaml
+
+from arm_application.dual_gripper import DualArmGripperManager, load_from_site
+
+
+def _site_path() -> Path:
+    env = os.environ.get("SITE_CONFIG", "").strip()
+    if env:
+        return Path(env).expanduser()
+    return Path.home() / "ros2" / "config" / "site.yaml"
+
+
+def _print_status(label: str, status) -> None:
+    print(
+        f"   {label}: state={status.state.name} "
+        f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
+        f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
+        f"gripped={status.is_gripped} init={status.initialized} "
+        f"err={status.error_msg or '-'}"
+    )
+
+
+def _load_site_exporter():
+    """复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。
+
+    这里**不自己再抄一份** ``89 -> SN`` 的映射：抄一份就多一处会漂移的地方（M2）。
+    """
+    root = Path(__file__).resolve().parents[3]
+    lib = str(root / "scripts" / "lib")
+    if lib not in sys.path:
+        sys.path.insert(0, lib)
+    import export_site_env  # type: ignore
+
+    return export_site_env
+
+
+def _normalize_arm(raw: str, data: dict) -> str:
+    """把 --arm 的写法归一成 ``left`` / ``right`` / ``both``。
+
+    别名（``89`` 这类）由 ``export_site_env`` 从 site.yaml 的 ``robots[]`` 现算
+    （臂号 = 该臂 SN 末两位）。代码里**不写死** SN，否则换台机器人后会指错臂，
+    而症状是"给右臂发命令，左臂动了"。
+    """
+    t = (raw or "").strip()
+    if t.lower() in ("both", "dual", "all"):
+        return "both"
+    site_env = _load_site_exporter()
+    sn, matched = site_env.resolve_robot_sn(data, t)
+    if not matched:
+        raise SystemExit(
+            f"未知的 --arm {raw!r}：不在 site.yaml 的 robot.robots[] 里。\n"
+            f"可用：\n{site_env.describe_arms(data)}\n"
+            f"也可以直接写 left|right|both。"
+        )
+    for row in (data.get("robot") or {}).get("robots") or []:
+        if str(row.get("sn") or "").strip() != sn:
+            continue
+        arm = str(row.get("arm") or "").strip().lower()
+        if arm in ("left", "right"):
+            return arm
+        raise SystemExit(
+            f"--arm {raw!r} 选中了 {sn}，但 site.yaml 里这台臂没写 arm: left|right，"
+            f"无法判断操作哪个夹爪。请补上这个键。"
+        )
+    raise SystemExit(
+        f"--arm {raw!r} 解析出 {sn}，但它不在 site.yaml 的 robot.robots[] 里，"
+        f"无法判断是哪个夹爪。"
+    )
+
+
+def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
+    gripper = manager.left
+    print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
+    gripper.connect()
+    _print_status("已连接", gripper.get_status())
+    if args.test in ("init", "probe", "all"):
+        if args.test != "probe":
+            gripper.initialize(full=args.init_full)
+            _print_status("初始化", gripper.get_status())
+    if args.test in ("open", "all"):
+        _print_status("张开", gripper.open(force=args.force, speed=args.speed))
+    if args.test in ("close", "all"):
+        _print_status("闭合", gripper.close(force=args.force, speed=args.speed))
+    if args.test in ("move", "pos") and args.pos is not None:
+        _print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
+    if args.test in ("status", "probe"):
+        _print_status("状态", gripper.get_status())
+
+
+def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
+    gripper = manager.right
+    print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
+    gripper.connect()
+    _print_status("已连接", gripper.get_status())
+    if args.test in ("init", "all"):
+        gripper.initialize(full=args.init_full)
+        _print_status("初始化", gripper.get_status())
+    if args.test in ("open", "all"):
+        _print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
+    if args.test in ("close", "all"):
+        _print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed))
+    if args.test in ("move", "pos") and args.pos is not None:
+        _print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
+    if args.test in ("rotate", "rotate_to", "all"):
+        if args.revolutions is not None:
+            _print_status(
+                "圈数",
+                gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
+            )
+        else:
+            angle = args.angle if args.angle is not None else 90.0
+            _print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
+    if args.test == "rotate_by":
+        delta = args.angle if args.angle is not None else 45.0
+        _print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
+    if args.test in ("status", "probe"):
+        _print_status("状态", gripper.get_status())
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
+    parser.add_argument(
+        "--arm",
+        # 默认取当前臂（parse_arm_args.sh / load_site_config.sh 已经把它放进环境）。
+        # **不**兜底成某个具体臂号：那等于"没选臂时也能跑，跑的是哪台看运气"。
+        default=(
+            os.environ.get("SITE_ROBOT_ARM")
+            or os.environ.get("ROBOT_SN")
+            or os.environ.get("SITE_ROBOT_SN")
+            or ""
+        ),
+        help="left|right|both，或该臂的臂号（= SN 末两位，由 site.yaml 决定）",
+    )
+    parser.add_argument(
+        "--test",
+        default="status",
+        help="probe|init|open|close|move|pos|rotate|rotate_by|status",
+    )
+    parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000")
+    parser.add_argument("--angle", type=float, default=None)
+    parser.add_argument("--revolutions", type=float, default=None)
+    parser.add_argument("--speed", type=int, default=25)
+    parser.add_argument("--force", type=int, default=33)
+    parser.add_argument("--init-full", action="store_true")
+    parser.add_argument("--site", default="")
+    args = parser.parse_args()
+
+    site = Path(args.site).expanduser() if args.site else _site_path()
+    if not site.is_file():
+        print(f"错误：site.yaml 缺失：{site}", file=sys.stderr)
+        return 1
+    data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
+    manager = DualArmGripperManager(load_from_site(data))
+    arm = _normalize_arm(str(args.arm), data)
+    print(f"site={site} arm={arm} test={args.test}")
+
+    try:
+        if arm == "left":
+            test_parallel(manager, args)
+        elif arm == "right":
+            test_rotary(manager, args)
+        else:
+            test_parallel(manager, args)
+            test_rotary(manager, args)
+    except KeyboardInterrupt:
+        print("收到中断 —— 停止")
+        manager.emergency_stop()
+        return 130
+    except Exception as exc:
+        print(f"错误：{exc}", file=sys.stderr)
+        return 1
+    finally:
+        manager.disconnect_all()
+        time.sleep(0.05)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

build_sim/arm_application/build/lib/arm_application/dual_gripper_node.py

@@ -0,0 +1,320 @@
+"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
+
+from __future__ import annotations
+
+import os
+import threading
+from pathlib import Path
+
+import rclpy
+import yaml
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_application.dual_gripper import (
+    DualArmGripperManager,
+    GripperState,
+    load_dual_arm_gripper_config,
+    load_from_site,
+)
+from arm_msgs.msg import GripperCommand as GripperCommandMsg
+from arm_msgs.msg import GripperStatus as GripperStatusMsg
+from arm_msgs.srv import GripperExecute
+
+from arm_application.ros_node import TolerantNode
+
+
+def _site_path() -> Path:
+    env = os.environ.get("SITE_CONFIG", "").strip()
+    if env:
+        return Path(env).expanduser()
+    return Path.home() / "ros2" / "config" / "site.yaml"
+
+
+def _as_bool(val, default: bool = True) -> bool:
+    if val is None:
+        return default
+    if isinstance(val, str):
+        return val.strip().lower() in ("1", "true", "yes", "on")
+    return bool(val)
+
+
+class DualGripperNode(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__("dual_gripper_driver")
+        self.declare_parameter("config_file", "")
+        self.declare_parameter("auto_connect", True)
+        self.declare_parameter("allow_offline", True)
+        self.declare_parameter("publish_hz", 10.0)
+        self.declare_parameter("sides", "left,right")
+
+        self._cb_group = ReentrantCallbackGroup()
+        self._locks = {"left": threading.Lock(), "right": threading.Lock()}
+        sides_raw = str(self.get_parameter("sides").value or "left,right")
+        self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
+        if not self._sides:
+            self._sides = ["left", "right"]
+
+        cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
+        if cfg_path:
+            self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
+        else:
+            site = _site_path()
+            data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
+            self._manager = DualArmGripperManager(load_from_site(data))
+            self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
+
+        self._connect_errors: dict[str, str] = {}
+        if _as_bool(self.get_parameter("auto_connect").value, True):
+            self._connect_all()
+
+        self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
+        self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
+        if "left" in self._sides:
+            self.create_subscription(
+                GripperCommandMsg,
+                "~/left/command",
+                lambda msg: self._on_command("left", msg),
+                10,
+                callback_group=self._cb_group,
+            )
+            self.create_service(
+                GripperExecute,
+                "~/left/execute",
+                lambda req, resp: self._on_execute("left", req, resp),
+                callback_group=self._cb_group,
+            )
+        if "right" in self._sides:
+            self.create_subscription(
+                GripperCommandMsg,
+                "~/right/command",
+                lambda msg: self._on_command("right", msg),
+                10,
+                callback_group=self._cb_group,
+            )
+            self.create_service(
+                GripperExecute,
+                "~/right/execute",
+                lambda req, resp: self._on_execute("right", req, resp),
+                callback_group=self._cb_group,
+            )
+
+        period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
+        self.create_timer(period, self._publish_status, callback_group=self._cb_group)
+        self.get_logger().info(
+            f"dual_gripper_driver ready sides={self._sides} "
+            "~/left|right/command status execute"
+        )
+
+    def _connect_all(self) -> None:
+        mapping = (("left", self._manager.left), ("right", self._manager.right))
+        for side, gripper in mapping:
+            if side not in self._sides:
+                continue
+            try:
+                gripper.connect()
+                self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}")
+            except Exception as exc:
+                self._connect_errors[side] = str(exc)
+                self.get_logger().error(f"{side} 夹爪连接失败：{exc}")
+        if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
+            raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
+
+    def _gripper(self, side: str):
+        return self._manager.left if side == "left" else self._manager.right
+
+    def _execute(self, side: str, msg: GripperCommandMsg):
+        gripper = self._gripper(side)
+        command = (msg.command or "").strip().lower()
+        timeout = float(msg.timeout or 10.0)
+        wait = bool(msg.wait_complete)
+
+        if side in self._connect_errors and not gripper.is_connected():
+            status = gripper.get_status()
+            status.error_msg = self._connect_errors[side]
+            return status
+
+        if command == "connect":
+            gripper.connect()
+            self._connect_errors.pop(side, None)
+            return gripper.get_status()
+        if command == "init":
+            gripper.initialize(full=False)
+            return gripper.get_status()
+        if command == "init_full":
+            gripper.initialize(full=True)
+            return gripper.get_status()
+        if command == "stop":
+            gripper.stop()
+            return gripper.get_status()
+
+        if side == "left":
+            if command == "open":
+                return gripper.open(
+                    force=int(msg.force) if msg.force else None,
+                    speed=int(msg.speed) if msg.speed else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "close":
+                return gripper.close(
+                    force=int(msg.force) if msg.force else None,
+                    speed=int(msg.speed) if msg.speed else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "move":
+                return gripper.move_to(
+                    float(msg.position),
+                    force=int(msg.force) if msg.force else None,
+                    speed=int(msg.speed) if msg.speed else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "set_force":
+                gripper.set_force(int(msg.force))
+                return gripper.get_status()
+            if command == "set_speed":
+                gripper.set_speed(int(msg.speed))
+                return gripper.get_status()
+        else:
+            if command == "open":
+                return gripper.open_fingers(
+                    force=int(msg.force) if msg.force else None,
+                    speed=int(msg.speed) if msg.speed else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "close":
+                return gripper.close_fingers(
+                    force=int(msg.force) if msg.force else None,
+                    speed=int(msg.speed) if msg.speed else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "move":
+                return gripper.move_fingers(
+                    int(msg.position),
+                    force=int(msg.force) if msg.force else None,
+                    speed=int(msg.speed) if msg.speed else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command in ("rotate", "rotate_to"):
+                return gripper.rotate_to(
+                    float(msg.position),
+                    speed=int(msg.speed) if msg.speed else None,
+                    torque=int(msg.force) if msg.force else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "rotate_by":
+                return gripper.rotate_by(
+                    float(msg.position),
+                    speed=int(msg.speed) if msg.speed else None,
+                    torque=int(msg.force) if msg.force else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "rotate_revolutions":
+                return gripper.rotate_revolutions(
+                    float(msg.position),
+                    speed=int(msg.speed) if msg.speed else None,
+                    torque=int(msg.force) if msg.force else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "home_rotation":
+                return gripper.home_rotation(
+                    speed=int(msg.speed) if msg.speed else None,
+                    torque=int(msg.force) if msg.force else None,
+                    wait_complete=wait,
+                    timeout=timeout,
+                )
+            if command == "set_force":
+                gripper.set_rotation_torque(int(msg.force))
+                return gripper.get_status()
+            if command == "set_speed":
+                gripper.set_rotation_speed(int(msg.speed))
+                return gripper.get_status()
+            if command == "set_finger_force":
+                gripper.set_finger_force(int(msg.force))
+                return gripper.get_status()
+            if command == "set_finger_speed":
+                gripper.set_finger_speed(int(msg.speed))
+                return gripper.get_status()
+
+        status = gripper.get_status()
+        status.state = GripperState.ERROR
+        status.error_msg = f"不支持的命令 {command!r}（side={side}）"
+        return status
+
+    def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
+        with self._locks[side]:
+            status = self._execute(side, msg)
+        if status.state == GripperState.ERROR:
+            self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}")
+
+    def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
+        with self._locks[side]:
+            status = self._execute(side, request.command)
+        response.success = status.state != GripperState.ERROR and status.connected
+        response.message = status.error_msg or "OK"
+        response.status = self._to_status_msg(side, status)
+        return response
+
+    def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
+        gripper = self._gripper(side)
+        msg = GripperStatusMsg()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = f"{side}_gripper"
+        msg.name = gripper.name
+        msg.gripper_type = gripper.gripper_type.name.lower()
+        msg.state = status.state.name
+        msg.connected = bool(status.connected)
+        msg.initialized = bool(status.initialized)
+        msg.is_moving = bool(status.is_moving)
+        msg.is_gripped = bool(status.is_gripped)
+        msg.position = float(status.position)
+        msg.finger_position_per_mille = float(status.finger_position_per_mille)
+        msg.rotation_deg = float(status.rotation_deg)
+        msg.gripper_state = int(status.gripper_state)
+        msg.rotation_state = int(status.rotation_state)
+        msg.velocity = float(status.velocity)
+        msg.force = float(status.force)
+        msg.error_msg = status.error_msg or ""
+        return msg
+
+    def _publish_status(self) -> None:
+        if "left" in self._sides:
+            left_status = self._manager.left.get_status()
+            if "left" in self._connect_errors and not left_status.connected:
+                left_status.error_msg = self._connect_errors["left"]
+            self._left_pub.publish(self._to_status_msg("left", left_status))
+        if "right" in self._sides:
+            right_status = self._manager.right.get_status()
+            if "right" in self._connect_errors and not right_status.connected:
+                right_status.error_msg = self._connect_errors["right"]
+            self._right_pub.publish(self._to_status_msg("right", right_status))
+
+    def destroy_node(self) -> bool:
+        try:
+            self._manager.disconnect_all()
+        finally:
+            return super().destroy_node()
+
+
+def main() -> None:
+    rclpy.init()
+    node = DualGripperNode()
+    executor = MultiThreadedExecutor(num_threads=4)
+    try:
+        rclpy.spin(node, executor=executor)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

build_sim/arm_application/build/lib/arm_application/gauge_boot.py

@@ -0,0 +1,405 @@
+"""S00 开机自检：把"能不能跑"这件事变成一张**可打印的清单**（纯逻辑部分）。
+
+为什么要有这一阶段
+------------------
+看板 S00 的要求是"各服务就绪后才开指令窗，任一不就绪即中止并报告"。它挡的是
+这个仓库最常见的一类事故：**流程跑起来了，只是没接上硬件**。例如：
+
+* 触觉没起 → 三阶段的力阈值全是 0 → 搜孔"一直没找到"（实际是没数据）；
+* 相机没起 → 精拍拿不到图 → 报"精拍未拿到图像"（离真正原因差三层）；
+* 夹爪没起 → S08 松爪/夹紧静默失败 → 棘轮循环空转，看起来像"拧不动"。
+
+放纯逻辑里的原因
+----------------
+"要检查哪些东西"是**事实**，会随现场（两台臂、两种夹爪后端、D435 有没有标定）
+变化；"怎么检查"是 ROS 机制。前者做成纯函数，就能被测试钉住：
+
+* 少一项检查 → 测试红（比如"忘了查触觉"）；
+* 检查项的顺序无关紧要，但**必检项**和**选检项**的划分必须明确 ——
+  把核心判据依赖的触觉标成选检，就会在没传感器时一路跑到力判据才失败。
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Dict, Iterable, List, Mapping, Optional, Sequence, Tuple
+
+#: 探针类型 —— 决定节点用哪种方式确认"就绪"。
+#:
+#: ``service``     服务在线且类型匹配
+#: ``action``      action server 在线
+#: ``topic``       话题在，且**真的收到过消息**（只看 topic list 会假阳性）
+#: ``tactile``     触觉状态话题在发，且 ``connected=true``
+#: ``camera``      图像话题在发（收到首帧）
+#: ``detector``    算法权重能加载（推理预热）
+PROBES = ("service", "action", "topic", "tactile", "camera", "detector")
+
+#: ``detector`` 自检除了"权重文件在"，还要求这些 Python 模块能 import。
+#: 为什么必须查：权重文件在 ≠ 能推理。实测现场 —— 仿真环境（ros_gz_sim）里
+#: 没有 torch/ultralytics，S00 的"检测算法预热"照样报 OK，然后 S04 空转 150s
+#: 才报"没锁定"，看起来像算法或标定问题（真正的错在很远的另一处）。
+#: ``ultralytics`` 会连带把 torch 拉进来，查它就够；真要细分再看 yolo_detector。
+DETECTOR_MODULES = ("ultralytics",)
+
+#: 感知来源（``site.yaml`` 顶层 ``perception.source``）—— 谁产生"视觉测量"。
+#:
+#: ``yolo``     本体 perception_node（要 torch/ultralytics）
+#: ``truth``    仿真真值：按场景里已知的物体位姿直接算目标点，不跑推理
+#: ``external`` 外部节点按同一话题（``visual_servo/filtered_point``）提供
+#:
+#: ⚠️ 别和 ``yolo.backend`` 混：那个是"**怎么**推理"（tensorrt/onnx）。
+PERCEPTION_SOURCES = ("yolo", "truth", "external")
+
+
+def perception_source_of(perception_cfg: Optional[Mapping] = None) -> str:
+    """读 ``perception.source`` 并校验。**未知值直接报错，不默默当 yolo**。
+
+    默默退回 yolo 的后果：仿真里 perception_node 起不来，S00 又去查
+    "推理依赖"，报出来的错跟真正的原因（source 拼错）隔着两层。
+    """
+    src = str(((perception_cfg or {}) or {}).get("source") or "yolo").strip().lower()
+    if src not in PERCEPTION_SOURCES:
+        raise ValueError(
+            f"site.yaml 的 perception.source = {src!r} 不认识。"
+            f"只支持 {' / '.join(PERCEPTION_SOURCES)}"
+            "（yolo=本体推理，truth=仿真真值，external=外部节点提供）。"
+        )
+    return src
+
+
+def missing_modules(modules: Sequence[str] = DETECTOR_MODULES) -> List[str]:
+    """哪些模块当前解释器里 import 不到（纯逻辑，可离线测试）。"""
+    import importlib.util
+
+    out: List[str] = []
+    for m in modules:
+        try:
+            if importlib.util.find_spec(m) is None:
+                out.append(m)
+        except (ImportError, ValueError):
+            # find_spec 对"父包不存在/名字非法"会抛，这同样等于"用不了"
+            out.append(m)
+    return out
+
+
+def dependency_advice(missing: Sequence[str]) -> str:
+    """缺依赖时给**两条**出路：装它、或换掉这个后端。
+
+    只写"请安装 X"是不够的：仿真环境常常是离线的（这台机器实测 pip 直接
+    SSL 失败），那样的话现场需要的是"换一条不依赖它的路"，而不是反复重试安装。
+    """
+    mods = ", ".join(missing)
+    return (
+        f"缺推理依赖 {mods} —— 权重文件在也没用，导入不了就跑不了推理。两条路：\n"
+        f"  1) 装上：pip install {' '.join(missing)}（需要联网；本机仿真环境离线时做不到）\n"
+        "  2) 换后端：仿真里可以不用 YOLO —— 由外部按同一话题/同一类型提供感知结果\n"
+        "     （见 devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节）"
+    )
+
+
+@dataclass(frozen=True)
+class BootCheck:
+    """一项自检。"""
+
+    key: str
+    """稳定标识（日志/结果 JSON 里用它，改名要同步文档）。"""
+
+    label: str
+    """人读的名字。"""
+
+    probe: str
+    """见 :data:`PROBES`。"""
+
+    target: str
+    """要探测的对象：服务名 / action 名 / 话题名 / 权重路径。"""
+
+    why: str
+    """**缺了会怎样**。自检失败的提示必须写这个 —— 只报"未就绪"没人知道该不该急。"""
+
+    required: bool = True
+    """必检 / 选检。选检失败只告警，不拦流程。"""
+
+    expect_type: Optional[str] = None
+    """期望的消息/服务类型（有值时类型不符即算失败）。"""
+
+
+#: 需要"绝对名字"的探针：相对名字会按节点命名空间解析，
+#: 节点被放进 namespace 之后 ``home`` 会变成 ``/ns/home`` —— 自检就会去探一个
+#: 不存在的名字，报"未就绪"。统一补前导斜杠，把这件事变成不可能发生。
+_ABSOLUTE_PROBES = frozenset({"service", "action", "topic", "tactile", "camera"})
+
+
+def abs_name(name: str) -> str:
+    """把 ROS 名字规范成绝对名（``home`` → ``/home``）。
+
+    对外公开：调用方（节点）也要用同一套规范化去比较名字，否则会出现
+    "清单里是 /foo、参数里是 foo，比较不相等"这类只在运行时暴露的错。
+    """
+    n = (name or "").strip()
+    if not n:
+        return n
+    return n if n.startswith("/") else "/" + n
+
+
+def _topic(prefix: str, suffix: str) -> str:
+    """拼话题名，容忍 ``prefix`` 带不带前导斜杠。"""
+    p = "/" + prefix.strip("/") if prefix else ""
+    return f"{p}/{suffix.strip('/')}"
+
+
+def build_checks(
+    *,
+    joint_action: str,
+    camera: str,
+    tactile_topic: str,
+    gripper_open: str,
+    gripper_close: str,
+    gripper_status: str,
+    gripper_status_type: str,
+    d435_camera: Optional[str] = None,
+    tool_name: str = "",
+    weights_path: str = "",
+    perception_source: str = "yolo",
+) -> List[BootCheck]:
+    """按现场情况生成检查清单。
+
+    :param joint_action: JTC action 名（``rizon_arm_controller/follow_joint_trajectory``）。
+    :param camera: 腕部相机命名空间（``d405_left``）。
+    :param tactile_topic: 触觉状态话题（``/tactile/left/status``）。
+    :param gripper_status: 夹爪状态话题，随后端不同（``/gripper/status`` 或 ``/gripper/states``）。
+    :param d435_camera: 手外相机命名空间。给 None 表示这台臂没有手外相机 →
+        不生成 S03 相关的检查项（**不生成**比"生成了一个必然失败的项"更诚实：
+        后者会让每次启动都报一条无关的错，很快所有人都不看自检输出了）。
+    :param tool_name: ``site.yaml`` 的 ``robot.tool_name``。空 = 没指定工具系，
+        则 S01 的"工具系垂直向下锁定"无法确认 → 单独列为选检告警。
+    :param weights_path: 检测权重路径。空 = 不检查推理预热。
+    :param perception_source: ``site.yaml`` 的 ``perception.source``。**只有
+        ``yolo`` 才查推理预热**：truth/external 根本不跑推理，查了必然是
+        假阴性（"缺 ultralytics"），而那是预期之内的。这里刻意**不生成**
+        这一项，而不是生成一个永远 OK 的项 —— 后者会让人以为"感知也验过了"。
+
+    清单的顺序 = 打印顺序 = 排障顺序：先臂、再夹爪、再相机、再触觉、最后算法。
+    """
+    checks: List[BootCheck] = [
+        BootCheck(
+            key="arm_jtc",
+            label="机械臂关节轨迹控制器",
+            probe="action",
+            target=joint_action,
+            why="没有它，S01/S06/S07/S10 的所有运动都发不出去",
+            # 类型必须是 **control_msgs**：本流程（和 stage2/stage3/pipeline 一样）
+            # 直接把轨迹发给 ros2_control 的 JTC 服务端，不经过 arm_control 的
+            # 轨迹适配器（那个用的是 arm_msgs/action/FollowJointTrajectory）。
+            # 写成 arm_msgs 的会得到一个"永远探不到服务端"的自检 —— 假阴性，
+            # 而且看起来像控制器没起来。
+            expect_type="control_msgs/action/FollowJointTrajectory",
+        ),
+        BootCheck(
+            key="arm_home",
+            label="回零服务",
+            probe="service",
+            target="/home",
+            why="S01 回 home、S11 复位都靠它",
+            expect_type="std_srvs/srv/Trigger",
+        ),
+        BootCheck(
+            key="arm_named_pose",
+            label="命名位姿服务",
+            probe="service",
+            target="/set_named_pose",
+            why="/home 内部转发给它；它不在时回零会超时",
+            expect_type="arm_msgs/srv/SetNamedPose",
+        ),
+        BootCheck(
+            key="arm_compliant",
+            label="柔顺下压 action",
+            probe="action",
+            target="/compliant_descent",
+            # 选检：S07 现在走节点内的"步进 + 力突变停"（与这个 action 内部
+            # 同一套做法），不依赖它；有它时驱动侧原语更顺，属于加分项。
+            # 标成必检会让仿真（arm_driver_flexiv 不参与）永远过不了自检。
+            why="有它时 S07 用驱动侧柔顺原语；没有则用节点内步进（不影响正确性）",
+            required=False,
+            expect_type="arm_msgs/action/CompliantDescent",
+        ),
+        BootCheck(
+            key="gripper_open",
+            label="夹爪张开服务",
+            probe="service",
+            target=gripper_open,
+            why="S08 棘轮的『松爪』一步靠它；失败会让回转把规拧回去",
+        ),
+        BootCheck(
+            key="gripper_close",
+            label="夹爪夹紧服务",
+            probe="service",
+            target=gripper_close,
+            why="S08 的『夹紧』一步靠它；失败会让慢转时规打滑（磨螺纹）",
+        ),
+        BootCheck(
+            key="camera_color",
+            label="腕部相机彩图",
+            probe="camera",
+            target=_topic(camera, "color/image_raw"),
+            why="S05 精拍定位 k1/k2 靠它；缺了只能靠 S03 粗定位（精度不够）",
+            expect_type="sensor_msgs/msg/Image",
+        ),
+        BootCheck(
+            key="camera_info",
+            label="腕部相机内参",
+            probe="topic",
+            target=_topic(camera, "color/camera_info"),
+            why="没有内参就无法把像素反投影成基座坐标（会退化成 60° 默认视场）",
+            expect_type="sensor_msgs/msg/CameraInfo",
+        ),
+        BootCheck(
+            key="tactile_status",
+            label="触觉三向力",
+            probe="tactile",
+            target=tactile_topic,
+            why="S07 切向力突变停、S08 拧紧判据、E1 掉件都依赖它；"
+            "缺了这几个判据全部失效（流程能走完但结果不可信）",
+            expect_type="arm_msgs/msg/TactileStatus",
+        ),
+        BootCheck(
+            key="gripper_status",
+            label="夹爪状态反馈",
+            probe="topic",
+            target=gripper_status,
+            why="E1 掉件检测靠它；缺了掉件只能靠人看",
+            expect_type=gripper_status_type,
+        ),
+    ]
+
+    if d435_camera:
+        checks.append(
+            BootCheck(
+                key="coarse_camera",
+                label="手外粗定位相机",
+                probe="camera",
+                target=_topic(d435_camera, "color/image_raw"),
+                why="S03 兜底粗定位用；缺了只能在 S04 跟踪上多试几次",
+                required=False,
+                expect_type="sensor_msgs/msg/Image",
+            )
+        )
+    if tool_name:
+        checks.append(
+            BootCheck(
+                key="tool_frame",
+                label="工具坐标系已指定",
+                probe="service",   # 具体内容由节点按 tool_name 复核（只查服务在不在）
+                target="/switch_tool",
+                why=f"要求 tool1（垂直向下、位姿锁定），site.yaml 里配的是 {tool_name!r}",
+                required=False,
+                expect_type="arm_msgs/srv/SwitchTool",
+            )
+        )
+        # 配了工具系就必须能切：S01 切不过去会直接 TIMEOUT 中止，
+        # 所以在自检里按**必检**拦下（早报比跑到 S01 再报少一次上电动作）。
+        checks.append(
+            BootCheck(
+                key="arm_switch_tool",
+                label="工具系切换服务",
+                probe="service",
+                target="/switch_tool",
+                why=f"site.yaml 指定了工具 {tool_name!r}，切不过去后面全是歪的",
+                expect_type="arm_msgs/srv/SwitchTool",
+            )
+        )
+    else:
+        # 没配工具系：S01 只会告警不会切，所以这条只算选检 ——
+        # 真机没配工具系是**配置问题**（看板要求 tool1），但仿真里根本没有
+        # 工具系切换服务，标必检会让仿真永远过不去。
+        checks.append(
+            BootCheck(
+                key="arm_switch_tool",
+                label="工具系切换服务",
+                probe="service",
+                target="/switch_tool",
+                why="本机没配 robot.tool_name：真机应配 tool1 并确认这个服务在；仿真不需要",
+                required=False,
+                expect_type="arm_msgs/srv/SwitchTool",
+            )
+        )
+    if weights_path and perception_source == "yolo":
+        checks.append(
+            BootCheck(
+                key="detector",
+                label="检测算法预热",
+                probe="detector",
+                target=weights_path,
+                why="S04 跟踪、S03 粗定位都要它；预热失败会在第一次跟踪时才炸",
+            )
+        )
+
+    # 网络端点统一成绝对名（见 _ABSOLUTE_PROBES 的说明）。
+    # detector 的 target 是文件路径，不参与规范化。
+    return [
+        c if c.probe not in _ABSOLUTE_PROBES
+        else BootCheck(
+            key=c.key, label=c.label, probe=c.probe, target=abs_name(c.target),
+            why=c.why, required=c.required, expect_type=c.expect_type,
+        )
+        for c in checks
+    ]
+
+
+def format_report(results: Sequence[Tuple[BootCheck, bool, str]]) -> Tuple[str, List[str]]:
+    """把逐项结果排成一张表，返回 ``(表文本, 失败项描述)``。
+
+    ``results`` 每项是 ``(检查项, 是否就绪, 细节)``。
+
+    两张输出分开是为了让调用方能按"必检失败"决定是否中止：表用来给人看，
+    失败项描述用来进日志/结果 JSON。
+    """
+    lines: List[str] = ["S00 自检："]
+    failures: List[str] = []
+    for item, ok, detail in results:
+        mark = "OK  " if ok else ("缺失" if item.required else "告警")
+        tag = "" if item.required else "（选检）"
+        lines.append(f"  [{mark}] {item.label}{tag}  {item.target}")
+        if detail:
+            lines.append(f"         {detail}")
+        if not ok:
+            prefix = "必检项未就绪" if item.required else "选检项未就绪"
+            failures.append(f"{prefix}：{item.label}（{item.target}）—— {item.why}")
+    if failures:
+        lines.append(f"  共 {len(failures)} 项未就绪")
+    else:
+        lines.append("  全部就绪，开指令窗")
+    return "\n".join(lines), failures
+
+
+def blocking_failures(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
+    """只挑出**必检且未就绪**的项 —— 决定要不要中止的就是它们。
+
+    和 :func:`format_report` 的 ``failures`` 的区别：那里两项都收（含选检），
+    这里只收必检。合成一个函数的话，调用方一不留神就会拿选检失败去中止流程，
+    于是"D435 没接"变成"装配跑不起来"。
+    """
+    return [
+        f"必检项未就绪：{item.label}（{item.target}）—— {item.why}"
+        for item, ok, _detail in results
+        if (not ok) and item.required
+    ]
+
+
+def optional_warnings(results: Iterable[Tuple[BootCheck, bool, str]]) -> List[str]:
+    """选检未就绪的告警（不拦流程，但必须打出来）。"""
+    return [
+        f"选检项未就绪：{item.label}（{item.target}）—— {item.why}"
+        for item, ok, _detail in results
+        if (not ok) and not item.required
+    ]
+
+
+def summary_counts(results: Sequence[Tuple[BootCheck, bool, str]]) -> Dict[str, int]:
+    """统计``必检/选检``各自通过多少，给启动日志一行摘要。"""
+    out = {"required_ok": 0, "required_total": 0, "optional_ok": 0, "optional_total": 0}
+    for item, ok, _detail in results:
+        slot = "required" if item.required else "optional"
+        out[f"{slot}_total"] += 1
+        if ok:
+            out[f"{slot}_ok"] += 1
+    return out

build_sim/arm_application/build/lib/arm_application/gauge_config.py

@@ -0,0 +1,591 @@
+"""通止规装配的配置：从 ``site.yaml`` 的 ``gauge_assembly:`` 段解析 + 校验。
+
+为什么单独一个模块
+------------------
+这一段的键**全是会动的量**（力阈值、角度、悬高、超时），也是现场最常改的地方。
+散在节点里读的话会有三个已知坑（都被这个仓库踩过）：
+
+1. **名字与单位不一致** —— ``hover`` 填了毫米、代码按米用（规则 M3）；
+2. **键没接线** —— YAML 里写着 ``screw_step_deg``，代码读的是 ``screw_steps``，
+   于是改了没效果，而报错也没有（``config_guard`` 模块的头注释就是这件事）；
+3. **拼错键名** —— YAML 静默接受，行为退回默认值，现场"改了参数没反应"。
+
+所以：所有键在一张表里声明（含单位），解析时**逐条校验**，多出来的键直接
+报错并给出最相近的候选名，缺的键按 required/default 明确处理。
+``describe()`` 把**派生**结果（比如"逆时针最多能转 147°"）打出来，让现场
+看到的是"实际生效的值"，而不是"我填的值"。
+"""
+
+from __future__ import annotations
+
+import difflib
+from dataclasses import dataclass, fields
+from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple
+
+from arm_application.gauge_fsm import descend_tol_m
+
+#: S05 稳定判据可用的特征来源。``auto`` 按 ``perception.source`` 自动选。
+#:
+#: 为什么需要有"motion"这一路：判据原本是**工件检测框中心**的抖动（像素），
+#: 那要求检测器真的出框。仿真里合成图过不了真机训练出来的模型（实测最高分
+#: 0.017），于是 S05 会永远"不稳定"地空转 —— 而真正该等的是"相机不再动"。
+#: 用法兰位移（mm）当特征既直接又不依赖模型，且判据本身（到均值的最远距离）不变。
+SNAP_STABLE_FEATURES = ("auto", "bbox", "motion")
+
+def snap_feature_of(want: str, perception_source: str) -> str:
+    """把 ``snap_stable_feature``（可能是 ``auto``）+ 感知来源 → 实际用哪个特征。
+
+    抽成纯函数是为了能脱离 ROS 节点测：这个选择错了，S05 会在真机上"永远不稳定"
+    或者在仿真里空转到超时，而两种表现都像是"机械臂在晃"，很难往配置上想。
+    """
+    want = str(want or "auto")
+    if want != "auto":
+        return want
+    return "bbox" if perception_source == "yolo" else "motion"
+
+
+#: 合法的单位后缀。键名必须以 ``_<单位>`` 结尾（``au`` = Orisys 任意单位增量）。
+_UNIT_SUFFIX: Dict[str, str] = {
+    "s": "_s",
+    "deg": "_deg",
+    "mm": "_mm",
+    "m": "_m",
+    "px": "_px",
+    "pct": "_pct",
+    "au": "_delta",
+    # 腕部 FT 的**真牛顿**。与 au 分开一个后缀是刻意的：两个口径混用不会报错，
+    # 只会"判据不触发"或"一碰就炸"，看起来像算法问题（见 site.yaml 的注释）。
+    "N": "_n",
+}
+
+#: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** ——
+#: 没有单位又不登记的键，会让"单位自检"变成一句空话（规则 M1）。
+_UNITLESS = frozenset(
+    {
+        "extreme_direction",
+        "grip_check",
+        "coarse_enabled",
+        "coarse_camera",
+        "coarse_conf",
+        "snap_min_frames",
+        "snap_stable_feature",
+        "screw_max_cycles",
+        "unscrew_extra_cycles",
+        "holes_total",
+        "dry_run",
+    }
+)
+
+#: 每个键的**类型**（用于类型校验，不做隐式转换：``"20"`` 不当 20 用）。
+_TYPES: Dict[str, type] = {
+    "boot_timeout_s": float,
+    "extreme_angle_deg": float,
+    "extreme_speed_deg_s": float,
+    "extreme_direction": int,
+    "extreme_margin_deg": float,
+    "grip_check": bool,
+    "grip_hold_s": float,
+    "coarse_enabled": bool,
+    "coarse_camera": str,
+    "coarse_hover_m": float,
+    "coarse_conf": float,
+    "coarse_timeout_s": float,
+    "snap_stable_s": float,
+    "snap_tol_px": float,
+    "snap_tol_mm": float,
+    "snap_min_frames": int,
+    "snap_stable_feature": str,
+    "align_tol_mm": float,
+    "contact_speed_m_s": float,
+    "contact_step_mm": float,
+    "contact_max_depth_mm": float,
+    "tool_tip_below_flange_m": float,
+    "workpiece_surface_z_m": float,
+    "contact_tangential_delta": float,
+    "contact_axial_delta_n": float,
+    "contact_relief_mm": float,
+    "contact_hold_n": float,
+    "contact_safe_delta": float,
+    "screw_step_deg": float,
+    "screw_speed_deg_s": float,
+    "screw_back_speed_deg_s": float,
+    "screw_max_cycles": int,
+    "screw_normal_delta": float,
+    "screw_shear_delta": float,
+    "screw_grip_force_pct": float,
+    "screw_grip_speed_pct": float,
+    "screw_release_wait_s": float,
+    "unscrew_extra_cycles": int,
+    "holes_total": int,
+    "dry_run": bool,
+    "reload_timeout_s": float,
+}
+
+#: 每个键的单位。``""`` 表示无量纲（须同时登记在 :data:`_UNITLESS`）。
+_UNITS: Dict[str, str] = {
+    "boot_timeout_s": "s",
+    "extreme_angle_deg": "deg",
+    "extreme_speed_deg_s": "deg/s",
+    "extreme_direction": "",
+    "extreme_margin_deg": "deg",
+    "grip_check": "",
+    "grip_hold_s": "s",
+    "coarse_enabled": "",
+    "coarse_camera": "",
+    "coarse_hover_m": "m",
+    "coarse_conf": "",
+    "coarse_timeout_s": "s",
+    "snap_stable_s": "s",
+    "snap_tol_px": "px",
+    "snap_tol_mm": "mm",
+    "snap_min_frames": "",
+    "snap_stable_feature": "",
+    "align_tol_mm": "mm",
+    "contact_speed_m_s": "m/s",
+    "contact_step_mm": "mm",
+    "contact_max_depth_mm": "mm",
+    # 这两个是**长度**不是"毫米量"：用 m 口径，与 hover_z 同一把尺
+    "tool_tip_below_flange_m": "m",
+    "workpiece_surface_z_m": "m",
+    "contact_tangential_delta": "au",
+    # 腕部 FT 是**真牛顿**（与触觉的 au 不是一个口径，所以键名也不带 taut 前缀）
+    "contact_axial_delta_n": "N",
+    # 卸载回退是**长度**（与 step_mm 同一把尺）
+    "contact_relief_mm": "mm",
+    # 残余轴向力是**牛顿**（腕部 FT 口径，不是触觉的 au）
+    "contact_hold_n": "N",
+    "contact_safe_delta": "au",
+    "screw_step_deg": "deg",
+    "screw_speed_deg_s": "deg/s",
+    "screw_back_speed_deg_s": "deg/s",
+    "screw_max_cycles": "",
+    "screw_normal_delta": "au",
+    "screw_shear_delta": "au",
+    "screw_grip_force_pct": "pct",
+    "screw_grip_speed_pct": "pct",
+    "screw_release_wait_s": "s",
+    "unscrew_extra_cycles": "",
+    "holes_total": "",
+    "dry_run": "",
+    "reload_timeout_s": "s",
+}
+
+#: 键名后缀与单位必须一致（防"填毫米当米用"）。``contact_speed_m_s`` 这类
+#: 复合单位只检查主单位段。
+_SUFFIX_OVERRIDE: Dict[str, str] = {
+    # 复合单位：后缀取"最后一个单位段"，键名里写全
+    "contact_speed_m_s": "_m_s",
+    "extreme_speed_deg_s": "_deg_s",
+    "screw_speed_deg_s": "_deg_s",
+    "screw_back_speed_deg_s": "_deg_s",
+}
+
+
+class GaugeConfigError(ValueError):
+    """``gauge_assembly:`` 段配置错误。消息里必须给出**改哪里**。"""
+
+
+# =============================================================================
+# 默认值
+# =============================================================================
+#: 缺省值。**只有真的有现场依据的才写默认值**：力阈值这类必须现场标定的，
+#: 默认值只是"能跑起来的起点"，必须在 ``describe()`` 里标出来让现场知道要改。
+DEFAULTS: Dict[str, Any] = {
+    "boot_timeout_s": 30.0,
+    # S01：逆时针预绕。符号靠 extreme_direction 定，不靠改代码里的负号。
+    "extreme_angle_deg": 150.0,
+    "extreme_speed_deg_s": 20.0,
+    "extreme_direction": 1,
+    "extreme_margin_deg": 3.0,
+    # S02（预留）
+    "grip_check": False,
+    "grip_hold_s": 0.5,
+    # S03
+    "coarse_enabled": True,
+    "coarse_camera": "d435",
+    "coarse_hover_m": 0.25,
+    "coarse_conf": 0.35,
+    "coarse_timeout_s": 20.0,
+    # S05
+    "snap_stable_s": 1.0,
+    "snap_tol_px": 2.0,
+    "snap_tol_mm": 0.4,
+    "snap_min_frames": 6,
+    "snap_stable_feature": "auto",
+    # S06
+    "align_tol_mm": 1.0,
+    # S07
+    "contact_speed_m_s": 0.02,
+    "contact_step_mm": 0.5,
+    "contact_max_depth_mm": 35.0,
+    "tool_tip_below_flange_m": 0.243,
+    "workpiece_surface_z_m": 0.0,   # 0 = 未知，运行期用 stage2 实测孔位兜底
+    "contact_tangential_delta": 300.0,
+    # 腕部 FT 的轴向力阈值 [N]。**这是 S07 的首选判据**，触觉只是兜底。
+    #
+    # 为什么默认是 8N 这么小：这是"接触了没有"的判据，不是"压到位了没有"。
+    # 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有 5N、0.2mm 有 10N ——
+    # 所以 8N 对应约 0.16mm 的过压，**在打滑之前**就能判到（打滑上限由
+    # µ·N 决定：µ=0.25、N≈46N → 两指合计 ~23N ≈ 0.46mm）。留了 3 倍余量。
+    # 现场标定法：空载慢慢压到台肩落面，看日志里"腕部轴向"从基线涨到多少，
+    # 取那个值的 1/3 作为阈值（要能区分"擦到孔壁"和"台肩落面"）。
+    "contact_axial_delta_n": 8.0,
+    # 接触到之后的**卸载回退** [mm]。
+    #
+    # 为什么必须有这一步（2026-09-24 一整轮排查换来的）：
+    #   位置控制的臂在"撞上硬面"时会把力顶到自己的**出力上限**才停住 ——
+    #   仿真实测：一步 0.5mm 的指令压在工件上，腕部 FT 读数直接跳到 **+244N**，
+    #   而臂这一步根本没走完（"实测只走了 +0.09mm"）。这不是仿真特有的：
+    #   接触刚度越高、步长越大，峰值力就越大，而位置环**不限制**它。
+    #
+    #   规是**靠夹持摩擦**抱住的，轴向能传的力上限是 µ·N（两指合计 ~73N@µ=0.8）。
+    #   244N 远超这个上限 ⇒ 规被轴向力**顶出爪外**（本次就是：S08 一转，
+    #   规直接掉到地上，报 E1）。所以"判到接触"之后不能就那样顶着，
+    #   必须先把下压量放掉，再交给 S08 拧入。
+    #
+    # 0.5mm 的来历：判到接触时的**真实**过压量通常只有 0.0x mm（刚度 5e4 N/m
+    # 时，10N 对应 0.2mm），回退 0.5mm 足够把力放干净，而规尖仍在孔里
+    # （通端穿板 18mm），不会脱出。
+    "contact_relief_mm": 0.5,
+    # 卸载后允许的残余轴向力 [N]。
+    # 超了不失败（可能只是回退量不够），但要**大声报**：它说明"夹持摩擦力不够
+    # 承受这次接触"，接着 S08 一转就会掉件。调法只有两条 —— 降峰值（调小
+    # contact_step_mm / contact_relief_mm）或加握力（改夹爪合拢力度/指垫）。
+    "contact_hold_n": 5.0,
+    "contact_safe_delta": 2000.0,
+    # S08
+    # 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
+    # 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
+    "screw_step_deg": 120.0,
+    "screw_speed_deg_s": 30.0,
+    "screw_back_speed_deg_s": 120.0,
+    "screw_max_cycles": 12,
+    "screw_normal_delta": 400.0,
+    "screw_shear_delta": 600.0,
+    "screw_grip_force_pct": 50.0,
+    "screw_grip_speed_pct": 50.0,
+    "screw_release_wait_s": 0.3,
+    # S09
+    "unscrew_extra_cycles": 1,
+    # 全局
+    "holes_total": 2,
+    "dry_run": False,
+    # E1（掉件）重放：等操作员放好并确认 /gauge/reload_ok 的最长时间。
+    # 超时 = 中止（臂在 home，人不在旁边时"继续等"比"停"更危险）。
+    "reload_timeout_s": 180.0,
+}
+
+
+@dataclass(frozen=True)
+class GaugeConfig:
+    """通止规装配的全部可调量（解析并校验之后）。"""
+
+    boot_timeout_s: float
+    extreme_angle_deg: float
+    extreme_speed_deg_s: float
+    extreme_direction: int
+    extreme_margin_deg: float
+    grip_check: bool
+    grip_hold_s: float
+    coarse_enabled: bool
+    coarse_camera: str
+    coarse_hover_m: float
+    coarse_conf: float
+    coarse_timeout_s: float
+    snap_stable_s: float
+    snap_tol_px: float
+    snap_tol_mm: float
+    snap_min_frames: int
+    snap_stable_feature: str
+    align_tol_mm: float
+    contact_speed_m_s: float
+    contact_step_mm: float
+    contact_max_depth_mm: float
+    tool_tip_below_flange_m: float
+    workpiece_surface_z_m: float
+    contact_tangential_delta: float
+    contact_axial_delta_n: float
+    contact_relief_mm: float
+    contact_hold_n: float
+    contact_safe_delta: float
+    screw_step_deg: float
+    screw_speed_deg_s: float
+    screw_back_speed_deg_s: float
+    screw_max_cycles: int
+    screw_normal_delta: float
+    screw_shear_delta: float
+    screw_grip_force_pct: float
+    screw_grip_speed_pct: float
+    screw_release_wait_s: float
+    unscrew_extra_cycles: int
+    holes_total: int
+    dry_run: bool
+    reload_timeout_s: float
+
+    # -- 派生量（不占 YAML 键，免得两处各写一份，规则 M2）-------------------
+    @property
+    def screw_step_rad(self) -> float:
+        import math
+
+        return math.radians(self.screw_step_deg)
+
+    @property
+    def total_screw_deg(self) -> float:
+        """一根规要拧进去的总角度 = 单次转角 × 循环数。"""
+        return self.screw_step_deg * self.screw_max_cycles
+
+    @property
+    def unscrew_cycles(self) -> int:
+        """拧出要多转几轮：多留 ``unscrew_extra_cycles`` 轮，确保螺纹完全脱开。"""
+        return self.screw_max_cycles + max(0, self.unscrew_extra_cycles)
+
+    def describe(self) -> List[str]:
+        """启动时打印的行：**实际生效的**值与派生结论。
+
+        特别是 S01 的"能转多少度"和 S08 的"要拧多少轮"—— 这两个数是现场
+        判断"参数是否真的生效"最直接的依据，比打印"我读了 site.yaml"有用。
+        """
+        return [
+            f"通止规装配：每孔孔数 {self.holes_total}，干跑={self.dry_run}",
+            f"  S01 预绕 {self.extreme_angle_deg:.1f}°（方向={'逆时针' if self.extreme_direction > 0 else '顺时针'}，"
+            f"限位余量 {self.extreme_margin_deg:.1f}°，关节速度 {self.extreme_speed_deg_s:.1f}°/s）",
+            f"  S03 粗定位={'开' if self.coarse_enabled else '关'} 相机={self.coarse_camera} "
+            f"悬高 {self.coarse_hover_m * 1000:.0f}mm 置信度≥{self.coarse_conf:.2f}",
+            f"  S05 稳定 {self.snap_stable_s:.1f}s / 至少 {self.snap_min_frames} 帧 / "
+            f"特征={self.snap_stable_feature}"
+            f"（检测框 ≤{self.snap_tol_px:.1f}px、法兰位移 ≤{self.snap_tol_mm:.2f}mm）"
+            "（稳定判据用「到均值的最远距离」，不是相邻帧之差）",
+            # 悬停高度**不在这里**：它取自 servo.hole_hover_z_m（stage2 那一层），
+            # 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。
+            f"  S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）",
+            f"  S07 腕部轴向突变阈值 +{self.contact_axial_delta_n:.0f}N（**首选判据**，"
+            "腕部 FT 在夹持摩擦的上游，工具打滑也测得准）,"
+            f"切向突变阈值 +{self.contact_tangential_delta:.0f}au（触觉兜底，两通道任一，现场标定），"
+            f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm",
+            f"  S07 接触后卸载：回退 {self.contact_relief_mm:.2f}mm，"
+            f"残余轴向力要求 ≤{self.contact_hold_n:.1f}N"
+            "（位置控制的臂撞上硬面会把力顶到出力上限再停住，实测能到 200N+；"
+            "规靠夹持摩擦抱住，轴向上限只有 µ·N≈73N，顶着不放 S08 一转就掉件）",
+            f"  S07 工具几何：尖端在法兰下方 {self.tool_tip_below_flange_m * 1000:.1f}mm，"
+            f"工件上表面 z={self.workpiece_surface_z_m:.3f}m"
+            "（启动时会按这两个值 + servo.hole_hover_z_m 算「悬停时工具到底入没入工件」，"
+            "入了会**明确报错**——见 gauge_fsm.tool_clearance_m）",
+            f"  S07 步进 {self.contact_step_mm:.2f}mm @ {self.contact_speed_m_s * 1000:.0f}mm/s，"
+            f"到位容差 {descend_tol_m(self.contact_step_mm / 1000.0) * 1000:.3f}mm"
+            "（=步长的 40%，**推导出来的、不是可调项**：容差 ≥ 步长会让每一步都被判成"
+            "「已在容差内」，臂一步都不走）",
+            f"  S08 棘轮 {self.screw_step_deg:.0f}°×{self.screw_max_cycles} 轮 = 累计 "
+            f"{self.total_screw_deg:.0f}°；停止判据「切向 >+{self.screw_shear_delta:.0f} 且 "
+            f"法向 >+{self.screw_normal_delta:.0f}」",
+            f"  S09 拧出 {self.unscrew_cycles} 轮（比拧入多 {self.unscrew_extra_cycles} 轮）",
+        ]
+
+
+def _resolve_one(key: str, raw: Mapping[str, Any]) -> Tuple[Any, bool]:
+    """取一个键的值，返回 ``(值, 是否来自 site.yaml)``。"""
+    if key in raw:
+        value = raw[key]
+    elif key in DEFAULTS:
+        return DEFAULTS[key], False
+    else:
+        raise GaugeConfigError(
+            f"gauge_assembly.{key} 既没在 site.yaml 里给，也没有默认值。"
+            f"请在 config/site.yaml 的 gauge_assembly: 段补上"
+            f"（改完跑一次 bash scripts/show_site_config.sh 确认解析）"
+        )
+
+    want = _TYPES[key]
+    if want is bool and not isinstance(value, bool):
+        raise GaugeConfigError(_type_msg(key, value, "布尔 true/false"))
+    if want is int and (isinstance(value, bool) or not isinstance(value, int)):
+        raise GaugeConfigError(_type_msg(key, value, "整数"))
+    if want is float and (isinstance(value, bool) or not isinstance(value, (int, float))):
+        raise GaugeConfigError(_type_msg(key, value, "数字（可带小数）"))
+    if want is str and not isinstance(value, str):
+        raise GaugeConfigError(_type_msg(key, value, "字符串"))
+    return (float(value) if want is float else value), True
+
+
+def _type_msg(key: str, value: Any, human: str) -> str:
+    unit = _UNITS[key]
+    hint = f"，单位 {unit}" if unit else ""
+    return (
+        f"gauge_assembly.{key} 的类型不对：期望 {human}{hint}，"
+        f"实际读到 {value!r}（{type(value).__name__}）。"
+        "本模块不做隐式转换 —— \"20\" 不会被当成 20 用，"
+        "因为那正是「填了字符串、跑起来是默认值」这类静默失效的入口"
+    )
+
+
+def from_site(site: Mapping[str, Any], *, overrides: Optional[Mapping[str, Any]] = None) -> GaugeConfig:
+    """解析并校验 ``site.yaml`` 的 ``gauge_assembly:`` 段。
+
+    :param site: ``site.yaml`` 解析后的整份字典。
+    :param overrides: CLI/launch 覆盖（只允许覆盖已知键，未知键报错）。
+    :raises GaugeConfigError: 缺键、类型错、未知键、单位与键名不符。
+
+    整段缺失**不报错**（全部走默认值），但会在 :meth:`GaugeConfig.describe`
+    之外由调用方打一条告警；理由是"默认值能不能用"取决于现场，
+    由节点决定要不要拒绝启动，配置模块只负责如实解析。
+    """
+    raw_any = site.get("gauge_assembly") or {}
+    if not isinstance(raw_any, Mapping):
+        raise GaugeConfigError(
+            "site.yaml 的 gauge_assembly: 应该是一段映射（键值对），"
+            f"实际是 {type(raw_any).__name__}"
+        )
+    raw: Dict[str, Any] = dict(raw_any)
+
+    if overrides:
+        bad = [k for k in overrides if k not in _TYPES]
+        if bad:
+            raise GaugeConfigError(
+                f"未知的覆盖参数: {', '.join(sorted(bad))}。"
+                f"可在 gauge_assembly 段覆盖的键见 gauge_config._TYPES"
+            )
+        raw.update({k: v for k, v in overrides.items() if v is not None})
+
+    _reject_unknown(raw)
+    values = {key: _resolve_one(key, raw)[0] for key in _TYPES}
+    cfg = GaugeConfig(**values)
+    _sanity(cfg)
+    return cfg
+
+
+def _reject_unknown(raw: Mapping[str, Any]) -> None:
+    """多余的键直接报错，并给出最相近的候选名。
+
+    静默忽略多余键的代价是：现场写 ``screw_step_degs``，改参数没反应，
+    而日志里一切正常。报错里带候选名，是因为拼错比写错新键常见得多。
+    """
+    unknown = sorted(set(raw) - set(_TYPES))
+    if not unknown:
+        return
+    lines: List[str] = []
+    for key in unknown:
+        close = difflib.get_close_matches(key, list(_TYPES), n=1, cutoff=0.6)
+        hint = f"，是不是想写 {close[0]}？" if close else ""
+        lines.append(f"  - {key}{hint}")
+    raise GaugeConfigError(
+        "site.yaml 的 gauge_assembly: 段里有本框架不认识的键：\n"
+        + "\n".join(lines)
+        + "\n多余的键不会被忽略，因为「写了没生效」比「写错报错」难查得多。"
+        "确认要新增配置项时，先在 gauge_config._TYPES / _UNITS 里登记"
+    )
+
+
+def _sanity(cfg: GaugeConfig) -> None:
+    """跨键的合理性检查：单键合法、组合起来没意义的，在这里挡住。"""
+    if cfg.holes_total < 1:
+        raise GaugeConfigError(f"gauge_assembly.holes_total 至少为 1，收到 {cfg.holes_total}")
+    if cfg.extreme_direction not in (-1, 1):
+        raise GaugeConfigError(
+            f"gauge_assembly.extreme_direction 只能是 +1（逆时针）或 -1（顺时针），"
+            f"收到 {cfg.extreme_direction}"
+        )
+    if cfg.screw_step_deg <= 0.0:
+        raise GaugeConfigError(
+            f"gauge_assembly.screw_step_deg 必须为正，收到 {cfg.screw_step_deg}"
+        )
+    if cfg.screw_max_cycles < 1:
+        raise GaugeConfigError(
+            f"gauge_assembly.screw_max_cycles 至少为 1，收到 {cfg.screw_max_cycles}"
+        )
+    if cfg.extreme_angle_deg <= 0.0:
+        raise GaugeConfigError(
+            f"gauge_assembly.extreme_angle_deg 必须为正（方向由 extreme_direction 决定，"
+            f"不要用负角度表方向），收到 {cfg.extreme_angle_deg}"
+        )
+    if cfg.contact_safe_delta <= cfg.contact_tangential_delta:
+        raise GaugeConfigError(
+            f"contact_safe_delta({cfg.contact_safe_delta}) 必须大于 "
+            f"contact_tangential_delta({cfg.contact_tangential_delta})："
+            "前者是「立刻抬臂」的安全上限，后者只是「停止下压」。"
+            "两者相等的话，接触一开始就会触发安全上限"
+        )
+    if cfg.tool_tip_below_flange_m <= 0.0:
+        raise GaugeConfigError(
+            f"tool_tip_below_flange_m 必须为正（工具尖端在法兰**下方**多少米），"
+            f"收到 {cfg.tool_tip_below_flange_m}。"
+            "填 0 会让「悬停时工具会不会已经插进工件」这个判断失效"
+        )
+    if cfg.snap_tol_px <= 0.0:
+        raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
+    if cfg.contact_relief_mm < 0.0:
+        raise GaugeConfigError(
+            f"gauge_assembly.contact_relief_mm 不能为负（0 = 接触到之后不卸载），"
+            f"收到 {cfg.contact_relief_mm}。\n"
+            "  它为什么值得一条检查：位置控制的臂撞上硬面会把轴向下压顶到**出力上限**"
+            "才停住（实测 200N+），而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。"
+            "卸载这一步省掉，S08 一转就掉件（报 E1）—— 而那时的日志只会说"
+            "「通止规掉了」，看不到真因。"
+        )
+    if cfg.contact_hold_n <= 0.0:
+        raise GaugeConfigError(
+            f"gauge_assembly.contact_hold_n 必须为正（卸载后的残余轴向力上限 [N]），"
+            f"收到 {cfg.contact_hold_n}。0 会让「卸载干不干净」永远判不过。"
+        )
+    if cfg.snap_tol_mm <= 0.0:
+        raise GaugeConfigError(f"gauge_assembly.snap_tol_mm 必须为正，收到 {cfg.snap_tol_mm}")
+    if cfg.snap_stable_feature not in SNAP_STABLE_FEATURES:
+        raise GaugeConfigError(
+            f"gauge_assembly.snap_stable_feature={cfg.snap_stable_feature!r} 不认识，"
+            f"只支持 {sorted(SNAP_STABLE_FEATURES)}。\n"
+            f"      auto   = 按 perception.source 自动选（真机 yolo→bbox、仿真/外部→motion）\n"
+            f"      bbox   = 用工件检测框中心（像素），需要检测器真的出框\n"
+            f"      motion = 用法兰位移（mm），不依赖检测器"
+        )
+
+
+# =============================================================================
+# 自检：键名必须体现单位（防"填了毫米按米用"）
+# =============================================================================
+def unit_violations(keys: Optional[Iterable[str]] = None) -> List[str]:
+    """返回"键名与单位不符"的清单（空 = 全部合规）。
+
+    规则：单位非空的键，**键名必须以对应的单位后缀结尾**（见 :data:`_UNIT_SUFFIX`）。
+    例外只有 :data:`_SUFFIX_OVERRIDE` 里显式登记的复合单位。
+
+    为什么值得一条专门的检查：``hover`` 填毫米、代码按米用，会给出一个
+    "看起来完全合理"的运动（悬停 5 变成悬停 5 米或 5 毫米），
+    而 :mod:`arm_interfaces.config_guard` 的头注释记录的正是这类事故。
+    """
+    bad: List[str] = []
+    for key in keys if keys is not None else _UNITS:
+        unit = _UNITS[key]
+        if not unit:
+            if key not in _UNITLESS:
+                bad.append(f"{key}: 无量纲但未登记在 _UNITLESS")
+            continue
+        suffix = _SUFFIX_OVERRIDE.get(key)
+        if suffix is None:
+            base = unit.split("/")[0]
+            suffix = _UNIT_SUFFIX.get(base)
+        if suffix is None:
+            bad.append(f"{key}: 单位 {unit} 没有登记后缀（补 _UNIT_SUFFIX 或 _SUFFIX_OVERRIDE）")
+            continue
+        if not key.endswith(suffix):
+            bad.append(f"{key}: 单位是 {unit}，键名应以 {suffix} 结尾")
+    return bad
+
+
+def missing_entries() -> List[str]:
+    """三张表（类型/单位/默认值）是否互相覆盖完整。"""
+    problems: List[str] = []
+    for key in _TYPES:
+        if key not in _UNITS:
+            problems.append(f"{key}: 缺 _UNITS 登记")
+        if key not in DEFAULTS:
+            problems.append(f"{key}: 缺 DEFAULTS（required 的键也应写一个起点值并在文档里标注必标定）")
+    for key in _UNITS:
+        if key not in _TYPES:
+            problems.append(f"{key}: 在 _UNITS 里但不在 _TYPES 里")
+    for key in DEFAULTS:
+        if key not in _TYPES:
+            problems.append(f"{key}: 在 DEFAULTS 里但不在 _TYPES 里")
+    return problems
+
+
+def dataclass_keys() -> List[str]:
+    """:class:`GaugeConfig` 的字段名 —— 与配置表比对，防"解析了但没存进对象"。"""
+    return [f.name for f in fields(GaugeConfig)]

build_sim/arm_application/build/lib/arm_application/gauge_fsm.py

@@ -0,0 +1,998 @@
+"""通止规装配的状态机 —— **纯逻辑**，不 import rclpy，可离线单测。
+
+为什么单独一个文件
+------------------
+状态机的价值全在"顺序"和"边界"上：S07 的力判据有没有在 S08 之前生效、
+E1 掉件之后到底回到哪一步、k2 是"回到 S05 记录的位姿"还是"重新拍照"。
+这些错**不会在运行时暴露成异常**，只会表现成"流程走得下去但结果不对"，
+和力轴向映射反了一样属于静默失效（规则 R5.1）。所以转移表写成**数据**，
+由测试逐条钉住，而不是散在节点的一堆 if/else 里。
+
+设计口径
+--------
+- 状态编号与看板《五、通止规定位装配：状态机设计》一致（S00–S11），
+  E1/E2 两条异常分支也在这里，**改名要同步改看板与文档**（规则 R6.1）。
+- 转移表只回答"给这个事件、从当前状态去哪"；**做不做得了**由节点判断
+  （比如 S03 需要 D435 手眼，没标定就报 COARSE_FAIL 走 S04 兜底）。
+  状态机不掺硬件条件，否则没法离线测。
+- 上下文里的两个字段（`hole_index` / `holes_total`）只影响 S09 的出口：
+  最后一个孔拧出后去 S11 复位，否则去 S10 换到下一个孔。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Dict, List, Optional, Sequence, Tuple
+
+
+# =============================================================================
+# 状态与事件
+# =============================================================================
+class State(str, Enum):
+    """S00–S11 全部状态 + 两个终态。"""
+
+    BOOT = "S00"            # 各服务自检（伺服/相机/夹爪/触觉图传/推理预热）
+    HOME_EXTREME = "S01"    # 回 home + 按 tool 系逆时针转到极限角
+    GRIP_CHECK = "S02"      # 夹持确认（预留，当前跳过）
+    COARSE = "S03"          # 手外 D435 粗定位
+    TRACK = "S04"           # 腕部 D405 跟踪 k0
+    SNAP = "S05"            # 像素稳定后拍照定位 k1/k2 + 记位姿
+    HOVER_K = "S06"         # 到当前孔正悬停
+    CONTACT = "S07"         # 柔顺接触（切向力突变停）
+    SCREW_IN = "S08"        # 旋转装配循环
+    UNSCREW = "S09"         # 反向拧出
+    NEXT_HOLE = "S10"       # 换到下一个孔（回到 S05 记录的位姿）
+    RESET = "S11"           # 复位退出
+    DONE = "DONE"           # 正常结束
+    ABORT = "ABORT"         # 中止（已按安全顺序退出到安全高度）
+
+    def __str__(self) -> str:  # 日志里直接印 S07 而不是 State.CONTACT
+        return self.value
+
+
+class Event(str, Enum):
+    """驱动状态机的事件。命名规则：``动作_结果``。"""
+
+    BOOT_OK = "boot_ok"
+    BOOT_FAIL = "boot_fail"
+
+    POSE_OK = "pose_ok"
+    JOINT_LIMIT = "joint_limit"          # S01 转不动 = 已经贴着关节极限
+
+    GRIP_PASS = "grip_pass"
+    GRIP_FAIL = "grip_fail"
+    GRIP_SKIPPED = "grip_skipped"        # S02 预留：没装/没标定，明确跳过
+
+    COARSE_OK = "coarse_ok"
+    COARSE_FAIL = "coarse_fail"
+
+    TRACK_LOCKED = "track_locked"
+    TRACK_LOST = "track_lost"            # 丢失超时 → 回 S03 兜底
+
+    SNAP_OK = "snap_ok"
+    SNAP_UNSTABLE = "snap_unstable"      # 画面还在动，继续等
+
+    ALIGN_OK = "align_ok"
+    ALIGN_OUT = "align_out"              # 对位超差 → 回 S05 重拍
+
+    CONTACT_OK = "contact_ok"
+    FORCE_OVER_LIMIT = "force_over_limit"  # 力超安全上限 → 抬起
+    #: S07 判到接触、但**下压深度远小于几何预期** ⇒ 规没进孔，顶在工件上表面。
+    #: 单列一个事件是为了让结论与日志都是实话：复用 FORCE_OVER_LIMIT 会把这种
+    #: 情况报成"力超安全上限"，而它的力根本没超限 —— 又是一句误导。
+    HOLE_LOST = "hole_lost"
+
+    SCREW_DONE = "screw_done"
+    SCREW_STOP = "screw_stop"            # 力判据满足：切向力>阈值 且 法向力突增
+    GAUGE_DROPPED = "gauge_dropped"      # E1：夹持丢了
+
+    UNSCREW_DONE = "unscrew_done"
+
+    RELOADED = "reloaded"                # E1：人工放好按 R 之后
+    TIMEOUT = "timeout"                  # E2：任意阶段超时
+    ABORT = "abort"                      # 用户 ^C / 外部中止
+
+
+#: 终态：到这里状态机不再接受事件。
+TERMINAL = frozenset({State.DONE, State.ABORT})
+
+
+# =============================================================================
+# 上下文：只放"影响去哪"的可变数据，不放硬件句柄
+# =============================================================================
+@dataclass
+class GaugeContext:
+    """状态机运行上下文。
+
+    :param holes_total: 一次任务要装的孔数（k1/k2 两个就是 2）。
+    :param hole_index: 当前孔序号，0 基。S10 会 +1。
+    :param relocations: E1 掉件后人工重放计数，用于日志与上限保护。
+    :param max_relocations: 允许的掉件重试次数，超了直接 ABORT，
+        避免"掉件 → 重放 → 又掉"无限循环。
+    """
+
+    holes_total: int = 2
+    hole_index: int = 0
+    relocations: int = 0
+    max_relocations: int = 3
+    notes: List[str] = field(default_factory=list)
+
+    @property
+    def is_last_hole(self) -> bool:
+        return self.hole_index >= self.holes_total - 1
+
+    def hole_name(self) -> str:
+        """给日志用的人读孔名：k1 / k2。"""
+        return f"k{self.hole_index + 1}"
+
+
+# =============================================================================
+# 转移表
+# =============================================================================
+#: ``(状态, 事件) -> 下一状态``。**没有列出的组合就是非法转移**，
+#: 会抛 :class:`FsmError` —— 宁可当场炸，也不要"悄悄停在原地"。
+TRANSITIONS: Dict[Tuple[State, Event], State] = {
+    # --- S00 自检 ---
+    (State.BOOT, Event.BOOT_OK): State.HOME_EXTREME,
+    (State.BOOT, Event.BOOT_FAIL): State.ABORT,
+    # --- S01 home + 极限角 ---
+    (State.HOME_EXTREME, Event.POSE_OK): State.GRIP_CHECK,
+    # 转不动不是失败：已经贴着极限，就按当前角继续（提示即可）
+    (State.HOME_EXTREME, Event.JOINT_LIMIT): State.GRIP_CHECK,
+    # E1 掉件后半段：S01 流程走完 → 提示人工放置 → 按 R → 回记录位姿继续
+    (State.HOME_EXTREME, Event.RELOADED): State.NEXT_HOLE,
+    # --- S02 夹持确认（预留）---
+    (State.GRIP_CHECK, Event.GRIP_SKIPPED): State.TRACK,
+    (State.GRIP_CHECK, Event.GRIP_PASS): State.TRACK,
+    (State.GRIP_CHECK, Event.GRIP_FAIL): State.ABORT,
+    # --- S03 手外粗定位（S04 的兜底，不是必经）---
+    (State.COARSE, Event.COARSE_OK): State.SNAP,
+    (State.COARSE, Event.COARSE_FAIL): State.TRACK,
+    # --- S04 跟踪 ---
+    (State.TRACK, Event.TRACK_LOCKED): State.SNAP,
+    (State.TRACK, Event.TRACK_LOST): State.COARSE,
+    # --- S05 稳定拍照 ---
+    (State.SNAP, Event.SNAP_OK): State.HOVER_K,
+    (State.SNAP, Event.SNAP_UNSTABLE): State.SNAP,   # 自环：继续等稳定
+    # --- S06 正悬停 ---
+    (State.HOVER_K, Event.ALIGN_OK): State.CONTACT,
+    (State.HOVER_K, Event.ALIGN_OUT): State.SNAP,    # 对位超差 → 回 S05 重拍
+    # --- S07 柔顺接触 ---
+    (State.CONTACT, Event.CONTACT_OK): State.SCREW_IN,
+    (State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT,
+    (State.CONTACT, Event.HOLE_LOST): State.ABORT,
+    # --- S08 旋转装配循环 ---
+    (State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW,
+    (State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW,
+    (State.SCREW_IN, Event.GAUGE_DROPPED): State.HOME_EXTREME,  # E1 前半段
+    # --- S09 反向拧出的出口与"是不是最后一个孔"有关，见 resolve_next() ---
+    # --- S10 换到下一个孔（= 回到 S05 记录的位姿），到了就重复 S06–S09 ---
+    (State.NEXT_HOLE, Event.POSE_OK): State.HOVER_K,
+    # --- S11 复位退出 ---
+    (State.RESET, Event.POSE_OK): State.DONE,
+}
+
+#: 兜底：任意非终态收到 ABORT / TIMEOUT 都去对应终态。
+#: 单列出来是因为"每个状态各自列一遍"最容易漏，而漏的那个状态恰恰会在
+#: 真机上表现为"按了急停但流程还在跑"。
+GLOBAL: Dict[Event, State] = {
+    Event.ABORT: State.ABORT,
+    Event.TIMEOUT: State.ABORT,
+}
+
+
+def resolve_next(state: State, event: Event, ctx: GaugeContext) -> State:
+    """算出下一状态。
+
+    绝大多数组合直接查 :data:`TRANSITIONS`；只有一处要结合上下文：
+
+    * ``S09 + UNSCREW_DONE``：最后一个孔 → S11 复位；否则 → S10 换到下一个孔。
+
+    其余"看起来需要上下文"的分支都用了**显式事件**，而不是让 resolve_next
+    去猜（比如 E1 重放用 ``RELOADED`` 而不是"看 relocations 是否大于 0"）。
+    猜出来的分支在测试里只能靠改内部计数复现，读代码的人也得跟着一起猜。
+    """
+    # 全局兜底优先：谁都得能被中止
+    if event in GLOBAL:
+        return GLOBAL[event]
+
+    if state is State.UNSCREW and event is Event.UNSCREW_DONE:
+        return State.RESET if ctx.is_last_hole else State.NEXT_HOLE
+
+    try:
+        return TRANSITIONS[(state, event)]
+    except KeyError:
+        raise FsmError(state, event, ctx) from None
+
+
+class FsmError(RuntimeError):
+    """非法转移。附带当前状态与事件，方便直接定位是哪一步没接上。"""
+
+    def __init__(self, state: State, event: Event, ctx: Optional[GaugeContext] = None) -> None:
+        self.state = state
+        self.event = event
+        ctx = ctx or GaugeContext()
+        super().__init__(
+            f"通止规装配状态机：状态 {state}（{STATE_DOC.get(state, '?')}）"
+            f"不接受事件 {event.value}。当前孔 {ctx.hole_name()}，"
+            f"已重放 {ctx.relocations} 次。"
+            "若是新增流程，先在 gauge_fsm.TRANSITIONS 里登记这条转移。"
+        )
+
+
+# =============================================================================
+# 状态机本体
+# =============================================================================
+@dataclass
+class Step:
+    """一次转移的记录，用于日志与事后复盘。"""
+
+    index: int
+    prev: State
+    event: Event
+    next: State
+
+    def __str__(self) -> str:
+        return f"#{self.index} {self.prev} --{self.event.value}--> {self.next}"
+
+
+class GaugeFsm:
+    """通止规装配状态机。
+
+    用法::
+
+        fsm = GaugeFsm(GaugeContext(holes_total=2))
+        fsm.handle(Event.BOOT_OK)          # S00 -> S01
+        print(fsm.state)                   # S01
+        fsm.handle(Event.ABORT)            # 任何状态都能中止
+    """
+
+    def __init__(self, ctx: Optional[GaugeContext] = None, start: State = State.BOOT) -> None:
+        self.ctx = ctx or GaugeContext()
+        self.state = start
+        self.history: List[Step] = []
+
+    # -- 查询 ---------------------------------------------------------------
+    @property
+    def is_terminal(self) -> bool:
+        return self.state in TERMINAL
+
+    @property
+    def ok(self) -> bool:
+        return self.state is State.DONE
+
+    def can(self, event: Event) -> bool:
+        """这个事件在当前状态下合法吗（不改状态，给"要不要试"用）。"""
+        if event in GLOBAL:
+            return not self.is_terminal
+        try:
+            resolve_next(self.state, event, self.ctx)
+        except FsmError:
+            return False
+        return True
+
+    # -- 推进 ---------------------------------------------------------------
+    def handle(self, event: Event) -> State:
+        """吃一个事件，返回新状态。非法转移抛 :class:`FsmError`。"""
+        if self.is_terminal:
+            raise FsmError(self.state, event, self.ctx)
+        prev = self.state
+        # 先算转移（非法转移在这里就抛，**不留副作用**）——
+        # 顺序反过来的话，一次手误的 GAUGE_DROPPED 会白白吃掉一次重放预算，
+        # 报错信息还说"状态不对"，而现场已经少了一次重试机会。
+        nxt = resolve_next(prev, event, self.ctx)
+
+        if event is Event.GAUGE_DROPPED:
+            self.ctx.relocations += 1
+            if self.ctx.relocations > self.ctx.max_relocations:
+                # **超过上限不抛异常**：掉件 4 次不是"程序非法状态"，是现场问题。
+                # 报错文案要指出现场（夹持/螺纹），而不是指着转移表说这条边不存在。
+                self.note(
+                    f"通止规已掉 {self.ctx.relocations} 次（上限 "
+                    f"{self.ctx.max_relocations}），不再自动重放，请检查夹持与螺纹"
+                )
+                nxt = State.ABORT
+
+        if nxt is State.NEXT_HOLE and prev is not State.NEXT_HOLE:
+            # 换孔动作：序号在**进入** S10 时推进；S10 完成就是新孔的 S06。
+            self.ctx.hole_index = min(self.ctx.hole_index + 1, max(0, self.ctx.holes_total - 1))
+
+        self.state = nxt
+        self.history.append(Step(len(self.history) + 1, prev, event, nxt))
+        return nxt
+
+    def note(self, text: str) -> None:
+        """挂一条人工说明（比如"跳过 S03：D435 未标定"），随结果一起输出。"""
+        self.ctx.notes.append(text)
+
+
+# =============================================================================
+# 判据（纯函数，被节点与测试共用）
+# =============================================================================
+def points_stable(samples: List[Sequence[float]], tol: float, min_frames: int) -> bool:
+    """稳定判据（任意维）：样本够多、且所有点到均值距离都 ≤ ``tol``。
+
+    与 :func:`pixel_stable` 是同一套判据，只是不限定二维。为什么需要：
+    S05 的"画面稳了再拍"在仿真里用不了检测框（合成图过不了真机训练出来的
+    工件模型），于是改用**法兰位移**（mm）当特征 —— 判据本身一模一样，
+    只是量纲从像素变成毫米，所以抽出来共用，而不是各写一份。
+    """
+    if len(samples) < max(2, int(min_frames)):
+        return False
+    dim = len(samples[0])
+    means = [sum(float(s[i]) for s in samples) / len(samples) for i in range(dim)]
+    if any(len(s) != dim for s in samples):
+        return False
+    for s in samples:
+        if math.sqrt(sum((float(s[i]) - means[i]) ** 2 for i in range(dim))) > tol:
+            return False
+    return True
+
+
+def pixel_stable(samples: List[Tuple[float, float]], tol_px: float, min_frames: int) -> bool:
+    """S05：像素稳定判据。
+
+    :param samples: 最近若干帧的 ``(u, v)`` 像素坐标。
+    :param tol_px: 允许的抖动半径（像素）。
+    :param min_frames: 至少要看多少帧才敢说"稳定"。
+    :returns: 样本够多、且所有点到它们的均值的距离都 ≤ ``tol_px``。
+
+    为什么不用"两帧之差"：跟踪输出的抖动是**零均值**的，相邻两帧可能恰好
+    很近，于是"稳定"判据会在画面明明在晃的时候早早触发，接着拍出一张糊的
+    定位图 —— 表现为"孔位偶尔偏 1mm"，随机且难复现。用"到均值的最远距离"
+    才对得上"画面不动"这个物理事实。
+    """
+    return points_stable(samples, tol_px, min_frames)
+
+
+def tangential_jump(
+    shear_now: Tuple[float, float], shear_ref: Tuple[float, float], delta: float
+) -> bool:
+    """S07：切向力突变判据 —— **两个通道都看，任一超阈值即算**。
+
+    用户明确过：切向力到底走 X 还是 Y 通道未确认。只监控一个通道的话，
+    赌错了就是整段流程静默失效（下发正常、日志正常、就是停不住）。
+    两个都监控的代价只是可能更早停一点点，这个方向的错是安全的。
+
+    :param shear_now: 当前两路切向读数。
+    :param shear_ref: 静止基线的两路切向读数。
+    :param delta: 触发阈值（相对基线的增量）。
+    """
+    return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref))
+
+
+def wrench_touch(
+    axial_now: float, axial_ref: float, delta: float
+) -> bool:
+    """S07（**首选判据**）：腕部 FT 的轴向力相对基线突变 → 判为接触到工件。
+
+    为什么这是首选、而不是手指触觉（2026-09-24 用一整轮排查换来的结论）：
+
+    通止规是**靠夹持摩擦抱住**的工具。轴向力要传到手指，必须过"指垫↔规"的
+    摩擦面，而摩擦能传的上限是 µ·N：
+        夹持段 µ=0.25、实测握力 N≈46N → 单指 ~11.5N、两指 ~23N
+    可台肩落面那一下，接触刚度 5e4 N/m 意味着**过压 0.5mm 就是 25N** ——
+    一步的步进量就足以让规在爪里打滑。**于是手指触觉物理上读不到那个力**
+    （读数停在 0，而它是对的），阈值怎么调都不对：调小被噪声骗，调大永远
+    达不到。当时确实是照"阈值调高了"去改的，白费一轮。
+
+    腕部 FT 在这条力路的**上游**：工件→规→夹爪→法兰→腕部关节。
+    工具打不打滑都要经过它，所以它测得到真实的轴向力。
+
+    判据用**绝对增量**而不是带符号压入量，理由与 ``tangential_jump`` 相同：
+    传感器坐标系的正负号方向取决于安装，赌错了就是"整段流程静默失效"
+    （下发正常、日志正常、就是停不住）。取绝对值只会更早停一点点，
+    这个方向的错是安全的 —— 与 ``stage2_locate_node._contact_detected``
+    的既有口径也一致（那里也是 ``abs(fz - baseline)``）。
+
+    :param axial_now: 当前轴向力读数 [N]。
+    :param axial_ref: 静止（还没碰工件）时的轴向基线 [N]。
+    :param delta: 触发阈值（相对基线的增量）[N]。
+    """
+    return abs(float(axial_now) - float(axial_ref)) >= float(delta)
+
+
+#: S07 腕部 FT 判据需要多"新"的数据才算活着 [s]。
+#: 真机 broadcaster 与仿真传感器都是 100Hz，0.5s 相当于容许掉 50 帧 ——
+#: 只用来排除"根本没在发"，不是用来做实时性判据的。
+WRENCH_MAX_AGE_S: float = 0.5
+
+
+def wrench_live(age_s: Optional[float]) -> bool:
+    """腕部 FT 是否有活的数据（够新）。
+
+    ``age_s`` 为 None 表示**一帧都没收到过**（话题名写错、传感器没起来）。
+    注意"收到过但数值为 0"**是活的** —— 静止时轴向力本来就接近 0。
+    这个区别很关键：拿数值大小判活，会让"安静的传感器"被误判成掉线，
+    而那正是 2026-09-24 在触觉上踩过的同一个坑（见 stage3 的 ``live_tactile``）。
+    """
+    return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S
+
+
+#: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
+#: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
+#: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
+#: → 臂原地不动、循环条件永远成立 → **静默自旋**（日志停在基线那行、CPU 跑满、
+#: 既不报错也不前进）。2026-09-24 在仿真里踩到过一次，真机同参数同结果。
+DESCEND_TOL_FRAC: float = 0.4
+#: 容差下限 [m]：再小就追不动了（伺服死区/噪声会让臂永远"差一点点"）。
+DESCEND_TOL_FLOOR_M: float = 2.0e-5
+#: 判定"这一步真的往下走了"的位移下限，占步长的比例。
+DESCEND_PROGRESS_FRAC: float = 0.2
+#: 卸载回退的到位容差占**回退量**的比例（见 :func:`relief_tol_m`）。
+RELIEF_TOL_FRAC: float = 0.2
+#: 允许连续多少步毫无位移后中止（防"没接触也一直转"的静默自旋）。
+DESCEND_STALL_STEPS: int = 5
+
+
+def relief_tol_m(dz_m: float) -> float:
+    """S07 判到接触后**卸载回退**该给 ``_goto`` 的到位容差 [m]。
+
+    为什么单独立一条（2026-09-24 实测踩到，代价是一轮假通过）：
+
+        回退量 0.50mm，而 ``_goto`` 的容差被写死成 0.0005 m（**同量级**）。
+        目标 z 是 ``Tee[2,3] + 0.0005``，浮点回减后 ``dist`` 会比 0.0005 略小
+        ⇒ ``_goto`` 的入口判据 ``dist < tol_m`` 当场成立 ⇒ **一步没走就返回 True**。
+        于是"卸载"变成空操作，而残余力判据比的是"回退前 vs 回退后"两个**相同**的
+        读数 ⇒ 报"残余 0.0N，已放干净"（假通过）。
+        接着 S08 带着 ~70N 的轴向力去转（夹持摩擦上限 µ·N 两指合计 ≈73N）
+        → 掉件(E1)，而 E1 的日志只说"规掉了"。
+
+    返回 ``回退量的 20%``，再夹到 ``[DESCEND_TOL_FLOOR_M, 回退量的一半]`` ——
+    后者保证**容差永远严格小于回退量**，`_goto` 不可能在入口就判"已到位"。
+    """
+    dz = float(dz_m)
+    if dz <= 0.0:
+        raise ValueError(f"卸载回退量必须为正，收到 {dz_m}")
+    return min(max(DESCEND_TOL_FLOOR_M, dz * RELIEF_TOL_FRAC), dz * 0.5)
+
+
+def descend_tol_m(step_m: float) -> float:
+    """S07 单步下压该给 ``_goto`` 的到位容差 [m]。
+
+    取步长的 40%，但不超过步长的一半、不低于 ``DESCEND_TOL_FLOOR_M``。
+    这样"走满这一步"才是循环推进的**唯一**出口 —— 容差比步长小，`_goto`
+    就不可能因为"已经在容差内"而空转（那正是上面注释里那个静默自旋）。
+    """
+    if step_m <= 0.0:
+        raise ValueError(f"下压步长必须为正，收到 {step_m}")
+    return max(step_m * DESCEND_TOL_FRAC, min(DESCEND_TOL_FLOOR_M, step_m * 0.5))
+
+
+def descend_iter_budget(max_depth_m: float, step_m: float, slack: int = 8) -> int:
+    """S07 下压循环的**硬**迭代上限：够走完整个下探深度，再多给几步余量。
+
+    为什么要有硬上限：循环条件写的是"实测高度还没到最深"，一旦臂因为死区、
+    被夹住、孔位偏了而**实际没动**，这个条件就恒真。上限不是用来"早点失败"
+    的（那是 ``contact_max_depth_mm`` 的活），而是**兜住静默自旋**：
+    宁可报"S07 没走完也没接触"，也不能让节点无声无息地空转到天亮。
+    """
+    if step_m <= 0.0:
+        raise ValueError(f"下压步长必须为正，收到 {step_m}")
+    if max_depth_m <= 0.0:
+        return 0
+    return int(math.ceil(max_depth_m / step_m)) + max(1, int(slack))
+
+
+def descend_progress(z_prev: float, z_now: float, step_m: float) -> bool:
+    """这一步是否真的往下走了（法兰 z 向下为负）。
+
+    判据是**实测**位移，不是"指令发出去了"：``_goto`` 因为容差而空转、
+    或步长被控制器死区吞掉时，实测位移≈0 而指令日志一切正常。
+    """
+    return (float(z_prev) - float(z_now)) >= step_m * DESCEND_PROGRESS_FRAC
+
+
+#: 通止规**台肩**（COLLAR）在规尖上方的距离 [m]。
+#: 取自 devices/sim/gazebo/models/gauge/model.sdf：通端 φ6×30mm，台肩在 30~33mm。
+#: 判"到底了"的那一下就是台肩落到工件上表面 —— 所以它决定了 S07 至少要下压
+#: 多少（见 descend_needed_m）。改规的模型时必须一起改这里，否则预算会算错。
+GAUGE_SHOULDER_ABOVE_TIP_M: float = 0.030
+
+
+def tool_clearance_m(
+    flange_z_m: float,
+    tool_tip_below_flange_m: float,
+    surface_z_m: float,
+) -> float:
+    """工具尖端离工件上表面还有多高 [m]。**负 = 已经插进去了。**
+
+    为什么需要这个：流程里"悬停高度"是按**相机**定的，不是按**工具**定的。
+    腕部相机要看清楚孔，就得离工件面 ~0.17m；而工具（装在法兰下面）可能比
+    相机长得多。两者一冲突，就会出现下面这种极难查的局面：
+
+        悬停高度 0.632（法兰）− 工具长 0.243 = 0.389
+        工件上表面                                  0.412
+        → 工具尖端已经插进工件 23mm
+
+    此时"悬停"这一步本身已经把工具捅进孔里了，S07 再往下压就一路压到顶、
+    等不到"台肩落面"那一下，最后报"下探 N mm 都没触发接触判据"然后 ABORT。
+    现象看起来像力阈值标错了，实际是**几何**错了。
+
+    真机同参数同结果（不是仿真特有的）：只要悬停高度是按相机定的、而工具
+    比相机长，就会这样。所以这个值要在**启动时**算出来并报出来。
+    """
+    tip_z = float(flange_z_m) - float(tool_tip_below_flange_m)
+    return tip_z - float(surface_z_m)
+
+
+def hover_z_for_tool_clearance(
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    clearance_m: float,
+) -> float:
+    """让工具尖端停在工件面上方 ``clearance_m`` 所需的**法兰**高度 [m]。
+
+    就是 ``tool_clearance_m`` 的逆运算 —— 写成一个函数是为了让"该用多高"这个
+    结论只有一个来源（下面 ``hover_z_guard`` 与调用方都用它，不各推一遍）。
+    """
+    return float(surface_z_m) + float(tool_tip_below_flange_m) + float(clearance_m)
+
+
+def hover_z_guard(
+    hover_z_m: float,
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    want_clearance_m: float,
+) -> Tuple[float, str]:
+    """二阶段悬停高度的自检 → ``(该用的悬停高度, 说明; 不用改时说明是空串)``。
+
+    **为什么必须有这条**（2026-09-24 实测栽在这上面）：
+
+        悬停高度是**按相机**定的（腕部相机要离工件面 ~0.17m 才看得清孔），
+        而工具（夹爪 + 通止规）可能比相机长得多 —— 仿真里实测
+        ``tool_tip_below_flange_m=0.245``，而悬停算的是
+        ``0.412 + 0.17 + 0.05 = 0.632`` ⇒ 工具尖端在工件上表面**下面 25mm**。
+
+        后果不在 S07（S07 有自己的几何自检），而在**更早的 S05**：
+        "悬停"这一步本身就把工具捅进工件里了，于是 S05 的横向对准等于
+        拿规尖去拨工件 —— 实测把工件从 x=0.520 拖到 0.4284（**92mm**）。
+        而真值孔位是按工件的**实测**位姿算的（``sim_live_pose``，这是对的），
+        工件一走，孔位就跟着工具走 ⇒ 残差恒定在 13mm、迭代 9 次永不收敛
+        （日志：``[fine 3..8] 残差=13.25/13.19/…/11.79mm``）。
+        现象看起来像"手眼标定坏了 / 检测不认孔"。
+
+    真机同参数同结果：这不是仿真专有，只要"悬停按相机定、工具比相机长"就会这样。
+
+    判据取"明显低于目标间隙"而不是"一毫米都不许低"：配置的悬停与实测的
+    工件面 z 都有几毫米的不确定度，拿 0 当界会把正常配置判成错的。
+    """
+    clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
+    if clear_m >= float(want_clearance_m) - 0.001:
+        return float(hover_z_m), ""
+    need_z = hover_z_for_tool_clearance(surface_z_m, tool_tip_below_flange_m, want_clearance_m)
+    why = (
+        f"悬停高度会让工具尖端钻进工件：法兰 {float(hover_z_m):.4f} − 工具长 "
+        f"{float(tool_tip_below_flange_m) * 1000:.1f}mm = 尖端 "
+        f"{float(hover_z_m) - float(tool_tip_below_flange_m):.4f}，工件上表面 "
+        f"{float(surface_z_m):.4f} → 尖端在面{'下' if clear_m < 0 else '上仅'} "
+        f"{abs(clear_m) * 1000:.1f}mm（要求 ≥ {float(want_clearance_m) * 1000:.1f}mm）。\n"
+        f"        悬停高度通常是按**相机**视距定的，而工具比相机长 —— 两者冲突时必须"
+        f"先保证不碰工件。\n"
+        f"        已按几何抬到 {need_z:.4f}（= {float(surface_z_m):.3f} 上表面 + "
+        f"{float(tool_tip_below_flange_m) * 1000:.0f}mm 工具 + "
+        f"{float(want_clearance_m) * 1000:.1f}mm 间隙）。\n"
+        f"        要固定这个值：改 site.yaml 的 robots[<sn>].servo.hole_hover_z_m = "
+        f"{need_z:.3f}（以及同组件的 ee_z_m ≥ 它 +0.05）。"
+    )
+    return need_z, why
+
+
+#: S05 精对位时判"工件被工具推走"的两级阈值 [m]。
+#
+#   为什么需要这条判据（2026-09-24 又栽了一次，见下面 drag_verdict 的说明）：
+#       S05 的对准是"看着孔走"的闭环，而孔位是**工件上**的。工具一旦碰到工件，
+#       闭环追的就不再是孔，而是"自己推着走的工件" —— 残差恒定（本轮 11mm）、
+#       迭代用满，日志报的是"检查手眼/相机安装"，与真因（工具比相机长、
+#       悬停时已经插进工件）差着十万八千里。
+#   取 3mm：工件是刚体，两拍之间它整体挪动 >3mm 只可能是被推/被撞（正常加工
+#   过程里它一动不动）。同一对孔的两拍测量噪声在 0.2mm 量级，离阈值很远。
+SNAP_DRAG_TOL_M: float = 0.003
+#: 单拍就这么大 ⇒ 不必等第二拍确认（刚体不会自己"挪回去"）。
+SNAP_DRAG_HARD_M: float = 0.006
+
+
+#: "已经到位"的容差：避免 ±1e-9 的浮点噪声变成"再转一整圈"。
+_RATCHET_FULL_TURN_EPS: float = 1e-9
+
+
+def ratchet_delta_rad(target_rad: float, q_now_rad: float, direction: int = 0) -> float:
+    """"目标角 − 当前角"，折成**一次**能转到的增量 [rad]。
+
+    **为什么要有这个函数**（2026-09-24 实测：棘轮把整条流程 ABORT 在 S09 第一步）：
+
+        S08/S09 给棘轮的是**关节 7 的目标角（绝对值）**（``q[-1] ± step``），
+        而棘轮此前把它当**增量**用。于是"关节在 +158.9°、目标 +338.9°"被算成
+        "要转 338.9°"（实际增量只有 180°）⇒ 误判行程不够 ⇒ 让余量那一步算到
+        限位外面（``158.9 − 338.9 = −180°``，比 −175° 还低）⇒ 报
+        "期望 −338.9°、实测 +26.0°，偏差过大" ⇒ ABORT。现场什么坏事都没发生。
+
+        差值必须折一圈：目标常常**故意**超出限位（这正是要"让余量"的原因），
+        ``+338.9°`` 与 ``−21.1°`` 是同一个关节位置。
+
+    **为什么还要 direction**（2026-09-24 又一次实测：S08 第 3 轮 ABORT）：
+
+        不给方向时这里取"最短的那条路"，而名义步长**正好是 180°** 时，
+        哪条路更短取决于 0.1° 级的读数差：调用方按自己读到的 ``q`` 算出目标
+        ``−18.0°``，棘轮再用**稍晚一点**读到的 ``q=+162.1°`` 去算差值，
+        ``−180.1°`` 折一圈就成了 ``+179.9°`` —— 方向**翻了个个儿**。
+        于是棘轮朝螺纹的反方向走，接着贴限位、让余量、重试，净位移 0 ⇒ ABORT。
+
+        棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给
+        "谁更短"这种数值巧合决定，所以由调用方明确给出 ``direction``。
+
+        注：``direction=0``（默认）保留旧的"最短路径"语义，给"目标本来就明确
+        在某一侧"的调用方用；棘轮的四个原子步一律显式给方向。
+
+    :param target_rad: 目标角（绝对值，可以是限位外的角度）。
+    :param q_now_rad: 当前关节角。
+    :param direction: 想要的转向：``+1`` 正转、``-1`` 反转、``0`` 取最短路径。
+    :returns: 增量 [rad]。``direction=0`` 时落在 ``(−π, π]``；给了方向时落在
+        ``[0, 2π)``（正转）或 ``(−2π, 0]``（反转）——**一定**沿指定方向，
+        且是能落到目标角的最小那一步。**同一个目标、同一个当前位置**只会有一个
+        增量 —— 棘轮的"让余量"与"重试"都按它算，两次不会互相矛盾。
+    """
+    base = float(target_rad) - float(q_now_rad)
+    two_pi = 2.0 * math.pi
+    if direction == 0:
+        return math.remainder(base, two_pi)
+    # 先折到 [0, 2π)，再按想要的转向取那一侧（与目标角差正好一整圈，是同一个位置）。
+    pos = math.fmod(base, two_pi)
+    if pos < 0.0:
+        pos += two_pi
+    if pos >= two_pi - _RATCHET_FULL_TURN_EPS:
+        pos = 0.0  # 已经（几乎）在目标上：不要为了凑方向白转一整圈
+    if direction > 0:
+        return pos
+    return pos - two_pi if pos > 0.0 else 0.0
+
+
+
+def drag_verdict(
+    mid_now: Tuple[float, float],
+    mid_prev: Optional[Tuple[float, float]],
+    mid_prev2: Optional[Tuple[float, float]] = None,
+    *,
+    tol_m: float = SNAP_DRAG_TOL_M,
+    hard_m: float = SNAP_DRAG_HARD_M,
+) -> Tuple[bool, float]:
+    """两孔中点是否在两拍之间"整体挪动" → ``(是否判定为被推走, 本拍位移 m)``。
+
+    **为什么看"中点"而不是看目标孔**：目标孔是**追**的对象，工具一动它就该动
+    （对准本意如此），看它分不出"正常对准"和"推着工件走"。而两孔**中点**在
+    对准过程中不该动 —— 它只有工件整体平移时才会动。所以中点位移干净地对应
+    "工件被搬动了"。
+
+    **为什么看"两孔中点"而不是单孔**：单孔在相机里会因分辨率/遮挡抖动，且
+    目标孔本来就在被逼近；中点是两者的平均，噪声更小、语义更明确。
+
+    :param mid_now: 本拍两孔中点 (x, y)[m]（基座系）。
+    :param mid_prev: 上一拍中点；``None`` = 还没有上一拍（第一拍不判）。
+    :param mid_prev2: 上上拍中点。用来确认"位移是持续的"（单拍可能是误检）。
+    :returns: ``(判定, 本拍位移 m)``。
+    """
+    if mid_prev is None:
+        return False, 0.0
+    dx = float(mid_now[0]) - float(mid_prev[0])
+    dy = float(mid_now[1]) - float(mid_prev[1])
+    d1 = math.hypot(dx, dy)
+    if d1 >= float(hard_m):
+        return True, d1
+    if d1 < float(tol_m) or mid_prev2 is None:
+        return False, d1
+    px = float(mid_prev[0]) - float(mid_prev2[0])
+    py = float(mid_prev[1]) - float(mid_prev2[1])
+    d2 = math.hypot(px, py)
+    # 第二拍确认：上一拍也挪了（≥ 阈值的一半），且方向一致（点积 > 0）。
+    # 为什么允许方向只"大致一致"：工件被推着走时前后两拍的推动量本来就不同
+    # （工件一让开，工具就少推一点），拿夹角严格相等会漏判。
+    if d2 >= float(tol_m) * 0.5 and (dx * px + dy * py) > 0.0:
+        return True, d1
+    return False, d1
+
+
+def descend_needed_m(
+    flange_z_m: float,
+    tool_tip_below_flange_m: float,
+    surface_z_m: float,
+    tool_shoulder_above_tip_m: float,
+) -> float:
+    """S07 要判"台肩落面"，从当前法兰高度起还需要下压多少 [m]。
+
+    判"到底了"的那一下不是工具尖端碰面，而是**台肩落到工件上表面**；
+    台肩在工具尖端上方 ``tool_shoulder_above_tip_m``（规是通端长 30mm）。
+    所以需要的下压量 = 台肩到面的距离，而不是尖端到面的距离。
+
+    返回值可能是负的（台肩已经在面以下）—— 那说明几何配错了，调用方应当
+    报错而不是"往下走一个负距离"。
+    """
+    shoulder_z = (
+        float(flange_z_m)
+        - float(tool_tip_below_flange_m)
+        + float(tool_shoulder_above_tip_m)
+    )
+    return shoulder_z - float(surface_z_m)
+
+
+#: S07 下探预算相对"实际需要下压量"的余量系数。
+#: 预算是**兜底**用的（不是用来判成败的）：真正让循环停下的是接触判据。
+#: 给 1.5 倍余量是为了容纳对位残差、板厚公差、台肩倒角还没完全落面等。
+DESCEND_BUDGET_FRAC: float = 1.5
+#: 下探预算的绝对下限 [m]：几何算出来很小（比如台肩已快贴面）时也要留够，
+#: 否则一旦对位有点偏，"预算用完"会先于"判到接触"触发，报出来的原因是假的。
+DESCEND_BUDGET_FLOOR_M: float = 0.008
+
+
+def descend_budget_m(needed_m: float) -> float:
+    """由"实际需要下压多少"推出 S07 的下探预算 [m]。
+
+    不要手工填这个数：填小了会在台肩落面**之前**就用完预算（报"下探 N mm
+    都没触发接触判据"，而真实原因只是预算不够）；填大了会在判据失灵时压得
+    更深（更危险）。由几何推出来，两个毛病都没有。
+    """
+    return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M)
+
+
+#: ``hole_entry_verdict`` 的结论。
+ENTRY_OK = "ok"        # 接触深度与几何预期相符 ⇒ 规是从孔里下去的、台肩落面
+ENTRY_HIGH = "high"    # 早早就接触 ⇒ 尖端顶在工件**上表面**上，根本没进孔
+
+#: 判"没进孔"的下压深度比例阈值。
+#: 实测两个锚点（2026-09-28，仿真）：
+#:   · 正常入孔：下压 28.6mm / 需要 31.9mm ⇒ 0.90
+#:   · 顶在平面上：下压 0.90mm / 需要 31.9mm ⇒ 0.03
+#: 两者差着一个台肩高度（30mm），取 0.5 时两侧余量都很大（正常那侧要掉到
+#: 一半才会误判）。**不要**为了"更灵敏"把它往 0.9 推 —— 台肩倒角没完全落面、
+#: 板厚公差都会让正常值在 0.85~0.95 之间晃。
+HOLE_ENTRY_MIN_RATIO: float = 0.5
+
+
+def hole_entry_verdict(
+    descended_m: float,
+    needed_m: float,
+    *,
+    min_ratio: float = HOLE_ENTRY_MIN_RATIO,
+) -> str:
+    """S07 判到接触的那一刻：规是**进了孔**、还是**顶在工件上表面**。
+
+    为什么需要它（2026-09-28 实测，同一命令三跑里坏一跑：DONE / ABORT / DONE）：
+      工件在 S08/S09/S10 阶段会被工具推走（仿真里实测被推 20~52mm）。而 k2 的
+      孔位是 **S05 一次性定下**的（``_step_hover_k`` 取 ``_hole_xy``），工件一动
+      它就失效。于是规落在孔**旁边的平面**上，只下压「悬停间隙」那么点就判到接触。
+
+    不拦的代价，实测完整发生过一次：
+      S08 干转 5 轮（切向增量 0.0、法向增量 ±0.0，**一点没进给**）→ 第 6 轮憋住 →
+      报出来的是「转轴偏差 7.6° 超过阈值 —— 关节7 轴与工具 Z 不共线，
+      先确认工具系是否垂直向下、有没有装转接件」。
+
+      **那句话会把所有人引到错误方向**：真因既不是工具系也不是转接件，是孔位失效。
+      而且为这句误导，白烧了 5 轮无效拧入（≈30 秒）和一次憋劲（有把规顶掉的风险）。
+
+    为什么比 ``needed_m`` 而不是比"尖端离面高度"：正常入孔时接触发生在**台肩落面**
+    那一刻 ≈ ``needed_m``；顶在平面上时接触发生在**尖端刚碰到面**那一刻，而台肩还在
+    尖端上方 30mm。两者差着一个台肩高度，所以用比例判最稳（见上面两个实测锚点）。
+
+    纯函数（R5.1）：判错两个方向都贵 —— 把正常的判成"没进孔"会白扔一整个孔；
+    把"没进孔"判成正常，就是上面那 30 秒 + 一次误导。
+
+    ``needed_m <= 0`` 时**不判**（几何本身已经错了，上面那条自检会报），
+    返回 ``ENTRY_OK`` 不在这里抢着报第二个错。
+    """
+    if needed_m <= 0.0:
+        return ENTRY_OK
+    if float(descended_m) < float(needed_m) * float(min_ratio):
+        return ENTRY_HIGH
+    return ENTRY_OK
+
+
+def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
+    """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
+
+    注意是"且"：只看切向力，规刚接触螺纹时会误停；只看法向力，规顶到孔底
+    时会误停。两者同时出现才说明"螺纹咬住了"，这是拧入到位的特征。
+    """
+    return shear_max > shear_th and normal_delta > normal_th
+
+
+#: S08 的四个原子步，顺序即执行顺序。
+#: **故意不封装成一个"拧螺丝"复合动作**：中途掉件（E1）时，只有每一步都能
+#: 单独调用、单独判断，才可能从任意一步精确恢复，而不是从头再来。
+RATCHET_STEPS: Tuple[str, ...] = (
+    "rotate_slow",   # 工具系顺时针慢转 step_deg（拧入，带着规一起转）
+    "release",       # 松爪（规留在孔里）
+    "rotate_back",   # 快回转 step_deg（空手回来）
+    "regrip",        # 夹紧
+)
+
+
+def ratchet_cycles(total_deg: float, step_deg: float) -> int:
+    """要拧 ``total_deg`` 需要多少轮棘轮。向上取整，``step_deg<=0`` 报错。"""
+    if step_deg <= 0.0:
+        raise ValueError(f"单次转角必须为正，收到 {step_deg}")
+    if total_deg <= 0.0:
+        return 0
+    return int(math.ceil(total_deg / step_deg))
+
+
+@dataclass(frozen=True)
+class RatchetTravel:
+    """棘轮步长与关节行程的**可用性核算**（S08 开头自检，别等到第 3 轮才炸）。"""
+
+    ok: bool
+    step_deg: float          # 配置的一步转角
+    travel_deg: float        # 关节行程（上限 − 下限）
+    need_deg: float          # 一步棘轮真正需要的行程
+    max_step_deg: float      # 行程允许的最大步长
+    why: str                 # 不 ok 时给人看的说明（ok 时为空串）
+
+
+def ratchet_travel_ok(
+    step_deg: float, limits_rad: Optional[Tuple[float, float]], margin_deg: float
+) -> RatchetTravel:
+    """一步棘轮 ``step_deg`` 放得进关节行程吗？
+
+    **为什么需要**：2026-09-24 实测，``screw_step_deg=180`` 配 Rizon4 的
+    关节 7（±175°，行程 350°）在第 3 轮 ABORT —— 慢转那一步要转 180°、
+    回转那一步还要 180°，两次加起来 360° > 350°，关节**必然**有一头贴限位；
+    更糟的是"让余量"也腾不出地方（要腾就得往反向再让一整步，还是 360°）。
+    日志里那一长串"贴限位 → 让余量 → 还是贴限位"就是这么来的。
+
+    判据取 ``2×step + 2×margin ≤ 行程``：
+    - 2×step：棘轮一个来回（慢转 + 回转）要的行程；
+    - 2×margin：两头都要留余量 —— 贴死限位再让余量时**一点空间都没有**，
+      而实测每轮回转还会比指令少走 4~5°（速度/加减速限制），
+      这点亏欠要靠余量吃，否则 7~8 轮后累计偏出限位、再也转不回来。
+
+    :param limits_rad: ``(lower, upper)`` [rad]；``None`` = 拿不到限位，放行
+        （返回 ``ok=True``，理由写清楚：真机靠驱动器报 JOINT_LIMIT 兜底）。
+    """
+    need = 2.0 * float(step_deg) + 2.0 * float(margin_deg)
+    if limits_rad is None:
+        return RatchetTravel(
+            ok=True, step_deg=float(step_deg), travel_deg=float("inf"),
+            need_deg=need, max_step_deg=float("inf"),
+            why="",
+        )
+    travel = math.degrees(float(limits_rad[1]) - float(limits_rad[0]))
+    max_step = 0.5 * (travel - 2.0 * float(margin_deg))
+    if need <= travel:
+        return RatchetTravel(
+            ok=True, step_deg=float(step_deg), travel_deg=travel,
+            need_deg=need, max_step_deg=max_step, why="",
+        )
+    # 给个能直接照着改的步长：往整十度取整（现场习惯填 120/150/180），
+    # 但不许把步长压到 30° 以下 —— 那样拧进/拧出的轮数会多到失控。
+    suggest = max(30.0, math.floor(max_step / 10.0) * 10.0)
+    why = (
+        f"棘轮一步 {step_deg:.0f}° 放不进关节 7 的行程：一步棘轮要 "
+        f"2×{step_deg:.0f}° + 2×{margin_deg:.0f}°(余量) = {need:.0f}°，"
+        f"而行程只有 {travel:.0f}°（上限 {math.degrees(limits_rad[1]):.0f}° − "
+        f"下限 {math.degrees(limits_rad[0]):.0f}°）。\n"
+        f"        改法：site.yaml 的 gauge_assembly.screw_step_deg 改到 "
+        f"{max_step:.0f}° 以内（例如 {suggest:.0f}°），并保证 "
+        f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍**（"
+        f"累计必须转整数圈，否则规停在螺纹中间、S09 也就少拧出几分之一圈）。"
+    )
+    return RatchetTravel(
+        ok=False, step_deg=float(step_deg), travel_deg=travel,
+        need_deg=need, max_step_deg=max_step, why=why,
+    )
+
+
+#: ``rotation_verdict`` 的结论。
+ROT_OK = "ok"
+ROT_LIMIT = "limit"   # 转了一部分、方向对、但没走完 ⇒ 最可能是贴到关节限位
+ROT_AXIS = "axis"     # 转轴与工具 Z 不共线 ⇒ 以「度」为单位的一切动作都不成立
+ROT_SHORT = "short"   # 其它偏差（方向不对/过冲）⇒ 真有问题，该停
+
+
+def rotation_verdict(
+    delta_deg: float,
+    got_deg: float,
+    mis_deg: float,
+    tol_deg: float,
+) -> str:
+    """绕工具 Z 转 ``delta_deg``、实测转 ``got_deg`` 的验收判据。
+
+    **纯函数**：这条判据 2026-09-24 把整条 S09 判死过一次
+    （"期望 +180.0°、实测 +16.4°，偏差过大" → ABORT），而真因只是
+    **关节7 的行程被前面的棘轮回转顶到了上限**（±175°，而棘轮一步就要 180°）。
+    判错两个方向都贵：
+      * 把"贴限位"判成"short" ⇒ 白扔一整轮（还可能把规撞歪）；
+      * 把"真有问题"判成"limit" ⇒ 该停的没停，继续拧。
+
+    还有一条**曾经漏判**的：只比绝对值时，"让它 +180、它 -170"会算成
+    10° 误差、落在 10% 容差里 ⇒ 判成功，而实际上一个孔都没拧。所以方向必须比。
+    所以它必须能脱离节点被钉住。
+    """
+    if abs(got_deg) < 3.0 and abs(delta_deg) > 10.0:
+        return ROT_LIMIT          # 几乎没动：与下面的"只走了一部分"同因不同余量
+    if mis_deg > tol_deg:
+        return ROT_AXIS
+    if abs(got_deg) >= 3.0 and abs(delta_deg) >= 3.0 and got_deg * delta_deg < 0:
+        # 方向反了。这条以前**漏判**：老判据只比绝对值
+        # （``abs(abs(got) - abs(delta))``），所以"让它 +180、它 -170"会算 10° 误差
+        # 落在 10% 容差里 → 判成功，而实际上一个孔都没拧。
+        return ROT_SHORT
+    slack = max(tol_deg, 0.1 * abs(delta_deg))
+    short = abs(delta_deg) - abs(got_deg)
+    if short > slack:
+        return ROT_LIMIT          # 方向对、只是没走完 ⇒ 被硬止点截住
+    if short < -slack:
+        return ROT_SHORT          # 过冲：转多了，同样不该当成到位
+    return ROT_OK
+
+
+def is_joint_limit_error(text: str) -> bool:
+    """S01：把驱动/规划报的字符串认成"撞到关节极限"。
+
+    认得出的理由：这一条在真机上不是异常而是**预期结果**（用户要求"转到不
+    触发关节极限的极限角"，那就一定会在某个角度贴住）。贴住时报 JOINT_LIMIT
+    继续流程；认不出来时按普通失败处理（更保守），但要把原文打印出来，
+    否则下次还得猜。
+    """
+    low = (text or "").lower()
+    return any(
+        key in low
+        for key in (
+            "joint limit",
+            "joint_limit",
+            "超出关节",
+            "关节极限",
+            "限位",
+            "out of range",
+            "position limit",
+            "exceed",
+        )
+    )
+
+
+# =============================================================================
+# 给人看的说明（文档 / 日志共用，避免两处各写一份，规则 M2）
+# =============================================================================
+STATE_DOC: Dict[State, str] = {
+    State.BOOT: "各服务自检（伺服/相机/夹爪/触觉图传/推理预热）",
+    State.HOME_EXTREME: "回 home + 按工具系逆时针转到极限角",
+    State.GRIP_CHECK: "夹持确认（预留，当前跳过）",
+    State.COARSE: "手外 D435 粗定位（S04 的兜底）",
+    State.TRACK: "腕部 D405 跟踪 k0",
+    State.SNAP: "像素稳定后拍照定位 k1/k2 并记位姿",
+    State.HOVER_K: "到当前孔正悬停",
+    State.CONTACT: "柔顺接触（切向力突变即停）",
+    State.SCREW_IN: "旋转装配循环（棘轮式拧入）",
+    State.UNSCREW: "反向拧出",
+    State.NEXT_HOLE: "换到下一个孔（回 S05 记录的位姿）",
+    State.RESET: "复位退出",
+    State.DONE: "正常结束",
+    State.ABORT: "中止（已退到安全高度）",
+}
+
+EVENT_DOC: Dict[Event, str] = {
+    Event.BOOT_OK: "全部服务就绪",
+    Event.BOOT_FAIL: "有服务不就绪",
+    Event.POSE_OK: "位姿就绪",
+    Event.JOINT_LIMIT: "已贴住关节极限",
+    Event.GRIP_PASS: "确认夹着通止规",
+    Event.GRIP_FAIL: "没夹住/判定不可信",
+    Event.GRIP_SKIPPED: "跳过夹持确认",
+    Event.COARSE_OK: "粗定位出坐标",
+    Event.COARSE_FAIL: "粗定位失败（回跟踪兜底）",
+    Event.TRACK_LOCKED: "跟踪锁定 k0",
+    Event.TRACK_LOST: "跟踪丢失超时",
+    Event.SNAP_OK: "定位到 k1/k2",
+    Event.SNAP_UNSTABLE: "画面还在动",
+    Event.ALIGN_OK: "对位在容差内",
+    Event.ALIGN_OUT: "对位超差",
+    Event.CONTACT_OK: "已接触工件面",
+    Event.FORCE_OVER_LIMIT: "力超安全上限",
+    Event.HOLE_LOST: "孔位失效（规没进孔）",
+    Event.SCREW_DONE: "达到目标圈数",
+    Event.SCREW_STOP: "力判据满足（咬住螺纹）",
+    Event.GAUGE_DROPPED: "通止规掉了",
+    Event.UNSCREW_DONE: "已拧出",
+    Event.RELOADED: "人工放好并按 R",
+    Event.TIMEOUT: "阶段超时",
+    Event.ABORT: "中止",
+}

build_sim/arm_application/build/lib/arm_application/gauge_gripper.py

@@ -0,0 +1,228 @@
+"""夹爪客户端：把"真机今天的接口"和"框架契约里的接口"收敛到一个出入口。
+
+为什么要这一层（这不是多余抽象）
+--------------------------------
+现场查证（2026-09-24）发现**同一个服务名后面挂着两种类型**：
+
+===============  ==========================  ================================
+服务             A：真机今天跑的              B：框架契约 / 仿真
+===============  ==========================  ================================
+``/gripper/open``   ``std_srvs/Trigger``      ``arm_msgs/srv/GripperExecute``
+``/gripper/close``  ``std_srvs/Trigger``      ``arm_msgs/srv/GripperExecute``
+状态             ``/gripper/states``         ``/gripper/status``
+                （Float32MultiArray，5 元）   （arm_msgs/GripperStatus）
+===============  ==========================  ================================
+
+A 来自 ``arm_application/dh_pgea_node.py``（真机在跑的那个节点，grip_state 直接
+带"咬住/掉落"）；B 来自 ``devices/grippers/dh_pgea/manifest.yaml`` 声明的
+``arm_driver_dh_gripper`` —— **那个包还不存在**（``src/`` 下没有），目前只有
+仿真适配器实现了它。
+
+于是有两个错法，都得避免：
+
+* 只按契约（B）写：真机上服务类型对不上，装配流程一步都走不了，
+  而且报错会像是"服务没起"（实际起了，只是类型不同）；
+* 只按真机（A）写：仿真跑不了，而且把"临时接口"固化成事实（规则 M2）。
+
+所以：**运行时看服务类型自己选**（:func:`pick_backend`），装配节点只依赖
+:class:`GripperClient` 的 ``open/close/wait/is_gripped`` 四个方法。
+等 ``arm_driver_dh_gripper`` 落地后，把 ``gauge_assembly.gripper_backend``
+改回 ``auto`` 即可自动切过去，应用层不用改一行。
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Dict, List, Mapping, Optional, Sequence
+
+#: 契约后端用到的服务名（``arm_msgs/srv/GripperExecute``）。
+CONTRACT_TYPE = "arm_msgs/srv/GripperExecute"
+#: 真机今天用的后端（``std_srvs/srv/Trigger``）。
+LEGACY_TYPE = "std_srvs/srv/Trigger"
+
+CONTRACT_OPEN = "/gripper/open"
+CONTRACT_CLOSE = "/gripper/close"
+CONTRACT_STATUS_TOPIC = "/gripper/status"
+LEGACY_STATUS_TOPIC = "/gripper/states"
+
+#: DH 状态字（与 ``arm_msgs/GripperStatus.msg`` 里的注释一致）。
+GRIP_STATE_MOVING = 0
+GRIP_STATE_REACHED = 1
+GRIP_STATE_CAUGHT = 2
+GRIP_STATE_DROPPED = 3
+GRIP_STATE_NAME = {
+    GRIP_STATE_MOVING: "运动中",
+    GRIP_STATE_REACHED: "到位",
+    GRIP_STATE_CAUGHT: "咬住物体",
+    GRIP_STATE_DROPPED: "物体掉落",
+}
+
+BACKENDS = ("contract", "legacy")
+
+
+class GripperInterfaceError(RuntimeError):
+    """夹爪接口无法确定。消息里必须写清"现在是什么状态、怎么修"。"""
+
+
+@dataclass(frozen=True)
+class BackendChoice:
+    """选择结果。"""
+
+    backend: str
+    """``contract`` 或 ``legacy``。"""
+
+    reason: str
+    """为什么这么选 —— 要能直接打进启动日志。"""
+
+    status_topic: str
+    """该后端下状态从哪个话题读。"""
+
+
+def pick_backend(
+    services: Mapping[str, Sequence[str]], prefer: str = "auto"
+) -> BackendChoice:
+    """按**实际服务类型**决定用哪种夹爪接口。
+
+    :param services: ``{服务名: [类型, ...]}``，直接来自
+        ``Node.get_service_names_and_types()``。
+    :param prefer: ``auto`` / ``contract`` / ``legacy``。
+        非 auto 时若实际类型与之不符，**不静默降级**，而是报错 ——
+        现场明确指定了后端却跑在另一个上，是最难查的一类"改了没生效"。
+    :raises GripperInterfaceError: 认不出来、两种都有、或指定的后端对不上。
+
+    用类型而不是名字判断，是因为两种后端的**服务名完全一样**（都是
+    ``/gripper/open``）：只看名字必然选不出来，而"随便选一个"的后果是
+    调用一个类型不匹配的服务，报出来的错与真实原因无关（规则 M1）。
+    """
+    if prefer not in ("auto",) + BACKENDS:
+        raise GripperInterfaceError(
+            f"gripper_backend 只能是 auto / {' / '.join(BACKENDS)}，收到 {prefer!r}"
+        )
+
+    open_types = list(services.get(CONTRACT_OPEN, ()))
+    close_types = list(services.get(CONTRACT_CLOSE, ()))
+    has_contract = CONTRACT_TYPE in open_types and CONTRACT_TYPE in close_types
+    has_legacy = LEGACY_TYPE in open_types and LEGACY_TYPE in close_types
+
+    if prefer == "contract" and not has_contract:
+        raise GripperInterfaceError(
+            f"配置要求 contract 后端，但 {CONTRACT_OPEN} 的类型是 "
+            f"{open_types or '（服务不存在）'}，不是 {CONTRACT_TYPE}。\n"
+            "  怎么修：① 起仿真（它有契约服务）或等 arm_driver_dh_gripper 落地；"
+            "② 改用 gauge_assembly.gripper_backend: legacy（真机今天的接口）；"
+            "③ 改回 auto 让本函数自己选"
+        )
+    if prefer == "legacy" and not has_legacy:
+        raise GripperInterfaceError(
+            f"配置要求 legacy 后端，但 {CONTRACT_OPEN} 的类型是 "
+            f"{open_types or '（服务不存在）'}，不是 {LEGACY_TYPE}。\n"
+            "  怎么修：确认真机侧起的是 arm_application 的 dh_pgea 节点"
+            "（bash scripts/run_gripper.sh --arm 89）"
+        )
+
+    if has_contract and has_legacy:
+        # 同名服务两种类型同时在线：ROS 2 允许，但调用方只能撞运气。
+        raise GripperInterfaceError(
+            f"{CONTRACT_OPEN} 同时存在两种类型：{open_types}。"
+            "是仿真适配器和真机节点同时起着（或旧的没停干净）。\n"
+            "  怎么修：只留一个 —— 仿真用 scripts/stop_sim_gz.sh 停干净，"
+            "真机 pkill -f dh_pgea"
+        )
+
+    if prefer == "auto":
+        if has_contract:
+            return BackendChoice("contract", f"{CONTRACT_OPEN} 是 {CONTRACT_TYPE}", CONTRACT_STATUS_TOPIC)
+        if has_legacy:
+            return BackendChoice(
+                "legacy",
+                f"{CONTRACT_OPEN} 是 {LEGACY_TYPE}（真机 dh_pgea 节点；"
+                "契约驱动 arm_driver_dh_gripper 还没落地）",
+                LEGACY_STATUS_TOPIC,
+            )
+        raise GripperInterfaceError(_unrecognised_msg(services, open_types))
+
+    if prefer == "contract":
+        return BackendChoice("contract", "配置指定 contract", CONTRACT_STATUS_TOPIC)
+    return BackendChoice("legacy", "配置指定 legacy", LEGACY_STATUS_TOPIC)
+
+
+def _unrecognised_msg(
+    services: Mapping[str, Sequence[str]], open_types: Sequence[str]
+) -> str:
+    """认不出来时，把"服务不在"和"服务在但类型不对"分开说。
+
+    两者都报成"服务没起"是这个仓库反复踩过的坑：现场明明起了夹爪节点，
+    却被提示"去起夹爪节点"，然后开始怀疑人生（规则 M1、R5.4）。
+    """
+    missing = [n for n in (CONTRACT_OPEN, CONTRACT_CLOSE) if n not in services]
+    if missing:
+        return (
+            f"{' / '.join(missing)} 不在，夹爪节点没起来。\n"
+            "  怎么修：真机 bash scripts/run_gripper.sh --arm 89；"
+            "仿真 bash scripts/run_sim_gz.sh（仿真自带契约服务）"
+        )
+    return (
+        f"{CONTRACT_OPEN} 在，但类型是 {list(open_types)}，"
+        f"既不认识 {CONTRACT_TYPE}（契约）也不认识 {LEGACY_TYPE}（真机 dh_pgea）。\n"
+        "  常见原因：这个服务名被别的节点占用了。查是谁：\n"
+        "    ros2 service list -t | grep gripper\n"
+        "  怎么修：停掉那个节点，或改 gauge_assembly.gripper_backend 明确指定后端"
+    )
+
+
+# =============================================================================
+# 状态解读（纯函数）
+# =============================================================================
+def is_gripped_from_state(grip_state: int) -> Optional[bool]:
+    """DH 状态字 → "爪里有没有东西"。
+
+    ``2`` 咬住 / ``3`` 掉落 是明确的；``0`` 运动中、``1`` 到位（空爪）都要
+    小心：**"到位"不等于"夹住了"** —— 空爪闭合也会到位。所以只有 2/3 给
+    明确答案，其余返回 None（"不知道"），由调用方决定要不要等。
+
+    返回 None 而不是 False 很重要：把"不知道"当成"没夹住"会让 E1 掉件检测
+    在任何一次开合过程中误报。
+    """
+    if grip_state == GRIP_STATE_CAUGHT:
+        return True
+    if grip_state == GRIP_STATE_DROPPED:
+        return False
+    return None
+
+
+def is_dropped_from_state(grip_state: int, was_gripped: bool) -> bool:
+    """E1 掉件判据：**曾经夹住过**，现在状态字变成"掉落"才是掉件。
+
+    为什么要 ``was_gripped``：装配前爪里本来就是空的，那时 ``grip_state``
+    可能是 3（掉落/无物），直接判成掉件会让流程一开始就报 E1。
+    """
+    return was_gripped and grip_state == GRIP_STATE_DROPPED
+
+
+def legacy_states_to_fields(data: Sequence[float]) -> Dict[str, float]:
+    """``/gripper/states`` 的 5 元素数组 → 具名字段。
+
+    超出长度或为空时返回空字典（而不是抛异常）：这是**状态话题**，
+    收到畸形数据时应该让上层走"不知道"分支，而不是让整个节点崩掉。
+    """
+    if not data or len(data) < 5:
+        return {}
+    return {
+        "initialized": float(data[0]),
+        "grip_state": float(data[1]),
+        "position": float(data[2]),
+        "target_position": float(data[3]),
+        "target_force": float(data[4]),
+    }
+
+
+def describe_backend_choice(choice: BackendChoice) -> List[str]:
+    """给启动日志用的多行说明（含"契约驱动还没落地"这类现场事实）。"""
+    lines = [f"夹爪后端 = {choice.backend}（{choice.reason}）"]
+    if choice.backend == "legacy":
+        lines.append(
+            "  注意：legacy 后端下 /gripper/open|close 是 std_srvs/Trigger，"
+            "开口是 1000‰ 的百分比语义，不是毫米；装配流程只用它开合，"
+            "不靠它量开口"
+        )
+    return lines

build_sim/arm_application/build/lib/arm_application/gauge_kin.py

@@ -0,0 +1,244 @@
+"""通止规装配用的小几何/解析工具 —— **纯函数**，可离线单测。
+
+放这里的都是"算错了不会抛异常、只会让动作做错"的东西（规则 M3、R5.1）：
+
+* ``parse_joint_limits``：从 URDF 文本里抠关节限位，给 S01 的"转到极限角"
+  做软夹取。为什么不用 ``urdf_parser_py``：本机没装，多一个依赖就多一个
+  "在真机上 import 失败"的机会；限位信息在 URDF 里是极规整的一行
+  ``<limit lower=... upper=...>``，用标准库解析足够，而且**可控**。
+* ``prewind``：算"逆时针转到不触发关节极限的极限角"到底能转多少度。
+* ``rotation_about_axis_deg``：事后验证那个动作**真的**是绕工具 Z 转的。
+  这条是 S01 的验收判据：不验证的话，"绕错轴"会表现成"转是转了，孔位
+  偏了"，而且要到几个阶段之后才看出来。
+"""
+
+from __future__ import annotations
+
+import math
+import re
+import xml.etree.ElementTree as ET
+from dataclasses import dataclass
+from typing import Dict, Iterable, Optional, Sequence, Tuple
+
+import numpy as np
+
+#: 限位夹取时留的余量（度）。贴到限位上再下发，驱动器会因为"位置超出限位"
+#: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
+DEFAULT_LIMIT_MARGIN_DEG = 3.0
+
+_JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
+_ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
+
+
+def _limits_by_lines(text: str) -> Dict[str, Tuple[float, float]]:
+    """逐行兜底解析：记住最近一个 ``<joint name=...>``，紧跟的 ``<limit>`` 归它。
+
+    为什么要"记住"而不是"要求同一行"：正常格式化的 URDF 里关节名和 limit
+    **不在同一行**，只认可同一行的兜底等于永远不生效（一个不会触发的兜底，
+    和没有兜底是一样的）。属性顺序也按出现顺序读，不假设 lower 在 upper 前。
+    """
+    out: Dict[str, Tuple[float, float]] = {}
+    current: Optional[str] = None
+    for line in text.splitlines():
+        m = _JOINT_RE.search(line)
+        if m:
+            current = m.group(1)
+        if current is None or "<limit" not in line:
+            continue
+        attrs = dict(_ATTR_RE.findall(line))
+        if "lower" in attrs and "upper" in attrs:
+            out[current] = (float(attrs["lower"]), float(attrs["upper"]))
+            current = None
+    return out
+
+
+@dataclass(frozen=True)
+class PrewindResult:
+    """S01 预绕量的计算结果。"""
+
+    target_rad: float
+    """目标关节角 [rad]。"""
+
+    applied_deg: float
+    """实际能转的角度（带符号，符合"逆时针为正"的约定）[deg]。"""
+
+    want_deg: float
+    """希望转的角度 [deg]。"""
+
+    clamped: bool
+    """是否因为关节限位被夹取过。被夹取时 ``applied_deg < want_deg``。"""
+
+    has_limits: bool
+    """是否拿到了可信的关节限位。False 表示没有任何夹取依据。"""
+
+
+def parse_joint_limits(urdf_xml: str) -> Dict[str, Tuple[float, float]]:
+    """从 URDF 文本里解析 ``关节名 -> (lower, upper)``（弧度）。
+
+    关节名**原样保留**（含 ``Rizon4-063989_`` 前缀），调用方自己决定怎么匹配；
+    这里不做"去前缀"之类的猜测 —— 猜测会在换臂时静默失配，然后限位夹取
+    悄悄失效（所有关节都"查不到限位"，于是不夹取，于是贴着限位下发）。
+
+    解析失败的关节直接不进结果集：``continuous`` 关节本来就没有限位，
+    不该被当成"解析失败"。调用方用"查不到 = 不夹取"来处理。
+    """
+    out: Dict[str, Tuple[float, float]] = {}
+    if not urdf_xml or "<robot" not in urdf_xml:
+        return out
+    try:
+        root = ET.fromstring(urdf_xml)
+    except ET.ParseError:
+        # URDF 里出现未展开的 xacro 或未转义的 ``&`` 是常见的（拿错话题就是
+        # 这种内容）。降级成逐行扫描仍能救回限位；救不回就返回空表（= 不夹取），
+        # 由调用方告警 —— **不会**返回半套数据。
+        return _limits_by_lines(urdf_xml)
+
+    for joint in root.findall(".//joint"):
+        name = joint.get("name")
+        limit = joint.find("limit")
+        if not name or limit is None:
+            continue
+        lo, up = limit.get("lower"), limit.get("upper")
+        if lo is None or up is None:
+            continue
+        try:
+            out[name] = (float(lo), float(up))
+        except ValueError:
+            continue
+    return out
+
+
+def match_limits(
+    limits: Dict[str, Tuple[float, float]], joint_names: Sequence[str]
+) -> Optional[Tuple[float, float]]:
+    """按名字给一串关节名找限位，**要求全部找到**才返回。
+
+    只有一部分找到时返回 None 而不是"把找到的那部分用上"：半套限位会让
+    "能不能转到极限角"的结论变得不可解释（有些关节夹了、有些没夹），
+    与其给出一个说不清的边界，不如明确地说"这次没有限位依据"。
+    """
+    found = [limits.get(n) for n in joint_names]
+    if any(v is None for v in found):
+        return None
+    return found[-1]  # 最后一个 = 我们要绕的那根轴（调用方传的顺序有意义）
+
+
+def prewind(
+    q_now_rad: float,
+    want_deg: float,
+    direction: int,
+    limits: Optional[Tuple[float, float]] = None,
+    margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
+) -> PrewindResult:
+    """算 S01 的预绕目标角。
+
+    :param q_now_rad: 当前关节角 [rad]。
+    :param want_deg: 想转的角度（**正值**）[deg]。
+    :param direction: ``+1`` = 绕 +Z 逆时针（右手定则）；``-1`` = 顺时针。
+        符号到底对应现场的哪个方向，用真机确认一次后改 ``site.yaml`` 即可，
+        不要靠"改代码里的负号"—— 那种改动没人 review 得到（规则 M2）。
+    :param limits: ``(lower, upper)`` [rad]。给 None 时不做夹取。
+    :param margin_deg: 距离限位保留的余量 [deg]。
+
+    :returns: :class:`PrewindResult`。**无论有没有限位，都返回目标角**，
+        因为"贴到限位再继续转"在现场是预期行为（用户要求转到极限角），
+        驱动器真拒绝时会由 :func:`arm_application.gauge_fsm.is_joint_limit_error`
+        认成 JOINT_LIMIT 事件继续流程。
+
+    为什么要在这里夹取（而不是直接下发、等驱动器报错）：
+    驱动器拒绝一条轨迹时，整个 action 失败，连带把已经转过的那部分**也**
+    变成"没转过"的语义（角度实际到位了，但流程认为失败）。夹取到
+    "限位 − 余量"能拿到一个明确成功、且确实接近极限的姿态度。
+    """
+    if want_deg <= 0.0:
+        raise ValueError(f"预绕角度必须为正，收到 {want_deg}")
+    if direction not in (-1, 1):
+        raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
+
+    signed = direction * abs(want_deg)
+    target = q_now_rad + math.radians(signed)
+    has_limits = limits is not None
+    clamped = False
+    if limits is not None:
+        lo = limits[0] + math.radians(margin_deg)
+        up = limits[1] - math.radians(margin_deg)
+        if lo > up:  # 限位本身比余量还窄（不该发生，但别让它变成静默走偏）
+            lo = up = 0.5 * (limits[0] + limits[1])
+        if target > up:
+            target, clamped = up, True
+        elif target < lo:
+            target, clamped = lo, True
+    applied = math.degrees(target - q_now_rad)
+    return PrewindResult(
+        target_rad=target,
+        applied_deg=applied,
+        want_deg=abs(want_deg),
+        clamped=clamped,
+        has_limits=has_limits,
+    )
+
+
+def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
+    """从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
+
+    S01 的验收判据就靠它：绕工具 Z 转 Δ 之后，如果实际绕的是别的轴，
+    这个函数给出的角度会与 Δ 明显不符（或者 axis 分量不占优）。
+    做法是把相对旋转投影到 ``axis`` 上：
+
+    ``θ = atan2( axis·(R_b·R_a⁻¹ 的反对称部分) , (trace-1)/2 )``
+
+    :param axis: **单位**轴（工具 Z 在世界系下的方向 = ``R[:, 2]``）。
+    """
+    a = np.asarray(axis, dtype=float)
+    n = float(np.linalg.norm(a))
+    if n < 1e-9:
+        raise ValueError("轴向量长度为 0，无法判定旋转")
+    a = a / n
+
+    Rb = np.asarray(R_before, dtype=float)[:3, :3]
+    Ra = np.asarray(R_after, dtype=float)[:3, :3]
+    Rrel = Ra @ Rb.T
+    # 反对称部分：sin(θ)·[axis]_×
+    v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
+    sin_t = 0.5 * float(np.dot(v, a))
+    cos_t = 0.5 * (float(np.trace(Rrel)) - 1.0)
+    return math.degrees(math.atan2(sin_t, cos_t))
+
+
+def axis_misalignment_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
+    """相对旋转的转轴与 ``axis`` 的夹角 [deg]。
+
+    纯绕轴旋转时这个值≈0；转错轴时它会接近 90°。用作 S01 的判错依据：
+    只看"转了多少度"是不够的 —— 绕正确的轴转 90° 和绕错误的轴转 90°
+    给出的角度读数一样。
+    """
+    Rb = np.asarray(R_before, dtype=float)[:3, :3]
+    Ra = np.asarray(R_after, dtype=float)[:3, :3]
+    Rrel = Ra @ Rb.T
+    # 旋转轴 = 反对称部分对应的向量（sinθ 因子不影响方向）
+    v = np.array([Rrel[2, 1] - Rrel[1, 2], Rrel[0, 2] - Rrel[2, 0], Rrel[1, 0] - Rrel[0, 1]])
+    nv = float(np.linalg.norm(v))
+    if nv < 1e-9:
+        return 0.0  # 几乎没转，谈不上"转错轴"
+    a = np.asarray(axis, dtype=float)
+    a = a / float(np.linalg.norm(a))
+    cos = abs(float(np.dot(v / nv, a)))  # 取绝对值：轴线的两个方向等价
+    return math.degrees(math.acos(max(-1.0, min(1.0, cos))))
+
+
+def unit_tool_z(R: np.ndarray) -> np.ndarray:
+    """工具坐标系的 Z 轴在世界系下的方向（旋转矩阵的第三列）。"""
+    return np.asarray(R, dtype=float)[:3, 2]
+
+
+def vertical_down_error_deg(R: np.ndarray, world_down: Iterable[float] = (0.0, 0.0, -1.0)) -> float:
+    """工具 Z 与"垂直向下"的夹角 [deg]。0 = 完全垂直向下。
+
+    看板里"工具系全程垂直向下、位姿全局锁定"这句话，只有能算出这个数才算
+    真的在执行 —— 否则每个阶段都得靠人眼看画面判断臂歪没歪。
+    """
+    z = unit_tool_z(R)
+    d = np.asarray(list(world_down), dtype=float)
+    d = d / float(np.linalg.norm(d))
+    cos = float(np.dot(z / max(1e-12, float(np.linalg.norm(z))), d))
+    return math.degrees(math.acos(max(-1.0, min(1.0, cos))))

build_sim/arm_application/build/lib/arm_application/geom_ik.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""向后兼容 shim —— 实现已移到 :mod:`arm_kinematics.geom_ik`。
+
+为什么搬走
+----------
+这套几何雅可比 + DLS 逆解同时被应用层（视觉伺服、搜孔装配）和驱动层
+（``arm_driver_flexiv`` 的柔顺下压）需要。留在 ``arm_application`` 里会让驱动层
+反过来依赖应用层，属于层次倒置。搬到不依赖 ROS 的 ``arm_kinematics`` 后，
+两边都依赖一个底层包。
+
+为什么保留这个文件
+------------------
+``stage2_locate_node`` / ``stage3_search_node`` / ``servo_node`` 等现有代码都在
+``from arm_application.geom_ik import ...``。保留 shim 就不用改动这些已经在跑的
+文件 —— 重构不应牵连能工作的代码。
+
+新代码请直接 ``from arm_kinematics import dls_twist``。
+"""
+
+from arm_kinematics.geom_ik import (  # noqa: F401
+    RIZON_LINK_AXES,
+    dls_position_step,
+    dls_translate_hold_ori,
+    dls_twist,
+    geometric_jacobian,
+    load_handeye_T,
+    position_jacobian,
+    quat_xyzw_to_R,
+)
+
+__all__ = [
+    "RIZON_LINK_AXES",
+    "quat_xyzw_to_R",
+    "geometric_jacobian",
+    "position_jacobian",
+    "dls_position_step",
+    "dls_twist",
+    "dls_translate_hold_ori",
+    "load_handeye_T",
+]

build_sim/arm_application/build/lib/arm_application/goto_ee_plane.py

@@ -0,0 +1,457 @@
+#!/usr/bin/env python3
+"""感知/伺服启动前的开机回初始位流程。
+
+流程：
+  [A] 若法兰低于 ``servo.safe_z_m``，先让它竖直上升（闭环 Z 小步跳），
+      以免回 home 时扫到墙面/工装；
+  [B] 调用 ``/set_named_pose``（Flexiv 命名位姿，默认 "home"）—— 由 Flexiv
+      自己的规划器把机械臂送回 Flexiv 初始位姿；
+  [C] 保持（pin）住，让伺服从一个干净、静止的位姿接手。
+"""
+
+from __future__ import annotations
+
+import sys
+import time
+from pathlib import Path
+from typing import List, Optional, Tuple
+
+import numpy as np
+import rclpy
+from arm_msgs.srv import SetNamedPose
+from control_msgs.action import FollowJointTrajectory
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from tf2_ros import Buffer, TransformListener
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+from arm_application.geom_ik import (
+    RIZON_LINK_AXES,
+    dls_translate_hold_ori,
+    dls_twist,
+    geometric_jacobian,
+    quat_xyzw_to_R,
+)
+from arm_application.site_io import (
+    base_frame_default,
+    ee_frame_default,
+    joint_prefix_default,
+)
+
+from arm_application.ros_node import TolerantNode
+
+
+
+def _load_ee_cfg() -> dict:
+    root = Path(__file__).resolve().parents[3]
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import load_site, resolve_robot  # type: ignore
+
+    data = load_site()
+    servo = (data.get("servo") or {})
+    robot = resolve_robot(data)
+    ee_z = float(servo.get("ee_z_m") or 0.24)
+    return {
+        "prefix": str(robot.get("joint_prefix") or joint_prefix_default()),
+        "base": str(robot.get("base_frame") or base_frame_default()),
+        "ee": str(robot.get("ee_frame") or ee_frame_default()),
+        "z": ee_z,
+        # 回 home 前先竖直升到这个 Z（避免撞到墙面/工装）；
+        # 然后再回到 Flexiv 的命名初始位姿。
+        "safe_z": float(servo.get("safe_z_m") or max(0.40, ee_z + 0.10)),
+        "home_pose": str(servo.get("home_pose_name") or "home"),
+        "home_speed": float(servo.get("home_pose_speed") or 0.30),
+        "v_z": float(servo.get("v_max_z") or 0.04),
+        # 仅用于（目前未使用的）verticalize 规划路径
+        "kp": float(servo.get("ee_hold_kp") or 2.0),
+        "w_max": float(servo.get("ee_w_max") or 0.50),
+    }
+
+
+def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
+    pt = JointTrajectoryPoint()
+    pt.positions = [float(v) for v in q.tolist()]
+    pt.velocities = [float(v) for v in qdot.tolist()]
+    pt.accelerations = [0.0] * int(q.size)
+    ns = int(round(max(0.0, float(t_sec)) * 1e9))
+    pt.time_from_start.sec = ns // 1_000_000_000
+    pt.time_from_start.nanosec = ns % 1_000_000_000
+    return pt
+
+
+class GotoEePlane(TolerantNode):
+    def __init__(self, cfg: dict) -> None:
+        super().__init__("goto_ee_plane")
+        self.cfg = cfg
+        prefix = cfg["prefix"]
+        self.joint_names = [f"{prefix}joint{i}" for i in range(1, 8)]
+        self.link_frames = [f"{prefix}link{i}" for i in range(1, 8)]
+        self._js: Optional[JointState] = None
+        self.create_subscription(JointState, "joint_states", self._on_js, 10)
+        self._tf = Buffer()
+        TransformListener(self._tf, self, spin_thread=True)
+        self._cli = ActionClient(
+            self, FollowJointTrajectory, "rizon_arm_controller/follow_joint_trajectory"
+        )
+
+    def _on_js(self, msg: JointState) -> None:
+        self._js = msg
+
+    def _spin(self, dt: float = 0.05) -> None:
+        rclpy.spin_once(self, timeout_sec=dt)
+
+    def _lookup(self, parent: str, child: str) -> Optional[np.ndarray]:
+        try:
+            tf = self._tf.lookup_transform(
+                parent, child, rclpy.time.Time(),
+                timeout=rclpy.duration.Duration(nanoseconds=80_000_000),
+            )
+        except Exception:
+            return None
+        t = tf.transform.translation
+        o = tf.transform.rotation
+        T = np.eye(4)
+        T[:3, :3] = quat_xyzw_to_R([o.x, o.y, o.z, o.w])
+        T[:3, 3] = [t.x, t.y, t.z]
+        return T
+
+    def _q(self) -> Optional[np.ndarray]:
+        if self._js is None:
+            return None
+        m = {n: p for n, p in zip(self._js.name, self._js.position)}
+        try:
+            return np.array([float(m[n]) for n in self.joint_names], dtype=float)
+        except KeyError:
+            return None
+
+    def _jacobian(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
+        link_R: List[np.ndarray] = []
+        link_p: List[np.ndarray] = []
+        for fr in self.link_frames:
+            Tl = self._lookup(self.cfg["base"], fr)
+            if Tl is None:
+                return None
+            link_R.append(Tl[:3, :3])
+            link_p.append(Tl[:3, 3])
+        return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
+
+    def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
+        z_now = R[:, 2]
+        z_des = np.array([0.0, 0.0, -1.0])
+        c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
+        ang = float(np.arccos(c))
+        if ang < 0.02:
+            return np.zeros(3), ang
+        axis = np.cross(z_now, z_des)
+        n = float(np.linalg.norm(axis))
+        axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
+        mag = float(np.clip(self.cfg["kp"] * ang, -self.cfg["w_max"], self.cfg["w_max"]))
+        return axis * mag, ang
+
+    def _target_err(
+        self, p: np.ndarray, R: np.ndarray, target: np.ndarray, verticalize: bool
+    ) -> Tuple[np.ndarray, float, float]:
+        dp = np.asarray(target, dtype=float).reshape(3) - np.asarray(p, dtype=float).reshape(3)
+        _, ang = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
+        xy = float(np.hypot(dp[0], dp[1]))
+        return dp, ang, xy
+
+    def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
+        t0 = time.monotonic()
+        q = None
+        Tee = None
+        while time.monotonic() - t0 < timeout_s:
+            self._spin(0.05)
+            q = self._q()
+            Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
+            if q is not None and Tee is not None and self._jacobian(Tee[:3, 3]) is not None:
+                return q, Tee
+        return q, Tee
+
+    def _send_action(self, traj: JointTrajectory, wait_s: float) -> bool:
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory = traj
+        fut = self._cli.send_goal_async(goal)
+        t0 = time.monotonic()
+        while not fut.done() and time.monotonic() - t0 < 4.0:
+            self._spin(0.05)
+        handle = fut.result() if fut.done() else None
+        if handle is None or not handle.accepted:
+            print("  错误：JTC 拒绝了抬升轨迹", flush=True)
+            return False
+        res = handle.get_result_async()
+        t0 = time.monotonic()
+        last = 0.0
+        while not res.done() and time.monotonic() - t0 < wait_s:
+            self._spin(0.05)
+            now = time.monotonic()
+            if now - last > 0.4:
+                Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
+                if Tee is not None:
+                    p = Tee[:3, 3]
+                    print(
+                        f"  运动中  ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})  "
+                        f"z_err={(self.cfg['z'] - p[2]) * 1000:+.0f}mm",
+                        flush=True,
+                    )
+                last = now
+        if not res.done():
+            print("  警告：JTC 结果超时 —— 仍然继续检查 TF", flush=True)
+            return False
+        return True
+
+    def _plan_and_send(
+        self,
+        q0: np.ndarray,
+        Tee: np.ndarray,
+        target: np.ndarray,
+        verticalize: bool,
+        tag: str = "",
+        max_seg: float = 0.08,
+        v: float = 0.05,
+    ) -> bool:
+        """规划并执行一段朝向 target 的短笛卡尔位移（≤ max_seg）。
+        长距离移动由调用方以闭环小步跳的方式完成，因为雅可比是在当前位姿
+        处冻结的 —— 超过 10 cm 后线性化误差会让一次性规划严重偏离目标。"""
+        p = Tee[:3, 3].copy()
+        R = Tee[:3, :3]
+        dp, ang, _ = self._target_err(p, R, target, verticalize)
+        dist = float(np.linalg.norm(dp))
+        if dist > max_seg:  # 限制本次小步跳的长度；调用方会用最新的 TF 循环执行
+            dp = dp * (max_seg / dist)
+            dist = max_seg
+        print(
+            f"  规划 {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm  "
+            f"倾角={ang * 180 / np.pi:.1f}deg",
+            flush=True,
+        )
+        J = self._jacobian(p)
+        if J is None:
+            print("错误：缺少用于雅可比的 link TF", flush=True)
+            return False
+
+        q = q0.copy()
+        waypoints = [q.copy()]
+        remain = dp.copy()
+        w_step, _ = (self._w_down(R) if verticalize else (np.zeros(3), 0.0))
+        n_max = int(np.ceil(dist / 0.012)) + 4
+        for _ in range(n_max):
+            if float(np.linalg.norm(remain)) < 0.0015 and (
+                (not verticalize) or ang < 0.03
+            ):
+                break
+            nseg = max(1, int(np.ceil(float(np.linalg.norm(remain)) / 0.012)))
+            step = remain / nseg
+            if verticalize and ang > 0.03:
+                qdot = dls_twist(
+                    J, step / 0.12, w_base=w_step, damping=0.08, max_qdot=0.45
+                )
+                dq = np.clip(qdot * 0.12, -0.06, 0.06)
+            else:
+                dq = dls_translate_hold_ori(J, step, damping=0.08, max_dq=0.06)
+            if float(np.max(np.abs(dq))) < 1e-5:
+                break
+            q = q + dq
+            waypoints.append(q.copy())
+            remain = remain - (J[:3, :] @ dq)
+            ang = max(0.0, ang - 0.05)
+
+        planned = dist - float(np.linalg.norm(remain))
+        if len(waypoints) < 2 or planned < 0.4 * dist:
+            print(
+                f"  警告 {tag}：规划器停滞 "
+                f"（已规划 {planned*1000:.0f}/{dist*1000:.0f}mm）—— 跳过本次小步跳",
+                flush=True,
+            )
+            return False
+        # 关节偏移量的合理性闸门：拒绝过于夸张的规划
+        if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0:
+            print(f"  错误 {tag}：关节偏移 >1 rad —— 放弃本次小步跳", flush=True)
+            return False
+
+        duration = max(1.6, dist / max(min(v, 0.06), 1e-3))
+        n = len(waypoints) - 1
+        traj = JointTrajectory()
+        traj.header.stamp.sec = 0
+        traj.header.stamp.nanosec = 0
+        traj.joint_names = list(self.joint_names)
+        traj.points.append(_pt(waypoints[0], np.zeros(7), 0.08))
+        zeros = np.zeros(7)
+        for i, qi in enumerate(waypoints[1:], start=1):
+            t = 0.08 + duration * (i / n)
+            qdot = zeros if i == n else (waypoints[min(i + 1, n)] - qi) / max(
+                duration / n, 0.05
+            )
+            qdot = np.clip(qdot, -0.40, 0.40)
+            traj.points.append(_pt(qi, qdot, t))
+        # 强制最后一个点的速度为零
+        traj.points[-1].velocities = [0.0] * 7
+        print(
+            f"  发送 JTC action {tag}，{n} 步  T={duration:.1f}s",
+            flush=True,
+        )
+        return self._send_action(traj, wait_s=duration + 6.0)
+
+    def _goto(
+        self,
+        q: np.ndarray,
+        Tee: np.ndarray,
+        target: np.ndarray,
+        verticalize: bool,
+        tol_m: float,
+        tol_tilt: float,
+        tag: str,
+        v: float,
+        max_hops: int = 8,
+    ) -> Tuple[bool, np.ndarray, np.ndarray]:
+        """闭环点到点：规划 ≤8 cm 的小步跳 → 执行 → 重新测量 TF → 重复，
+        直到落在容差内。这样对长距离下的雅可比线性化误差更鲁棒。"""
+        for hop in range(max_hops):
+            p = Tee[:3, 3]
+            dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
+            dist = float(np.linalg.norm(dp))
+            if dist < tol_m and ((not verticalize) or ang < tol_tilt):
+                return True, q, Tee
+            print(
+                f"  {tag} 第 {hop + 1} 跳  remain={dist*1000:.0f}mm  "
+                f"倾角={ang * 180 / np.pi:.1f}deg",
+                flush=True,
+            )
+            if not self._plan_and_send(q, Tee, target, verticalize, tag, v=v):
+                return False, q, Tee
+            t_end = time.monotonic() + 0.5
+            while time.monotonic() < t_end:
+                self._spin(0.05)
+            q2, Tee2 = self._wait_state(2.0)
+            if q2 is None or Tee2 is None:
+                print("错误：移动过程中丢失状态", flush=True)
+                return False, q, Tee
+            q, Tee = q2, Tee2
+        p = Tee[:3, 3]
+        dp, ang, _ = self._target_err(p, Tee[:3, :3], target, verticalize)
+        ok = float(np.linalg.norm(dp)) < (tol_m * 2.0) and (
+            (not verticalize) or ang < (tol_tilt * 2.0)
+        )
+        return ok, q, Tee
+
+    def _pin(self, q: np.ndarray) -> None:
+        if not self._cli.wait_for_server(timeout_sec=3.0):
+            return
+        traj = JointTrajectory()
+        traj.header.stamp.sec = 0
+        traj.header.stamp.nanosec = 0
+        traj.joint_names = list(self.joint_names)
+        traj.points = [_pt(q, np.zeros(7), 0.08), _pt(q, np.zeros(7), 0.45)]
+        self._send_action(traj, wait_s=3.0)
+
+    def _named_pose(self, name: str, speed: float) -> bool:
+        """调用 Flexiv 的 ``/set_named_pose``，并等待机械臂静止。"""
+        cli = self.create_client(SetNamedPose, "/set_named_pose")
+        if not cli.wait_for_server(timeout_sec=8.0):
+            print("错误：``/set_named_pose`` 不可用 —— 请启动 run_real.sh", flush=True)
+            return False
+        req = SetNamedPose.Request()
+        req.pose_name = name
+        req.velocity_scale = float(min(max(speed, 0.05), 1.0))
+        req.acceleration_scale = 0.0
+        print(f"  [B] set_named_pose '{name}' speed={req.velocity_scale:.2f}", flush=True)
+        fut = cli.call_async(req)
+        t0 = time.monotonic()
+        while not fut.done() and time.monotonic() - t0 < 90.0:
+            self._spin(0.1)
+        if not fut.done():
+            print("错误：set_named_pose 超时", flush=True)
+            return False
+        resp = fut.result()
+        print(
+            f"  [B] 响应 success={resp.success}  msg={resp.message}",
+            flush=True,
+        )
+        if not resp.success:
+            return False
+        # 等待法兰停止运动（约 0.8 s 内 Δp < 1 mm）
+        last = None
+        still = 0.0
+        t0 = time.monotonic()
+        while time.monotonic() - t0 < 30.0:
+            self._spin(0.2)
+            Tee = self._lookup(self.cfg["base"], self.cfg["ee"])
+            if Tee is not None:
+                p = Tee[:3, 3]
+                if last is not None and float(np.linalg.norm(p - last)) < 0.001:
+                    still += 0.2
+                    if still >= 0.8:
+                        return True
+                else:
+                    still = 0.0
+                last = p.copy()
+        print("  警告：等待静止超时 —— 仍然继续", flush=True)
+        return True
+
+    def run(self, timeout_s: float = 25.0) -> int:
+        cfg = self.cfg
+        q, Tee = self._wait_state(4.0)
+        if q is None or Tee is None:
+            print("错误：没有 joint_states / TF —— 请启动 run_real.sh", flush=True)
+            return 2
+        if not self._cli.wait_for_server(timeout_sec=8.0):
+            print("错误：JTC action 不可用 —— 请启动 run_real.sh", flush=True)
+            return 3
+
+        p = Tee[:3, 3]
+        safe_z = float(cfg["safe_z"])
+        print(
+            f"=== 回到 Flexiv 初始位姿 '{cfg['home_pose']}'  "
+            f"safe_z={safe_z*1000:.0f}mm ===",
+            flush=True,
+        )
+        print(
+            f"  当前 ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
+            flush=True,
+        )
+
+        # --- 阶段 A：回 home 前先沿纯 Z 方向升到安全高度 ---
+        if p[2] < safe_z - 0.01:
+            lift_tgt = np.array([p[0], p[1], safe_z], dtype=float)
+            print(f"  [A] 抬升 Z {p[2]*1000:.0f} → {safe_z*1000:.0f}mm", flush=True)
+            ok, q, Tee = self._goto(
+                q, Tee, lift_tgt, False, 0.010, 0.0, "[A]lift", v=0.05
+            )
+            if not ok:
+                print("错误：抬升阶段失败 —— 就地 pin 住", flush=True)
+                self._pin(q)
+                return 4
+
+        # --- 阶段 B：Flexiv 命名位姿 → 真正的初始位姿，姿态良好 ---
+        if not self._named_pose(cfg["home_pose"], cfg["home_speed"]):
+            print("错误：回命名位姿失败 —— 就地 pin 住", flush=True)
+            self._pin(q)
+            return 4
+
+        # --- 阶段 C：保持住，让伺服从一个静止位姿接手 ---
+        q, Tee = self._wait_state(2.0)
+        if q is not None:
+            self._pin(q)
+        if Tee is not None:
+            p = Tee[:3, 3]
+            print(
+                f"  已到初始位姿  ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
+                flush=True,
+            )
+        return 0
+
+def main() -> int:
+    cfg = _load_ee_cfg()
+    rclpy.init()
+    node = GotoEePlane(cfg)
+    try:
+        return node.run()
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

build_sim/arm_application/build/lib/arm_application/handeye_calibrate.py

@@ -0,0 +1,687 @@
+#!/usr/bin/env python3
+"""完整手眼标定流水线：移动 → 棋盘稳定判定 → 采集 → 求解 → 清理。
+
+支持 eye_in_hand / eye_to_hand。
+
+相机内参优先级：
+  1) ROS 的 CameraInfo 话题（若正在发布）
+  2) 通过 pyrealsense2 读取 RealSense D405/D435 的 EEPROM（camera_source）
+  3) YAML 中手写的 camera_fx/fy/cx/cy
+
+棋盘尺寸（board_cols / board_rows / square_size_m）可由用户配置。
+"""
+
+from __future__ import annotations
+
+import json
+import shutil
+import time
+from datetime import datetime, timezone
+from pathlib import Path
+from typing import List, Optional, Tuple
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image, JointState
+from std_msgs.msg import String
+
+from arm_application.handeye_lib import (
+    BoardConfig,
+    CameraIntrinsics,
+    ChessboardStabilizer,
+    calibrate_hand_eye,
+    detect_chessboard,
+    format_transform_yaml,
+    is_pose_filled,
+    pose_to_Rt,
+    solve_board_pose,
+)
+from arm_application.handeye_realsense import (
+    is_realsense_source,
+    query_realsense_color_intrinsics,
+    resolve_realsense_topics,
+)
+from arm_application.site_io import joint_prefix_default
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import GetArmState
+
+from arm_application.ros_node import TolerantNode
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+
+
+def _image_to_bgr(msg: Image) -> np.ndarray:
+    h, w = int(msg.height), int(msg.width)
+    enc = (msg.encoding or '').lower()
+    data = np.frombuffer(msg.data, dtype=np.uint8)
+
+    if enc in ('bgr8', 'rgb8'):
+        img = data.reshape(h, w, 3)
+        return img[:, :, ::-1].copy() if enc == 'rgb8' else img
+    if enc in ('mono8', '8uc1'):
+        gray = data.reshape(h, w)
+        return np.stack([gray, gray, gray], axis=-1)
+    if enc in ('bgra8', 'rgba8'):
+        img = data.reshape(h, w, 4)
+        bgr = img[:, :, :3]
+        return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
+    raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
+
+
+def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
+    K = np.array(msg.k, dtype=float).reshape(3, 3)
+    dist = np.array(msg.d, dtype=float).reshape(-1, 1) if msg.d else np.zeros((5, 1))
+    return CameraIntrinsics(K=K, dist=dist)
+
+
+class HandeyeCalibrate(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('handeye_calibrate')
+
+        self.declare_parameter('mode', 'eye_in_hand')  # eye_in_hand | eye_to_hand
+        self.declare_parameter('joint_prefix', joint_prefix_default())
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('velocity_scale', 0.10)
+        self.declare_parameter('action_name', 'follow_joint_trajectory')
+        self.declare_parameter('move_time_sec', 4.0)
+        self.declare_parameter('start_index', 0)
+
+        self.declare_parameter('num_poses', 12)
+        self.declare_parameter('waypoint_positions', [0.0] * (12 * 7))
+        self.declare_parameter('min_poses', 5)
+
+        # 相机来源：manual | d405 | d435
+        self.declare_parameter('camera_source', 'manual')
+        self.declare_parameter('camera_serial', '')
+        self.declare_parameter('camera_namespace', '')
+        self.declare_parameter('realsense_width', 1280)
+        self.declare_parameter('realsense_height', 720)
+        self.declare_parameter('image_topic', '')
+        self.declare_parameter('camera_info_topic', '')
+        self.declare_parameter('camera_fx', 0.0)
+        self.declare_parameter('camera_fy', 0.0)
+        self.declare_parameter('camera_cx', 0.0)
+        self.declare_parameter('camera_cy', 0.0)
+        self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
+
+        # 棋盘（可自定义）
+        # 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
+        # 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
+        # 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
+        # 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
+        self.declare_parameter('board_cols', 8)  # 内角点
+        self.declare_parameter('board_rows', 11)
+        self.declare_parameter('square_size_m', 0.015)
+
+        self.declare_parameter('stable_window', 8)
+        self.declare_parameter('stable_pixel_std', 0.45)
+        self.declare_parameter('stable_timeout_sec', 25.0)
+        self.declare_parameter('settle_sec', 0.4)
+        # 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。
+        # 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明，
+        # ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现，
+        # 实际只靠一次固定 settle_sec。现在补上。
+        self.declare_parameter('settle_qd_max', 0.015)      # |qd|_max [rad/s] 低于此值算静
+        self.declare_parameter('settle_stable_sec', 0.6)    # 需要连续静多久
+        self.declare_parameter('settle_timeout_sec', 8.0)   # 等静止的总超时
+        self.declare_parameter('pin_holds', 2)              # 静止后再保持几拍
+        self.declare_parameter('pin_hold_sec', 0.45)        # 每拍多长
+        self.declare_parameter('skip_unstable_poses', True)
+
+        self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
+        self.declare_parameter('result_file', '')
+        self.declare_parameter('cleanup_images', True)
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names = [f'{prefix}{n}' for n in base]
+        self.mode = str(self.get_parameter('mode').value).strip().lower()
+        self.velocity_scale = float(self.get_parameter('velocity_scale').value)
+        self.action_name = str(self.get_parameter('action_name').value)
+        self.move_time_sec = float(self.get_parameter('move_time_sec').value)
+        self.start_index = max(0, int(self.get_parameter('start_index').value))
+        self.min_poses = int(self.get_parameter('min_poses').value)
+        self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
+
+        self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
+        self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
+        self.camera_namespace = str(
+            self.get_parameter('camera_namespace').value or ''
+        ).strip()
+        self.rs_width = int(self.get_parameter('realsense_width').value)
+        self.rs_height = int(self.get_parameter('realsense_height').value)
+
+        self.board = BoardConfig(
+            cols=int(self.get_parameter('board_cols').value),
+            rows=int(self.get_parameter('board_rows').value),
+            square_size=float(self.get_parameter('square_size_m').value),
+        )
+        self.stabilizer = ChessboardStabilizer(
+            window=int(self.get_parameter('stable_window').value),
+            max_pixel_std=float(self.get_parameter('stable_pixel_std').value),
+        )
+        self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value)
+        self.settle_sec = float(self.get_parameter('settle_sec').value)
+        self.settle_qd_max = float(self.get_parameter('settle_qd_max').value)
+        self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value)
+        self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value)
+        self.pin_holds = max(0, int(self.get_parameter('pin_holds').value))
+        self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value))
+        self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value)
+
+        out = Path(str(self.get_parameter('output_dir').value)).expanduser()
+        self.output_dir = out
+        self.images_dir = out / 'images'
+        rf = str(self.get_parameter('result_file').value or '').strip()
+        self.result_file = Path(rf).expanduser() if rf else (out / 'handeye_result.yaml')
+
+        self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
+        self._get_state = self.create_client(GetArmState, 'get_arm_state')
+        self._capture_pub = self.create_publisher(String, 'handeye/capture', 10)
+
+        self._latest_image: Optional[Image] = None
+        self._latest_info: Optional[CameraInfo] = None
+        self._latest_js: Optional[JointState] = None
+        self._intrinsics: Optional[CameraIntrinsics] = None
+
+        img_topic = str(self.get_parameter('image_topic').value or '').strip()
+        info_topic = str(self.get_parameter('camera_info_topic').value or '').strip()
+        if is_realsense_source(self.camera_source):
+            img_topic, info_topic = resolve_realsense_topics(
+                self.camera_source,
+                camera_namespace=self.camera_namespace,
+                image_topic=img_topic,
+                camera_info_topic=info_topic,
+            )
+        self._info_topic = info_topic
+
+        self.create_subscription(JointState, '/joint_states', self._on_js, 10)
+
+        if not img_topic:
+            self.get_logger().error(
+                'image_topic 为空。请设置 camera_source:=d405|d435，或填写 image_topic。'
+            )
+        else:
+            self.create_subscription(
+                Image, img_topic, self._on_image, qos_profile_sensor_data
+            )
+            self.get_logger().info(f'已订阅图像：{img_topic}（BEST_EFFORT）')
+        if info_topic:
+            self.create_subscription(
+                CameraInfo, info_topic, self._on_info, qos_profile_sensor_data
+            )
+            self.get_logger().info(f'已订阅 camera_info：{info_topic}（BEST_EFFORT）')
+
+    def _on_js(self, msg: JointState) -> None:
+        self._latest_js = msg
+
+    def _on_image(self, msg: Image) -> None:
+        self._latest_image = msg
+
+    def _on_info(self, msg: CameraInfo) -> None:
+        self._latest_info = msg
+
+    def run(self) -> int:
+        if self.mode not in ('eye_in_hand', 'eye_to_hand'):
+            self.get_logger().error(
+                f'未知的 mode={self.mode!r}；请使用 eye_in_hand 或 eye_to_hand'
+            )
+            return 1
+
+        poses = self._load_poses()
+        if len(poses) < self.min_poses:
+            self.get_logger().error(
+                f'只有 {len(poses)} 个已填写位姿（需要 >= {self.min_poses}）。'
+                '示教命令：bash ~/ros2/scripts/run_handeye.sh teach && merge。'
+            )
+            return 1
+        if self.start_index >= len(poses):
+            self.get_logger().error(
+                f'start_index {self.start_index} >= 已填写位姿数 {len(poses)}'
+            )
+            return 1
+
+        if not self._resolve_intrinsics():
+            return 1
+
+        if not self._client.wait_for_server(timeout_sec=15.0):
+            self.get_logger().error(
+                f'Action [{self.action_name}] 不可用。'
+                '请启动：bash ~/ros2/scripts/run_real.sh'
+            )
+            return 2
+
+        self.output_dir.mkdir(parents=True, exist_ok=True)
+        self.images_dir.mkdir(parents=True, exist_ok=True)
+
+        eye_to_hand = self.mode == 'eye_to_hand'
+        self.get_logger().info(
+            f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
+            f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
+            f'方格={self.board.square_size}m，v={self.velocity_scale}'
+        )
+        self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
+
+        Rg: List[np.ndarray] = []
+        tg: List[np.ndarray] = []
+        Rc: List[np.ndarray] = []
+        tc: List[np.ndarray] = []
+        meta = []
+
+        try:
+            for i in range(self.start_index, len(poses)):
+                q = poses[i]
+                self._log_progress(i, len(poses), '移动中...')
+                code = self._move_one(q, self.move_time_sec)
+                if code != 0:
+                    return code
+
+                if not self._verify_joints(q, tol=0.08):
+                    self.get_logger().error(
+                        f'[{i + 1}] 机械臂未到达目标关节位置 '
+                        '（控制器报告成功，但关节没有变化）。'
+                        '请重启 run_real.sh；示教器切 AUTO Remote；清除故障。'
+                        '不要相信这次标定结果。'
+                    )
+                    return 10
+
+                end = time.monotonic() + max(0.0, self.settle_sec)
+                while rclpy.ok() and time.monotonic() < end:
+                    rclpy.spin_once(self, timeout_sec=0.05)
+
+                # 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。
+                self._wait_settled()
+                self._pin_hold()
+
+                ok, std, corners, pattern, bgr = self._wait_stable()
+                if not ok or corners is None or bgr is None:
+                    msg = f'[{i + 1}] 棋盘不稳定（std={std:.3f}）'
+                    if self.skip_unstable:
+                        self.get_logger().warn(f'{msg} —— 跳过')
+                        continue
+                    self.get_logger().error(f'{msg}。中止。')
+                    return 7
+
+                ee = self._read_ee_pose()
+                if ee is None:
+                    self.get_logger().error('读取 ee_pose 失败')
+                    return 8
+
+                assert self._intrinsics is not None
+                R_t2c, t_t2c = solve_board_pose(
+                    corners, self.board, self._intrinsics, pattern=pattern
+                )
+                xyz = [ee.pose.position.x, ee.pose.position.y, ee.pose.position.z]
+                quat = [
+                    ee.pose.orientation.x,
+                    ee.pose.orientation.y,
+                    ee.pose.orientation.z,
+                    ee.pose.orientation.w,
+                ]
+                R_g2b, t_g2b = pose_to_Rt(xyz, quat)
+
+                stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
+                img_path = self.images_dir / f'pose_{i:02d}_{stamp}.png'
+                self._save_png(bgr, img_path)
+
+                msg = String()
+                msg.data = f'capture:{i}:{len(poses)}'
+                self._capture_pub.publish(msg)
+
+                Rg.append(R_g2b)
+                tg.append(t_g2b)
+                Rc.append(R_t2c)
+                tc.append(t_t2c)
+                meta.append(
+                    {
+                        'index': i,
+                        'joints': list(q),
+                        'ee_xyz': xyz,
+                        'ee_quat_xyzw': quat,
+                        'image': str(img_path),
+                        'corner_std_px': std,
+                    }
+                )
+                self.get_logger().info(
+                    f'[{i + 1}/{len(poses)}] 已采集 '
+                    f'(corner_std={std:.3f}px) -> {img_path.name}'
+                )
+        except KeyboardInterrupt:
+            self.get_logger().info('已中断')
+            return 0
+
+        if len(Rg) < self.min_poses:
+            self.get_logger().error(
+                f'只采集到 {len(Rg)} 次；求解至少需要 {self.min_poses} 次'
+            )
+            return 9
+
+        self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
+        R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
+        name = (
+            'T_cam_to_base (eye_to_hand)'
+            if eye_to_hand
+            else 'T_cam_to_gripper (eye_in_hand)'
+        )
+        yaml_txt = format_transform_yaml(R, t, name)
+        header = (
+            f'# 手眼标定结果\n'
+            f'# mode: {self.mode}\n'
+            f'# camera_source: {self.camera_source}\n'
+            f'# views: {len(Rg)}\n'
+            f'# utc: {datetime.now(timezone.utc).isoformat()}\n'
+            f'# board: {self.board.cols}x{self.board.rows} '
+            f'square_m={self.board.square_size}\n'
+        )
+        self.result_file.parent.mkdir(parents=True, exist_ok=True)
+        self.result_file.write_text(header + yaml_txt, encoding='utf-8')
+        (self.output_dir / 'captures.json').write_text(
+            json.dumps(meta, indent=2), encoding='utf-8'
+        )
+
+        self.get_logger().info('======== 手眼标定结果 ========')
+        for line in (header + yaml_txt).strip().splitlines():
+            self.get_logger().info(line)
+        self.get_logger().info(f'已保存 -> {self.result_file}')
+
+        if self.cleanup_images:
+            if self.images_dir.exists():
+                shutil.rmtree(self.images_dir, ignore_errors=True)
+            self.get_logger().info('标定图像已删除')
+        else:
+            self.get_logger().info(f'图像保留在 {self.images_dir}')
+        return 0
+
+    def _load_poses(self) -> List[List[float]]:
+        flat = list(self.get_parameter('waypoint_positions').value)
+        n_j = len(self.joint_names)
+        n_cfg = int(self.get_parameter('num_poses').value)
+        if n_cfg < 1:
+            return []
+        need = n_cfg * n_j
+        if len(flat) < need:
+            self.get_logger().warn(
+                f'waypoint_positions 长度 {len(flat)} < num_poses*{n_j}={need}；'
+                '用零补齐'
+            )
+            flat = flat + [0.0] * (need - len(flat))
+        poses: List[List[float]] = []
+        for i in range(n_cfg):
+            q = [float(v) for v in flat[i * n_j : (i + 1) * n_j]]
+            if is_pose_filled(q):
+                poses.append(q)
+            else:
+                self.get_logger().warn(f'位姿槽 {i} 为空（全零）—— 已跳过')
+        return poses
+
+    def _resolve_intrinsics(self) -> bool:
+        # 1) CameraInfo
+        deadline = time.monotonic() + 3.0
+        while rclpy.ok() and self._latest_info is None and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.1)
+
+        if self._latest_info is not None:
+            self._intrinsics = _camera_info_to_intrinsics(self._latest_info)
+            K = self._intrinsics.K
+            self.get_logger().info(
+                f'内参来自 CameraInfo（{self._info_topic}）：'
+                f'fx={K[0, 0]:.2f} fy={K[1, 1]:.2f} cx={K[0, 2]:.2f} cy={K[1, 2]:.2f}'
+            )
+            return True
+
+        # 2) RealSense EEPROM
+        if is_realsense_source(self.camera_source):
+            try:
+                intr = query_realsense_color_intrinsics(
+                    self.camera_source,
+                    serial=self.camera_serial,
+                    width=self.rs_width,
+                    height=self.rs_height,
+                )
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f'RealSense 内参查询失败：{exc}')
+            else:
+                self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
+                self.get_logger().info(
+                    f'内参来自 RealSense {intr.model.upper()} '
+                    f'serial={intr.serial} {intr.width}x{intr.height}：'
+                    f'fx={intr.fx:.2f} fy={intr.fy:.2f} '
+                    f'cx={intr.cx:.2f} cy={intr.cy:.2f}'
+                )
+                if (intr.width, intr.height) != (self.rs_width, self.rs_height):
+                    self.get_logger().warn(
+                        f'请求 {self.rs_width}x{self.rs_height}，'
+                        f'实际得到 {intr.width}x{intr.height}（最接近的 profile）'
+                    )
+                return True
+
+        # 3) Manual YAML
+        fx = float(self.get_parameter('camera_fx').value)
+        fy = float(self.get_parameter('camera_fy').value)
+        cx = float(self.get_parameter('camera_cx').value)
+        cy = float(self.get_parameter('camera_cy').value)
+        if fx <= 0 or fy <= 0:
+            self.get_logger().error(
+                '没有内参。请设置 camera_source:=d405|d435，'
+                '或提供 camera_info_topic，或填写 camera_fx/fy/cx/cy。'
+            )
+            return False
+        dist = np.array(
+            list(self.get_parameter('camera_dist').value), dtype=float
+        ).reshape(-1, 1)
+        K = np.array([[fx, 0.0, cx], [0.0, fy, cy], [0.0, 0.0, 1.0]], dtype=float)
+        self._intrinsics = CameraIntrinsics(K=K, dist=dist)
+        self.get_logger().info(
+            f'内参来自 YAML：fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
+        )
+        return True
+
+    def _log_progress(self, i: int, total: int, suffix: str = '') -> None:
+        """带进度条的日志。
+
+        自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照），
+        单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。
+        """
+        frac = (i + 1) / max(1, total)
+        width = 24
+        filled = int(round(frac * width))
+        bar = '█' * filled + '░' * (width - filled)
+        self.get_logger().info(
+            f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}%  {suffix}'
+        )
+
+    def _wait_settled(self) -> bool:
+        """等机械臂真的停下来，再进入拍照。
+
+        判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。
+        为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同，
+        刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动），
+        等太长则整轮标定被无谓拉长。直接看速度更准。
+
+        超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。
+        """
+        if self.settle_qd_max <= 0.0:
+            return True
+        deadline = time.monotonic() + self.settle_timeout
+        stable_since: Optional[float] = None
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.05)
+            js = self._latest_js
+            if js is None or not list(js.velocity):
+                continue
+            try:
+                vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float))))
+            except Exception:  # noqa: BLE001
+                continue
+            now = time.monotonic()
+            if vmax <= self.settle_qd_max:
+                if stable_since is None:
+                    stable_since = now
+                elif now - stable_since >= self.settle_stable_sec:
+                    return True
+            else:
+                stable_since = None  # 又动了，重新计时
+        self.get_logger().warn(
+            f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；'
+            '交给棋盘稳定性检查兜底'
+        )
+        return False
+
+    def _pin_hold(self) -> None:
+        """静止后再保持几拍，抑制到位后的微小回弹/漂移。
+
+        语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹，
+        而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。
+        """
+        total = self.pin_holds * self.pin_hold_sec
+        end = time.monotonic() + total
+        while rclpy.ok() and time.monotonic() < end:
+            rclpy.spin_once(self, timeout_sec=0.05)
+
+    def _wait_stable(
+        self,
+    ) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]:
+        self.stabilizer.reset()
+        deadline = time.monotonic() + self.stable_timeout
+        last_std = float('inf')
+        last_corners = None
+        last_pattern = None
+        last_bgr = None
+        last_log = 0.0
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if self._latest_image is None:
+                continue
+            try:
+                bgr = _image_to_bgr(self._latest_image)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f'图像转换失败：{exc}')
+                return False, last_std, None, None, None
+            detected = detect_chessboard(bgr, self.board)
+            corners = detected[0] if detected is not None else None
+            pattern = detected[1] if detected is not None else None
+            ok, std = self.stabilizer.push(corners)
+            last_std = std
+            if corners is not None:
+                last_corners = corners
+                last_pattern = pattern
+                last_bgr = bgr
+            now = time.monotonic()
+            if now - last_log > 1.0:
+                seen = '有' if corners is not None else '无'
+                self.get_logger().info(
+                    f'  棋盘={seen}  角点标准差='
+                    f'{std if std < 1e8 else float("nan"):.3f}px'
+                )
+                last_log = now
+            if ok:
+                return True, std, last_corners, last_pattern, last_bgr
+        return False, last_std, last_corners, last_pattern, last_bgr
+
+
+    def _verify_joints(self, target: List[float], tol: float = 0.08) -> bool:
+        """确认 ``joint_states`` 真的走到了目标位置。
+
+        用来识破"假的成功"：有些执行接口会立刻返回成功，但关节其实没动
+        （或只动了一半）。这里在 2 秒内轮询关节角，逐轴比对容差 ``tol``（弧度），
+        只有全部落在容差内才算真到位。
+        """
+        deadline = time.monotonic() + 2.0
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if self._latest_js is None:
+                continue
+            name_to_q = dict(zip(list(self._latest_js.name), list(self._latest_js.position)))
+            actual = []
+            for n in self.joint_names:
+                if n not in name_to_q:
+                    break
+                actual.append(float(name_to_q[n]))
+            if len(actual) != len(target):
+                continue
+            err = float(np.sqrt(np.mean([(a - b) ** 2 for a, b in zip(actual, target)])))
+            self.get_logger().info(
+                f'  关节相对目标的 RMSE：{err:.4f} rad（tol={tol}）'
+            )
+            return err <= tol
+        self.get_logger().error('校验运动时没有 joint_states')
+        return False
+
+    def _move_one(self, positions: List[float], duration: float) -> int:
+        goal = FollowJointTrajectory.Goal()
+        goal.joint_names = self.joint_names
+        goal.num_points = 1
+        goal.velocity_scale = self.velocity_scale
+        goal.positions = list(positions)
+        goal.time_from_start = [float(duration)]
+
+        send = self._client.send_goal_async(goal)
+        if not self._wait_future(send, 10.0, '发送目标'):
+            return 3
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('目标被拒绝')
+            return 4
+        res_fut = handle.get_result_async()
+        if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
+            return 5
+        result = res_fut.result().result
+        if not result.success:
+            self.get_logger().error(f'移动失败：{result.message}')
+            return 6
+        return 0
+
+    def _wait_future(self, fut, timeout: float, label: str) -> bool:
+        deadline = time.monotonic() + timeout
+        while rclpy.ok() and not fut.done():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if time.monotonic() > deadline:
+                self.get_logger().error(f'等待 {label} 超时')
+                return False
+        return fut.done()
+
+    def _read_ee_pose(self) -> Optional[PoseStamped]:
+        if not self._get_state.wait_for_service(timeout_sec=2.0):
+            return None
+        fut = self._get_state.call_async(GetArmState.Request())
+        if not self._wait_future(fut, 2.0, 'get_arm_state'):
+            return None
+        res = fut.result()
+        if res is None or not res.success:
+            return None
+        return res.ee_pose
+
+    def _save_png(self, bgr: np.ndarray, path: Path) -> None:
+        import cv2
+
+        if not cv2.imwrite(str(path), bgr):
+            raise RuntimeError(f'cv2.imwrite 失败：{path}')
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = HandeyeCalibrate()
+    code = 1
+    try:
+        code = node.run()
+    except KeyboardInterrupt:
+        code = 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    raise SystemExit(code)
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/handeye_lib.py

@@ -0,0 +1,331 @@
+#!/usr/bin/env python3
+"""手眼标定辅助工具：棋盘稳定判定 + AX=XB 求解。
+
+有 ``cv2.calibrateHandEye`` 时用它，否则用 Tsai-Lenz（NumPy 实现）。
+棋盘检测始终依赖 OpenCV。
+"""
+
+from __future__ import annotations
+
+import math
+from collections import deque
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Deque, List, Optional, Sequence, Tuple
+
+import numpy as np
+
+try:
+    import cv2
+except Exception:  # noqa: BLE001
+    cv2 = None  # type: ignore
+
+
+@dataclass
+class BoardConfig:
+    cols: int  # 内角点数（宽）
+    rows: int  # 内角点数（高）
+    square_size: float  # 米
+
+
+@dataclass
+class CameraIntrinsics:
+    K: np.ndarray
+    dist: np.ndarray
+
+
+def R_to_quat_xyzw(R: np.ndarray) -> Tuple[float, float, float, float]:
+    """3×3 旋转 → ``(x, y, z, w)`` 四元数（Shepperd 法，按最大分量选分支）。
+
+    为什么放在库里而不是各个 launch 里：这段换算**已经在两份 launch 里各抄了
+    一遍**，而且抄错过（对角分支的公式被写歪，症状是手眼 TF 差了 30°，
+    定位看起来"总能到附近"但永远差一点）。换算只有一份才可能被测试：
+    ``test_handeye_lib_roundtrip.py`` 拿真实标定文件验证
+    ``quat_xyzw_to_R(R_to_quat_xyzw(R)) == R``。
+    """
+    R = np.asarray(R, dtype=float).reshape(3, 3)
+    tr = float(R[0, 0] + R[1, 1] + R[2, 2])
+    if tr > 0.0:
+        s = math.sqrt(tr + 1.0) * 2.0
+        w = 0.25 * s
+        x = (R[2, 1] - R[1, 2]) / s
+        y = (R[0, 2] - R[2, 0]) / s
+        z = (R[1, 0] - R[0, 1]) / s
+    elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+        s = math.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2.0
+        w = (R[2, 1] - R[1, 2]) / s
+        x = 0.25 * s
+        y = (R[0, 1] + R[1, 0]) / s
+        z = (R[0, 2] + R[2, 0]) / s
+    elif R[1, 1] > R[2, 2]:
+        s = math.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2.0
+        w = (R[0, 2] - R[2, 0]) / s
+        x = (R[0, 1] + R[1, 0]) / s
+        y = 0.25 * s
+        z = (R[1, 2] + R[2, 1]) / s
+    else:
+        s = math.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2.0
+        w = (R[1, 0] - R[0, 1]) / s
+        x = (R[0, 2] + R[2, 0]) / s
+        y = (R[1, 2] + R[2, 1]) / s
+        z = 0.25 * s
+    n = math.sqrt(x * x + y * y + z * z + w * w)
+    if n <= 0.0:
+        raise ValueError("旋转矩阵无法转成四元数（矩阵为零？）")
+    return x / n, y / n, z / n, w / n
+
+
+def read_transform_yaml(path: str | Path) -> Tuple[np.ndarray, Tuple[float, float, float, float]]:
+    """读手眼标定产物 → ``(xyz_m, quat_xyzw)``。
+
+    文件格式见 :func:`format_transform_yaml`：``T_4x4`` 是 4 行 4 列的列表。
+    读取时**必须校验形状**：手眼文件是现场人手拷来拷去的，形状不对时
+    直接报错，比"取到一堆 nan 然后发了个全零 TF"好得多（后者会让定位
+    永远差一个常量，而且看起来像相机标定不准）。
+    """
+    import yaml  # 局部导入：本模块其它函数不需要 yaml（标定计算本身不依赖）
+
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f) or {}
+    T = data.get("T_4x4")
+    if T is None:
+        raise ValueError(f"{path}: 缺少 T_4x4 字段")
+    arr = np.asarray(T, dtype=float)
+    if arr.shape != (4, 4):
+        raise ValueError(f"{path}: T_4x4 应为 4x4，实际 {arr.shape}")
+    xyz = np.array([arr[0, 3], arr[1, 3], arr[2, 3]], dtype=float)
+    return xyz, R_to_quat_xyzw(arr[:3, :3])
+
+
+def quat_xyzw_to_R(q: Sequence[float]) -> np.ndarray:
+    x, y, z, w = [float(v) for v in q]
+    n = np.sqrt(x * x + y * y + z * z + w * w) + 1e-12
+    x, y, z, w = x / n, y / n, z / n, w / n
+    return np.array(
+        [
+            [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+            [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+            [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+        ],
+        dtype=float,
+    )
+
+
+def pose_to_Rt(
+    xyz: Sequence[float], quat_xyzw: Sequence[float]
+) -> Tuple[np.ndarray, np.ndarray]:
+    R = quat_xyzw_to_R(quat_xyzw)
+    t = np.array([[float(xyz[0])], [float(xyz[1])], [float(xyz[2])]], dtype=float)
+    return R, t
+
+
+def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = t.reshape(3)
+    return T
+
+
+def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+    Ri = R.T
+    ti = -Ri @ t.reshape(3, 1)
+    return Ri, ti
+
+
+def board_object_points(cfg: BoardConfig) -> np.ndarray:
+    obj = np.zeros((cfg.rows * cfg.cols, 3), np.float32)
+    xs, ys = np.meshgrid(np.arange(cfg.cols), np.arange(cfg.rows))
+    obj[:, 0] = xs.reshape(-1) * cfg.square_size
+    obj[:, 1] = ys.reshape(-1) * cfg.square_size
+    return obj
+
+
+def detect_chessboard(
+    image_bgr: np.ndarray, cfg: BoardConfig
+) -> Optional[Tuple[np.ndarray, Tuple[int, int]]]:
+    """返回 (角点, pattern_size) 或 None。pattern 为实际使用的 (cols, rows)。"""
+    if cv2 is None:
+        raise RuntimeError('棋盘检测需要 OpenCV（cv2）')
+    gray = cv2.cvtColor(image_bgr, cv2.COLOR_BGR2GRAY)
+    pattern = (int(cfg.cols), int(cfg.rows))
+    flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE
+    ok, corners = cv2.findChessboardCorners(gray, pattern, flags)
+    used = pattern
+    if not ok:
+        ok, corners = cv2.findChessboardCorners(
+            gray, (pattern[1], pattern[0]), flags
+        )
+        used = (pattern[1], pattern[0])
+    if not ok or corners is None:
+        return None
+    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 40, 1e-3)
+    corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
+    return np.asarray(corners, dtype=np.float32), used
+
+
+def solve_board_pose(
+    corners: np.ndarray,
+    cfg: BoardConfig,
+    cam: CameraIntrinsics,
+    pattern: Optional[Tuple[int, int]] = None,
+) -> Tuple[np.ndarray, np.ndarray]:
+    if cv2 is None:
+        raise RuntimeError('solvePnP 需要 OpenCV（cv2）')
+    if pattern is None:
+        pattern = (cfg.cols, cfg.rows)
+    board = BoardConfig(pattern[0], pattern[1], cfg.square_size)
+    obj = board_object_points(board)
+    ok, rvec, tvec = cv2.solvePnP(
+        obj, corners, cam.K, cam.dist, flags=cv2.SOLVEPNP_ITERATIVE
+    )
+    if not ok:
+        raise RuntimeError('solvePnP 失败')
+    R, _ = cv2.Rodrigues(rvec)
+    return R.astype(float), np.asarray(tvec, dtype=float).reshape(3, 1)
+
+
+class ChessboardStabilizer:
+    def __init__(self, window: int = 8, max_pixel_std: float = 0.45) -> None:
+        self.window = int(window)
+        self.max_pixel_std = float(max_pixel_std)
+        self._buf: Deque[np.ndarray] = deque(maxlen=self.window)
+
+    def reset(self) -> None:
+        self._buf.clear()
+
+    def push(self, corners: Optional[np.ndarray]) -> Tuple[bool, float]:
+        if corners is None:
+            self._buf.clear()
+            return False, float('inf')
+        self._buf.append(corners.reshape(-1, 2).astype(float))
+        if len(self._buf) < self.window:
+            return False, float('inf')
+        stack = np.stack(list(self._buf), axis=0)
+        std = float(np.mean(np.std(stack, axis=0)))
+        return std <= self.max_pixel_std, std
+
+
+def _skew(v: np.ndarray) -> np.ndarray:
+    x, y, z = v.reshape(3)
+    return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
+
+
+def _mat_log(R: np.ndarray) -> np.ndarray:
+    c = float(np.clip((np.trace(R) - 1.0) * 0.5, -1.0, 1.0))
+    theta = float(np.arccos(c))
+    if theta < 1e-8:
+        return np.zeros(3)
+    w = np.array(
+        [R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]], dtype=float
+    )
+    return w * (theta / (2.0 * np.sin(theta)))
+
+
+def _rodrigues(rvec: np.ndarray) -> np.ndarray:
+    r = rvec.reshape(3)
+    theta = float(np.linalg.norm(r))
+    if theta < 1e-12:
+        return np.eye(3)
+    k = r / theta
+    K = _skew(k)
+    return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)
+
+
+def calibrate_hand_eye_tsai(
+    R_gripper2base: List[np.ndarray],
+    t_gripper2base: List[np.ndarray],
+    R_target2cam: List[np.ndarray],
+    t_target2cam: List[np.ndarray],
+) -> Tuple[np.ndarray, np.ndarray]:
+    """Tsai-Lenz 方法。返回 R_cam2gripper、t_cam2gripper。"""
+    n = len(R_gripper2base)
+    if n < 3:
+        raise ValueError('至少需要 3 个位姿')
+
+    Hg = [Rt_to_T(R, t) for R, t in zip(R_gripper2base, t_gripper2base)]
+    Hc = [Rt_to_T(R, t) for R, t in zip(R_target2cam, t_target2cam)]
+
+    A_rows: List[np.ndarray] = []
+    b_rows: List[np.ndarray] = []
+    for i in range(n - 1):
+        A = np.linalg.inv(Hg[i + 1]) @ Hg[i]
+        B = Hc[i + 1] @ np.linalg.inv(Hc[i])
+        ra = _mat_log(A[:3, :3])
+        rb = _mat_log(B[:3, :3])
+        A_rows.append(_skew(ra + rb))
+        b_rows.append(rb - ra)
+    A_mat = np.vstack(A_rows)
+    b_vec = np.concatenate(b_rows)
+    p, _, _, _ = np.linalg.lstsq(A_mat, b_vec, rcond=None)
+    denom = np.sqrt(1.0 + float(p @ p))
+    P = 2.0 * p / denom
+    Rx = _rodrigues(P)
+
+    C_rows: List[np.ndarray] = []
+    d_rows: List[np.ndarray] = []
+    for i in range(n - 1):
+        A = np.linalg.inv(Hg[i + 1]) @ Hg[i]
+        B = Hc[i + 1] @ np.linalg.inv(Hc[i])
+        Ra, ta = A[:3, :3], A[:3, 3:4]
+        _Rb, tb = B[:3, :3], B[:3, 3:4]
+        C_rows.append(Ra - np.eye(3))
+        d_rows.append(Rx @ tb - ta)
+    C = np.vstack(C_rows)
+    d = np.vstack(d_rows)
+    tx, _, _, _ = np.linalg.lstsq(C, d, rcond=None)
+    return Rx.astype(float), tx.reshape(3, 1).astype(float)
+
+
+def calibrate_hand_eye(
+    R_gripper2base: List[np.ndarray],
+    t_gripper2base: List[np.ndarray],
+    R_target2cam: List[np.ndarray],
+    t_target2cam: List[np.ndarray],
+    *,
+    eye_to_hand: bool = False,
+) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    eye_in_hand  -> X = cam -> gripper（相机装在法兰上）
+    eye_to_hand  -> X = cam -> base（相机固定安装）
+    """
+    Rg = list(R_gripper2base)
+    tg = list(t_gripper2base)
+    if eye_to_hand:
+        Rg = []
+        tg = []
+        for R, t in zip(R_gripper2base, t_gripper2base):
+            Ri, ti = invert_Rt(R, t)
+            Rg.append(Ri)
+            tg.append(ti)
+
+    if cv2 is not None and hasattr(cv2, 'calibrateHandEye'):
+        try:
+            R, t = cv2.calibrateHandEye(
+                Rg, tg, R_target2cam, t_target2cam, method=cv2.CALIB_HAND_EYE_TSAI
+            )
+            return np.asarray(R, float), np.asarray(t, float).reshape(3, 1)
+        except Exception:
+            pass  # 退回 NumPy 版 Tsai
+    return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
+
+
+def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
+    T = Rt_to_T(R, t)
+    lines = [
+        f'# {name}',
+        f'translation_m: [{t[0, 0]:.8f}, {t[1, 0]:.8f}, {t[2, 0]:.8f}]',
+        'T_4x4:',
+    ]
+    for row in T:
+        lines.append('  - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
+    return '\n'.join(lines) + '\n'
+
+
+def is_pose_filled(joints: Sequence[float]) -> bool:
+    """若全为零 / NaN 占位则返回 False。"""
+    arr = np.asarray(joints, dtype=float)
+    if arr.size != 7:
+        return False
+    if np.any(np.isnan(arr)):
+        return False
+    return float(np.linalg.norm(arr)) > 1e-6

build_sim/arm_application/build/lib/arm_application/handeye_realsense.py

@@ -0,0 +1,251 @@
+#!/usr/bin/env python3
+"""通过 pyrealsense2 查询 RealSense（D405 / D435）的出厂彩色内参。"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import List, Tuple
+
+import numpy as np
+
+
+@dataclass
+class RealsenseIntrinsics:
+    model: str
+    serial: str
+    width: int
+    height: int
+    fx: float
+    fy: float
+    cx: float
+    cy: float
+    dist: List[float]  # OpenCV 顺序，长度 5
+
+    def as_K(self) -> np.ndarray:
+        return np.array(
+            [[self.fx, 0.0, self.cx], [0.0, self.fy, self.cy], [0.0, 0.0, 1.0]],
+            dtype=float,
+        )
+
+    def as_dist(self) -> np.ndarray:
+        d = list(self.dist) + [0.0] * 5
+        return np.array(d[:5], dtype=float).reshape(-1, 1)
+
+
+# camera_source 为 d405/d435 时的默认 ROS 话题命名空间。
+REALSENSE_TOPIC_DEFAULTS = {
+    'd405': {
+        'image_topic': '/d405/color/image_raw',
+        'camera_info_topic': '/d405/color/camera_info',
+    },
+    'd435': {
+        'image_topic': '/d435/color/image_raw',
+        'camera_info_topic': '/d435/color/camera_info',
+    },
+}
+
+
+def _norm_model(model: str) -> str:
+    m = model.strip().lower().replace(' ', '').replace('-', '')
+    if m.startswith('d405') or m in ('realsensed405', 'intelrealsensed405'):
+        return 'd405'
+    if m.startswith('d435') or m in ('realsensed435', 'intelrealsensed435'):
+        return 'd435'
+    return m
+
+
+def list_realsense_devices() -> List[Tuple[str, str]]:
+    """返回 [(name, serial), ...]。"""
+    try:
+        import pyrealsense2 as rs
+    except Exception as exc:  # noqa: BLE001
+        raise RuntimeError(
+            '未安装 pyrealsense2。'
+            '请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
+        ) from exc
+    out: List[Tuple[str, str]] = []
+    for d in rs.context().devices:
+        name = d.get_info(rs.camera_info.name)
+        serial = d.get_info(rs.camera_info.serial_number)
+        out.append((name, serial))
+    return out
+
+
+def query_realsense_color_intrinsics(
+    model: str,
+    *,
+    serial: str = '',
+    width: int = 1280,
+    height: int = 720,
+) -> RealsenseIntrinsics:
+    """
+    在给定分辨率下读取 D405/D435 设备上的彩色内参。
+
+    不会启动流管道（只读取 profile 元数据）。
+    """
+    try:
+        import pyrealsense2 as rs
+    except Exception as exc:  # noqa: BLE001
+        raise RuntimeError(
+            '未安装 pyrealsense2。'
+            '请在 ros_flexiv 环境中安装：pip/conda install pyrealsense2'
+        ) from exc
+
+    want = _norm_model(model)
+    if want not in ('d405', 'd435'):
+        raise ValueError(f'不支持的型号 {model!r}；请使用 d405 或 d435')
+
+    hint = want.upper()  # D405 / D435
+    serial = (serial or '').strip()
+    devices = list(rs.context().devices)
+    if not devices:
+        raise RuntimeError('未连接任何 RealSense 设备')
+
+    chosen = None
+    for d in devices:
+        name = d.get_info(rs.camera_info.name)
+        sn = d.get_info(rs.camera_info.serial_number)
+        if serial and sn != serial:
+            continue
+        name_u = name.upper().replace(' ', '')
+        if hint in name_u or hint in name.upper():
+            chosen = d
+            break
+
+    if chosen is None:
+        avail = ', '.join(
+            f'{d.get_info(rs.camera_info.name)}'
+            f'({d.get_info(rs.camera_info.serial_number)})'
+            for d in devices
+        )
+        raise RuntimeError(
+            f'没有匹配 model={want!r} serial={serial!r} 的 RealSense。'
+            f'已连接的设备：{avail or "无"}'
+        )
+
+    name = chosen.get_info(rs.camera_info.name)
+    sn = chosen.get_info(rs.camera_info.serial_number)
+
+    best = None  # (评分, width, height, fx, fy, cx, cy, 畸变系数)
+    for s in chosen.sensors:
+        for p in s.profiles:
+            if p.stream_type() != rs.stream.color:
+                continue
+            vp = p.as_video_stream_profile()
+            try:
+                intr = vp.get_intrinsics()
+            except Exception:  # noqa: BLE001
+                continue
+            w, h = int(intr.width), int(intr.height)
+            if w == int(width) and h == int(height):
+                score = 0
+            else:
+                score = abs(w * h - int(width) * int(height)) + 1
+            cand = (
+                score,
+                w,
+                h,
+                float(intr.fx),
+                float(intr.fy),
+                float(intr.ppx),
+                float(intr.ppy),
+                [float(c) for c in list(intr.coeffs)[:5]],
+            )
+            if best is None or cand[0] < best[0]:
+                best = cand
+
+    if best is None:
+        raise RuntimeError(f'{name} ({sn}) 上没有彩色流 profile')
+
+    _, w, h, fx, fy, cx, cy, coeffs = best
+    while len(coeffs) < 5:
+        coeffs.append(0.0)
+    return RealsenseIntrinsics(
+        model=want,
+        serial=sn,
+        width=w,
+        height=h,
+        fx=fx,
+        fy=fy,
+        cx=cx,
+        cy=cy,
+        dist=coeffs[:5],
+    )
+
+
+def is_realsense_source(name: str) -> bool:
+    """对 d405 / d435 及带命名空间的别名（d405_left、d405_right）返回 True。"""
+    return _norm_model(name or "") in ("d405", "d435")
+
+
+def resolve_realsense_topics(
+    model: str,
+    *,
+    camera_namespace: str = '',
+    image_topic: str = '',
+    camera_info_topic: str = '',
+) -> Tuple[str, str]:
+    """为空时，为 d405/d435 填充默认的 image/camera_info 话题。
+
+    ``model`` 可以是 ``d405_left`` / ``d405_right`` 这类话题命名空间。
+    """
+    ns = (camera_namespace or model or 'd405').strip().strip('/')
+    img = (image_topic or '').strip()
+    info = (camera_info_topic or '').strip()
+    if not img and ns:
+        img = f'/{ns}/color/image_raw'
+    if not info and ns:
+        info = f'/{ns}/color/camera_info'
+    return img, info
+
+
+def apply_realsense_exposure(
+    device,
+    *,
+    auto_exposure: bool = True,
+    ae_limit_us: float = 3000.0,
+    emitter_enabled: bool = False,
+    laser_power: float = 0.0,
+    exposure_us: float | None = None,
+    gain: float | None = None,
+    log=None,
+) -> list[str]:
+    """限制自动曝光 / 关闭投影器。腕部 D405 过曝通常是自动曝光过长 + 发射器导致。"""
+    import pyrealsense2 as rs
+
+    notes: list[str] = []
+
+    def _set(sensor, opt, val) -> bool:
+        try:
+            if sensor.supports(opt):
+                sensor.set_option(opt, float(val))
+                return True
+        except Exception:
+            return False
+        return False
+
+    for sensor in device.query_sensors():
+        if _set(sensor, rs.option.emitter_enabled, 1.0 if emitter_enabled else 0.0):
+            notes.append(f"emitter={int(bool(emitter_enabled))}")
+        laser_opt = getattr(rs.option, "laser_power", None)
+        if laser_opt is not None and not emitter_enabled:
+            _set(sensor, laser_opt, float(laser_power))
+        # 必须在写曝光*之前*关闭自动曝光；D405 固件在自动曝光开启时会忽略曝光设置。
+        if _set(sensor, rs.option.enable_auto_exposure, 1.0 if auto_exposure else 0.0):
+            notes.append(f"ae={int(bool(auto_exposure))}")
+        pri = getattr(rs.option, "auto_exposure_priority", None)
+        # priority 0 = 保帧压暗；无上限时不要设，让自动曝光自己变亮
+        if pri is not None and ae_limit_us and auto_exposure:
+            _set(sensor, pri, 0.0)
+        limit = getattr(rs.option, "auto_exposure_limit", None)
+        if auto_exposure and limit is not None and ae_limit_us and ae_limit_us > 0:
+            if _set(sensor, limit, float(ae_limit_us)):
+                notes.append(f"ae_limit={int(ae_limit_us)}us")
+        if not auto_exposure:
+            if exposure_us is not None and _set(sensor, rs.option.exposure, float(exposure_us)):
+                notes.append(f"exposure={int(exposure_us)}us")
+            if gain is not None and _set(sensor, rs.option.gain, float(gain)):
+                notes.append(f"gain={gain}")
+    if log and notes:
+        log("RealSense 曝光设置：" + ", ".join(dict.fromkeys(notes)))
+    return notes

build_sim/arm_application/build/lib/arm_application/handeye_teach.py

@@ -0,0 +1,645 @@
+#!/usr/bin/env python3
+"""手眼示教：实时预览 + Free Drive + 按 Enter 拍照/记录关节。
+
+两种后端
+------------
+ros（默认，整套栈正在运行）：
+  - 关节来自 ``/get_arm_state``
+  - 拖拽：示教器 Free Drive + 按住使能键
+  需保持：bash ~/ros2/scripts/run_real.sh
+
+rdk（软件 Free Drive）：
+  - 先停掉 run_real.sh（RDK 独占）
+  - 按 'd' 通过 RDK 启用 FloatingJoint 拖拽
+  - 关节来自 RDK 的 robot.states
+
+  source ~/ros2/scripts/env.sh
+  ros2 run arm_application handeye_teach --ros-args -p backend:=rdk
+
+界面按键（预览窗口）
+  Enter / Space  拍照 + 记录关节
+  d             启用 Free Drive（rdk）/ 显示示教器提示（ros）
+  s             停止 Free Drive（rdk）/ 保持
+  q / Esc       结束
+"""
+
+from __future__ import annotations
+
+import os
+import threading
+import time
+from datetime import datetime, timezone
+from pathlib import Path
+from typing import List, Optional
+
+import numpy as np
+import rclpy
+from rcl_interfaces.msg import ParameterDescriptor
+from rclpy.node import Node
+
+from arm_application.ros_node import TolerantNode, param_as_str
+
+HELP = """
+================================================
+ 手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
+================================================
+ 窗口按键：
+   Enter / Space  = 拍照 + 保存关节
+   d              = 开启 Free Drive
+   s              = 关闭 Free Drive / 保持
+   q / Esc        = 结束
+ 提示：让棋盘清晰，并占到画面的约 30-70%
+ 建议采 8-12 个角度有变化的位姿
+================================================
+"""
+
+
+def _setup_matplotlib_font():
+    """避免中文乱码：优先选用存在的中日韩（CJK）字体。"""
+    import matplotlib
+
+    matplotlib.use('QtAgg')
+    from matplotlib import font_manager, rcParams
+
+    candidates = [
+        'Noto Sans CJK SC',
+        'Noto Sans CJK JP',
+        'Noto Serif CJK SC',
+        'AR PL UMing CN',
+        'AR PL UKai CN',
+        'WenQuanYi Micro Hei',
+        'Source Han Sans CN',
+        'DejaVu Sans',
+    ]
+    available = {f.name for f in font_manager.fontManager.ttflist}
+    chosen = None
+    for name in candidates:
+        if name in available:
+            chosen = name
+            break
+    if chosen is None:
+        # 按文件路径再试一次
+        for path in (
+            '/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc',
+            '/usr/share/fonts/opentype/noto/NotoSansCJK-Medium.ttc',
+            '/usr/share/fonts/opentype/noto/NotoSansCJK-DemiLight.ttc',
+            '/usr/share/fonts/truetype/arphic/uming.ttc',
+        ):
+            if os.path.isfile(path):
+                font_manager.fontManager.addfont(path)
+                prop = font_manager.FontProperties(fname=path)
+                chosen = prop.get_name()
+                break
+    if chosen:
+        rcParams['font.family'] = chosen
+        rcParams['axes.unicode_minus'] = False
+    return chosen
+
+
+class RdkBridge:
+    """通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
+
+    def __init__(self, robot_sn: str, logger=None) -> None:
+        import json
+        import subprocess
+
+        self._json = json
+        self._logger = logger
+        self.robot_sn = robot_sn
+        self._last_q: Optional[List[float]] = None
+        self._io = threading.Lock()
+        py = os.environ.get('RDK_PYTHON', '/home/cc/rdk_py310/bin/python')
+        bridge = Path.home() / 'ros2' / 'scripts' / 'rdk_drag_bridge.py'
+        if not bridge.is_file():
+            bridge = Path('/home/cc/ros2/scripts/rdk_drag_bridge.py')
+        if not bridge.is_file():
+            raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
+
+        # 只保留 Flexiv 的库 —— 不要继承 conda 的 LD_LIBRARY_PATH（会让 RDK 卡死）。
+        flexiv_lib = os.path.expanduser('~/flexiv_install/lib')
+        env = {
+            'HOME': os.environ.get('HOME', ''),
+            'PATH': f'{Path(py).parent}:/usr/bin:/bin',
+            'LD_LIBRARY_PATH': flexiv_lib,
+            'LANG': 'C.UTF-8',
+        }
+        # 不要继承 conda 的 PYTHONPATH。
+        # 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
+        # （stdout 永远不返回）。改成写入日志文件。
+        log_path = Path('/tmp/rdk_drag_bridge.log')
+        self._err_file = open(log_path, 'w', encoding='utf-8')
+        self._proc = subprocess.Popen(
+            [py, str(bridge)],
+            stdin=subprocess.PIPE,
+            stdout=subprocess.PIPE,
+            stderr=self._err_file,
+            text=True,
+            bufsize=1,
+            env=env,
+        )
+        if logger:
+            logger.info(f'RDK 桥接已启动；日志 → {log_path}')
+        pong = self._ask({'cmd': 'ping'}, timeout=8.0)
+        if not pong.get('ok'):
+            raise RuntimeError(
+                f'RDK 桥接 ping 失败：{pong}。参见 {log_path}'
+            )
+        if logger:
+            logger.info('RDK 桥接 ping 正常，正在连接机器人...')
+        conn = self._ask(
+            {
+                'cmd': 'connect',
+                'sn': robot_sn,
+                'iface': os.environ.get('SITE_NET_IFACE', ''),
+            },
+            timeout=45.0,
+        )
+        if not conn.get('ok'):
+            raise RuntimeError(
+                f'RDK 连接失败：{conn.get("error")}。'
+                f'请先停掉 run_real.sh；切 AUTO Remote 并清除故障。参见 {log_path}'
+            )
+        if logger:
+            logger.info(f'RDK 桥接就绪（py={pong.get("py")}）sn={robot_sn}')
+        q0 = conn.get('q') or []
+        if len(q0) >= 7:
+            self._last_q = [float(v) for v in q0[:7]]
+
+    def _ask(self, msg: dict, timeout: float = 10.0) -> dict:
+        import select
+
+        # 同一时刻只允许一条在途命令：相机/GUI 绝不能共用这个管道。
+        if not self._io.acquire(timeout=max(timeout, 0.2)):
+            return {'ok': False, 'error': '桥接忙'}
+        try:
+            assert self._proc.stdin and self._proc.stdout
+            self._proc.stdin.write(self._json.dumps(msg) + '\n')
+            self._proc.stdin.flush()
+            end = time.monotonic() + timeout
+            while time.monotonic() < end:
+                if self._proc.poll() is not None:
+                    tip = '/tmp/rdk_drag_bridge.log'
+                    return {
+                        'ok': False,
+                        'error': f'桥接已退出 rc={self._proc.returncode}；参见 {tip}',
+                    }
+                ready, _, _ = select.select([self._proc.stdout], [], [], 0.05)
+                if not ready:
+                    continue
+                line = self._proc.stdout.readline()
+                if not line:
+                    continue
+                line = line.strip()
+                if not line.startswith('{'):
+                    continue
+                try:
+                    return self._json.loads(line)
+                except Exception as exc:  # noqa: BLE001
+                    return {'ok': False, 'error': f'应答异常：{exc} {line!r}'}
+            return {'ok': False, 'error': '桥接超时'}
+        finally:
+            self._io.release()
+
+    def drag_on(self, primitive: str = 'FloatingJoint', mode: str = 'soft') -> dict:
+        return self._ask(
+            {'cmd': 'drag_on', 'primitive': primitive, 'mode': mode},
+            timeout=20.0,
+        )
+
+    def drag_off(self) -> dict:
+        return self._ask({'cmd': 'drag_off'}, timeout=6.0)
+
+    def joints(self) -> Optional[List[float]]:
+        res = self._ask({'cmd': 'joints'}, timeout=2.0)
+        if res.get('ok'):
+            q = res.get('q') or []
+            if len(q) >= 7:
+                self._last_q = [float(v) for v in q[:7]]
+                return self._last_q
+        return self._last_q
+
+    def close(self) -> None:
+        try:
+            self._ask({'cmd': 'quit'}, timeout=3.0)
+        except Exception:
+            pass
+        try:
+            self._proc.terminate()
+        except Exception:
+            pass
+        try:
+            if getattr(self, '_err_file', None) is not None:
+                self._err_file.close()
+        except Exception:
+            pass
+
+
+class HandeyeTeach(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('handeye_teach')
+        self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
+        self.declare_parameter('width', 1280)
+        self.declare_parameter('height', 720)
+        self.declare_parameter('fps', 15)
+        self.declare_parameter('backend', 'ros')  # ros | rdk
+        self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
+        self.declare_parameter('drag_primitive', 'FloatingJoint')  # or FloatingCartesian
+        self._serial = param_as_str(self, 'camera_serial', '')
+        self.declare_parameter('auto_exposure', True)
+        self.declare_parameter('ae_limit_us', 1800.0)
+        self.declare_parameter('emitter_enabled', False)
+        self.declare_parameter('exposure_us', 0.0)
+        self.declare_parameter('gain', 0.0)
+        # 目标采集点数。0 = 不限（按 q 结束）。
+        # 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
+        # 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
+        self.declare_parameter('target_points', 0)
+
+        self.backend = str(self.get_parameter('backend').value).strip().lower()
+        self.robot_sn = str(self.get_parameter('robot_sn').value)
+        self.drag_primitive = str(self.get_parameter('drag_primitive').value).strip()
+
+        self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
+        self.images_dir = self.output_dir / 'images'
+        self.poses_file = self.output_dir / 'taught_poses.txt'
+        self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
+        self.images_dir.mkdir(parents=True, exist_ok=True)
+
+        self._captures: List[dict] = []
+        self._latest_bgr: Optional[np.ndarray] = None
+        self._frame_lock = threading.Lock()
+        self._quit = False
+        self._busy = False
+        self._cmd_pending = False
+        self._drag_on = False
+        self._robot = None
+        self._rdk = None
+        self._get_state = None
+        self._cam_thread: Optional[threading.Thread] = None
+        self._pipe = None
+        self._fps = 15
+
+        if self.backend == 'rdk':
+            self._init_rdk()
+        else:
+            self._init_ros_state()
+
+        self._start_camera()
+        font = _setup_matplotlib_font()
+        self._start_gui()
+        print(HELP)
+        if self.backend == 'rdk':
+            print('[backend=rdk] 按 d = 软拖拽（在 AUTO Remote 下可用，无需 Floating 许可）')
+            print('              run_real.sh 必须已停止。')
+            print('              拖着就能按 Enter 拍照，不必先按 s')
+        else:
+            print('[backend=ros] 用示教器 Free Drive + 按住使能键来拖拽。')
+            print('             或停掉 run_real.sh 后改用：-p backend:=rdk')
+        if font:
+            self.get_logger().info(f'界面字体：{font}')
+        self.create_timer(1.0 / max(1, self._fps), self._tick)
+
+    def _init_ros_state(self) -> None:
+        from arm_msgs.srv import GetArmState
+
+        self._get_state = self.create_client(GetArmState, 'get_arm_state')
+        if not self._get_state.wait_for_service(timeout_sec=10.0):
+            raise RuntimeError(
+                'get_arm_state 不可用。请启动 run_real.sh，'
+                '或改用：-p backend:=rdk（需先停掉整套栈）'
+            )
+
+    def _init_rdk(self) -> None:
+        self.get_logger().info(f'正在把 RDK 桥接连到 {self.robot_sn} ...')
+        self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
+        self._robot = True  # 标记：RDK 后端已就绪
+        self.get_logger().info('RDK 已连接。按 d 启用 Free Drive。')
+
+    def _start_camera(self) -> None:
+        import pyrealsense2 as rs
+
+        w = int(self.get_parameter('width').value)
+        h = int(self.get_parameter('height').value)
+        fps = int(self.get_parameter('fps').value)
+        pipe = rs.pipeline()
+        profile = None
+        last_err = None
+        for ww, hh, ff in [
+            (w, h, fps), (w, h, 15), (w, h, 10),
+            (640, 480, 30), (640, 480, 15),
+        ]:
+            try:
+                cfg = rs.config()
+                if self._serial:
+                    cfg.enable_device(self._serial)
+                cfg.enable_stream(rs.stream.color, ww, hh, rs.format.bgr8, ff)
+                profile = pipe.start(cfg)
+                self._fps = ff
+                self.get_logger().info(f'相机 {ww}x{hh}@{ff}Hz')
+                break
+            except Exception as exc:  # noqa: BLE001
+                last_err = exc
+                try:
+                    pipe.stop()
+                except Exception:  # noqa: BLE001
+                    pass
+                pipe = rs.pipeline()
+        if profile is None:
+            raise RuntimeError(
+                f'无法打开 D405 —— 请先停掉 realsense_color_pub。（{last_err}）'
+            )
+        self._pipe = pipe
+        try:
+            from arm_application.handeye_realsense import apply_realsense_exposure
+
+            apply_realsense_exposure(
+                profile.get_device(),
+                auto_exposure=bool(self.get_parameter('auto_exposure').value),
+                ae_limit_us=float(self.get_parameter('ae_limit_us').value),
+                emitter_enabled=bool(self.get_parameter('emitter_enabled').value),
+                exposure_us=(
+                    float(self.get_parameter('exposure_us').value) or None
+                ),
+                gain=(float(self.get_parameter('gain').value) or None),
+                log=self.get_logger().info,
+            )
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'曝光设置应用失败：{exc}')
+        self._cam_thread = threading.Thread(target=self._cam_loop, name='d405', daemon=True)
+        self._cam_thread.start()
+
+    def _cam_loop(self) -> None:
+        while not self._quit:
+            try:
+                frames = self._pipe.wait_for_frames(timeout_ms=400)
+                color = frames.get_color_frame()
+            except Exception:  # noqa: BLE001
+                time.sleep(0.02)
+                continue
+            if not color:
+                continue
+            bgr = np.asanyarray(color.get_data()).copy()
+            with self._frame_lock:
+                self._latest_bgr = bgr
+
+    def _start_gui(self) -> None:
+        import matplotlib.pyplot as plt
+
+        self._plt = plt
+        self._fig, self._ax = plt.subplots(1, 1, figsize=(11, 7))
+        self._fig.canvas.manager.set_window_title(
+            'handeye_teach | Enter=拍照  d=拖拽  s=停止  q=退出'
+        )
+        self._im = None
+        self._status = self._ax.text(
+            0.02,
+            0.98,
+            self._status_text(),
+            transform=self._ax.transAxes,
+            color='lime',
+            fontsize=13,
+            va='top',
+            fontweight='bold',
+            bbox=dict(boxstyle='round', facecolor='black', alpha=0.6),
+        )
+        self._ax.axis('off')
+        self._fig.canvas.mpl_connect('key_press_event', self._on_key)
+        plt.ion()
+        plt.show(block=False)
+
+    def _status_text(self) -> str:
+        drag = '拖拽开启' if self._drag_on else '拖拽关闭'
+        if self.backend == 'rdk':
+            tip = 'd=软拖拽  s=保持'
+        else:
+            tip = '示教器 FreeDrive+使能'
+        return (
+            f'已采集:{len(self._captures)}  [{drag}]  '
+            f'{tip}  |  Enter=拍照  q=退出'
+        )
+
+    def _on_key(self, event) -> None:
+        key = (event.key or '').lower()
+        if key in ('q', 'escape'):
+            self._quit = True
+            self.get_logger().info('收到退出请求')
+            return
+        if self._cmd_pending:
+            return
+        if key in ('enter', ' ', 'space', 'd', 's'):
+            self._cmd_pending = True
+            threading.Thread(target=self._run_key, args=(key,), daemon=True).start()
+
+    def _run_key(self, key: str) -> None:
+        try:
+            if key in ('enter', ' ', 'space'):
+                self._capture_once()
+            elif key == 'd':
+                self._enable_drag()
+            elif key == 's':
+                self._disable_drag()
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().error(f'按键 {key}：{exc}')
+        finally:
+            self._cmd_pending = False
+
+    def _enable_drag(self) -> None:
+        if self.backend != 'rdk':
+            self._drag_on = True
+            self.get_logger().warn(
+                'ROS 后端：请用示教器的 Free Drive + 按住使能键。'
+                '想用软件拖拽：先停掉 run_real.sh，然后执行 '
+                'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk'
+            )
+            self._status.set_text(self._status_text())
+            return
+        self.get_logger().info('正在启动软拖拽（预览保持实时刷新）...')
+        # soft = AUTO Remote 模式下的关节阻抗拖拽（不需要 Floating 许可）
+        res = self._rdk.drag_on(self.drag_primitive, mode='soft')
+        if res.get('ok'):
+            self._drag_on = True
+            self.get_logger().info(
+                f'拖拽已开启 mode={res.get("mode")} —— 推着机械臂走，Enter=拍照（s=保持，可选）'
+            )
+            if res.get('hint'):
+                self.get_logger().info(str(res.get('hint')))
+        else:
+            self._drag_on = False
+            self.get_logger().error(f'拖拽失败：{res.get("error")}')
+        # 状态文字由 GUI 线程里的 _tick 刷新
+
+    def _disable_drag(self) -> None:
+        if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
+            res = self._rdk.drag_off()
+            if not res.get('ok'):
+                self.get_logger().warn(f'停止拖拽：{res.get("error")}')
+        self._drag_on = False
+        self.get_logger().info('Free Drive 已关闭 / 保持')
+
+    def _tick(self) -> None:
+        if self._quit:
+            return
+        with self._frame_lock:
+            bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
+        if bgr is None:
+            try:
+                self._fig.canvas.flush_events()
+            except Exception:  # noqa: BLE001
+                pass
+            return
+        rgb = bgr[:, :, ::-1]
+        if self._im is None:
+            self._im = self._ax.imshow(rgb)
+        else:
+            self._im.set_data(rgb)
+        try:
+            self._status.set_text(self._status_text())
+            self._fig.canvas.draw_idle()
+            self._fig.canvas.flush_events()
+        except Exception:  # noqa: BLE001
+            pass
+
+    def _read_joints(self) -> Optional[List[float]]:
+        if self.backend == 'rdk' and getattr(self, '_rdk', None) is not None:
+            q = self._rdk.joints()
+            if q is None:
+                self.get_logger().error('读取 RDK 关节失败')
+            return q
+        from arm_msgs.srv import GetArmState
+
+        fut = self._get_state.call_async(GetArmState.Request())
+        end = time.monotonic() + 2.0
+        while rclpy.ok() and not fut.done() and time.monotonic() < end:
+            rclpy.spin_once(self, timeout_sec=0.02)
+        res = fut.result() if fut.done() else None
+        if res is None or not res.success:
+            return None
+        q = list(res.joint_positions)
+        return [float(v) for v in q[:7]] if len(q) >= 7 else None
+
+    def _capture_once(self) -> None:
+        if self._busy:
+            return
+        self._busy = True
+        try:
+            with self._frame_lock:
+                bgr = None if self._latest_bgr is None else self._latest_bgr.copy()
+            if bgr is None:
+                self.get_logger().warn('还没有相机帧')
+                return
+            q = self._read_joints()
+            if q is None:
+                self.get_logger().error('读取关节失败')
+                return
+
+            idx = len(self._captures)
+            stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
+            img_path = self.images_dir / f'teach_{idx:02d}_{stamp}.png'
+            try:
+                import cv2
+
+                cv2.imwrite(str(img_path), bgr)
+            except Exception:  # noqa: BLE001
+                from PIL import Image as PILImage
+
+                PILImage.fromarray(bgr[:, :, ::-1]).save(str(img_path))
+
+            line = ', '.join(f'{v:.6f}' for v in q) + ','
+            self._captures.append(
+                {'index': idx, 'joints': q, 'image': str(img_path), 'line': line}
+            )
+            with self.poses_file.open('a', encoding='utf-8') as f:
+                f.write(f'# pose {idx}  {stamp}\n')
+                f.write(line + '\n')
+                f.write(f'# image {img_path}\n')
+            print(f'\n# pose {idx}  {img_path.name}')
+            print(line)
+            n = idx + 1
+            try:
+                target = int(self.get_parameter('target_points').value)
+            except Exception:  # noqa: BLE001
+                target = 0
+            if target > 0:
+                if n < target:
+                    self.get_logger().info(
+                        f'[{n}/{target}] 已保存照片+关节  —— 还差 {target - n} 个点'
+                    )
+                elif n == target:
+                    self.get_logger().info(
+                        f'[{n}/{target}] 已采够 {target} 个点。'
+                        ' 建议按 q 结束，然后跑 merge + calibrate；'
+                        ' 想多采几个点也可以继续按 Enter。'
+                    )
+                else:
+                    self.get_logger().info(
+                        f'[{n}/{target}] 已保存照片+关节  —— 已超出目标 '
+                        f'{n - target} 个点（会用全部点求解）'
+                    )
+            else:
+                self.get_logger().info(f'[{n}] 已保存照片+关节')
+            self._write_yaml_snippet()
+        finally:
+            self._busy = False
+
+    def _write_yaml_snippet(self) -> None:
+        lines = [
+            '# 粘贴到 handeye_calibrate.yaml -> waypoint_positions',
+            f'# num_poses: {len(self._captures)}',
+            'waypoint_positions: [',
+        ]
+        for c in self._captures:
+            lines.append(f'  # pose {c["index"]}')
+            lines.append(f'  {c["line"]}')
+        lines.append(']')
+        self.yaml_file.write_text('\n'.join(lines) + '\n', encoding='utf-8')
+
+    def finish(self) -> None:
+        self._quit = True
+        if self._cam_thread is not None:
+            self._cam_thread.join(timeout=1.5)
+        if self._drag_on:
+            self._disable_drag()
+        if getattr(self, '_rdk', None) is not None:
+            try:
+                self._rdk.close()
+            except Exception:
+                pass
+        n = len(self._captures)
+        print('\n========== 示教完成 ==========')
+        print(f'位姿: {n}')
+        print(f'关节: {self.poses_file}')
+        print(f'yaml:   {self.yaml_file}')
+        print(f'图像: {self.images_dir}')
+        if n >= 1:
+            print('\n--- 粘贴到 handeye_calibrate.yaml ---\n')
+            print(self.yaml_file.read_text(encoding='utf-8'))
+            print('同时设置 num_poses:', n)
+        if n < 5:
+            self.get_logger().warn(f'只有 {n} 个位姿；至少需要 5 个（建议 8-12 个）。')
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = HandeyeTeach()
+        while rclpy.ok() and not node._quit:
+            rclpy.spin_once(node, timeout_sec=0.05)
+    except SystemExit:
+        pass
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.finish()
+            try:
+                node._pipe.stop()
+            except Exception:  # noqa: BLE001
+                pass
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/handeye_waypoints.py

@@ -0,0 +1,525 @@
+#!/usr/bin/env python3
+"""手眼标定路点执行器。
+
+逐个移动到每个关节路点，等到位后停留一段时间，
+让相机（或操作者）采集一张标定图像。
+
+capture_mode:
+  auto      — 停留 dwell_sec 后进入下一个位姿
+  keypress  — 在终端按 Enter 继续
+  preview   — OpenCV 实时相机 + Enter/Space 确认（示教时推荐）
+
+处于 ``preview`` 模式时，可在预览窗口打开期间用示教器的 **Free Drive**
+拖拽微调（ros2_control 保持 AUTO Remote；拖拽经由示教器，而非 RDK）。
+"""
+
+from __future__ import annotations
+
+import csv
+import array
+import sys
+import threading
+import time
+from datetime import datetime, timezone
+from pathlib import Path
+from typing import List, Optional
+
+import numpy as np
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+from arm_application.handeye_realsense import (
+    is_realsense_source,
+    resolve_realsense_topics,
+)
+from arm_application.site_io import joint_prefix_default
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import GetArmState
+
+from arm_application.ros_node import TolerantNode
+
+try:
+    import cv2
+except Exception:  # noqa: BLE001
+    cv2 = None  # type: ignore
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+
+DEFAULT_TIMES = [4.0] * 12
+DEFAULT_POSITIONS = [
+    0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
+    0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
+    -0.35, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
+    0.00, -0.90, 0.00, 1.70, 0.00, 0.85, 0.00,
+    0.00, -0.50, 0.00, 1.40, 0.00, 0.55, 0.00,
+    0.20, -0.70, 0.15, 1.57, 0.40, 0.70, 0.30,
+    -0.20, -0.70, -0.15, 1.57, -0.40, 0.70, -0.30,
+    0.25, -0.80, 0.10, 1.65, 0.25, 0.90, -0.40,
+    -0.25, -0.80, -0.10, 1.65, -0.25, 0.90, 0.40,
+    0.15, -0.60, 0.20, 1.50, -0.35, 0.60, 0.50,
+    -0.15, -0.60, -0.20, 1.50, 0.35, 0.60, -0.50,
+    0.00, -0.70, 0.00, 1.57, 0.00, 0.70, 0.00,
+]
+
+PREVIEW_HELP_GUI = """
+=== 位姿预览 / 拖拽调整（OpenCV 窗口）===
+1. 示教器：Free Drive + 按住使能键拖拽微调
+2. 预览窗口：确认棋盘在画面内且清晰
+3. 键盘：Enter / Space = 下一个位姿；q / Esc = 退出
+"""
+
+PREVIEW_HELP_HEADLESS = """
+=== 位姿预览 / 拖拽调整（无 OpenCV 窗口）===
+1. 另开终端查看画面：
+   ros2 run rqt_image_view rqt_image_view /handeye/preview_image
+2. 示教器：Free Drive + 按住使能键拖拽微调
+3. 本终端：Enter = 确认下一个位姿；输入 q 回车 = 退出
+"""
+
+
+def _cv_gui_available() -> bool:
+    if cv2 is None:
+        return False
+    try:
+        cv2.namedWindow('_handeye_gui_probe', cv2.WINDOW_NORMAL)
+        cv2.destroyWindow('_handeye_gui_probe')
+        return True
+    except Exception:  # noqa: BLE001
+        return False
+
+
+def _image_to_bgr(msg: Image) -> np.ndarray:
+    h, w = int(msg.height), int(msg.width)
+    enc = (msg.encoding or '').lower()
+    data = np.frombuffer(msg.data, dtype=np.uint8)
+    if enc in ('bgr8', 'rgb8'):
+        img = data.reshape(h, w, 3)
+        return img[:, :, ::-1].copy() if enc == 'rgb8' else img
+    if enc in ('mono8', '8uc1'):
+        gray = data.reshape(h, w)
+        return np.stack([gray, gray, gray], axis=-1)
+    if enc in ('bgra8', 'rgba8'):
+        img = data.reshape(h, w, 4)
+        bgr = img[:, :, :3]
+        return bgr[:, :, ::-1].copy() if enc == 'rgba8' else bgr
+    raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
+
+
+class HandeyeWaypoints(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('handeye_waypoints')
+        self.declare_parameter('joint_prefix', joint_prefix_default())
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('velocity_scale', 0.12)
+        self.declare_parameter('action_name', 'follow_joint_trajectory')
+        self.declare_parameter('waypoint_times', DEFAULT_TIMES)
+        self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
+        self.declare_parameter('dwell_sec', 2.5)
+        # auto | keypress | preview
+        self.declare_parameter('capture_mode', 'auto')
+        self.declare_parameter('start_index', 0)
+        self.declare_parameter('log_csv', '')
+        self.declare_parameter('capture_topic', 'handeye/capture')
+        self.declare_parameter('camera_source', 'd405')
+        self.declare_parameter('camera_namespace', '')
+        self.declare_parameter('image_topic', '')
+        self.declare_parameter('preview_window', True)
+        self.declare_parameter('append_poses_file', '/tmp/handeye_taught_poses.txt')
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names = [f'{prefix}{n}' for n in base]
+        self.velocity_scale = float(self.get_parameter('velocity_scale').value)
+        self.action_name = str(self.get_parameter('action_name').value)
+        self.dwell_sec = float(self.get_parameter('dwell_sec').value)
+        self.capture_mode = str(self.get_parameter('capture_mode').value).strip().lower()
+        self.start_index = max(0, int(self.get_parameter('start_index').value))
+        self.log_csv = str(self.get_parameter('log_csv').value or '').strip()
+        topic = str(self.get_parameter('capture_topic').value)
+        self.preview_window = bool(self.get_parameter('preview_window').value)
+        self.append_poses_file = str(
+            self.get_parameter('append_poses_file').value or ''
+        ).strip()
+
+        cam_src = str(self.get_parameter('camera_source').value).strip().lower()
+        img_topic = str(self.get_parameter('image_topic').value or '').strip()
+        ns = str(self.get_parameter('camera_namespace').value or '').strip()
+        if is_realsense_source(cam_src) and not img_topic:
+            img_topic, _ = resolve_realsense_topics(
+                cam_src, camera_namespace=ns, image_topic='', camera_info_topic=''
+            )
+        self.image_topic = img_topic
+
+        self._client = ActionClient(self, FollowJointTrajectory, self.action_name)
+        self._get_state = self.create_client(GetArmState, 'get_arm_state')
+        self._capture_pub = self.create_publisher(String, topic, 10)
+        self._preview_pub = self.create_publisher(Image, 'handeye/preview_image', 10)
+        self._latest_bgr: Optional[np.ndarray] = None
+        self._cv_gui = _cv_gui_available() and self.preview_window
+        if self.capture_mode == 'preview' and not self._cv_gui:
+            self.get_logger().warn(
+                'OpenCV 没有 GUI（无头环境）。请在 /handeye/preview_image 上预览，'
+                '在本终端按 Enter 确认。'
+            )
+        if self.image_topic and self.capture_mode == 'preview':
+            self.create_subscription(
+                Image, self.image_topic, self._on_image, qos_profile_sensor_data
+            )
+            self.get_logger().info(f'预览图像：{self.image_topic}')
+
+    def _on_image(self, msg: Image) -> None:
+        try:
+            self._latest_bgr = _image_to_bgr(msg)
+        except Exception:  # noqa: BLE001
+            pass
+
+    def run(self) -> int:
+        if self.capture_mode == 'preview':
+            if cv2 is None:
+                self.get_logger().error('preview 模式需要 OpenCV（cv2）')
+                return 1
+            if not self.image_topic:
+                self.get_logger().error('preview 模式需要 image_topic 或 camera_source')
+                return 1
+
+        times = list(self.get_parameter('waypoint_times').value)
+        positions = list(self.get_parameter('waypoint_positions').value)
+        n_j = len(self.joint_names)
+        n_pts = len(times)
+        if n_pts < 1:
+            self.get_logger().error('waypoint_times 为空')
+            return 1
+        if len(positions) != n_pts * n_j:
+            self.get_logger().error(
+                f'waypoint_positions 长度 {len(positions)} != {n_pts}*{n_j}'
+            )
+            return 1
+        if self.start_index >= n_pts:
+            self.get_logger().error(
+                f'start_index {self.start_index} >= 路点总数 {n_pts}'
+            )
+            return 1
+
+        if not self._client.wait_for_server(timeout_sec=15.0):
+            self.get_logger().error(
+                f'Action [{self.action_name}] 不可用。'
+                '请先启动整套栈：bash scripts/run_real.sh'
+            )
+            return 2
+
+        csv_file = None
+        csv_writer: Optional[csv.writer] = None
+        if self.log_csv:
+            path = Path(self.log_csv).expanduser()
+            path.parent.mkdir(parents=True, exist_ok=True)
+            csv_file = path.open('w', newline='', encoding='utf-8')
+            csv_writer = csv.writer(csv_file)
+            csv_writer.writerow(
+                ['index', 'utc_iso', 'move_duration_s']
+                + [f'target_{n}' for n in self.joint_names]
+                + [f'measured_{n}' for n in self.joint_names]
+            )
+            self.get_logger().info(f'正在记录采集数据 → {path}')
+
+        if self.capture_mode == 'preview':
+            print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS)
+
+        self.get_logger().info(
+            f'手眼标定：{n_pts} 个位姿，start={self.start_index}，'
+            f'v_scale={self.velocity_scale}，dwell={self.dwell_sec}s，'
+            f'mode={self.capture_mode}'
+        )
+        self.get_logger().warn(
+            '清理工作空间；让棋盘保持在视野内。Ctrl+C 可中止。'
+        )
+
+        win = 'handeye_preview'
+        try:
+            for i in range(self.start_index, n_pts):
+                q = positions[i * n_j : (i + 1) * n_j]
+                move_t = max(1.0, float(times[i]))
+                self.get_logger().info(
+                    f'[{i + 1}/{n_pts}] 移动中（约 {move_t:.1f}s）...'
+                )
+                code = self._move_one(q, move_t)
+                if code != 0:
+                    return code
+
+                measured = self._read_joints()
+                self._announce_capture(i, n_pts, q, measured)
+
+                if csv_writer is not None and csv_file is not None:
+                    csv_writer.writerow(
+                        [i, datetime.now(timezone.utc).isoformat(), move_t]
+                        + list(q)
+                        + list(measured if measured else [float('nan')] * n_j)
+                    )
+                    csv_file.flush()
+
+                if not self._wait_capture(i, n_pts):
+                    self.get_logger().info('用户已中止')
+                    return 0
+
+                # 在 preview 模式下确认后，保存示教关节
+                if self.capture_mode == 'preview':
+                    taught = self._read_joints()
+                    if taught:
+                        self._save_taught_pose(i, taught)
+        finally:
+            if csv_file is not None:
+                csv_file.close()
+            if cv2 is not None:
+                try:
+                    cv2.destroyWindow(win)
+                except Exception:  # noqa: BLE001
+                    pass
+
+        self.get_logger().info('手眼标定路点序列执行完毕')
+        if self.append_poses_file and self.capture_mode == 'preview':
+            self.get_logger().info(
+                f'示教位姿已保存 → {self.append_poses_file}'
+            )
+        return 0
+
+    def _save_taught_pose(self, index: int, joints: List[float]) -> None:
+        line = ', '.join(f'{v:.6f}' for v in joints[:7]) + ','
+        print(f'# pose {index}')
+        print(line)
+        if not self.append_poses_file:
+            return
+        path = Path(self.append_poses_file).expanduser()
+        path.parent.mkdir(parents=True, exist_ok=True)
+        with path.open('a', encoding='utf-8') as f:
+            f.write(f'# pose {index}\n')
+            f.write(line + '\n')
+        self.get_logger().info(f'已保存位姿 {index} → {path}')
+
+    def _move_one(self, positions: List[float], duration: float) -> int:
+        goal = FollowJointTrajectory.Goal()
+        goal.joint_names = self.joint_names
+        goal.num_points = 1
+        goal.velocity_scale = self.velocity_scale
+        goal.positions = list(positions)
+        goal.time_from_start = [float(duration)]
+
+        send = self._client.send_goal_async(goal)
+        if not self._wait_future(send, 10.0, '发送目标'):
+            return 3
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('目标被拒绝')
+            return 4
+
+        res_fut = handle.get_result_async()
+        if not self._wait_future(res_fut, max(60.0, duration * 3.0), '结果'):
+            return 5
+        result = res_fut.result().result
+        if not result.success:
+            self.get_logger().error(f'移动失败：{result.message}')
+            return 6
+        return 0
+
+    def _wait_future(self, fut, timeout: float, label: str) -> bool:
+        deadline = time.monotonic() + timeout
+        while rclpy.ok() and not fut.done():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if time.monotonic() > deadline:
+                self.get_logger().error(f'等待 {label} 超时')
+                return False
+        return fut.done()
+
+    def _read_joints(self) -> Optional[List[float]]:
+        if not self._get_state.wait_for_service(timeout_sec=0.5):
+            return None
+        fut = self._get_state.call_async(GetArmState.Request())
+        if not self._wait_future(fut, 2.0, 'get_arm_state'):
+            return None
+        res = fut.result()
+        if res is None or not res.success:
+            return None
+        return list(res.joint_positions)
+
+    def _announce_capture(
+        self,
+        index: int,
+        n_pts: int,
+        target: List[float],
+        measured: Optional[List[float]],
+    ) -> None:
+        msg = String()
+        msg.data = f'capture:{index}:{n_pts}'
+        self._capture_pub.publish(msg)
+        q_str = ', '.join(f'{v:.4f}' for v in (measured or target))
+        if self.capture_mode == 'preview':
+            self.get_logger().info(
+                f'[{index + 1}/{n_pts}] 已到位 —— 用 Free Drive 调整，然后按 Enter/Space'
+            )
+        else:
+            self.get_logger().info(
+                f'[{index + 1}/{n_pts}] 已到位 —— 现在采集  q=[{q_str}]'
+            )
+
+    def _wait_capture(self, index: int, n_pts: int) -> bool:
+        if self.capture_mode == 'preview':
+            return self._wait_preview(index, n_pts)
+
+        if self.capture_mode in ('key', 'keypress', 'manual'):
+            self.get_logger().info(
+                '采集完成后按 Enter（q+Enter 退出）...'
+            )
+            for _ in range(5):
+                rclpy.spin_once(self, timeout_sec=0.05)
+            line = sys.stdin.readline()
+            if not line or line.strip().lower() in ('q', 'quit', 'exit'):
+                return False
+            return True
+
+        end = time.monotonic() + max(0.0, self.dwell_sec)
+        while rclpy.ok() and time.monotonic() < end:
+            rclpy.spin_once(self, timeout_sec=0.05)
+        return rclpy.ok()
+
+    def _annotate_preview(
+        self, bgr: np.ndarray, index: int, n_pts: int
+    ) -> np.ndarray:
+        vis = bgr.copy()
+        if cv2 is None:
+            return vis
+        cv2.putText(
+            vis,
+            f'位姿 {index + 1}/{n_pts}',
+            (12, 28),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.7,
+            (0, 255, 0),
+            2,
+        )
+        cv2.putText(
+            vis,
+            'Free Drive 拖拽 | Enter=下一个 q=退出',
+            (12, 56),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.6,
+            (0, 255, 255),
+            2,
+        )
+        return vis
+
+    def _publish_preview(self, bgr: np.ndarray) -> None:
+        msg = Image()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = 'camera'
+        msg.height, msg.width = bgr.shape[0], bgr.shape[1]
+        msg.encoding = 'bgr8'
+        msg.is_bigendian = 0
+        msg.step = msg.width * 3
+        msg.data = array.array('B', bgr.tobytes())  # 快速路径；用 bytes 会多花约 40 ms
+        self._preview_pub.publish(msg)
+
+    def _wait_preview(self, index: int, n_pts: int) -> bool:
+        if self._cv_gui:
+            return self._wait_preview_opencv(index, n_pts)
+        return self._wait_preview_terminal(index, n_pts)
+
+    def _wait_preview_terminal(self, index: int, n_pts: int) -> bool:
+        confirmed = threading.Event()
+        quit_req = threading.Event()
+
+        def _stdin_reader() -> None:
+            try:
+                line = sys.stdin.readline()
+            except Exception:  # noqa: BLE001
+                quit_req.set()
+                return
+            if not line or line.strip().lower() in ('q', 'quit', 'exit'):
+                quit_req.set()
+            else:
+                confirmed.set()
+
+        self.get_logger().info(
+            f'[{index + 1}/{n_pts}] 用 Free Drive 调整，然后按 Enter '
+            '（q+Enter 退出）'
+        )
+        reader = threading.Thread(target=_stdin_reader, daemon=True)
+        reader.start()
+
+        no_frame_warn = False
+        while rclpy.ok():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if confirmed.is_set():
+                return True
+            if quit_req.is_set():
+                return False
+            if not reader.is_alive() and not confirmed.is_set() and not quit_req.is_set():
+                return False
+
+            bgr = self._latest_bgr
+            if bgr is None:
+                if not no_frame_warn:
+                    self.get_logger().warn(
+                        f'正在等待 {self.image_topic} ...',
+                        throttle_duration_sec=3.0,
+                    )
+                    no_frame_warn = True
+                continue
+            vis = self._annotate_preview(bgr, index, n_pts)
+            self._publish_preview(vis)
+        return False
+
+    def _wait_preview_opencv(self, index: int, n_pts: int) -> bool:
+        assert cv2 is not None
+        win = 'handeye_preview'
+        cv2.namedWindow(win, cv2.WINDOW_NORMAL)
+
+        no_frame_warn = False
+        while rclpy.ok():
+            rclpy.spin_once(self, timeout_sec=0.02)
+            bgr = self._latest_bgr
+            if bgr is None:
+                if not no_frame_warn:
+                    self.get_logger().warn(
+                        f'正在等待 {self.image_topic} ...',
+                        throttle_duration_sec=3.0,
+                    )
+                    no_frame_warn = True
+                key = cv2.waitKey(30) & 0xFF
+            else:
+                vis = self._annotate_preview(bgr, index, n_pts)
+                self._publish_preview(vis)
+                cv2.imshow(win, vis)
+                key = cv2.waitKey(30) & 0xFF
+
+            if key in (13, 10, 32):
+                return True
+            if key in (27, ord('q'), ord('Q')):
+                return False
+        return False
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = HandeyeWaypoints()
+    code = 1
+    try:
+        code = node.run()
+    except KeyboardInterrupt:
+        code = 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    raise SystemExit(code)
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/hole_detect_hough.py

@@ -0,0 +1,754 @@
+"""工件二阶段检测：YOLO 定位工件 + HoughCircles 检测左右螺纹孔中心。"""
+
+from __future__ import annotations
+
+import logging
+
+from pathlib import Path
+from typing import Any
+
+import cv2
+import numpy as np
+import yaml
+
+_PKG_CONFIG = Path(__file__).resolve().parents[1] / "config" / "hole_detect.yaml"
+_WS_CONFIG = Path.home() / "ros2" / "src" / "arm_application" / "config" / "hole_detect.yaml"
+DEFAULT_CONFIG = _PKG_CONFIG if _PKG_CONFIG.is_file() else _WS_CONFIG
+
+
+def load_config(path: str | Path | None = None) -> dict[str, Any]:
+    cfg_path = Path(path) if path else DEFAULT_CONFIG
+    with open(cfg_path, encoding="utf-8") as f:
+        data = yaml.safe_load(f) or {}
+    if not isinstance(data, dict):
+        raise ValueError(f"配置格式错误: {cfg_path}")
+    return data
+
+
+logger = logging.getLogger(__name__)
+
+
+def _fail(
+    msg: str,
+    draw_img: np.ndarray,
+    bbox: tuple[int, int, int, int] | None = None,
+    gate_block: bool = False,
+) -> dict[str, Any]:
+    return {
+        "status": False,
+        "msg": msg,
+        "hole_points": [],
+        "hole_radii": [],
+        "bbox": list(bbox) if bbox is not None else None,
+        "gate_block": gate_block,
+        "draw_img": draw_img,
+    }
+
+
+def _draw_bbox(
+    img: np.ndarray,
+    bbox: tuple[int, int, int, int],
+    color=(0, 255, 0),
+    label: str = "",
+) -> None:
+    """画工件框。``label`` 是模型给出的类别名（由标签文件决定）。
+
+    这里刻意**不写死类名**：以前写死了 "gongjian"，但实际模型的类别是
+    "workpiece"，图上的字和模型的类名对不上，排查时会被误导。
+    现在用 ``Detection.cls_name``，也就是 ``weights.sh labels`` 里能读到的那份。
+    """
+    x1, y1, x2, y2 = bbox
+    cv2.rectangle(img, (x1, y1), (x2, y2), color, 2)
+    text = (label or "").strip()
+    if text:
+        cv2.putText(img, text, (x1, max(20, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
+
+
+def _best_label(dets) -> str:
+    """取置信度最高那个检测的类名（由标签文件决定）。没有则返回空串。"""
+    if not dets:
+        return ""
+    try:
+        best = max(dets, key=lambda d: float(d.conf))
+    except Exception:  # noqa: BLE001
+        return ""
+    return str(getattr(best, "cls_name", "") or "").strip()
+
+
+def _draw_center_gate(img: np.ndarray, max_dx: float, max_dy: float, ok: bool) -> None:
+    h, w = img.shape[:2]
+    cx, cy = w * 0.5, h * 0.5
+    x1 = int(round(cx - max_dx))
+    y1 = int(round(cy - max_dy))
+    x2 = int(round(cx + max_dx))
+    y2 = int(round(cy + max_dy))
+    color = (0, 255, 0) if ok else (0, 165, 255)
+    cv2.rectangle(img, (x1, y1), (x2, y2), color, 1)
+    cv2.drawMarker(img, (int(cx), int(cy)), color, markerType=cv2.MARKER_CROSS, markerSize=18, thickness=1)
+
+
+def _bbox_center_ok(
+    bbox: tuple[int, int, int, int],
+    img_w: int,
+    img_h: int,
+    cfg: dict[str, Any],
+) -> tuple[bool, float, float, float, float]:
+    """返回 (是否居中, bbox_cx, bbox_cy, max_dx, max_dy)。"""
+    x1, y1, x2, y2 = bbox
+    bx = 0.5 * (x1 + x2)
+    by = 0.5 * (y1 + y2)
+    max_dx = float(cfg.get("center_max_offset_ratio_x", 0.12)) * img_w
+    max_dy = float(cfg.get("center_max_offset_ratio_y", 0.12)) * img_h
+    ok = abs(bx - img_w * 0.5) <= max_dx and abs(by - img_h * 0.5) <= max_dy
+    return ok, bx, by, max_dx, max_dy
+
+
+def _draw_holes(
+    img: np.ndarray,
+    points: list[list[float]],
+    radii: list[float] | None = None,
+) -> None:
+    """画孔轮廓圆 + 中心点（十字+实心点）+ 坐标。"""
+    for i, (cx, cy) in enumerate(points):
+        px, py = int(round(cx)), int(round(cy))
+        r = 12.0 if radii is None or i >= len(radii) else float(radii[i])
+        rr = max(4, int(round(r)))
+        # 轮廓
+        cv2.circle(img, (px, py), rr, (0, 0, 255), 2)
+        # 中心十字 + 黄点
+        cv2.drawMarker(img, (px, py), (0, 0, 255), markerType=cv2.MARKER_CROSS, markerSize=16, thickness=2)
+        cv2.circle(img, (px, py), 3, (0, 255, 255), -1)
+        label = f"P{i + 1}({px},{py}) r={rr}"
+        cv2.putText(img, label, (px + 12, py - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 0, 255), 2)
+
+
+def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
+    """最多取 1 个工件：取置信度最高且 >= conf 的框。"""
+    if not dets:
+        return None
+    ordered = sorted(dets, key=lambda d: d.conf, reverse=True)
+    for det in ordered:
+        if float(det.conf) < conf_thr:
+            continue
+        return tuple(int(v) for v in det.xyxy)  # type: ignore[return-value]
+    return None
+
+
+def _refine_circle_center(
+    gray: np.ndarray,
+    cx: float,
+    cy: float,
+    r: float,
+) -> tuple[float, float]:
+    """Hough 整数圆心 → 暗孔连通域一阶矩（亚像素）。失败则退回原圆心。"""
+    h, w = gray.shape[:2]
+    rad = max(5.0, float(r))
+    half = int(round(2.2 * rad))
+    x0 = max(0, int(round(cx)) - half)
+    y0 = max(0, int(round(cy)) - half)
+    x1 = min(w, int(round(cx)) + half)
+    y1 = min(h, int(round(cy)) + half)
+    if x1 - x0 < 8 or y1 - y0 < 8:
+        return cx, cy
+    roi = gray[y0:y1, x0:x1]
+    blur = cv2.GaussianBlur(roi, (5, 5), 1.0)
+    _, bw = cv2.threshold(blur, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
+    n, labels, stats, cents = cv2.connectedComponentsWithStats(bw, 8)
+    if n <= 1:
+        return cx, cy
+    ex, ey = cx - x0, cy - y0
+    min_area = np.pi * (0.35 * rad) ** 2
+    max_area = np.pi * (1.8 * rad) ** 2
+    best_i = 0
+    best_d = 1e18
+    for i in range(1, n):
+        area = float(stats[i, cv2.CC_STAT_AREA])
+        if area < min_area or area > max_area:
+            continue
+        d = (float(cents[i][0]) - ex) ** 2 + (float(cents[i][1]) - ey) ** 2
+        if d < best_d:
+            best_d = d
+            best_i = i
+    if best_i <= 0 or best_d > (1.2 * rad) ** 2:
+        return cx, cy
+    mask = np.uint8(labels == best_i)
+    M = cv2.moments(mask)
+    if M["m00"] < 1.0:
+        return cx, cy
+    return x0 + M["m10"] / M["m00"], y0 + M["m01"] / M["m00"]
+
+
+# ==================== 螺纹嵌件对检测（亮环签名 + 关于中心孔对称）====================
+# 目标件是圆形法兰：中心一个大螺纹孔，两个目标螺纹嵌件在中心孔两侧、关于中心对称。
+# 旧 Hough"间距最大的一对"在这类多孔板上会锁到普通安装孔且逐帧跳变。
+# 新路径：金属嵌件有"暗孔芯 + 亮金属环 + 暗法兰背景"的明确签名；
+#         两个目标嵌件关于中心孔对称（中点≈中心孔、半径相近）——据此唯一锁定，抗干扰抗跳变。
+
+
+def _disc_kernel(r: float) -> np.ndarray:
+    R = int(np.ceil(r))
+    y, x = np.mgrid[-R:R + 1, -R:R + 1]
+    k = (x * x + y * y <= r * r).astype(np.float32)
+    return k / max(1.0, float(k.sum()))
+
+
+def _ring_kernel(a: float, b: float) -> np.ndarray:
+    R = int(np.ceil(b))
+    y, x = np.mgrid[-R:R + 1, -R:R + 1]
+    d = x * x + y * y
+    k = ((d >= a * a) & (d <= b * b)).astype(np.float32)
+    return k / max(1.0, float(k.sum()))
+
+
+def _insert_score_map(gray: np.ndarray, scales) -> np.ndarray:
+    """亮环签名评分（多尺度取 max）：环越亮、芯越暗、外背景越暗，分越高。"""
+    g = gray.astype(np.float32)
+    best = None
+    for ri, rr, ro in scales:
+        inner = cv2.filter2D(g, -1, _disc_kernel(ri))
+        ringm = cv2.filter2D(g, -1, _ring_kernel(ri + 1, rr))
+        outer = cv2.filter2D(g, -1, _ring_kernel(rr + 2, ro))
+        s = ringm - 0.5 * (inner + outer)
+        best = s if best is None else np.maximum(best, s)
+    return best
+
+
+def _refine_dark_bore(gray: np.ndarray, cx: float, cy: float, half: int = 26):
+    """暗孔连通域一阶矩 → 亚像素孔心 + 等效半径。失败退回原心。"""
+    h, w = gray.shape
+    x0 = max(0, int(round(cx)) - half)
+    y0 = max(0, int(round(cy)) - half)
+    x1 = min(w, int(round(cx)) + half)
+    y1 = min(h, int(round(cy)) + half)
+    if x1 - x0 < 12 or y1 - y0 < 12:
+        return float(cx), float(cy), 12.0
+    roi = gray[y0:y1, x0:x1].astype(np.float32)
+    blur = cv2.GaussianBlur(roi, (5, 5), 1.0)
+    thr = float(np.percentile(blur, 20))
+    mask = (blur < thr).astype(np.uint8) * 255
+    n, _lab, st, ce = cv2.connectedComponentsWithStats(mask, 8)
+    best_i, best_d = 0, 1e18
+    ex, ey = cx - x0, cy - y0
+    for i in range(1, n):
+        a = float(st[i, cv2.CC_STAT_AREA])
+        if a < 15:
+            continue
+        d = (float(ce[i][0]) - ex) ** 2 + (float(ce[i][1]) - ey) ** 2
+        if d < best_d:
+            best_d, best_i = d, i
+    if best_i <= 0:
+        return float(cx), float(cy), 12.0
+    area = float(st[best_i, cv2.CC_STAT_AREA])
+    r_eq = float(np.sqrt(area / np.pi))
+    return x0 + float(ce[best_i][0]), y0 + float(ce[best_i][1]), max(6.0, r_eq)
+
+
+def _detect_insert_pair(gray: np.ndarray, bbox, cfg, depth_m=None, intr=None):
+    """找"关于工件中心对称的一对螺纹嵌件"。返回 ((k1x,k1y),(k2x,k2y),r1,r2) 或 None。
+
+    尺度自适应：用深度把物理螺栓圆半径（~29mm，两孔间距~58mm）换算成像素半径，
+    在中心 C 周围的圆环上搜候选，再选"中点≈C、都在环上、间距≈直径"的对称对。
+    不依赖 YOLO 框是否盖住嵌件（框只用来定中心 C）。k1=左孔(x 小)，k2=右孔(x 大)。
+    """
+    h, w = gray.shape
+    x1, y1, x2, y2 = [int(v) for v in bbox]
+    C = np.array([0.5 * (x1 + x2), 0.5 * (y1 + y2)], dtype=float)
+    # ---- 螺栓圆像素半径 bolt_px ----
+    bolt_mm = float(cfg.get("insert_bolt_radius_mm", 29.0))
+    bolt_px = 0.0
+    if depth_m is not None and intr is not None:
+        # 工件表面深度：C 附近小邻域的浅分位（孔底更深，表面偏浅）
+        r_s = max(6, int(round(0.25 * min(x2 - x1, y2 - y1))))
+        cxi, cyi = int(round(C[0])), int(round(C[1]))
+        sx1, sx2 = max(0, cxi - r_s), min(w, cxi + r_s)
+        sy1, sy2 = max(0, cyi - r_s), min(h, cyi + r_s)
+        zreg = depth_m[sy1:sy2, sx1:sx2]
+        zreg = zreg[np.isfinite(zreg) & (zreg > 0.05)]
+        if zreg.size >= 20:
+            z_plate = float(np.percentile(zreg, 30))
+            z_surf = max(0.05, z_plate - float(cfg.get("plate_thickness_m", 0.004)))
+            bolt_px = bolt_mm / 1000.0 * float(intr["fx"]) / z_surf
+    if bolt_px <= 0:
+        # 无深度时退化为按框尺寸粗估（嵌件约在 0.7×框半宽处）
+        bolt_px = 0.70 * 0.5 * max(x2 - x1, y2 - y1)
+    bolt_px = float(np.clip(bolt_px, 24.0, 0.42 * min(h, w)))
+    # ---- 亮环分数图 ----
+    scales = cfg.get("insert_scales") or [
+        (3, 6, 10), (5, 9, 14), (7, 12, 18), (10, 16, 23), (14, 21, 30),
+    ]
+    score = _insert_score_map(gray, scales)
+    # ---- 圆环掩码：嵌件在 [0.45, 1.55]×bolt_px 的环上 ----
+    yy, xx = np.mgrid[0:h, 0:w]
+    dist = np.hypot(xx - C[0], yy - C[1])
+    ring = ((dist > 0.45 * bolt_px) & (dist < 1.55 * bolt_px)).astype(np.float32)
+    scm = score * ring
+    pos = scm[scm > 0]
+    if pos.size < 20:
+        return None
+    thr = float(np.percentile(pos, float(cfg.get("insert_percentile", 96))))
+    pk = (scm > thr).astype(np.uint8) * 255
+    n, _lab, st, ce = cv2.connectedComponentsWithStats(pk, 8)
+    cands = []  # (pos, score)
+    for i in range(1, n):
+        if st[i, cv2.CC_STAT_AREA] < 8:
+            continue
+        cx_i, cy_i = float(ce[i][0]), float(ce[i][1])
+        cands.append((np.array([cx_i, cy_i], dtype=float),
+                      float(scm[int(round(cy_i)), int(round(cx_i))])))
+    if len(cands) < 2:
+        return None
+    # ---- 对称对：两孔都在环上、间距≈直径、中点大致≈C（对称门）。
+    # 在几何合格的对里选"亮环总分最高"的——真嵌件亮环最强，稳定选中真孔对，
+    # 不被"更对称但更弱"的假对带偏（C 与真中心有几像素偏差时仍稳健，消除双稳态跳变）。
+    best = None
+    best_score = -1.0
+    sym_gate = float(cfg.get("insert_sym_gate_ratio", 0.40)) * bolt_px
+    for i in range(len(cands)):
+        for j in range(i + 1, len(cands)):
+            pi, si = cands[i]
+            pj, sj = cands[j]
+            di = float(np.linalg.norm(pi - C))
+            dj = float(np.linalg.norm(pj - C))
+            if not (0.40 * bolt_px < di < 1.65 * bolt_px):
+                continue
+            if not (0.40 * bolt_px < dj < 1.65 * bolt_px):
+                continue
+            sep = float(np.linalg.norm(pi - pj))
+            if not (1.2 * bolt_px < sep < 2.9 * bolt_px):
+                continue
+            mid = 0.5 * (pi + pj)
+            if float(np.linalg.norm(mid - C)) > sym_gate:
+                continue
+            tot = si + sj
+            if tot > best_score:
+                best_score = tot
+                best = (pi, pj)
+    if best is None:
+        return None
+    p, q = best
+    # 稳定分配 k1/k2：沿主方向，避免竖直对时"x 小者"被噪声翻转。
+    # 竖直对(|dy|>=|dx|) → k1=上(y 小)、k2=下；水平对 → k1=左(x 小)、k2=右。
+    if abs(p[1] - q[1]) >= abs(p[0] - q[0]):
+        k1raw, k2raw = (p, q) if p[1] < q[1] else (q, p)
+    else:
+        k1raw, k2raw = (p, q) if p[0] < q[0] else (q, p)
+    k1x, k1y, r1 = _refine_dark_bore(gray, k1raw[0], k1raw[1])
+    k2x, k2y, r2 = _refine_dark_bore(gray, k2raw[0], k2raw[1])
+    return (k1x, k1y), (k2x, k2y), r1, r2
+
+
+def _pad_roi(
+    img_w: int,
+    img_h: int,
+    bbox: tuple[int, int, int, int],
+    padding: int,
+) -> tuple[int, int, int, int, int, int]:
+    """返回 x1,y1,x2,y2,offset_x,offset_y（裁剪左上角在原图坐标）。"""
+    x1, y1, x2, y2 = bbox
+    px1 = max(0, x1 - padding)
+    py1 = max(0, y1 - padding)
+    px2 = min(img_w, x2 + padding)
+    py2 = min(img_h, y2 + padding)
+    if px2 <= px1 or py2 <= py1:
+        raise ValueError("ROI 无效（宽或高为 0）")
+    return px1, py1, px2, py2, px1, py1
+
+
+def _render_success(
+    image: np.ndarray,
+    bbox: tuple[int, int, int, int],
+    hole_points: list[list[float]],
+    hole_radii: list[float],
+    dist: float,
+    gate: tuple[float, float] | None = None,
+    label: str = "",
+) -> np.ndarray:
+    draw_img = image.copy()
+    if gate is not None:
+        _draw_center_gate(draw_img, gate[0], gate[1], True)
+    _draw_bbox(draw_img, bbox, label=label)
+    _draw_holes(draw_img, hole_points, hole_radii)
+    cv2.putText(
+        draw_img,
+        f"OK dist={dist:.1f}px",
+        (16, 36),
+        cv2.FONT_HERSHEY_SIMPLEX,
+        0.8,
+        (0, 255, 0),
+        2,
+    )
+    return draw_img
+
+
+def detect_holes(
+    image: np.ndarray,
+    detector,
+    config: dict[str, Any] | None = None,
+    device: str | int = 0,
+    skip_center_gate: bool = False,
+    depth_m: np.ndarray | None = None,
+    intr: dict | None = None,
+    allow_full_frame_roi: bool = False,
+) -> dict[str, Any]:
+    """
+    二阶段检测螺纹孔中心。
+
+    detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[Detection]
+              （YOLODetector / FasterRCNNDetector / ONNX 均可）
+
+    :param allow_full_frame_roi: 检测器**没给出工件框**时，是否退回"整幅画面当 ROI"。
+        默认 False（保持原行为：没有框就不做，报「未检测到工件」）。
+        置 True 的场景是**调用方明确要试一把**：知道检测器可能漏检，但孔本身
+        在画面里已经看得见（例如工件检测模型与当前相机/画面不匹配、或工件贴着
+        画面中心）。此时 ROI 的约束由孔自身的判据接手 —— 环半径、两孔间距、
+        居中门控都还在，所以不会凭空多出"随便一个圆"。
+
+    返回:
+      status, msg, hole_points [[cx,cy],...], hole_radii [r1,r2], bbox, draw_img(BGR)
+    """
+    try:
+        if image is None or not isinstance(image, np.ndarray) or image.size == 0:
+            return _fail("输入图像无效", np.zeros((480, 640, 3), dtype=np.uint8))
+
+        cfg = config if config is not None else load_config()
+        draw_img = image.copy()
+        h, w = image.shape[:2]
+
+        conf = float(cfg.get("conf", 0.25))
+        iou = float(cfg.get("iou", 0.7))
+        imgsz = int(cfg.get("imgsz", 640))
+        padding = int(cfg.get("bbox_padding", 25))
+        ksize = int(cfg.get("gaussian_ksize", 5))
+        if ksize % 2 == 0:
+            ksize += 1
+        sigma = float(cfg.get("gaussian_sigma", 1.5))
+
+        # 1) 检测器定位工件 bbox（最多 1 个）— 兼容 YOLO / Faster R-CNN
+        dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
+        bbox = _parse_best_bbox(dets, conf)
+        if bbox is None and allow_full_frame_roi:
+            # 退回整幅画面：ROI 从"检测器给的框"换成"整幅"，孔自身的判据
+            #（环半径区间、两孔间距、居中门控）继续把关。
+            bbox = (0, 0, int(w - 1), int(h - 1))
+            logger.info(
+                "工件检测器没给框，按调用方要求用整幅画面当 ROI 继续找孔"
+                "（允许的原因见 allow_full_frame_roi 的说明）"
+            )
+        if bbox is None:
+            return _fail("未检测到工件", draw_img)
+
+        # 类别名来自标签文件（见 model_labels / weights.sh labels），不再写死。
+        label = _best_label(dets)
+        _draw_bbox(draw_img, bbox, label=label)
+
+        # 1.5) 居中门控：bbox 中心接近画面中心才进入二阶检测。
+        # 精对准 k1/k2 时工件会离开画面中心，调用方应 skip_center_gate=True。
+        center_enable = bool(cfg.get("center_gate_enable", True)) and not skip_center_gate
+        centered, bx, by, max_dx, max_dy = _bbox_center_ok(bbox, w, h, cfg)
+        if bool(cfg.get("center_gate_draw", True)):
+            _draw_center_gate(draw_img, max_dx, max_dy, centered if center_enable else True)
+        if center_enable and not centered:
+            dx = abs(bx - w * 0.5)
+            dy = abs(by - h * 0.5)
+            cv2.putText(
+                draw_img,
+                "MOVE TO CENTER",
+                (16, 36),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.8,
+                (0, 165, 255),
+                2,
+            )
+            return _fail(
+                f"工件未居中：offset=({dx:.0f},{dy:.0f})px，允许<=({max_dx:.0f},{max_dy:.0f})px",
+                draw_img,
+                bbox,
+                gate_block=True,
+            )
+
+        # ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
+        if bool(cfg.get("insert_pair_enable", True)):
+            gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
+            pair = _detect_insert_pair(gray_full, bbox, cfg, depth_m=depth_m, intr=intr)
+            if pair is not None:
+                (k1, k2, r1, r2) = pair
+                hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
+                hole_radii = [float(r1), float(r2)]
+                dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
+                gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
+                draw_img = _render_success(
+                    image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
+                )
+                return {
+                    "status": True,
+                    "msg": f"螺纹嵌件对，两孔距离 {dist:.1f}px",
+                    "hole_points": hole_points,
+                    "hole_radii": hole_radii,
+                    "bbox": list(bbox),
+                    "label": label,
+                    "draw_img": draw_img,
+                }
+        # ---- 退路：原 Hough 老路径 ----
+
+        # YOLO 框经常只包住半边法兰；扩到能同时容纳两个螺纹孔
+        min_w = int(cfg.get("bbox_min_width", 280))
+        min_h = int(cfg.get("bbox_min_height", 220))
+        x1, y1, x2, y2 = bbox
+        bw, bh = x2 - x1, y2 - y1
+        if bw < min_w or bh < min_h:
+            cxb, cyb = 0.5 * (x1 + x2), 0.5 * (y1 + y2)
+            bw, bh = max(bw, min_w), max(bh, min_h)
+            x1 = int(round(cxb - 0.5 * bw))
+            y1 = int(round(cyb - 0.5 * bh))
+            x2 = int(round(cxb + 0.5 * bw))
+            y2 = int(round(cyb + 0.5 * bh))
+            bbox = (
+                max(0, x1), max(0, y1),
+                min(w, x2), min(h, y2),
+            )
+
+        # 2) padding ROI + 边界保护
+        rx1, ry1, rx2, ry2, offset_x, offset_y = _pad_roi(w, h, bbox, padding)
+        roi = image[ry1:ry2, rx1:rx2]
+        if roi.size == 0:
+            return _fail("ROI 裁剪失败", draw_img, bbox)
+
+        # 3) 灰度 + 高斯模糊
+        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) if roi.ndim == 3 else roi
+        blur = cv2.GaussianBlur(gray, (ksize, ksize), sigma)
+
+        # 4) HoughCircles
+        circles = cv2.HoughCircles(
+            blur,
+            cv2.HOUGH_GRADIENT,
+            dp=float(cfg.get("hough_dp", 1.2)),
+            minDist=float(cfg.get("hough_min_dist", 40)),
+            param1=float(cfg.get("hough_param1", 100)),
+            param2=float(cfg.get("hough_param2", 28)),
+            minRadius=int(cfg.get("hough_min_radius", 6)),
+            maxRadius=int(cfg.get("hough_max_radius", 45)),
+        )
+        if circles is None:
+            return _fail("HoughCircles 未检测到圆孔", draw_img, bbox)
+
+        # 5) ROI 坐标 -> 原图坐标，再亚像素细化孔心
+        circles = np.asarray(circles[0], dtype=np.float64)
+        gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
+        do_refine = bool(cfg.get("refine_hole_centers", True))
+        mapped: list[tuple[float, float, float]] = []
+        for cx, cy, r in circles:
+            fx = float(cx + offset_x)
+            fy = float(cy + offset_y)
+            fr = float(r)
+            if do_refine:
+                fx, fy = _refine_circle_center(gray_full, fx, fy, fr)
+            mapped.append((fx, fy, fr))
+
+        # 6) 工业校验（含 bbox padding 区内的孔，避免螺纹孔落在框外）
+        bx1, by1, bx2, by2 = bbox
+        vx1 = max(0, bx1 - padding)
+        vy1 = max(0, by1 - padding)
+        vx2 = min(w, bx2 + padding)
+        vy2 = min(h, by2 + padding)
+        min_r = float(cfg.get("min_radius", 6))
+        max_r = float(cfg.get("max_radius", 45))
+        dist_min = float(cfg.get("hole_distance_min", 80))
+        dist_max = float(cfg.get("hole_distance_max", 900))
+
+        valid: list[tuple[float, float, float]] = []
+        for cx, cy, r in mapped:
+            if not (vx1 <= cx <= vx2 and vy1 <= cy <= vy2):
+                continue
+            if not (min_r <= r <= max_r):
+                continue
+            valid.append((cx, cy, r))
+
+        # 丢掉法兰中心大孔：螺纹孔在工件两端，中心孔靠近 bbox 中心
+        bcx = 0.5 * (bx1 + bx2)
+        bcy = 0.5 * (by1 + by2)
+        diag = max(40.0, float(np.hypot(bx2 - bx1, by2 - by1)))
+        ring = [
+            c for c in valid
+            if float(np.hypot(c[0] - bcx, c[1] - bcy)) >= 0.18 * diag
+        ]
+        if len(ring) >= 2:
+            valid = ring
+
+        # 多于 2 个：在间距窗内取半径接近、间距最大的一对（左右螺纹孔，避开中心大孔边缘）
+        if len(valid) >= 2:
+            best = None
+            best_d = -1.0
+            for i in range(len(valid)):
+                for j in range(i + 1, len(valid)):
+                    d = float(np.hypot(valid[i][0] - valid[j][0], valid[i][1] - valid[j][1]))
+                    if not (dist_min <= d <= dist_max):
+                        continue
+                    if abs(float(valid[i][2]) - float(valid[j][2])) > 10.0:
+                        continue
+                    if d > best_d:
+                        best_d = d
+                        best = (valid[i], valid[j])
+            if best is None:
+                return _fail(
+                    f"候选圆孔={len(valid)}，没有间距在 [{dist_min}, {dist_max}] 且半径接近的一对",
+                    draw_img,
+                    bbox,
+                )
+            valid = [best[0], best[1]]
+
+        if len(valid) != 2:
+            return _fail(f"有效圆孔数量={len(valid)}，要求严格等于 2", draw_img, bbox)
+
+        ordered = sorted(valid, key=lambda c: c[0])
+        (x1, y1, r1), (x2, y2, r2) = ordered
+        dist = float(np.hypot(x1 - x2, y1 - y2))
+        if not (dist_min <= dist <= dist_max):
+            return _fail(
+                f"两孔距离={dist:.1f}px，不在 [{dist_min}, {dist_max}]",
+                draw_img,
+                bbox,
+            )
+
+        hole_points = [[x1, y1], [x2, y2]]
+        hole_radii = [r1, r2]
+        gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
+        draw_img = _render_success(image, bbox, hole_points, hole_radii, dist, gate=gate, label=label)
+        return {
+            "status": True,
+            "msg": f"检测成功，两孔距离 {dist:.1f}px",
+            "hole_points": hole_points,
+            "hole_radii": hole_radii,
+            "bbox": list(bbox),
+            "label": label,
+            "draw_img": draw_img,
+        }
+    except Exception as exc:
+        fallback = image.copy() if isinstance(image, np.ndarray) and image.size else np.zeros((480, 640, 3), dtype=np.uint8)
+        return _fail(f"异常: {exc}", fallback)
+
+
+class HoleStabilizer:
+    """时序稳帧：EMA 平滑中心/半径，短暂失败时沿用上一帧结果，抑制闪烁。"""
+
+    def __init__(self, config: dict[str, Any] | None = None) -> None:
+        cfg = config or {}
+        self.cfg = cfg
+        self.hold_frames = int(cfg.get("stabilize_hold_frames", 15))
+        self.ema_alpha = float(cfg.get("stabilize_ema_alpha", 0.35))
+        self.max_jump = float(cfg.get("stabilize_max_jump", 60))
+        self.reset()
+
+    def reset(self) -> None:
+        self.points: list[list[float]] | None = None
+        self.radii: list[float] | None = None
+        self.bbox: tuple[int, int, int, int] | None = None
+        self.miss = 0
+
+    def update(self, image: np.ndarray, result: dict[str, Any]) -> dict[str, Any]:
+        try:
+            # 未居中：不做二阶、不沿用旧孔，立即清空稳帧
+            if result.get("gate_block"):
+                self.reset()
+                out = dict(result)
+                out["hole_points"] = []
+                out["hole_radii"] = []
+                return out
+
+            bbox = result.get("bbox")
+            if bbox is not None and len(bbox) == 4:
+                self.bbox = (int(bbox[0]), int(bbox[1]), int(bbox[2]), int(bbox[3]))
+
+            if result.get("status") and len(result.get("hole_points") or []) == 2:
+                pts = [[float(p[0]), float(p[1])] for p in result["hole_points"]]
+                pts = sorted(pts, key=lambda p: p[0])
+                rad = result.get("hole_radii") or [12.0, 12.0]
+                rad = [float(rad[0]), float(rad[1])] if len(rad) >= 2 else [12.0, 12.0]
+
+                if self.points is not None:
+                    jump = max(
+                        float(np.hypot(pts[0][0] - self.points[0][0], pts[0][1] - self.points[0][1])),
+                        float(np.hypot(pts[1][0] - self.points[1][0], pts[1][1] - self.points[1][1])),
+                    )
+                    if jump > self.max_jump:
+                        # 跳变过大，当作本帧失败，沿用稳定值
+                        self.miss += 1
+                    else:
+                        a = self.ema_alpha
+                        self.points = [
+                            [
+                                (1 - a) * self.points[0][0] + a * pts[0][0],
+                                (1 - a) * self.points[0][1] + a * pts[0][1],
+                            ],
+                            [
+                                (1 - a) * self.points[1][0] + a * pts[1][0],
+                                (1 - a) * self.points[1][1] + a * pts[1][1],
+                            ],
+                        ]
+                        self.radii = [
+                            (1 - a) * self.radii[0] + a * rad[0],
+                            (1 - a) * self.radii[1] + a * rad[1],
+                        ]
+                        self.miss = 0
+                else:
+                    self.points = pts
+                    self.radii = rad
+                    self.miss = 0
+            else:
+                self.miss += 1
+                if self.miss > self.hold_frames:
+                    self.points = None
+                    self.radii = None
+
+            if self.points is None or self.radii is None:
+                # 彻底丢失：只保留当前帧绘制（可能仅有 bbox）
+                out = dict(result)
+                out["hole_points"] = []
+                out["hole_radii"] = []
+                return out
+
+            dist = float(
+                np.hypot(
+                    self.points[0][0] - self.points[1][0],
+                    self.points[0][1] - self.points[1][1],
+                )
+            )
+            bbox_draw = self.bbox
+            if bbox_draw is None and result.get("bbox") is not None:
+                b = result["bbox"]
+                bbox_draw = (int(b[0]), int(b[1]), int(b[2]), int(b[3]))
+            if bbox_draw is None:
+                # 无框时仍画孔
+                draw_img = image.copy()
+                _draw_holes(draw_img, self.points, self.radii)
+            else:
+                gate = None
+                if bool(self.cfg.get("center_gate_draw", True)):
+                    ih, iw = image.shape[:2]
+                    max_dx = float(self.cfg.get("center_max_offset_ratio_x", 0.12)) * iw
+                    max_dy = float(self.cfg.get("center_max_offset_ratio_y", 0.12)) * ih
+                    gate = (max_dx, max_dy)
+                draw_img = _render_success(
+                    image,
+                    bbox_draw,
+                    self.points,
+                    self.radii,
+                    dist,
+                    gate=gate,
+                    label=str(result.get("label") or ""),
+                )
+
+            held = self.miss > 0
+            return {
+                "status": True,
+                "msg": (
+                    f"检测成功，两孔距离 {dist:.1f}px"
+                    + (f"（稳帧 hold={self.miss}）" if held else "")
+                ),
+                "hole_points": [[p[0], p[1]] for p in self.points],
+                "hole_radii": [float(self.radii[0]), float(self.radii[1])],
+                "bbox": list(bbox_draw) if bbox_draw is not None else result.get("bbox"),
+                "draw_img": draw_img,
+            }
+        except Exception as exc:
+            fallback = image.copy() if isinstance(image, np.ndarray) and image.size else np.zeros((480, 640, 3), dtype=np.uint8)
+            return _fail(f"稳帧异常: {exc}", fallback)

build_sim/arm_application/build/lib/arm_application/hole_detect_node.py

@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+"""在当前机械臂的 D405 彩色话题上做入孔检测。只检测，不做插入动作。"""
+
+from __future__ import annotations
+
+import os
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import Image
+from std_msgs.msg import Float32MultiArray
+
+from arm_application.handeye_realsense import resolve_realsense_topics
+from arm_application.hole_detect_hough import HoleStabilizer
+from arm_application.hole_detector import (
+    HoleDetection,
+    HoleDetectResult,
+    HoleDetector,
+    config_from_site,
+    weights_from_site,
+)
+from arm_application.site_io import load_site_dict
+from arm_msgs.msg import HoleDetection as HoleDetectionMsg
+from arm_msgs.msg import HoleDetections
+
+from arm_application.ros_node import TolerantNode
+
+
+def _as_bool(val, default: bool = False) -> bool:
+    if val is None:
+        return default
+    if isinstance(val, str):
+        return val.strip().lower() in ("1", "true", "yes", "on")
+    return bool(val)
+
+
+def _bgr_copy(msg: Image) -> np.ndarray:
+    h, w = int(msg.height), int(msg.width)
+    enc = (msg.encoding or "").lower()
+    buf = np.frombuffer(msg.data, dtype=np.uint8)
+    if enc == "bgr8":
+        return buf.reshape(h, w, 3).copy()
+    if enc == "rgb8":
+        return buf.reshape(h, w, 3)[:, :, ::-1].copy()
+    raise RuntimeError(f"Unsupported encoding {msg.encoding!r}")
+
+
+def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
+    msg = Image()
+    msg.header.stamp = stamp
+    msg.header.frame_id = frame_id
+    msg.height = int(image.shape[0])
+    msg.width = int(image.shape[1])
+    msg.encoding = "bgr8"
+    msg.step = int(image.shape[1] * 3)
+    msg.data = image.tobytes()
+    return msg
+
+
+class HoleDetectNode(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__("hole_detect")
+        self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")
+        self.declare_parameter("image_topic", "")
+        self.declare_parameter("weights", "")
+        self.declare_parameter("config_file", "")
+        self.declare_parameter("backend", "two_stage")
+        self.declare_parameter("device", os.environ.get("SITE_YOLO_DEVICE") or "0")
+        self.declare_parameter("preview", False)
+
+        site = load_site_dict()
+        cam = str(self.get_parameter("camera_source").value or "d405")
+        image_topic, _info = resolve_realsense_topics(cam, image_topic=str(self.get_parameter("image_topic").value or ""))
+        cfg = config_from_site(site)
+        param_backend = str(self.get_parameter("backend").value or "").strip().lower()
+        if param_backend:
+            cfg["backend"] = param_backend
+        backend = str(cfg.get("backend") or "two_stage")
+        weights = str(self.get_parameter("weights").value or "").strip() or weights_from_site(site)
+        self._preview = _as_bool(self.get_parameter("preview").value, False)
+        self._detector = HoleDetector(
+            model_path=weights or None,
+            config=cfg,
+            device=str(self.get_parameter("device").value or "0"),
+        )
+        self._stabilizer = (
+            HoleStabilizer(cfg) if backend in ("two_stage", "hough_roi") else None
+        )
+        self._pub = self.create_publisher(HoleDetections, "/hole_detect/result", 10)
+        self._arr_pub = self.create_publisher(Float32MultiArray, "/hole_detect/holes", 10)
+        self._overlay_pub = self.create_publisher(Image, "/hole_detect/overlay", 10)
+        self.create_subscription(Image, image_topic, self._on_image, qos_profile_sensor_data)
+        self.get_logger().info(
+            f"hole_detect camera={cam} topic={image_topic} backend={backend} "
+            f"weights={self._detector.model_path} site overlay=hole_detect"
+        )
+
+    def _on_image(self, msg: Image) -> None:
+        try:
+            bgr = _bgr_copy(msg)
+        except Exception as exc:
+            self.get_logger().error(str(exc))
+            return
+        result = self._detector.detect(bgr)
+        if self._stabilizer is not None:
+            result = self._stabilize(bgr, result)
+        out = HoleDetections()
+        out.header = msg.header
+        out.ok = bool(result.status)
+        out.message = result.msg
+        for hole in result.holes:
+            hmsg = HoleDetectionMsg()
+            hmsg.u = float(hole.center_px[0])
+            hmsg.v = float(hole.center_px[1])
+            hmsg.radius_px = float(hole.radius_px)
+            hmsg.confidence = float(hole.confidence)
+            hmsg.label = hole.label or ""
+            if hole.bbox:
+                hmsg.bbox = [int(v) for v in hole.bbox]
+            out.holes.append(hmsg)
+        self._pub.publish(out)
+
+        arr = Float32MultiArray()
+        data = [1.0 if result.status else 0.0, float(len(result.holes))]
+        for hole in result.holes:
+            data.extend([hole.center_px[0], hole.center_px[1], hole.radius_px, hole.confidence])
+        arr.data = data
+        self._arr_pub.publish(arr)
+
+        overlay = result.draw_img if result.draw_img is not None else bgr
+        self._overlay_pub.publish(_bgr_to_msg(overlay, msg.header.stamp, msg.header.frame_id))
+        if self._preview:
+            import cv2
+
+            cv2.imshow("hole_detect", overlay)
+            cv2.waitKey(1)
+
+    def _stabilize(self, bgr, result: HoleDetectResult) -> HoleDetectResult:
+        raw = result.raw if isinstance(result.raw, dict) else {}
+        payload = {
+            "status": result.status,
+            "msg": result.msg,
+            "hole_points": raw.get("hole_points")
+            or [list(h.center_px) for h in result.holes],
+            "hole_radii": raw.get("hole_radii") or [h.radius_px for h in result.holes],
+            "bbox": raw.get("bbox") or (list(result.holes[0].bbox) if result.holes and result.holes[0].bbox else None),
+            "draw_img": result.draw_img,
+            "gate_block": bool(raw.get("gate_block")),
+        }
+        stab = self._stabilizer.update(bgr, payload)
+        holes: list[HoleDetection] = []
+        radii = stab.get("hole_radii") or []
+        bbox = stab.get("bbox")
+        bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
+        for i, pt in enumerate(stab.get("hole_points") or []):
+            rr = float(radii[i]) if i < len(radii) else 0.0
+            holes.append(
+                HoleDetection(
+                    center_px=(float(pt[0]), float(pt[1])),
+                    radius_px=rr,
+                    confidence=1.0,
+                    bbox=bbox_t,
+                    label="left" if i == 0 else "right" if i == 1 else f"h{i + 1}",
+                )
+            )
+        return HoleDetectResult(
+            bool(stab.get("status")),
+            str(stab.get("msg") or ""),
+            holes,
+            stab.get("draw_img", result.draw_img),
+            raw=stab,
+        )
+
+
+def main() -> None:
+    rclpy.init()
+    node = HoleDetectNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

build_sim/arm_application/build/lib/arm_application/hole_detector.py

@@ -0,0 +1,367 @@
+"""孔定位模块：加载已训练模型，输出孔中心像素坐标与置信度。
+
+与工件定位（thread_gauge）完全分离，可独立替换模型与参数。
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any
+
+import cv2
+import numpy as np
+import yaml
+
+_PKG_SHARE_FALLBACK = Path(__file__).resolve().parents[1] / "config"
+DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
+
+# 仓库里**实际存在**的孔/工件检测模型，相对仓库根。
+# 以前这个位置上写的是 ``models/hole_insert/best.pt`` —— 文件根本不存在，属于悬空引用：
+# 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
+# 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
+# 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
+_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
+
+
+@dataclass
+class HoleDetection:
+    """单孔检测结果。"""
+
+    center_px: tuple[float, float]
+    radius_px: float
+    confidence: float
+    bbox: tuple[int, int, int, int] | None = None
+    cls_id: int = 0
+    label: str = ""  # left / right
+
+
+@dataclass
+class HoleDetectResult:
+    """孔定位整体结果。"""
+
+    status: bool
+    msg: str
+    holes: list[HoleDetection]
+    draw_img: np.ndarray
+    raw: dict[str, Any] | None = None
+
+
+def load_hole_config(path: str | Path | None = None) -> dict[str, Any]:
+    cfg_path = Path(path) if path else DEFAULT_CONFIG
+    with open(cfg_path, encoding="utf-8") as f:
+        data = yaml.safe_load(f) or {}
+    if not isinstance(data, dict):
+        raise ValueError(f"孔定位配置格式错误: {cfg_path}")
+    return data
+
+
+_HOLE_SITE_SKIP = {"weights", "workpiece_weights", "config"}
+
+
+def config_from_site(data: dict[str, Any] | None = None) -> dict[str, Any]:
+    """先取包内 yaml 默认值，再用 ``site.yaml`` 的 ``hole_detect:`` 覆盖。
+
+    以 ``site.yaml`` 为准（operator 的唯一数据源）；``data`` 参数用于测试时直接喂字典。
+    """
+    from arm_application.site_io import deep_merge, load_site_dict, ros2_root
+
+    site = data if data is not None else load_site_dict()
+    hd = dict(site.get("hole_detect") or {})
+    backend = str(hd.get("backend") or "two_stage").strip().lower()
+    cfg_path = str(hd.get("config") or "").strip()
+    if not cfg_path:
+        root = ros2_root()
+        if backend in ("two_stage", "hough_roi"):
+            cfg_path = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
+        else:
+            cfg_path = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
+    base = load_hole_config(cfg_path) if Path(cfg_path).is_file() else {}
+    overlay = {k: v for k, v in hd.items() if k not in _HOLE_SITE_SKIP and v != ""}
+    merged = deep_merge(base, overlay)
+    merged["backend"] = backend
+    return merged
+
+
+def weights_from_site(data: dict[str, Any] | None = None) -> str:
+    from arm_application.site_io import load_site_dict, ros2_root
+
+    site = data if data is not None else load_site_dict()
+    hd = site.get("hole_detect") or {}
+    paths = site.get("paths") or {}
+    backend = str(hd.get("backend") or "two_stage").strip().lower()
+    if backend in ("two_stage", "hough_roi"):
+        return str(
+            hd.get("workpiece_weights")
+            or paths.get("hole_workpiece_weights")
+            or paths.get("yolo_weights")
+            or ""
+        )
+    return str(
+        hd.get("weights")
+        or paths.get("hole_weights")
+        or str(ros2_root() / _CANONICAL_HOLE_MODEL)
+    )
+
+
+def _default_model_path() -> Path:
+    """未显式给 ``model_path`` 时的兜底模型路径。
+
+    以前这里枚举了 ``hole_insert/best.pt`` / ``hole_best.pt`` / ``yolo/detect_best.pt``，
+    三个文件在本仓库里**都不存在**，函数却会 ``return candidates[0]`` ——
+    于是永远返回一个不存在的路径（同 ``weights_from_site`` 里那个悬空的 best.pt）。
+
+    现在以 site.yaml 为准（它才是真正的来源），它给的路径存在就用它；
+    否则退到仓库里实际存在的规范模型。这样报错信息里的路径至少是对的。
+    """
+    configured = weights_from_site()
+    if configured:
+        p = Path(configured).expanduser()
+        if p.is_file():
+            return p
+
+    from arm_application.site_io import ros2_root
+
+    return ros2_root() / _CANONICAL_HOLE_MODEL
+
+
+class HoleDetector:
+    """孔定位检测器：支持 YOLO / HoughCircles 双模式，可配置切换。"""
+
+    def __init__(
+        self,
+        model_path: str | Path | None = None,
+        config: dict[str, Any] | None = None,
+        device: str | int = 0,
+    ) -> None:
+        self.cfg = config or load_hole_config()
+        self.device = device
+        self.model_path = Path(model_path) if model_path else _default_model_path()
+        self._detector = None
+        self._backend = self.cfg.get("backend", "yolo").lower()
+
+    def _load_yolo(self):
+        if self._detector is not None:
+            return self._detector
+        from arm_application.yolo_detector import YoloDetector, resolve_weights
+
+        weights = resolve_weights(
+            self.model_path,
+            backend=str(self.cfg.get("yolo_backend") or "auto"),
+        )
+        self._detector = YoloDetector(
+            weights,
+            device=str(self.device),
+            conf=float(self.cfg.get("conf", 0.25)),
+            iou=float(self.cfg.get("iou", 0.7)),
+            imgsz=int(self.cfg.get("imgsz", 640)),
+        )
+        return self._detector
+
+    def detect(self, image_bgr: np.ndarray) -> HoleDetectResult:
+        """检测图像中的孔，返回像素坐标列表。"""
+        if image_bgr is None or image_bgr.size == 0:
+            return HoleDetectResult(False, "输入图像无效", [], np.zeros((480, 640, 3), dtype=np.uint8))
+
+        draw_img = image_bgr.copy()
+        backend = self._backend
+
+        if backend in {"yolo", "faster_rcnn"}:
+            return self._detect_by_model(image_bgr, draw_img)
+        if backend in {"two_stage", "hough_roi"}:
+            return self._detect_two_stage(image_bgr, draw_img)
+        if backend == "hough":
+            return self._detect_by_hough(image_bgr, draw_img)
+        return HoleDetectResult(False, f"未知 backend: {backend}", [], draw_img)
+
+    def _detect_two_stage(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
+        from arm_application.hole_detect_hough import detect_holes
+
+        try:
+            detector = self._load_yolo()
+        except Exception as exc:
+            return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
+
+        raw = detect_holes(image, detector, config=self.cfg, device=self.device)
+        holes: list[HoleDetection] = []
+        points = raw.get("hole_points") or []
+        radii = raw.get("hole_radii") or []
+        bbox = raw.get("bbox")
+        bbox_t = tuple(int(v) for v in bbox) if bbox and len(bbox) == 4 else None
+        for i, pt in enumerate(points):
+            r = float(radii[i]) if i < len(radii) else 0.0
+            label = "left" if i == 0 else "right" if i == 1 else f"h{i + 1}"
+            holes.append(
+                HoleDetection(
+                    center_px=(float(pt[0]), float(pt[1])),
+                    radius_px=r,
+                    confidence=1.0,
+                    bbox=bbox_t,
+                    label=label,
+                )
+            )
+        draw = raw.get("draw_img", draw_img)
+        return HoleDetectResult(bool(raw.get("status")), str(raw.get("msg") or ""), holes, draw, raw=raw)
+
+    def _detect_by_model(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
+        try:
+            detector = self._load_yolo()
+        except Exception as exc:
+            return HoleDetectResult(False, f"模型加载失败: {exc}", [], draw_img)
+
+        conf = float(self.cfg.get("conf", 0.25))
+        iou = float(self.cfg.get("iou", 0.7))
+        imgsz = int(self.cfg.get("imgsz", 640))
+
+        dets = detector.detect(image, conf=conf, iou=iou, imgsz=imgsz)
+        if not dets:
+            return HoleDetectResult(False, "未检测到孔", [], draw_img)
+
+        holes: list[HoleDetection] = []
+        for d in dets:
+            x1, y1, x2, y2 = [int(v) for v in d.xyxy]
+            cx = 0.5 * (x1 + x2)
+            cy = 0.5 * (y1 + y2)
+            r = 0.5 * max(x2 - x1, y2 - y1)
+            holes.append(
+                HoleDetection(
+                    center_px=(float(cx), float(cy)),
+                    radius_px=float(r),
+                    confidence=float(d.conf),
+                    bbox=(x1, y1, x2, y2),
+                    cls_id=int(getattr(d, "cls_id", 0)),
+                )
+            )
+        return _finalize_holes(holes, draw_img, raw={"dets": dets}, cfg=self.cfg)
+
+    def _detect_by_hough(self, image: np.ndarray, draw_img: np.ndarray) -> HoleDetectResult:
+        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image
+        ksize = int(self.cfg.get("gaussian_ksize", 5))
+        if ksize % 2 == 0:
+            ksize += 1
+        blur = cv2.GaussianBlur(gray, (ksize, ksize), float(self.cfg.get("gaussian_sigma", 1.5)))
+
+        circles = cv2.HoughCircles(
+            blur,
+            cv2.HOUGH_GRADIENT,
+            dp=float(self.cfg.get("hough_dp", 1.2)),
+            minDist=float(self.cfg.get("hough_min_dist", 40)),
+            param1=float(self.cfg.get("hough_param1", 100)),
+            param2=float(self.cfg.get("hough_param2", 28)),
+            minRadius=int(self.cfg.get("hough_min_radius", 6)),
+            maxRadius=int(self.cfg.get("hough_max_radius", 45)),
+        )
+        if circles is None:
+            return HoleDetectResult(False, "HoughCircles 未检测到孔", [], draw_img)
+
+        holes: list[HoleDetection] = []
+        for c in np.asarray(circles[0], dtype=np.float64):
+            cx, cy, r = float(c[0]), float(c[1]), float(c[2])
+            holes.append(HoleDetection(center_px=(cx, cy), radius_px=r, confidence=1.0))
+        return _finalize_holes(holes, draw_img, cfg=self.cfg)
+
+    def pixel_to_camera(
+        self,
+        hole: HoleDetection,
+        depth_m: float | None = None,
+        intrinsics: dict[str, float] | None = None,
+    ) -> np.ndarray:
+        depth = float(depth_m if depth_m is not None else self.cfg.get("default_depth_m", 0.10))
+        intr = intrinsics or self.cfg.get("camera_intrinsics") or {}
+        return pixel_to_camera(hole, depth, intr)
+
+
+def pair_two_holes(holes: list[HoleDetection], cfg: dict[str, Any] | None = None) -> list[HoleDetection]:
+    """取左右两孔：优先最高置信度，再按图像 x 排序并打 left/right 标签。"""
+    cfg = cfg or {}
+    expected = int(cfg.get("expected_holes", 2))
+    if expected != 2:
+        ordered = sorted(holes, key=lambda h: h.center_px[0])
+        for i, hole in enumerate(ordered):
+            hole.label = f"h{i + 1}"
+        return ordered
+
+    if len(holes) < 2:
+        return holes
+
+    picked = sorted(holes, key=lambda h: h.confidence, reverse=True)
+    if len(picked) > 2:
+        by_x = sorted(picked, key=lambda h: h.center_px[0])
+        picked = [by_x[0], by_x[-1]]
+    else:
+        picked = picked[:2]
+    picked.sort(key=lambda h: h.center_px[0])
+    dist = float(
+        np.hypot(
+            picked[1].center_px[0] - picked[0].center_px[0],
+            picked[1].center_px[1] - picked[0].center_px[1],
+        )
+    )
+    min_sep = float(cfg.get("min_hole_sep_px", 20.0))
+    max_sep = float(cfg.get("max_hole_sep_px", 2000.0))
+    if not (min_sep <= dist <= max_sep):
+        return []
+    picked[0].label = "left"
+    picked[1].label = "right"
+    return picked
+
+
+def _draw_holes(draw_img: np.ndarray, holes: list[HoleDetection]) -> None:
+    for hole in holes:
+        cx, cy = int(round(hole.center_px[0])), int(round(hole.center_px[1]))
+        rr = max(4, int(round(hole.radius_px)))
+        cv2.circle(draw_img, (cx, cy), rr, (0, 0, 255), 2)
+        cv2.drawMarker(draw_img, (cx, cy), (0, 255, 255), cv2.MARKER_CROSS, 16, 2)
+        cv2.circle(draw_img, (cx, cy), 3, (0, 255, 255), -1)
+        name = hole.label or "hole"
+        cv2.putText(
+            draw_img,
+            f"{name} ({cx},{cy}) {hole.confidence:.2f}",
+            (cx + 12, cy - 8),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.55,
+            (0, 0, 255),
+            2,
+        )
+
+
+def _finalize_holes(
+    holes: list[HoleDetection],
+    draw_img: np.ndarray,
+    raw: dict[str, Any] | None = None,
+    cfg: dict[str, Any] | None = None,
+) -> HoleDetectResult:
+    cfg = cfg or {}
+    paired = pair_two_holes(holes, cfg)
+    if len(holes) >= 2 and not paired:
+        _draw_holes(draw_img, holes)
+        return HoleDetectResult(False, "两孔间距不在允许范围", holes, draw_img, raw=raw)
+    if not paired:
+        _draw_holes(draw_img, holes)
+        return HoleDetectResult(False, f"有效孔数量={len(holes)}，需要 2 个", holes, draw_img, raw=raw)
+    _draw_holes(draw_img, paired)
+    dist = float(
+        np.hypot(
+            paired[1].center_px[0] - paired[0].center_px[0],
+            paired[1].center_px[1] - paired[0].center_px[1],
+        )
+    )
+    msg = f"检测到双孔 left={paired[0].center_px} right={paired[1].center_px} dist={dist:.1f}px"
+    return HoleDetectResult(True, msg, paired, draw_img, raw=raw)
+
+
+def pixel_to_camera(
+    hole: HoleDetection,
+    depth_m: float,
+    intrinsics: dict[str, float],
+) -> np.ndarray:
+    """像素坐标 + 深度 -> 相机系三维坐标（m）。"""
+    fx = float(intrinsics["fx"])
+    fy = float(intrinsics["fy"])
+    cx = float(intrinsics["cx"])
+    cy = float(intrinsics["cy"])
+    u, v = hole.center_px
+    z = float(depth_m)
+    x = (u - cx) * z / fx
+    y = (v - cy) * z / fy
+    return np.array([x, y, z], dtype=float)

build_sim/arm_application/build/lib/arm_application/image_utils.py

@@ -0,0 +1,149 @@
+"""把 numpy 数组打包成 ``sensor_msgs/Image``。
+
+为什么需要
+----------
+Orisys 视触觉传感器的数据分两类：
+
+* **三力**（``fnormal`` / ``fshearx`` / ``fsheary``）—— 每帧几个标量，走子进程
+  队列几乎没有开销。
+* **图像类**（形变图 / 深度图 / 力矢量场）—— 每帧几百 KB 到几 MB。原先
+  ``tactile_worker`` 为了性能**故意不过队列**，于是这些数据从未离开子进程、
+  也从未发布到 ROS 话题上。
+
+本模块只负责最后一公里：把已经拿到的数组正确打包成 ``Image``。放在单独文件
+里是为了能脱离相机单测 —— 编码、``step``、通道顺序这些细节一旦写错，下游
+拿到的图会静默错位（图像撕裂、颜色通道互换），而不是报错。
+
+支持的形状与编码
+----------------
+===============  ==============  ==========================================
+形状              dtype           编码
+===============  ==============  ==========================================
+``(H, W)``       ``uint8``       ``mono8``
+``(H, W)``       ``uint16``      ``mono16``
+``(H, W)``       ``float32``     ``32FC1``
+``(H, W, 1)``    任意            ``mono8``
+``(H, W, 2)``    ``float32``     ``32FC2``（力矢量场：每像素 dx, dy）
+``(H, W, 3)``    ``uint8``       ``rgb8``
+``(H, W, 4)``    ``uint8``       ``rgba8``
+===============  ==============  ==========================================
+
+2 通道的 ``32FC2`` 是 ROS 里表达矢量场的标准做法，``step = W * 4 * 2``。
+"""
+
+from __future__ import annotations
+
+from typing import Optional, Tuple
+
+import numpy as np
+from sensor_msgs.msg import Image
+
+__all__ = ["ndarray_to_image", "infer_encoding", "UnsupportedImageError"]
+
+
+class UnsupportedImageError(ValueError):
+    """数组的形状/类型没有对应的 ROS 编码。
+
+    宁可显式报错也不要猜 —— 猜错的结果是下游拿到一张静默错位的图。
+    错误消息里带上实际形状与 dtype，方便对照本文档的表格。
+    """
+
+
+#: (通道数, dtype) -> ROS 编码
+_ENCODING_BY_CHANNELS = {
+    (1, np.dtype(np.uint8)): "mono8",
+    (1, np.dtype(np.uint16)): "mono16",
+    (1, np.dtype(np.float32)): "32FC1",
+    (2, np.dtype(np.float32)): "32FC2",
+    (3, np.dtype(np.uint8)): "rgb8",
+    (4, np.dtype(np.uint8)): "rgba8",
+}
+
+
+def infer_encoding(array: np.ndarray) -> str:
+    """推断 ROS 编码。推不出来时抛 :class:`UnsupportedImageError`。"""
+    arr = np.asarray(array)
+    if arr.ndim == 2:
+        channels = 1
+    elif arr.ndim == 3:
+        channels = arr.shape[2]
+    else:
+        raise UnsupportedImageError(
+            f"只支持 2D（灰度）或 3D（H,W,C）数组，收到 {arr.ndim}D，形状 {arr.shape}"
+        )
+
+    key = (channels, arr.dtype)
+    encoding = _ENCODING_BY_CHANNELS.get(key)
+    if encoding is None:
+        supported = ", ".join(
+            f"{enc}({c}通道/{d.name})"
+            for (c, d), enc in sorted(_ENCODING_BY_CHANNELS.items(), key=lambda kv: kv[1])
+        )
+        raise UnsupportedImageError(
+            f"没有 {channels} 通道 + {arr.dtype.name} 对应的 ROS 编码。"
+            f"支持: {supported}。"
+            f"（收到形状 {arr.shape}）"
+        )
+    return encoding
+
+
+def _layout(array: np.ndarray) -> Tuple[int, int, int]:
+    """返回 ``(height, width, channels)``，并把 (H,W,1) 归一化成 2D 语义。"""
+    arr = np.asarray(array)
+    if arr.ndim == 2:
+        h, w = arr.shape
+        return int(h), int(w), 1
+    h, w, c = arr.shape
+    return int(h), int(w), int(c)
+
+
+def ndarray_to_image(
+    array: np.ndarray,
+    *,
+    frame_id: str = "",
+    stamp=None,
+    encoding: Optional[str] = None,
+) -> Image:
+    """把数组打包成 ``sensor_msgs/Image``。
+
+    参数
+    ----
+    array:
+        图像或矢量场。必须是 C 连续（``np.ascontiguousarray`` 会自动处理）。
+    encoding:
+        显式指定编码以覆盖推断。仅在确实知道自己在做什么时使用 ——
+        编码与 dtype 不匹配会让下游按错误的元素大小解读 ``data``。
+    """
+    arr = np.asarray(array)
+    if encoding is None:
+        encoding = infer_encoding(arr)
+
+    height, width, channels = _layout(arr)
+
+    # (H,W,1) 在内存里与 (H,W) 连续一致，压平即可。
+    flat = np.ascontiguousarray(arr).reshape(-1)
+
+    msg = Image()
+    if stamp is not None:
+        msg.header.stamp = stamp
+    msg.header.frame_id = frame_id
+    msg.height = height
+    msg.width = width
+    msg.encoding = encoding
+    msg.is_bigendian = False
+    msg.step = int(width * channels * flat.dtype.itemsize)
+    msg.data = flat.tobytes()
+    return msg
+
+
+def expected_step(array: np.ndarray, encoding: Optional[str] = None) -> int:
+    """算出应当写入 ``Image.step`` 的值。测试与自检用。"""
+    arr = np.asarray(array)
+    _, width, channels = _layout(arr)
+    if encoding is not None:
+        channels = {"mono8": 1, "mono16": 1, "32FC1": 1, "32FC2": 2,
+                    "rgb8": 3, "rgba8": 4}.get(encoding, channels)
+        itemsize = {"mono8": 1, "mono16": 2, "32FC1": 4, "32FC2": 4,
+                    "rgb8": 1, "rgba8": 1}.get(encoding, arr.dtype.itemsize)
+        return int(width * channels * itemsize)
+    return int(width * channels * arr.dtype.itemsize)

build_sim/arm_application/build/lib/arm_application/kalman_filter.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+"""三维点的匀速模型卡尔曼滤波（坐标系为相机系）。"""
+
+from __future__ import annotations
+
+from typing import Optional
+
+import numpy as np
+
+
+class KalmanFilter3D:
+    """状态量为 ``[x, y, z, vx, vy, vz]``；观测只有位置，没有速度。"""
+
+    def __init__(
+        self,
+        *,
+        q_pos: float = 1e-4,
+        q_vel: float = 5e-3,
+        r_pos: float = 4e-4,
+    ) -> None:
+        self.q_pos = float(q_pos)
+        self.q_vel = float(q_vel)
+        self.r_pos = float(r_pos)
+        self.x = np.zeros(6, dtype=float)
+        self.P = np.eye(6, dtype=float)
+        self._t: Optional[float] = None
+        self.initialized = False
+
+    def reset(self, p: np.ndarray, t: Optional[float] = None) -> None:
+        p = np.asarray(p, dtype=float).reshape(3)
+        self.x[:] = 0.0
+        self.x[:3] = p
+        self.P = np.diag([1e-3, 1e-3, 1e-3, 1e-2, 1e-2, 1e-2])
+        self._t = t
+        self.initialized = True
+
+    @property
+    def position(self) -> np.ndarray:
+        return self.x[:3].copy()
+
+    @property
+    def velocity(self) -> np.ndarray:
+        return self.x[3:].copy()
+
+    def advance(self, t: float) -> None:
+        """把状态预测到时刻 ``t``（秒，必须单调递增）。
+
+        超过 0.5 秒的空档按"目标可能已经跑掉"处理：位置不动、速度清零，
+        否则旧速度会把位置外推得很离谱。
+        """
+        if not self.initialized or self._t is None:
+            self._t = t
+            return
+        dt = float(t - self._t)
+        if dt <= 1e-6:
+            return
+        if dt > 0.5:
+            # 空档太长——位置保持不动，速度清零。
+            self.x[3:] = 0.0
+            self._t = t
+            return
+        self._predict(dt)
+        self._t = t
+
+    def update(self, z: np.ndarray) -> None:
+        """用一次三维位置观测做量测更新。"""
+        z = np.asarray(z, dtype=float).reshape(3)
+        if not self.initialized:
+            self.reset(z)
+            return
+        H = np.zeros((3, 6), dtype=float)
+        H[0, 0] = H[1, 1] = H[2, 2] = 1.0
+        R = np.eye(3, dtype=float) * self.r_pos
+        y = z - H @ self.x
+        S = H @ self.P @ H.T + R
+        K = self.P @ H.T @ np.linalg.inv(S)
+        self.x = self.x + K @ y
+        I = np.eye(6)
+        self.P = (I - K @ H) @ self.P
+
+    def _predict(self, dt: float) -> None:
+        F = np.eye(6, dtype=float)
+        F[0, 3] = F[1, 4] = F[2, 5] = dt
+        q_p = self.q_pos
+        q_v = self.q_vel
+        Q = np.diag([q_p, q_p, q_p, q_v, q_v, q_v])
+        self.x = F @ self.x
+        self.P = F @ self.P @ F.T + Q

build_sim/arm_application/build/lib/arm_application/model_labels.py

@@ -0,0 +1,558 @@
+"""模型类别标签（class names）的统一读写。
+
+## 为什么需要这个模块
+
+推理代码需要把模型输出的**类别索引**（整数 0/1/2…）翻译成**人能看懂的名字**。
+这件事原先没有统一做法，导致同一个模型在不同后端下名字都不一样：
+
+- `.pt`：从 Ultralytics 的 `model.names` 读（来源是 checkpoint 内嵌 metadata）
+- `.onnx`：ONNX 文件里有 `names` metadata，但代码没用，而是**硬编码** `{0: "workpiece"}`
+- `.engine`：**标签直接丢了**
+
+最后一条是本模块存在的主要原因。TensorRT engine 要用 `trt.OnnxParser` 重新构建，
+而**当前安装的 TensorRT（10.13）Python API 根本没有 metadata 读写接口** ——
+`INetworkDefinition` / `IBuilderConfig` / `ICudaEngine` 上都没有任何 metadata 方法。
+所以 engine 构建完就只剩张量，标签无从附着。实测：
+
+    detect_best_0.engine   二进制里搜不到 "workpiece"  → 标签已丢
+    detect_hc90.engine     能搜到 "workpiece"          → 旧版 TRT 建的，但当前环境已无法加载
+
+（`detect_hc90` 已作为零引用模型移出仓库，这里保留这行是因为它同时印证了两件事：
+旧版 TRT 建的 engine 里其实是有标签的，以及 engine 与 TRT 版本绑死。）
+
+后果很隐蔽：`yolo_detector` 读不到名字时会退化成 `str(cid)`，也就是类名变成字符串
+`"0"`；而预览界面又不绘制类名，所以**没有任何地方看得出标签丢了**。
+
+## 解决办法：旁挂标签文件
+
+在每个权重旁边放一个同名文本文件 `<模型名>.labels.yaml`：
+
+    models/yolo/detect_best_0.onnx
+    models/yolo/detect_best_0.engine
+    models/yolo/detect_best_0.labels.yaml      ← 就是这个
+
+选文本文件而不是想办法塞进 engine，理由有二：
+
+1. **可修改**。标签是配置，不是模型的一部分。塞进 engine 就只能重建 engine 才能改。
+2. **跨格式统一**。engine / onnx / pt 三种格式共用同一份标签，不会出现
+   "换后端名字就变了"。engine 丢标签的问题也一并绕过了。
+
+标签文件是纯文本，进 git（体积小、属于配置）；权重二进制不进 git。
+
+## 读取优先级
+
+    sidecar（旁挂文件） > onnx（ONNX metadata） > model（模型自身 metadata） > placeholder
+
+`read_labels()` 会连**来源**一起返回。这点很重要：调用方可以据此提示
+"标签来自占位符，请补一个 .labels.yaml"，而不是悄悄给出一个错的类名。
+"""
+
+from __future__ import annotations
+
+import re
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Tuple, Union
+
+# 标签文件的扩展名。刻意用双后缀：既能被 *.yaml 工具识别，
+# 又能和模型自身的配置（如果有）区分开。
+LABELS_SUFFIX = ".labels.yaml"
+
+# read_labels() 的来源标记。顺序即优先级（高 → 低）。
+SOURCE_SIDECAR = "sidecar"        # 旁挂 <模型名>.labels.yaml（权威，用户可直接改）
+SOURCE_ONNX = "onnx"              # ONNX 文件内嵌 metadata
+SOURCE_MODEL = "model"            # 模型自身 metadata（Ultralytics 的 .names）
+SOURCE_PLACEHOLDER = "placeholder"  # 都没有 → 占位符 class_0/class_1…
+
+SOURCE_LABELS_ZH = {
+    SOURCE_SIDECAR: "旁挂标签文件",
+    SOURCE_ONNX: "ONNX 内嵌 metadata",
+    SOURCE_MODEL: "模型内嵌 metadata",
+    SOURCE_PLACEHOLDER: "占位符（缺标签）",
+}
+
+
+@dataclass(frozen=True)
+class LabelSet:
+    """一次标签读取的结果。
+
+    ``names`` 是索引 → 名字；``source`` 说明它从哪来（见 SOURCE_* 常量）。
+
+    调用方**应该**检查 ``source``：如果是 ``placeholder``，说明这个模型缺标签，
+    此时给出的是 ``class_0`` 这样的占位名，只适合占位显示，不该当成真名使用。
+    """
+
+    names: Dict[int, str]
+    source: str
+    path: Optional[Path] = None  # 实际读到的文件（placeholder 时为 None）
+
+    @property
+    def is_placeholder(self) -> bool:
+        return self.source == SOURCE_PLACEHOLDER
+
+    def get(self, cid: int, default: Optional[str] = None) -> str:
+        if cid in self.names:
+            return self.names[cid]
+        return default if default is not None else f"class_{cid}"
+
+    def sorted_names(self) -> List[str]:
+        return [self.names[k] for k in sorted(self.names)]
+
+
+# ---------------------------------------------------------------------------
+# 路径
+# ---------------------------------------------------------------------------
+
+
+def labels_path_for(weights: Union[str, Path]) -> Path:
+    """给出权重对应的旁挂标签文件路径。
+
+    只换后缀，目录不变：``models/yolo/detect_best_0.engine`` →
+    ``models/yolo/detect_best_0.labels.yaml``。
+
+    这样无论 ``resolve_weights()`` 最后选中了 .pt/.onnx/.engine 中的哪一个，
+    算出来的标签路径都相同 —— 一份标签服务同一个模型的所有格式。
+    """
+    p = Path(weights).expanduser()
+    # 用 stem 手工拼接，而不是 with_suffix('.labels.yaml')：
+    # 后者对 `a.b.onnx` 这类含点文件名容易踩坑，手工拼没有歧义。
+    return p.parent / (p.stem + LABELS_SUFFIX)
+
+
+# ---------------------------------------------------------------------------
+# 解析：把各种形状的标签定义统一成 {索引: 名字}
+# ---------------------------------------------------------------------------
+
+
+def _clean(text: object) -> str:
+    # None 必须先拦掉：str(None) == "None" 是 truthy 的，
+    # 直接 str() 会让空项变成一个字面量为 "None" 的类名。
+    if text is None:
+        return ""
+    return str(text).strip()
+
+
+def _try_parse_structured(text: str) -> Optional[Dict[int, str]]:
+    """尝试把字符串当成 dict/list 的字面量解析。
+
+    为什么需要：ONNX metadata 里的 ``names`` 存的**不是** JSON，也不是逗号列表，
+    而是 Python 字典的 repr（单引号）：
+
+        "{0: 'workpiece'}"
+
+    Ultralytics 是直接 ``str(names)`` 写进去的，所以 ``json.loads`` 会失败。
+    两种都试一遍（JSON 标准、literal_eval 兜底），都不行才交给逗号解析。
+    ``ast.literal_eval`` 只解析字面量、不执行代码，是安全的。
+    """
+    s = text.strip()
+    if not s:
+        return None
+    import ast
+    import json
+
+    for loader in (json.loads, ast.literal_eval):
+        try:
+            obj = loader(s)
+        except Exception:  # noqa: BLE001
+            continue
+        if isinstance(obj, (dict, list, tuple)):
+            return parse_names_field(obj)
+    return None
+
+
+def _parse_freeform_text(text: str) -> Dict[int, str]:
+    """解析"自由文本"形式的标签内容。
+
+    两种情况要分开，否则会互相吃掉：
+
+    - 一行一个名字（有人把 ``classes.txt`` 直接改名成 ``.labels.yaml``）：
+      ``"a\\nb"`` → ``{0:'a', 1:'b'}``
+    - 单行逗号串：``"a, b"`` → ``{0:'a', 1:'b'}``
+
+    判据是"有没有多行"。只有一行时才按逗号切，
+    否则 ``"a, b"`` 这种文件名里带逗号的写法会被切错。
+    """
+    items = [
+        ln.strip()
+        for ln in text.splitlines()
+        if ln.strip() and not ln.strip().startswith("#")
+    ]
+    if len(items) > 1:
+        return {i: n for i, n in enumerate(items)}
+    return parse_label_list(text)
+
+
+def parse_names_field(value: object) -> Dict[int, str]:
+    """把 YAML/metadata 里的 ``names`` 字段规范成 ``{int: str}``。
+
+    容忍三种写法，因为它们各自都很常见：
+
+    - 字典（Ultralytics data.yaml 的写法）：``{0: workpiece, 1: hole}``
+    - 列表（本模块写出的规范形式，索引即顺序）：``[workpiece, hole]``
+    - 逗号分隔的字符串：``"workpiece, hole"``
+
+    跳过空项但**保留其索引位置**，这样 ``[a, "", c]`` 不会让 c 变成 1 号。
+    无法解析成整数键的字典项会被忽略（不抛异常 —— 标签读不出来不该让程序崩）。
+    """
+    if value is None:
+        return {}
+    if isinstance(value, dict):
+        out: Dict[int, str] = {}
+        for k, v in value.items():
+            try:
+                idx = int(k)
+            except (TypeError, ValueError):
+                continue
+            name = _clean(v)
+            if name:
+                out[idx] = name
+        return out
+    if isinstance(value, (list, tuple)):
+        out = {}
+        for i, v in enumerate(value):
+            name = _clean(v)
+            if name:
+                out[i] = name
+        return out
+    if isinstance(value, str):
+        # 字符串可能是三种东西：dict/list 的字面量（ONNX metadata）、
+        # 逗号列表、或者多行文本。先试结构化的，再退到逗号/多行解析。
+        structured = _try_parse_structured(value)
+        if structured is not None:
+            return structured
+        return _parse_freeform_text(value)
+    return {}
+
+
+def parse_label_list(text: str) -> Dict[int, str]:
+    """解析逗号（或中文逗号、分号）分隔的类别名。
+
+    ``"workpiece, hole"`` → ``{0: 'workpiece', 1: 'hole'}``
+
+    同时兼容 ``"0:workpiece, 1:hole"`` 这种带显式索引的写法，
+    因为用户从别处拷配置过来时经常带着索引。空项被跳过。
+    """
+    if not text or not text.strip():
+        return {}
+    parts = [p for p in re.split(r"[,，;；\n]", text)]
+    out: Dict[int, str] = {}
+    next_idx = 0
+    for raw in parts:
+        item = raw.strip()
+        if not item:
+            continue
+        m = re.match(r"^\s*(\d+)\s*[:：]\s*(.+)$", item)
+        if m:
+            idx = int(m.group(1))
+            name = m.group(2).strip()
+            if name:
+                out[idx] = name
+                next_idx = max(next_idx, idx + 1)
+            continue
+        out[next_idx] = item
+        next_idx += 1
+    return out
+
+
+def read_label_text_file(path: Union[str, Path]) -> Dict[int, str]:
+    """读纯文本标签文件：一行一个类名（YOLO 的 classes.txt 就是这个格式）。
+
+    空行和 ``#`` 开头的注释行被忽略，且**不占索引**。
+    这样用户可以在文件里随意加注释而不破坏索引对应关系。
+    """
+    p = Path(path).expanduser()
+    out: Dict[int, str] = {}
+    idx = 0
+    with p.open("r", encoding="utf-8") as f:
+        for line in f:
+            s = line.strip()
+            if not s or s.startswith("#"):
+                continue
+            out[idx] = s
+            idx += 1
+    return out
+
+
+def placeholder_labels(nc: int) -> Dict[int, str]:
+    """没有标签可用时的占位名。
+
+    刻意用 ``class_0`` 而不是直接回退成字符串形式的数字 ``"0"``：
+    后者和"真的有一个叫 0 的类"无法区分，正是原先难以发现问题所在。
+    ``class_`` 前缀一眼就能看出是没配标签。
+    """
+    n = max(0, int(nc))
+    return {i: f"class_{i}" for i in range(n)}
+
+
+# ---------------------------------------------------------------------------
+# 旁挂标签文件读写
+# ---------------------------------------------------------------------------
+
+
+def format_labels_yaml(names: Dict[int, str], model: Optional[str] = None) -> str:
+    """生成旁挂标签文件的内容（带使用说明的注释头）。
+
+    注释头是刻意保留的：这个文件会被不熟悉的人直接打开编辑，
+    把"第几行 = 第几类"写在文件里比写在文档里有效得多。
+    """
+    lines: List[str] = []
+    lines.append("# 模型类别标签（class names）")
+    lines.append("#")
+    lines.append("# 一个模型对应一份标签，放在权重旁边、同名前缀：")
+    lines.append("#   detect_best_0.engine  ← 权重")
+    lines.append("#   detect_best_0.labels.yaml  ← 本文件")
+    lines.append("#")
+    lines.append("# 写法：names 下面一行一个类名，第 1 行对应类别 0，第 2 行对应类别 1，依次类推。")
+    lines.append("# 改完保存即生效，不需要重新转换模型、也不用重启环境（重跑节点即可）。")
+    lines.append("#")
+    lines.append("# 命令行查看/修改（推荐，不用手改本文件）：")
+    lines.append("#   bash ~/ros2/scripts/weights.sh labels <模型名>")
+    lines.append("#   bash ~/ros2/scripts/weights.sh labels <模型名> --set \"工件,通止规\"")
+    lines.append("")
+    if model:
+        # 记下所属模型，便于发现"标签文件被拷给了另一个模型"这种错配。
+        lines.append(f"model: {model}")
+    lines.append("names:")
+    if names:
+        for k in sorted(names):
+            lines.append(f"  - {names[k]}")
+    else:
+        lines.append("  []")
+    return "\n".join(lines) + "\n"
+
+
+def write_labels(
+    weights: Union[str, Path],
+    names: Dict[int, str],
+    *,
+    path: Optional[Union[str, Path]] = None,
+) -> Path:
+    """把标签写到权重旁边的旁挂文件，返回写入路径。"""
+    dst = Path(path).expanduser() if path else labels_path_for(weights)
+    dst.parent.mkdir(parents=True, exist_ok=True)
+    model = Path(weights).stem
+    dst.write_text(format_labels_yaml(names, model=model), encoding="utf-8")
+    return dst
+
+
+def read_label_file(path: Union[str, Path]) -> Optional[Dict[int, str]]:
+    """读旁挂标签文件。文件不存在或内容为空返回 None（不抛异常）。"""
+    p = Path(path).expanduser()
+    if not p.is_file():
+        return None
+    try:
+        raw = p.read_text(encoding="utf-8")
+    except Exception:  # noqa: BLE001
+        return None
+    try:
+        import yaml
+
+        data = yaml.safe_load(raw)
+    except Exception:  # noqa: BLE001
+        # 文件存在但读不动（语法错、编码错、权限）。返回 None 让调用方按
+        # "没有标签"处理，而不是让整个推理进程崩掉 —— 标签缺失不该是致命的。
+        return None
+
+    if isinstance(data, dict):
+        # 规范形式：names: [...]
+        names = parse_names_field(data.get("names"))
+        if names:
+            return names
+        # 容忍 {0: 'a', 1: 'b'} 这种"顶层直接就是索引字典"的写法（Ultralytics data.yaml）
+        names = {
+            int(k): str(v).strip()
+            for k, v in data.items()
+            if str(k).lstrip("-").isdigit() and str(v).strip()
+        }
+        # 是合法 YAML 字典却读不出类别名 → 就是没有标签。
+        # 这里**不能**再回落到"按行读文件"，否则会把 `names: []` 里的
+        # 'names:' 这几个字当成类名。
+        return names or None
+
+    if isinstance(data, (list, tuple)):
+        return parse_names_field(data) or None
+
+    if isinstance(data, str):
+        # 整个文件就是一个标量 —— 可能是"改名过来的 classes.txt"。
+        # 注意必须用**原始文本**：YAML 会把多行纯标量折成一行
+        # （"a\nb\n" → "a b"），用 data 的话换行已经没了，
+        # "一行一个类名"会被当成一个带空格的长名字。
+        return _parse_freeform_text(raw) or None
+
+    # 空文件 / 只有注释 / 其它标量类型：按行读（能正确处理注释行）。
+    try:
+        return read_label_text_file(p) or None
+    except Exception:  # noqa: BLE001
+        return None
+
+
+def label_file_model(path: Union[str, Path]) -> Optional[str]:
+    """读旁挂标签文件里记录的 ``model:`` 字段（用于发现标签错配）。"""
+    p = Path(path).expanduser()
+    if not p.is_file():
+        return None
+    try:
+        import yaml
+
+        data = yaml.safe_load(p.read_text(encoding="utf-8"))
+    except Exception:  # noqa: BLE001
+        return None
+    if isinstance(data, dict):
+        v = data.get("model")
+        return _clean(v) if v else None
+    return None
+
+
+# ---------------------------------------------------------------------------
+# 从模型自身读标签
+# ---------------------------------------------------------------------------
+
+
+def onnx_metadata(path: Union[str, Path]) -> Dict[str, str]:
+    """读 ONNX 的 ``metadata_props``（全是字符串），读不到返回空字典。
+
+    用 ``load_external_data=False``：只读图结构不读权重，对 38MB 的模型也是毫秒级。
+    Ultralytics 导出时会写入 ``names`` / ``imgsz`` / ``nc`` / ``task`` 等。
+    """
+    p = Path(path).expanduser()
+    if not p.is_file():
+        return {}
+    try:
+        import onnx
+
+        m = onnx.load(str(p), load_external_data=False)
+    except Exception:  # noqa: BLE001
+        return {}
+    return {x.key: x.value for x in m.metadata_props}
+
+
+def labels_from_onnx(path: Union[str, Path]) -> Optional[Tuple[Dict[int, str], int]]:
+    """从 ONNX 的 ``metadata_props`` 读 ``names`` 与类别数 ``nc``。
+
+    返回 ``(names, nc)``；读不到返回 None。
+
+    注意 ``names`` 在 ONNX 里存的是 **Python 字典的 repr 字符串**（单引号），
+    形如 ``"{0: 'workpiece'}"`` —— 不是 JSON。解析细节见 ``parse_names_field``。
+    """
+    meta = onnx_metadata(path)
+    if not meta:
+        return None
+
+    names = parse_names_field(meta.get("names"))
+    nc = 0
+    raw_nc = meta.get("nc")
+    if raw_nc is not None:
+        try:
+            nc = int(str(raw_nc).strip())
+        except (TypeError, ValueError):
+            nc = 0
+    if not nc and names:
+        nc = max(names) + 1
+    return (names, nc) if names else None
+
+
+def labels_from_model(path: Union[str, Path]) -> Optional[Tuple[Dict[int, str], int]]:
+    """从模型自身 metadata 读标签（走 Ultralytics，主要给 .pt 用）。
+
+    对 .engine / .onnx 也能用（Ultralytics 会自己反序列化），但代价高：
+    要加载整个模型。所以 ``read_labels()`` 只在必要时才调它。
+
+    Ultralytics 的 model 对象除了 ``names`` 还有 ``nc``；拿不到 nc 时用 names 推断。
+    """
+    p = Path(path).expanduser()
+    if not p.is_file():
+        return None
+    try:
+        from ultralytics import YOLO
+
+        model = YOLO(str(p), task="detect")
+        raw = getattr(model, "names", None) or {}
+        names = parse_names_field(raw)
+        nc = 0
+        for attr in ("nc",):
+            try:
+                nc = int(getattr(model, attr) or 0)
+            except Exception:  # noqa: BLE001
+                nc = 0
+        if not nc and names:
+            nc = max(names) + 1
+        return (names, nc) if names else None
+    except Exception:  # noqa: BLE001
+        return None
+
+
+# ---------------------------------------------------------------------------
+# 统一入口
+# ---------------------------------------------------------------------------
+
+
+def read_labels(
+    weights: Union[str, Path],
+    *,
+    allow_model_metadata: bool = True,
+    nc_hint: int = 0,
+) -> LabelSet:
+    """按优先级读出一个模型该用的标签。
+
+    优先级：旁挂文件 > ONNX metadata > 模型自身 metadata > 占位符。
+
+    旁挂优先是刻意的：它是用户能直接编辑的那一份，必须覆盖模型内嵌的旧值，
+    否则"我明明改了标签为什么不生效"就会变成一个很难查的问题。
+
+    绝不抛异常。读不到就返回占位符并把原因记在 ``source`` 里，
+    由调用方决定是记一条警告还是直接显示。
+    """
+    p = Path(weights).expanduser()
+
+    side = labels_path_for(p)
+    names = read_label_file(side)
+    if names:
+        return LabelSet(names=names, source=SOURCE_SIDECAR, path=side)
+
+    suffix = p.suffix.lower()
+    # .pt 的标签只可能来自模型自身；.onnx 先看内嵌 metadata（快）；
+    # .engine / .trt 没有可用 metadata（见模块头说明），直接落到占位符或模型加载。
+    if suffix == ".onnx":
+        got = labels_from_onnx(p)
+        if got:
+            return LabelSet(names=got[0], source=SOURCE_ONNX, path=p)
+    elif suffix in (".pt", ".pth"):
+        got = labels_from_model(p)
+        if got:
+            return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
+
+    if allow_model_metadata and suffix in (".engine", ".trt"):
+        # engine 一般读不到（当前 TRT 没 metadata API），但也别直接放弃：
+        # 若用户是用 Ultralytics 导出的 engine，metadata 可能在。
+        got = labels_from_model(p)
+        if got:
+            return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
+
+    return LabelSet(names=placeholder_labels(nc_hint), source=SOURCE_PLACEHOLDER, path=None)
+
+
+def describe(labels: LabelSet) -> str:
+    """一句人能看懂的说明，用于日志：``workpiece (来源: 旁挂标签文件)``。"""
+    names = ", ".join(labels.sorted_names()) or "(空)"
+    src = SOURCE_LABELS_ZH.get(labels.source, labels.source)
+    return f"{names}  (来源: {src})"
+
+
+def label_file_status(
+    weights: Union[str, Path],
+) -> Tuple[bool, Optional[Path], Optional[str]]:
+    """检查旁挂标签文件状态，供 doctor 用。
+
+    返回 ``(存在, 路径, 提示)``；``提示`` 为 None 表示没有问题。
+    """
+    p = Path(weights).expanduser()
+    side = labels_path_for(p)
+    if not side.is_file():
+        return False, side, f"缺标签文件：{side.name}"
+    recorded = label_file_model(side)
+    if recorded and Path(weights).stem != recorded:
+        return True, side, (
+            f"标签文件记录的 model 是 '{recorded}'，但当前权重是 "
+            f"'{Path(weights).stem}' —— 可能是拷错了标签文件"
+        )
+    return True, side, None

build_sim/arm_application/build/lib/arm_application/move_to_pose_client.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+"""``arm_msgs/action/MoveToPose``（笛卡尔目标）的示例客户端。
+
+等你有了 MoveIt 或自己的 IK 求解 action server 后，把它接上去即可。
+"""
+
+from __future__ import annotations
+
+import sys
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+
+from arm_msgs.action import MoveToPose
+
+from arm_application.ros_node import TolerantNode
+
+
+class MoveToPoseClient(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('move_to_pose_client')
+        self._client = ActionClient(self, MoveToPose, 'move_to_pose')
+
+    def send(self, x: float, y: float, z: float) -> None:
+        if not self._client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error(
+                'move_to_pose action 不可用。'
+                '请启动一个实现了它的笛卡尔规划器（例如 MoveIt）。'
+            )
+            return
+
+        goal = MoveToPose.Goal()
+        goal.target_pose = PoseStamped()
+        goal.target_pose.header.frame_id = 'base_link'
+        goal.target_pose.header.stamp = self.get_clock().now().to_msg()
+        goal.target_pose.pose.position.x = x
+        goal.target_pose.pose.position.y = y
+        goal.target_pose.pose.position.z = z
+        # 单位姿态（不旋转）
+        goal.target_pose.pose.orientation.w = 1.0
+        goal.velocity_scale = 0.3
+        goal.acceleration_scale = 0.3
+        goal.avoid_collisions = True
+
+        self.get_logger().info(f'发送笛卡尔目标：({x:.3f}, {y:.3f}, {z:.3f})')
+        future = self._client.send_goal_async(goal)
+        rclpy.spin_until_future_complete(self, future)
+        handle = future.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('目标被拒绝')
+            return
+
+        result_future = handle.get_result_async()
+        rclpy.spin_until_future_complete(self, result_future)
+        result = result_future.result().result
+        self.get_logger().info(f'success={result.success} msg={result.message}')
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = MoveToPoseClient()
+    # 默认演示位姿；可通过命令行覆盖：x y z
+    vals = [0.35, 0.0, 0.35]
+    if len(sys.argv) >= 4:
+        vals = [float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])]
+    node.send(*vals)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/onnx_workpiece.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+"""Direct ONNX Runtime inference for Ultralytics end2end detect models.
+
+为什么不走 ultralytics.YOLO：它对 .onnx 会按发行包名检查 onnxruntime，
+环境装的是 onnxruntime-gpu 时会触发 pip AutoUpdate（可能卡死/破坏环境）。
+本模块用 onnxruntime 直接推理，显式 CPU provider，一次性拍照场景足够快。
+
+模型要求：Ultralytics 导出时 end2end=True（NMS 已融合），
+输出形状 [1, N, 6] = x1, y1, x2, y2, conf, cls（letterbox 坐标系）。
+detect_best_1.onnx 满足（metadata: end2end=True, nms=False）。
+"""
+
+from __future__ import annotations
+
+import logging
+from pathlib import Path
+from typing import List, Optional, Union
+
+import numpy as np
+
+from arm_application.model_labels import (
+    SOURCE_MODEL,
+    LabelSet,
+    parse_names_field,
+    read_labels,
+)
+from arm_application.yolo_detector import Detection
+
+logger = logging.getLogger(__name__)
+
+
+class OnnxWorkpieceDetector:
+    """给 detect_holes() 用的最小接口：detect(image, conf, iou, imgsz)。"""
+
+    def __init__(
+        self,
+        weights: Union[str, Path],
+        *,
+        conf: float = 0.25,
+        imgsz: int = 640,
+        class_names: Optional[dict] = None,
+    ) -> None:
+        import onnxruntime as ort
+
+        path = Path(weights).expanduser()
+        if not path.is_file():
+            raise FileNotFoundError(f"ONNX weights not found: {path}")
+        so = ort.SessionOptions()
+        so.log_severity_level = 3  # 关掉 ORT 刷屏告警
+        self._sess = ort.InferenceSession(
+            str(path), sess_options=so, providers=["CPUExecutionProvider"]
+        )
+        inp = self._sess.get_inputs()[0]
+        self._iname = inp.name
+        shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape]
+        self._in_h, self._in_w = int(shape[-2]), int(shape[-1])
+        self.conf = float(conf)
+        self.imgsz = int(imgsz)
+        # 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。
+        # 原先这里是硬编码 {0: "workpiece"}，等于把类名写死在代码里 ——
+        # 换一个多类模型就会展示错误的名字，而且没人改得动。
+        if class_names:
+            names = parse_names_field(class_names)
+            self._labels = LabelSet(names=names, source=SOURCE_MODEL, path=path)
+        else:
+            self._labels = read_labels(path, allow_model_metadata=False)
+            if self._labels.is_placeholder:
+                logger.warning(
+                    "ONNX %s 没有类别标签，将用占位名显示。"
+                    "补法: bash ~/ros2/scripts/weights.sh labels %s --from-model",
+                    path.name,
+                    path.stem,
+                )
+        # 兼容旧属性名，避免外部代码引用时报 AttributeError。
+        self._names = dict(self._labels.names)
+
+    def _letterbox(self, img: np.ndarray):
+        h, w = img.shape[:2]
+        scale = min(self._in_w / w, self._in_h / h)
+        nw, nh = int(round(w * scale)), int(round(h * scale))
+        import cv2
+
+        resized = cv2.resize(img, (nw, nh), interpolation=cv2.INTER_LINEAR)
+        canvas = np.full((self._in_h, self._in_w, 3), 114, dtype=np.uint8)
+        top = (self._in_h - nh) // 2
+        left = (self._in_w - nw) // 2
+        canvas[top : top + nh, left : left + nw] = resized
+        return canvas, scale, left, top
+
+    def detect(
+        self,
+        image_bgr: np.ndarray,
+        conf: Optional[float] = None,
+        iou: Optional[float] = None,   # end2end 模型已内置 NMS，忽略
+        imgsz: Optional[int] = None,   # 输入尺寸由模型决定，忽略
+        **_kwargs,
+    ) -> List[Detection]:
+        del iou, imgsz
+        thr = float(conf if conf is not None else self.conf)
+        canvas, scale, left, top = self._letterbox(image_bgr)
+        blob = canvas[:, :, ::-1].transpose(2, 0, 1)[None].astype(np.float32) / 255.0
+        out = self._sess.run(None, {self._iname: blob})[0]  # [1, N, 6]
+        dets: List[Detection] = []
+        if out is None or out.size == 0:
+            return dets
+        arr = np.asarray(out).reshape(-1, out.shape[-1])
+        h, w = image_bgr.shape[:2]
+        for row in arr:
+            if row.shape[0] < 6:
+                continue
+            x1, y1, x2, y2, score, cls = [float(v) for v in row[:6]]
+            if score < thr:
+                continue
+            # letterbox 坐标 → 原图坐标
+            xa = (x1 - left) / scale
+            ya = (y1 - top) / scale
+            xb = (x2 - left) / scale
+            yb = (y2 - top) / scale
+            xyxy = np.array(
+                [
+                    float(np.clip(xa, 0, w - 1)),
+                    float(np.clip(ya, 0, h - 1)),
+                    float(np.clip(xb, 0, w - 1)),
+                    float(np.clip(yb, 0, h - 1)),
+                ]
+            )
+            cid = int(cls)
+            dets.append(
+                Detection(
+                    xyxy=xyxy,
+                    conf=float(score),
+                    cls_id=cid,
+                    cls_name=self._labels.get(cid),
+                )
+            )
+        dets.sort(key=lambda d: d.conf, reverse=True)
+        return dets

build_sim/arm_application/build/lib/arm_application/orisys_tactile.py

@@ -0,0 +1,587 @@
+"""Orisys 视触觉适配器。SDK 通过 pip 安装；设备 id 存放在 ``site.yaml`` 里。"""
+
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Any, Optional
+
+import numpy as np
+
+from arm_application.site_io import ros2_root
+from arm_application.tactile_interface import TactileCommand, TactileInterface, TactileSample
+
+
+@dataclass
+class OrisysSensorConfig:
+    vid_src: int | str = 0
+    config_name: str = "dd03"
+    rotation_config_path: str = ""
+    cuda: bool = True
+    isstitch: bool = True
+    backend: str = "auto"
+    motion_threshold: float = 0.0
+    target_fps: float = 30.0
+
+
+@dataclass
+class OrisysDualConfig:
+    left: OrisysSensorConfig
+    right: OrisysSensorConfig
+    baseline_seconds: float = 2.0
+    filter_alpha: float = 0.25
+    backend: str = "orisys"  # orisys | mock
+    image_hz: float = 0.0
+    """视触觉图像（深度/形变图）的发布频率。0 = 不发布图像。
+
+    为什么默认关：图像每帧几百 KB，跨进程序列化 + ROS 发布的开销远大于三力。
+    三力路径必须跑在 30fps（接触判据靠它），图像只是给人看和给算法备用，
+    默认不开以免动到已经调好的三力时序。装配现场按需在 site.yaml 打开。"""
+
+
+def resolve_orisys_config(config_name: str, config_file: str = "") -> str:
+    """把型号名解析成实际的 config.json 路径。
+
+    查找顺序（先命中先用）：
+
+    1. 显式传入的 ``config_file``
+    2. 参数本身就是存在的文件路径
+    3. ``devices/sensors/orisys/models/<型号>/config.json`` —— 型号配置随 SDK 放，
+       这是现在唯一推荐的位置
+    4. 老位置 ``src/arm_application/config/orisys/<型号>.json``（兼容保留：
+       站点自己放的定制配置可能还在这里）
+    5. Orisys 包自带的 ``configs/<型号>.json``
+    6. 都没找到就把原样返回，交给 SDK 自己解析（它支持内置型号名）
+    """
+    if config_file:
+        p = Path(config_file).expanduser()
+        if p.is_file():
+            return str(p)
+    raw = (config_name or "dd03").strip()
+    p = Path(raw).expanduser()
+    if p.is_file():
+        return str(p)
+
+    root = ros2_root()
+    for cand in (
+        root / "devices" / "sensors" / "orisys" / "models" / raw / "config.json",
+        root / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json",
+    ):
+        if cand.is_file():
+            return str(cand)
+    try:
+        import orisys
+
+        pkg_root = Path(orisys.__file__).resolve().parent.parent
+        for cand in (pkg_root / "configs" / f"{raw}.json", pkg_root / "config" / f"{raw}.json"):
+            if cand.is_file():
+                return str(cand)
+    except Exception:
+        pass
+    return raw
+
+
+def load_orisys_from_site(data: dict) -> OrisysDualConfig:
+    t = data.get("tactile") or {}
+    left_raw = t.get("left") if isinstance(t.get("left"), dict) else {}
+    right_raw = t.get("right") if isinstance(t.get("right"), dict) else {}
+    cfg_file = str(t.get("config_file") or "")
+    # model 是设备契约层选型号用的键（对应 models/<型号>/ 目录名）；
+    # config 是它的旧名，仅为兼容老站点文件保留，站点里应统一用 model。
+    cfg_name = str(t.get("model") or t.get("config") or "dd03")
+    resolved = resolve_orisys_config(cfg_name, cfg_file)
+    common = dict(
+        config_name=resolved,
+        rotation_config_path=str(t.get("rotation_config") or ""),
+        cuda=bool(t.get("cuda", True)),
+        isstitch=bool(t.get("isstitch", True)),
+        backend=str(t.get("infer_backend") or t.get("backend_infer") or "auto"),
+        motion_threshold=float(t.get("motion_threshold") or 0.0),
+        target_fps=float(t.get("target_fps") or 30.0),
+    )
+    left_vid = left_raw.get("video", t.get("left_video", 0))
+    right_vid = right_raw.get("video", t.get("right_video", 2))
+    left = OrisysSensorConfig(vid_src=_parse_video(left_vid), **common)
+    right = OrisysSensorConfig(vid_src=_parse_video(right_vid), **common)
+    if left_raw.get("config") or left_raw.get("config_file"):
+        left.config_name = resolve_orisys_config(
+            str(left_raw.get("config") or cfg_name),
+            str(left_raw.get("config_file") or ""),
+        )
+    if right_raw.get("config") or right_raw.get("config_file"):
+        right.config_name = resolve_orisys_config(
+            str(right_raw.get("config") or cfg_name),
+            str(right_raw.get("config_file") or ""),
+        )
+    left.rotation_config_path = str(
+        left_raw.get("rotation_config") or t.get("left_rotation") or t.get("rotation_config") or ""
+    )
+    right.rotation_config_path = str(
+        right_raw.get("rotation_config") or t.get("right_rotation") or t.get("rotation_config") or ""
+    )
+    return OrisysDualConfig(
+        left=left,
+        right=right,
+        baseline_seconds=float(t.get("baseline_seconds") or 2.0),
+        filter_alpha=float(t.get("filter_alpha") or 0.25),
+        backend=str(t.get("backend") or "orisys"),
+        image_hz=float(t.get("image_hz") or 0.0),
+    )
+
+
+def read_vector_map(sensor: Any) -> Optional[np.ndarray]:
+    """尝试从传感器读出力矢量场（每像素的切向力/位移矢量）。
+
+    为什么要"尝试"
+    --------------
+    Orisys 的 Python 绑定是编译过的，公开接口里没有一个明确叫
+    "force vector map" 的方法。按语义最接近的是 ``compute_flow()``
+    （形变光流 ≈ 切向位移场 ≈ 力矢量场），其次是 ``compute_hhd()``。
+    不同 SDK 版本这两个方法的返回值也不一样 —— 有的是 ``(H,W,2)`` 数组，
+    有的返回 ``bool``（只给"形变是否超阈值"）或可视化图。
+
+    所以这里逐个试，只接受真正是二维以上数组的结果；都拿不到就返回 ``None``，
+    上层跳过该话题而不是发一张假图。哪一路成功会打到日志里，便于现场核对。
+    """
+    for name in ("compute_flow", "compute_hhd", "compute_deformation"):
+        fn = getattr(sensor, name, None)
+        if fn is None:
+            continue
+        try:
+            result = fn()
+        except Exception:
+            continue
+        if isinstance(result, np.ndarray) and result.ndim >= 2 and result.size:
+            return result
+    return None
+
+
+def _parse_video(value) -> int | str:
+    if isinstance(value, int):
+        return value
+    text = str(value).strip()
+    if text.isdigit():
+        return int(text)
+    return text
+
+
+class OrisysTactile(TactileInterface):
+    name = "orisys_tactile"
+
+    def __init__(self, config: OrisysDualConfig | None = None) -> None:
+        self.cfg = config or OrisysDualConfig(
+            left=OrisysSensorConfig(vid_src=0),
+            right=OrisysSensorConfig(vid_src=2),
+        )
+        self._sensors: list[dict[str, Any]] = []
+        self._baseline: dict[str, float] = {}
+        self._connected = False
+
+    def connect(self, sides=("left", "right")) -> None:
+        """连接指定侧的传感器。注意：SDK 单进程双开会严重掉帧（30→0.8fps），
+        双指请拆两个进程各连一侧（tactile.launch.py 已是双进程）。"""
+        try:
+            import orisys
+        except ImportError as exc:
+            raise RuntimeError(
+                "缺少 orisys SDK。请执行 pip install -e /path/to/orisys-sdk  "
+                "（git.orisys-tech.cn/orisys/test）。配置依旧从 ``site.yaml`` 的 ``tactile`` 段读取。"
+            ) from exc
+
+        self._sensors = []
+        for side, cfg in (("left", self.cfg.left), ("right", self.cfg.right)):
+            if side not in sides:
+                continue
+            kwargs = dict(
+                cuda=cfg.cuda,
+                isstitch=cfg.isstitch,
+                verbose=False,
+                config_name=cfg.config_name,
+                backend=cfg.backend,
+            )
+            if cfg.rotation_config_path:
+                kwargs["rotation_config_path"] = cfg.rotation_config_path
+            sensor = orisys.Sensor(cfg.vid_src, **kwargs)
+            self._sensors.append({"side": side, "sensor": sensor, "cfg": cfg})
+        self._calibrate_baseline()
+        self._connected = True
+
+    def read_side(self, side: str, want_vector: bool = False) -> dict[str, Any]:
+        """读指定侧（单侧进程模式）；未连接/未知侧返回全零。
+
+        ``want_vector=True`` 时额外计算力矢量场（逐像素运算，较贵）——
+        只应在需要发布矢量图的那些帧上打开。
+        """
+        if not self._connected:
+            return self._empty_sample(side)
+        for item in self._sensors:
+            if item["side"] == side:
+                return self._read_single(item, want_vector=want_vector)
+        return self._empty_sample(side)
+
+    def disconnect(self) -> None:
+        for item in self._sensors:
+            try:
+                item["sensor"].disconnect()
+            except Exception:
+                pass
+        self._sensors = []
+        self._connected = False
+
+    def reset(self, hole_id: int) -> None:
+        del hole_id
+
+    def step(self, sample: TactileSample) -> TactileCommand:
+        del sample
+        return TactileCommand(
+            delta_xyz=np.zeros(3),
+            delta_rpy=np.zeros(3),
+            done=False,
+            ok=True,
+            message="orisys step",
+        )
+
+    def read_sample(self) -> TactileSample:
+        if not self._connected or len(self._sensors) != 2:
+            return TactileSample(
+                timestamp=time.time(),
+                force_xyz=np.zeros(3),
+                torque_xyz=np.zeros(3),
+                contact=False,
+            )
+        left = self._read_single(self._sensors[0])
+        right = self._read_single(self._sensors[1])
+        force_xyz = np.array(
+            [
+                (left["fnormal"] + right["fnormal"]) * 0.5,
+                (left["fshearx"] + right["fshearx"]) * 0.5,
+                (left["fsheary"] + right["fsheary"]) * 0.5,
+            ]
+        )
+        torque_xyz = np.array(
+            [
+                right["fnormal"] - left["fnormal"],
+                right["fshearx"] - left["fshearx"],
+                right["fsheary"] - left["fsheary"],
+            ]
+        )
+        return TactileSample(
+            timestamp=time.time(),
+            force_xyz=force_xyz,
+            torque_xyz=torque_xyz,
+            contact=bool(left["is_contact"] or right["is_contact"]),
+            embedding=np.array(
+                [
+                    left["fnormal"],
+                    right["fnormal"],
+                    left["fshearx"],
+                    right["fshearx"],
+                    left["fsheary"],
+                    right["fsheary"],
+                ]
+            ),
+            raw={"left": left, "right": right, "centroid": self._estimate_centroid(left, right)},
+        )
+
+    def _read_single(self, item: dict, want_vector: bool = False) -> dict[str, Any]:
+        sensor = item["sensor"]
+        cfg = item["cfg"]
+        img = sensor.get_img()
+        if img is None:
+            return self._empty_sample(item["side"])
+        is_contact = sensor.compute_deformation(check_motion=True, threshold=cfg.motion_threshold)
+        fps, fnormal, fshearx, fsheary = sensor.read_info(
+            sensor.info.FPS,
+            sensor.info.FNORMAL,
+            sensor.info.FSHEARX,
+            sensor.info.FSHEARY,
+        )
+        side = item["side"]
+        fnormal = max(0.0, float(fnormal) - self._baseline.get(f"{side}_normal", 0.0))
+        fshearx = float(fshearx) - self._baseline.get(f"{side}_shearx", 0.0)
+        fsheary = float(fsheary) - self._baseline.get(f"{side}_sheary", 0.0)
+        centroid = (0.0, 0.0)
+        centroid_found = False
+        try:
+            sensor.compute_contact()
+            c, found = sensor.read_info(sensor.info.CENTROID)
+            if found:
+                centroid = c
+                centroid_found = True
+        except Exception:
+            pass
+        return {
+            "side": side,
+            "fnormal": fnormal,
+            "fshearx": fshearx,
+            "fsheary": fsheary,
+            "fps": float(fps),
+            "is_contact": bool(is_contact),
+            "centroid": centroid,
+            "centroid_found": centroid_found,
+            "img": img,
+            # 矢量场只在被要求时才算：compute_flow 是逐像素运算，每帧都做会
+            # 明显增加 worker 耗时，可能拖累三力的 30fps。
+            "vector": read_vector_map(sensor) if want_vector else None,
+        }
+
+    def _empty_sample(self, side: str = "unknown") -> dict[str, Any]:
+        return {
+            "side": side,
+            "fnormal": 0.0,
+            "fshearx": 0.0,
+            "fsheary": 0.0,
+            "fps": 0.0,
+            "is_contact": False,
+            "centroid": (0.0, 0.0),
+            "centroid_found": False,
+            "img": None,
+            "vector": None,
+        }
+
+    def _calibrate_baseline(self) -> None:
+        seconds = self.cfg.baseline_seconds
+        if seconds <= 0:
+            return
+        buckets: dict[str, list[float]] = {
+            "left_normal": [],
+            "right_normal": [],
+            "left_shearx": [],
+            "right_shearx": [],
+            "left_sheary": [],
+            "right_sheary": [],
+        }
+        deadline = time.perf_counter() + seconds
+        while time.perf_counter() < deadline:
+            for item in self._sensors:
+                sensor = item["sensor"]
+                side = item["side"]
+                img = sensor.get_img()
+                if img is None:
+                    continue
+                sensor.compute_deformation(check_motion=False)
+                fnormal, fshearx, fsheary = sensor.read_info(
+                    sensor.info.FNORMAL,
+                    sensor.info.FSHEARX,
+                    sensor.info.FSHEARY,
+                )
+                buckets[f"{side}_normal"].append(float(fnormal))
+                buckets[f"{side}_shearx"].append(float(fshearx))
+                buckets[f"{side}_sheary"].append(float(fsheary))
+            time.sleep(0.01)
+        self._baseline = {k: float(np.mean(v)) if v else 0.0 for k, v in buckets.items()}
+
+    def _estimate_centroid(self, left: dict, right: dict) -> tuple[float, float]:
+        if left["centroid_found"] and right["centroid_found"]:
+            return (
+                (left["centroid"][0] + right["centroid"][0]) * 0.5,
+                (left["centroid"][1] + right["centroid"][1]) * 0.5,
+            )
+        if left["centroid_found"]:
+            return left["centroid"]
+        if right["centroid_found"]:
+            return right["centroid"]
+        return (0.0, 0.0)
+
+
+class MockOrisysTactile(OrisysTactile):
+    name = "mock_orisys"
+
+    def __init__(self, config: OrisysDualConfig | None = None) -> None:
+        super().__init__(config)
+        self._n = 0
+
+    def connect(self) -> None:
+        self._connected = True
+        self._baseline = {}
+        self._n = 0
+        self._sensors = [
+            {"side": "left", "sensor": None, "cfg": self.cfg.left},
+            {"side": "right", "sensor": None, "cfg": self.cfg.right},
+        ]
+
+    def read_sample(self) -> TactileSample:
+        self._n += 1
+        contact = self._n >= 8
+        force = 0.2 if contact else 0.0
+        return TactileSample(
+            timestamp=time.time(),
+            force_xyz=np.array([force, 0.01, 0.005]),
+            torque_xyz=np.zeros(3),
+            contact=contact,
+            raw={
+                "left": {"fnormal": force, "fshearx": 0.0, "fsheary": 0.0, "is_contact": contact, "fps": 30.0, "centroid": (0, 0), "centroid_found": False},
+                "right": {"fnormal": force, "fshearx": 0.0, "fsheary": 0.0, "is_contact": contact, "fps": 30.0, "centroid": (0, 0), "centroid_found": False},
+            },
+        )
+
+
+def make_tactile(data: dict) -> TactileInterface:
+    cfg = load_orisys_from_site(data)
+    backend = (cfg.backend or "orisys").strip().lower()
+    if backend in ("mock", "mock_orisys"):
+        return MockOrisysTactile(cfg)
+    return OrisysTactile(cfg)
+
+
+# ==================== 多进程工作器（厂家推荐架构，每进程独立 CUDA 上下文） ====================
+# 实测：单进程双开两路传感器会互相拖垮（30→0.8fps）；两个独立进程各开一路则稳定 30fps。
+
+def _usb_devpath_for_video(vid_src) -> Optional[str]:
+    """/dev/v4l/by-path/... 或 /dev/videoN → sysfs USB 设备号（如 1-3.4.1）。"""
+    import re
+
+    try:
+        real = os.path.realpath(str(vid_src))
+        base = os.path.basename(real)
+        if not base.startswith("video"):
+            return None
+        sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device")
+        for part in reversed(sysdev.split("/")):
+            if re.fullmatch(r"\d+-\d+(\.\d+)*", part):
+                return part
+    except Exception:
+        pass
+    return None
+
+
+def _usb_self_heal(vid_src, side: str) -> None:
+    """内核级卡死（select 超时循环）只有 unbind/bind 能救。orisys-usb-reset
+    是 root 拥有的白名单脚本（sudoers NOPASSWD），只认 Orisys 的 vid/pid。"""
+    import subprocess
+
+    dev = _usb_devpath_for_video(vid_src)
+    if not dev:
+        return f"[{side}] USB 自愈：解析不到设备号（{vid_src}）"
+    try:
+        r = subprocess.run(
+            ["sudo", "-n", "/usr/local/bin/orisys-usb-reset", dev],
+            capture_output=True, text=True, timeout=15.0,
+        )
+        note = (r.stdout or r.stderr).strip().splitlines()
+        return f"[{side}] USB 自愈复位 {dev}: {note[-1] if note else 'ok'}"
+    except Exception as exc:
+        return f"[{side}] USB 自愈失败 {dev}: {exc}"
+
+
+def tactile_worker(side: str, cfg: OrisysDualConfig, q, stop_event, image_q=None) -> None:
+    """独立进程：开一侧传感器，把样本推进队列。
+
+    q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）—— 三力等轻量数据，每帧都推。
+    image_q: mp.Queue 或 None —— 视触觉图像与力矢量场。
+
+        图像默认**不过队列**（``image_q=None``）：每帧几百 KB，跨进程序列化 +
+        ROS 发布的开销会挤占三力的 30fps 时序。需要图像时调用方传一个
+        ``maxsize=1`` 的队列，并按 ``cfg.image_hz`` 节流推送 —— 队列满时丢旧留新，
+        主进程来不及消费也只会丢图像，不会影响三力。
+
+    stop_event: mp.Event，置位后干净退出（先 disconnect 释放 V4L2 设备）
+
+    自动重连：摄像头被强占/USB 抖动后会出现 get_img 持续 select 超时（空读，
+    单次最长阻塞 ~10s）。连续 2 次空读或单次读取 >2s 即断开重连，直到恢复。
+    """
+    try:  # 给进程起个可识别的 comm 名，便于 pkill 清理孤儿（否则占死 /dev/video*）
+        import ctypes
+        ctypes.CDLL("libc.so.6").prctl(15, b"orisys_tac_w", 0, 0, 0)  # PR_SET_NAME
+    except Exception:
+        pass
+
+    def _put(item: dict) -> None:
+        try:
+            if q.full():
+                try:
+                    q.get_nowait()
+                except Exception:
+                    pass
+            q.put(item)
+        except Exception:
+            pass
+
+    def _put_image(item: dict) -> None:
+        """图像入队：只保留最新一帧。队列满说明主进程没在消费，丢掉旧的即可。"""
+        try:
+            if image_q is None:
+                return
+            if image_q.full():
+                try:
+                    image_q.get_nowait()
+                except Exception:
+                    pass
+            image_q.put(item)
+        except Exception:
+            pass
+
+    period = 1.0 / max(1.0, float(getattr(cfg.left if side == "left" else cfg.right, "target_fps", 30.0)))
+    img_period = (1.0 / cfg.image_hz) if (image_q is not None and cfg.image_hz > 0) else 0.0
+    last_img_t = 0.0
+    vid_src = getattr(cfg.left if side == "left" else cfg.right, "vid_src", None)
+    tac: Optional[OrisysTactile] = None
+    bad = 0
+    dead_cycles = 0  # 连续“重连了但读不到数据”的轮数 → 内核级卡死 → USB 自愈
+    backoff = 0.5
+    while not stop_event.is_set():
+        if tac is None:
+            try:
+                tac = OrisysTactile(cfg)
+                tac.connect(sides=(side,))
+                _put({"side": side, "error": f"[{side}] (re)connected"})
+                backoff = 0.5
+            except Exception as exc:
+                _put({"side": side, "error": f"connect failed: {exc}"})
+                tac = None
+                dead_cycles += 1
+                if dead_cycles >= 2:
+                    _put({"side": side, "error": _usb_self_heal(vid_src, side)})
+                    dead_cycles = 0
+                time.sleep(min(backoff, 3.0))
+                backoff *= 2.0
+                continue
+        # 只有图像帧才算矢量场：compute_flow 是逐像素运算，每帧都算会拖累三力。
+        want_img = image_q is not None and (time.perf_counter() - last_img_t) >= img_period
+        t0 = time.perf_counter()
+        try:
+            d = tac.read_side(side, want_vector=want_img)
+            empty = d.get("img") is None
+        except Exception:
+            d, empty = None, True
+        dt = time.perf_counter() - t0
+        if empty or dt > 2.0:
+            bad += 1
+            if bad >= 2:
+                _put({"side": side, "error": f"[{side}] 相机断流，自动重连中…"})
+                try:
+                    tac.disconnect()
+                except Exception:
+                    pass
+                tac = None
+                bad = 0
+                dead_cycles += 1
+                if dead_cycles >= 2:
+                    _put({"side": side, "error": _usb_self_heal(vid_src, side)})
+                    dead_cycles = 0
+                continue
+        else:
+            bad = 0
+            dead_cycles = 0
+        if d is not None and not empty:
+            if want_img:
+                last_img_t = time.perf_counter()
+                _put_image({
+                    "side": side,
+                    "img": d.get("img"),
+                    "vector": d.get("vector"),
+                    "stamp": time.time(),
+                })
+            # 图像与矢量场已单独走 image_q；三力这条轻量队列不背它们。
+            d = {k: v for k, v in d.items() if k not in ("img", "vector")}
+            d["error"] = ""
+            _put(d)
+        rest = period - (time.perf_counter() - t0)
+        if rest > 0:
+            time.sleep(rest)
+    try:
+        if tac is not None:
+            tac.disconnect()
+    except Exception:
+        pass

build_sim/arm_application/build/lib/arm_application/perception_node.py

@@ -0,0 +1,917 @@
+#!/usr/bin/env python3
+"""perception_node —— YOLO + 深度 + 卡尔曼滤波（独立进程）。
+
+硬性规则：
+- 图像回调**只**存最新帧（绝不在回调里跑 YOLO）
+- YOLO 跑在专门的工作线程上，保证 ``MutuallyExclusive`` 的 I/O 回调永远不会被阻塞
+- 用 ``SensorDataQoS`` ``BEST_EFFORT`` ``keep_last(5)``
+- 滤波后的三维点用 ``geometry_msgs/PointStamped`` 发布（不用 Service）
+- 动态物体 TF 只在视觉帧率下发布
+- 高置信度检测始终重新捕获（快速运动时不会冻结）
+"""
+
+from __future__ import annotations
+
+import os
+import threading
+import time
+from typing import Optional
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import PointStamped, TransformStamped
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import (
+    HistoryPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+    qos_profile_sensor_data,
+)
+from sensor_msgs.msg import CameraInfo, Image
+from std_msgs.msg import Float32MultiArray
+from tf2_ros import TransformBroadcaster
+
+from arm_application.box_tracker import BoxTracker
+from arm_application.handeye_realsense import (
+    is_realsense_source,
+    resolve_realsense_topics,
+)
+from arm_application.kalman_filter import KalmanFilter3D
+from arm_application.plane_feature import geometric_center_on_plane, workpiece_xyz_in_box
+from arm_application.yolo_detector import (
+    INFER_BUSY,
+    YoloDetector,
+    YoloInferProcess,
+    pick_best,
+    resolve_weights,
+)
+
+from arm_application.ros_node import TolerantNode
+
+
+def _bgr_copy(msg: Image) -> np.ndarray:
+    """返回调用方持有的 BGR uint8 数组（对异步 YOLO 安全；ROS 可能把原消息丢掉）。"""
+    h, w = int(msg.height), int(msg.width)
+    enc = (msg.encoding or "").lower()
+    buf = np.frombuffer(msg.data, dtype=np.uint8)
+    if enc == "bgr8":
+        return buf.reshape(h, w, 3).copy()
+    if enc == "rgb8":
+        return buf.reshape(h, w, 3)[:, :, ::-1].copy()
+    raise RuntimeError(f"不支持的编码 {msg.encoding!r}")
+
+
+class PerceptionNode(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__("perception_node")
+        tid = threading.get_ident()
+        self.get_logger().warn(
+            f"perception_node 已启动 pid={os.getpid()} thread_id={tid}"
+        )
+
+        self.declare_parameter("camera_source", "d405")
+        self.declare_parameter("camera_namespace", "")
+        self.declare_parameter("image_topic", "")
+        self.declare_parameter("camera_info_topic", "")
+        self.declare_parameter("depth_topic", "/d405/aligned_depth_to_color/image_raw")
+        self.declare_parameter("weights", "/home/cc/ros2/models/yolo/detect_best_0.onnx")
+        self.declare_parameter("conf", 0.10)
+        self.declare_parameter("iou", 0.45)
+        self.declare_parameter("imgsz", 640)
+        self.declare_parameter("device", "0")
+        self.declare_parameter("min_conf", 0.12)
+        self.declare_parameter("vision_hz", 30.0)
+        self.declare_parameter("z_ref_m", 0.40)
+        self.declare_parameter("jump_px", 280.0)
+        self.declare_parameter("jump_z_m", 0.20)
+        self.declare_parameter("min_depth_m", 0.04)
+        self.declare_parameter("max_depth_m", 0.55)
+        self.declare_parameter("work_z_max_m", 0.50)  # 高于此值 = 孔 / 背景
+        self.declare_parameter("object_frame", "tracked_object")
+        self.declare_parameter("camera_frame", "d405_color_optical_frame")
+        self.declare_parameter("kf_q_pos", 2.0e-3)
+        self.declare_parameter("kf_q_vel", 5.0e-2)
+        self.declare_parameter("kf_r_pos", 8.0e-4)
+        self.declare_parameter("meas_blend", 0.65)  # 1 = 原始测量值，0 = 纯卡尔曼滤波
+        self.declare_parameter("half_precision", True)
+        self.declare_parameter("backend", "auto")  # auto|torch|tensorrt
+        self.declare_parameter("mp_infer", True)  # YOLO 跑在 spawn() 子进程里
+        # 自适应 Z 低通滤波：alpha_z = clamp(z_alpha_min + |vel_z|*z_alpha_gain, min, max)
+        self.declare_parameter("z_alpha_min", 0.10)
+        self.declare_parameter("z_alpha_max", 0.60)
+        self.declare_parameter("z_alpha_gain", 2.0)
+        # 加速 YOLO：推理前先降采样；检测框再缩放回全分辨率
+        self.declare_parameter("infer_max_width", 0)
+        # 检测框 EMA（1 = 每帧都用原始 YOLO —— 紧跟目标；更低 = 更平滑但滞后）
+        self.declare_parameter("box_ema_alpha", 0.85)
+        # YOLO 短暂漏检：预览里保留上一个框；只有连续 N 帧都漏检才清除
+        self.declare_parameter("miss_clear_frames", 12)
+        self.declare_parameter("overlay_hold_on_miss", True)
+        self.declare_parameter("hold_point_on_miss", True)
+        self.declare_parameter("miss_hold_point_frames", 2)
+        # plane = 立体表面质心（推荐）；box = YOLO 检测框中心
+        self.declare_parameter("feature_mode", "plane")
+        self.declare_parameter("plane_inlier_m", 0.008)
+        self.declare_parameter("plane_band_m", 0.06)
+
+        self.z_ref = float(self.get_parameter("z_ref_m").value)
+        self.min_conf = float(self.get_parameter("min_conf").value)
+        self.jump_px = float(self.get_parameter("jump_px").value)
+        self.jump_z_m = float(self.get_parameter("jump_z_m").value)
+        self.min_depth = float(self.get_parameter("min_depth_m").value)
+        self.max_depth = float(self.get_parameter("max_depth_m").value)
+        self.work_z_max = float(self.get_parameter("work_z_max_m").value)
+        self.hold_point_on_miss = bool(self.get_parameter("hold_point_on_miss").value)
+        self.object_frame = str(self.get_parameter("object_frame").value)
+        self.camera_frame = str(self.get_parameter("camera_frame").value)
+        self.meas_blend = float(np.clip(float(self.get_parameter("meas_blend").value), 0.0, 1.0))
+        self.z_alpha_min = float(self.get_parameter("z_alpha_min").value)
+        self.z_alpha_max = float(self.get_parameter("z_alpha_max").value)
+        self.z_alpha_gain = float(self.get_parameter("z_alpha_gain").value)
+        self.infer_max_width = int(self.get_parameter("infer_max_width").value)
+        self.backend_name = str(self.get_parameter("backend").value or "auto")
+        self.mp_infer = bool(self.get_parameter("mp_infer").value)
+        self.box_ema_alpha = float(np.clip(float(self.get_parameter("box_ema_alpha").value), 0.2, 1.0))
+        self.miss_clear_frames = max(1, int(self.get_parameter("miss_clear_frames").value))
+        self.overlay_hold_on_miss = bool(self.get_parameter("overlay_hold_on_miss").value)
+        self.miss_hold_point_frames = max(
+            1, int(self.get_parameter("miss_hold_point_frames").value)
+        )
+        self.feature_mode = str(self.get_parameter("feature_mode").value or "plane").strip().lower()
+        self.plane_inlier_m = float(self.get_parameter("plane_inlier_m").value)
+        self.plane_band_m = float(self.get_parameter("plane_band_m").value)
+
+        self._fx = self._fy = self._cx = self._cy = 0.0
+        self._latest_image: Optional[Image] = None
+        self._latest_depth: Optional[Image] = None
+        self._last_uv: Optional[np.ndarray] = None
+        self._last_good_z: Optional[float] = None
+        self._last_plane_xyz: Optional[np.ndarray] = None
+        self._last_filt: Optional[np.ndarray] = None
+        self._z_filt: Optional[float] = None
+        self._z_filt_t: Optional[float] = None
+        self._box_ema: Optional[np.ndarray] = None  # xyxy
+        self._last_raw_xyxy: Optional[np.ndarray] = None
+        self._box_trk = BoxTracker()
+        self._last_box_cls = 0.0
+        self._last_box_conf = 0.0
+        self._last_yolo_t: Optional[float] = None
+        self._miss_count = 0
+        self._last_det_payload: Optional[list] = None
+        self._frame_seq = 0
+        self._done_seq = -1
+        self._frame_t: dict = {}
+        self._infer_meta: dict = {}
+        self._submitted_seq = -1
+        self._last_gpu_t: Optional[float] = None
+        self._last_restart_t = 0.0
+        self._lock = threading.Lock()
+        self._infer_busy = False
+        self._stop = threading.Event()
+        self._wake = threading.Event()
+
+        # I/O 回调组：只做轻量的“存最新帧”（绝不跑 YOLO）
+        self._io_cbg = MutuallyExclusiveCallbackGroup()
+        self._wake_cbg = MutuallyExclusiveCallbackGroup()
+
+        self.kf = KalmanFilter3D(
+            q_pos=float(self.get_parameter("kf_q_pos").value),
+            q_vel=float(self.get_parameter("kf_q_vel").value),
+            r_pos=float(self.get_parameter("kf_r_pos").value),
+        )
+
+        weights_raw = str(self.get_parameter("weights").value)
+        weights = str(resolve_weights(weights_raw, self.backend_name))
+        yolo_cfg = {
+            "weights": weights,
+            "backend": self.backend_name,
+            "conf": float(self.get_parameter("conf").value),
+            "iou": float(self.get_parameter("iou").value),
+            "device": str(self.get_parameter("device").value or ""),
+            "imgsz": int(self.get_parameter("imgsz").value),
+            "half": bool(self.get_parameter("half_precision").value),
+        }
+        self._yolo_half = bool(yolo_cfg["half"])
+        self._yolo_proc: Optional[YoloInferProcess] = None
+        if self.mp_infer:
+            self._yolo_proc = YoloInferProcess(yolo_cfg)
+            self.detector = self._yolo_proc  # 鸭子类型，只要求有 infer()
+            imgsz = self._yolo_proc.imgsz
+            self.get_logger().info(
+                f"YOLO worker pid={self._yolo_proc._p.pid} weights={self._yolo_proc.weights_path}"
+            )
+        else:
+            self.detector = YoloDetector(
+                weights,
+                conf=float(yolo_cfg["conf"]),
+                iou=float(yolo_cfg["iou"]),
+                device=str(yolo_cfg["device"]),
+                imgsz=int(yolo_cfg["imgsz"]),
+            )
+            if self._yolo_half:
+                try:
+                    import torch
+
+                    if torch.cuda.is_available():
+                        self.detector._model.to("cuda")
+                    else:
+                        self._yolo_half = False
+                except Exception:  # noqa: BLE001
+                    self._yolo_half = False
+            try:
+                self.detector.warmup(half=self._yolo_half)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().warn(f"YOLO 预热失败：{exc}")
+            imgsz = self.detector.imgsz
+
+        image_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+        )
+        assert image_qos.reliability == qos_profile_sensor_data.reliability
+
+        cam_src = str(self.get_parameter("camera_source").value).strip().lower()
+        img_topic = str(self.get_parameter("image_topic").value or "").strip()
+        info_topic = str(self.get_parameter("camera_info_topic").value or "").strip()
+        ns = str(self.get_parameter("camera_namespace").value or "").strip()
+        if is_realsense_source(cam_src) or is_realsense_source(ns):
+            img_topic, info_topic = resolve_realsense_topics(
+                cam_src,
+                camera_namespace=ns,
+                image_topic=img_topic,
+                camera_info_topic=info_topic,
+            )
+        depth_topic = str(self.get_parameter("depth_topic").value or "").strip()
+        cam_ns = (ns or cam_src).strip().strip("/")
+        if not depth_topic or (
+            is_realsense_source(cam_ns)
+            and depth_topic
+            in (
+                "/d405/aligned_depth_to_color/image_raw",
+                "/d435/aligned_depth_to_color/image_raw",
+            )
+            and cam_ns not in ("d405", "d435")
+        ):
+            depth_topic = f"/{cam_ns}/aligned_depth_to_color/image_raw"
+        if not img_topic:
+            raise RuntimeError(
+                f"image_topic 为空（camera_source={cam_src!r} ns={ns!r}）；"
+                "请使用 d405 / d405_left / d405_right"
+            )
+        self.get_logger().info(
+            f"perception image={img_topic} depth={depth_topic} info={info_topic or '-'}"
+        )
+
+        self.create_subscription(
+            Image, img_topic, self._on_image, image_qos, callback_group=self._io_cbg
+        )
+        if info_topic:
+            self.create_subscription(
+                CameraInfo, info_topic, self._on_info, image_qos,
+                callback_group=self._io_cbg,
+            )
+        self.create_subscription(
+            Image, depth_topic, self._on_depth, image_qos, callback_group=self._io_cbg
+        )
+
+        point_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+        )
+        self._point_pub = self.create_publisher(
+            PointStamped, "visual_servo/filtered_point", point_qos
+        )
+        # RELIABLE + keep_last(5)：相机预览能容忍短暂的 DDS 丢包间隙
+        det_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+        self._det_pub = self.create_publisher(Float32MultiArray, "workpiece/detection", det_qos)
+        self._tf_br = TransformBroadcaster(self)
+
+        vision_hz = max(5.0, min(40.0, float(self.get_parameter("vision_hz").value)))
+        self.create_timer(1.0 / vision_hz, self._wake_infer, callback_group=self._wake_cbg)
+        self._worker = threading.Thread(target=self._infer_loop, name="yolo_worker", daemon=True)
+        self._worker.start()
+
+        self.get_logger().info(
+            f"perception 就绪 vision≤{vision_hz:.0f}Hz imgsz={imgsz} "
+            f"infer_max_w={self.infer_max_width} mp={int(self.mp_infer)} "
+            f"backend={self.backend_name} half={self._yolo_half} "
+            f"feature={self.feature_mode} image={img_topic}"
+        )
+
+    def destroy_node(self) -> bool:
+        self._stop.set()
+        self._wake.set()
+        if self._yolo_proc is not None:
+            self._yolo_proc.close()
+            self._yolo_proc = None
+        return super().destroy_node()
+
+    def _on_image(self, msg: Image) -> None:
+        self._latest_image = msg
+        self._frame_seq += 1
+        seq = self._frame_seq
+        self._frame_t[seq] = time.monotonic()
+        if len(self._frame_t) > 40:
+            cut = seq - 30
+            self._frame_t = {k: v for k, v in self._frame_t.items() if k >= cut}
+            self._infer_meta = {k: v for k, v in self._infer_meta.items() if k >= cut}
+        if self._cx <= 1.0:
+            self._cx = float(msg.width) * 0.5
+            self._cy = float(msg.height) * 0.5
+        if self._fx <= 1.0:
+            w = float(msg.width)
+            self._fx = 654.8 if w >= 1200 else 393.9 * (w / 640.0)
+            self._fy = 654.0 if w >= 1200 else 393.4 * (float(msg.height) / 480.0)
+        # 千万不要在这里把图像 pickle 进 YOLO 队列 —— 那会卡住相机回调，
+        # 看起来就像是推理冻结。由推理线程负责提交。
+        self._wake.set()
+
+    def _on_info(self, msg: CameraInfo) -> None:
+        self._fx = float(msg.k[0])
+        self._fy = float(msg.k[4])
+        self._cx = float(msg.k[2])
+        self._cy = float(msg.k[5])
+        if msg.header.frame_id:
+            self.camera_frame = msg.header.frame_id
+
+    def _on_depth(self, msg: Image) -> None:
+        self._latest_depth = msg
+
+    def _wake_infer(self) -> None:
+        if self._latest_image is None:
+            return
+        self._wake.set()
+
+    def _submit_latest(self) -> None:
+        """复制最新的相机帧，交给 GPU 进程。"""
+        msg = self._latest_image
+        seq = int(self._frame_seq)
+        if msg is None or seq <= self._submitted_seq:
+            return
+        try:
+            bgr = _bgr_copy(msg)
+            small, scale = self._prepare_infer(bgr)
+            self._infer_meta[seq] = scale
+            self.detector.submit(small, seq)
+            self._submitted_seq = seq
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"yolo 提交失败：{exc}", throttle_duration_sec=2.0)
+
+    def _restart_yolo(self, why: str) -> None:
+        """自愈：重启卡死 / 挂掉的 GPU 工作进程（重新加载 engine 需要几秒）。"""
+        now = time.monotonic()
+        if now - self._last_restart_t < 5.0:
+            return
+        if not hasattr(self.detector, "restart"):
+            return
+        self._last_restart_t = now
+        self.get_logger().error(f"YOLO 工作进程 {why} —— 正在重启（检测框靠惯性滑行）")
+        try:
+            self.detector.restart()
+            self._submitted_seq = -1
+            self._last_gpu_t = time.monotonic()
+            self.get_logger().info("YOLO 工作进程已重启")
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().error(f"YOLO 重启失败：{exc}")
+
+    def _infer_loop(self) -> None:
+        """提交最新帧，收集 GPU 结果，再做立体匹配。
+
+        GPU 绝不能等立体匹配：如果我们短暂正忙，工作进程会直接丢掉旧结果。
+        我们还会**先**提交再去等，这样下一次推理已经排进队列了。
+        """
+        while not self._stop.is_set():
+            self._submit_latest()
+            try:
+                got = self.detector.wait_result(timeout=0.030)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f"推理：{exc}", throttle_duration_sec=2.0)
+                if not getattr(self.detector, "alive", True):
+                    self._restart_yolo("died")
+                time.sleep(0.02)
+                continue
+            now = time.monotonic()
+            if got is None:
+                silent = (now - self._last_gpu_t) if self._last_gpu_t else 0.0
+                if self._last_gpu_t is not None and silent > 0.25:
+                    self.get_logger().warn(
+                        f"YOLO 静默 {silent*1000:.0f}ms "
+                        f"(worker={'存活' if getattr(self.detector, 'alive', True) else '已死'})",
+                        throttle_duration_sec=1.0,
+                    )
+                outstanding = self._submitted_seq > self._done_seq
+                if outstanding and self._last_gpu_t is not None and silent > 2.0:
+                    self._restart_yolo(f"卡死 {silent:.1f}s")
+                    continue
+                if (
+                    self._last_yolo_t is not None
+                    and (now - self._last_yolo_t) > 0.070
+                    and self._last_raw_xyxy is not None
+                ):
+                    self._coast_box("gpu")
+                self._wake.wait(timeout=0.015)
+                self._wake.clear()
+                continue
+            self._last_gpu_t = now
+            seq, dets, gpu_ms = got
+            if dets is INFER_BUSY:
+                continue
+            scale = float(self._infer_meta.get(int(seq), 1.0))
+            t_img = float(self._frame_t.get(int(seq), time.monotonic()))
+            t0 = time.monotonic()
+            try:
+                self._apply_detections(dets, scale, t_img)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0)
+            self._done_seq = int(seq)
+            self._submit_latest()
+            dt_ms = (time.monotonic() - t0) * 1000.0
+            lag_ms = (time.monotonic() - t_img) * 1000.0
+            self.get_logger().info(
+                f"infer gpu={gpu_ms:.0f}ms apply={dt_ms:.0f}ms lag={lag_ms:.0f}ms seq={seq}",
+                throttle_duration_sec=1.0,
+            )
+
+    def _decode_depth_m(self, msg: Image) -> Optional[np.ndarray]:
+        w, h = int(msg.width), int(msg.height)
+        enc = (msg.encoding or "").lower()
+        if enc in ("16uc1", "mono16"):
+            raw = np.frombuffer(msg.data, dtype=np.uint16)
+            if raw.size < w * h:
+                return None
+            return raw[: w * h].reshape(h, w)
+        if enc == "32fc1":
+            raw = np.frombuffer(msg.data, dtype=np.float32)
+            if raw.size < w * h:
+                return None
+            return raw[: w * h].reshape(h, w)
+        return None
+
+    def _depth_in_box(self, xyxy) -> tuple:
+        msg = self._latest_depth
+        if msg is None:
+            return None, "无深度"
+        if not self._depth_fresh(msg):
+            return None, "深度过期"
+        enc = (msg.encoding or "").lower()
+        buf = self._decode_depth_m(msg)
+        if buf is None:
+            return None, "深度无效"
+        h, w = buf.shape
+        x1, y1, x2, y2 = [float(v) for v in xyxy]
+        img = self._latest_image
+        if img is not None and (int(img.width) != w or int(img.height) != h):
+            sx, sy = w / float(img.width), h / float(img.height)
+            x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
+        bw, bh = max(8.0, x2 - x1), max(8.0, y2 - y1)
+        # 内部 ROI —— 避开检测框边缘；但对有孔的工件仍要保留足够像素
+        ix0 = int(np.clip(x1 + 0.20 * bw, 0, w - 1))
+        iy0 = int(np.clip(y1 + 0.20 * bh, 0, h - 1))
+        ix1 = int(np.clip(x2 - 0.20 * bw, ix0 + 2, w))
+        iy1 = int(np.clip(y2 - 0.20 * bh, iy0 + 2, h))
+        roi = buf[iy0:iy1, ix0:ix1].reshape(-1).astype(np.float32)
+        scale = 0.001
+        if enc in ("16uc1", "mono16"):
+            pos = roi[roi > 0]
+            if pos.size >= 8:
+                med_raw = float(np.median(pos))
+                scale = 0.001 if 20.0 <= med_raw <= 8000.0 else (1.0 if med_raw < 20 else 0.0001)
+                meters = pos * scale
+            else:
+                meters = np.array([], dtype=np.float32)
+        else:
+            meters = roi
+            scale = 1.0
+        vals = meters[np.isfinite(meters) & (meters > self.min_depth) & (meters < self.max_depth)]
+        if vals.size < 8:
+            # 内部 ROI 经常落在孔上；改用整个检测框重试。
+            y0 = int(np.clip(y1, 0, h - 1))
+            x0 = int(np.clip(x1, 0, w - 1))
+            y1b = int(np.clip(y2, y0 + 2, h))
+            x1b = int(np.clip(x2, x0 + 2, w))
+            roi2 = buf[y0:y1b, x0:x1b].reshape(-1).astype(np.float32)
+            if enc in ("16uc1", "mono16"):
+                pos2 = roi2[roi2 > 0]
+                if pos2.size >= 8:
+                    med_raw = float(np.median(pos2))
+                    scale = 0.001 if 20.0 <= med_raw <= 8000.0 else (1.0 if med_raw < 20 else 0.0001)
+                meters2 = pos2 * scale
+            else:
+                meters2 = roi2
+            vals = meters2[np.isfinite(meters2) & (meters2 > self.min_depth) & (meters2 < self.max_depth)]
+            if vals.size < 8:
+                med_m = float(np.median(meters)) if meters.size else 0.0
+                return None, f"ok={vals.size} scale={scale} med={med_m:.3f}"
+        # 只取近表面：孔 / 桌面 / 远处背景绝不能把 Z 拉偏。
+        # 先取最近的 40%，再围绕第 10 百分位保留一个 6 cm 的带宽。
+        vals = np.sort(vals)
+        n_near = max(8, int(0.40 * vals.size))
+        near = vals[:n_near]
+        z_p10 = float(np.percentile(near, 10))
+        band = near[near <= (z_p10 + 0.06)]
+        if band.size < 4:
+            band = near
+        z = float(np.median(band))
+        if z > self.work_z_max:
+            return None, f"ok={vals.size} far={z:.3f}"
+        return z, f"ok={vals.size} near={band.size}"
+
+    def _adaptive_filter_z(self, z_raw: float, now: float) -> float:
+        """第 4 项：对深度做自适应低通滤波 —— 静止时更稳，运动时更灵敏。"""
+        if self._z_filt is None or self._z_filt_t is None:
+            self._z_filt = float(z_raw)
+            self._z_filt_t = now
+            return self._z_filt
+        dt = max(1e-3, float(now - self._z_filt_t))
+        vel_z = abs(float(z_raw) - self._z_filt) / dt
+        alpha_z = float(
+            np.clip(
+                self.z_alpha_min + vel_z * self.z_alpha_gain,
+                self.z_alpha_min,
+                self.z_alpha_max,
+            )
+        )
+        self._z_filt = alpha_z * float(z_raw) + (1.0 - alpha_z) * self._z_filt
+        self._z_filt_t = now
+        return float(self._z_filt)
+
+    def _pixel_to_cam(self, u: float, v: float, z: float) -> np.ndarray:
+        return np.array(
+            [
+                (u - self._cx) * z / max(self._fx, 1.0),
+                (v - self._cy) * z / max(self._fy, 1.0),
+                z,
+            ],
+            dtype=float,
+        )
+
+    def _cam_to_pixel(self, p: np.ndarray) -> np.ndarray:
+        z = max(float(p[2]), 1e-3)
+        return np.array(
+            [
+                self._fx * float(p[0]) / z + self._cx,
+                self._fy * float(p[1]) / z + self._cy,
+            ],
+            dtype=float,
+        )
+
+    def _publish_detection_clear(self) -> None:
+        """空检测 → 预览清掉绿框（绝不能复用上一次的 xyxy）。"""
+        self._last_det_payload = None
+        msg = Float32MultiArray()
+        msg.data = []
+        self._det_pub.publish(msg)
+
+    def _publish_detection_box(
+        self, xyxy: np.ndarray, conf: float, cls_id: float, uv: np.ndarray, z: float
+    ) -> None:
+        det = Float32MultiArray()
+        det.data = [
+            float(xyxy[0]), float(xyxy[1]), float(xyxy[2]), float(xyxy[3]),
+            float(conf), float(cls_id),
+            float(uv[0]), float(uv[1]), float(z),
+        ]
+        self._last_det_payload = list(det.data)
+        self._det_pub.publish(det)
+
+    def _republish_last_box(self) -> None:
+        if not self._last_det_payload:
+            return
+        msg = Float32MultiArray()
+        msg.data = list(self._last_det_payload)
+        self._det_pub.publish(msg)
+
+    def _prepare_infer(self, bgr: np.ndarray) -> tuple:
+        """可选的 YOLO 降采样；返回 (infer_bgr, scale)，全分辨率 = 推理图 * scale。"""
+        max_w = self.infer_max_width
+        h, w = bgr.shape[:2]
+        if max_w <= 0 or w <= max_w:
+            return bgr, 1.0
+        scale = w / float(max_w)
+        new_w = int(max_w)
+        new_h = max(1, int(round(h / scale)))
+        try:
+            import cv2
+            small = cv2.resize(bgr, (new_w, new_h), interpolation=cv2.INTER_AREA)
+        except Exception:  # noqa: BLE001
+            small = bgr
+            scale = 1.0
+        return small, scale
+
+    def _img_wh(self) -> Optional[tuple]:
+        msg = self._latest_image
+        if msg is None:
+            return None
+        return (int(msg.width), int(msg.height))
+
+    def _coast_box(self, note: str) -> bool:
+        """让绿框按上一次的速度继续移动；不要用它驱动三维。"""
+        now = time.monotonic()
+        pred = self._box_trk.predict(now, self._img_wh())
+        if pred is None and self._last_raw_xyxy is not None:
+            pred = np.asarray(self._last_raw_xyxy, dtype=float).reshape(4)
+        if pred is None:
+            return False
+        uv = np.array(
+            [0.5 * (pred[0] + pred[2]), 0.5 * (pred[1] + pred[3])], dtype=float
+        )
+        z = float(self._z_filt if self._z_filt is not None else self.z_ref)
+        self._publish_detection_box(
+            pred, float(self._last_box_conf), float(self._last_box_cls), uv, z
+        )
+        self.get_logger().info(f"检测框惯滑 {note}", throttle_duration_sec=0.5)
+        return True
+
+    def _clear_track(self) -> None:
+        self._box_ema = None
+        self._last_raw_xyxy = None
+        self._last_uv = None
+        self._last_filt = None
+        self._last_plane_xyz = None
+        self._box_trk.reset()
+        self._last_yolo_t = None
+        self._publish_detection_clear()
+
+    def _apply_detections(self, dets, scale: float, t_img: float) -> None:
+        """匹配 YOLO 结果，发布按当前时刻预测的检测框，再在该框上做立体匹配。"""
+        if scale != 1.0:
+            for d in dets:
+                d.xyxy = d.xyxy * scale
+        now = time.monotonic()
+        pred_at_img = self._box_trk.predict(t_img, self._img_wh())
+        with self._lock:
+            prev_uv = None if self._last_uv is None else self._last_uv.copy()
+        prev_xyxy = pred_at_img if pred_at_img is not None else self._last_raw_xyxy
+        best = pick_best(
+            dets,
+            prefer_center=(self._cx, self._cy),
+            prev_uv=prev_uv,
+            prev_xyxy=prev_xyxy,
+            max_track_px=max(400.0, self.jump_px),
+            reacquire_conf=self.min_conf,
+            min_conf=self.min_conf,
+            lock_iou=0.05,
+        )
+        if best is None:
+            self._miss_count += 1
+            if self._miss_count >= self.miss_clear_frames:
+                self._clear_track()
+                self._last_yolo_t = None
+                self.get_logger().info(
+                    "检测漏检 —— 已清除叠加显示",
+                    throttle_duration_sec=1.0,
+                )
+            else:
+                if not self._coast_box(f"miss={self._miss_count}"):
+                    if self.overlay_hold_on_miss:
+                        self._republish_last_box()
+                if (
+                    self.hold_point_on_miss
+                    and self._miss_count <= self.miss_hold_point_frames
+                ):
+                    self._republish_held_point()
+            return
+
+        self._miss_count = 0
+        self._last_yolo_t = now
+        xyxy_raw = np.asarray(best.xyxy, dtype=float).reshape(4)
+        a = self.box_ema_alpha
+        if self._box_ema is None:
+            self._box_ema = xyxy_raw.copy()
+        else:
+            self._box_ema = a * xyxy_raw + (1.0 - a) * self._box_ema
+        self._last_raw_xyxy = xyxy_raw.copy()
+        self._last_box_conf = float(best.conf)
+        self._last_box_cls = float(best.cls_id)
+        self._box_trk.update(xyxy_raw, t_img)
+        # 补偿推理延迟：画工件**当前**所在的位置，而不是 YOLO 看到它时的位置
+        xyxy = self._box_trk.predict(now, self._img_wh())
+        if xyxy is None:
+            xyxy = xyxy_raw
+        uv = np.array(
+            [0.5 * (xyxy[0] + xyxy[2]), 0.5 * (xyxy[1] + xyxy[3])],
+            dtype=float,
+        )
+        box_uv = uv.copy()
+        z_preview = float(self._z_filt if self._z_filt is not None else self.z_ref)
+        self._publish_detection_box(
+            xyxy, float(best.conf), float(best.cls_id), box_uv, z_preview
+        )
+
+        # 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线，
+        # 也不是 z_ref）。红点 = 该三维点。
+        feat = None
+        p_cam: Optional[np.ndarray] = None
+        note = "box"
+        z_meas = None
+        uv = box_uv
+        if self.feature_mode != "box":
+            feat = self._workpiece_xyz(xyxy)
+            if feat is None:
+                feat = self._plane_center(xyxy)
+        if feat is not None:
+            p_cam = np.asarray(feat.xyz, dtype=float).reshape(3)
+            uv = np.asarray(feat.uv, dtype=float).reshape(2)
+            z_meas = float(p_cam[2])
+            note = feat.note
+        else:
+            z_meas, note = self._depth_in_box(xyxy)
+            note = f"{note} box_z"
+        z_ok = z_meas is not None
+        z, note = self._metric_z(z_meas, note)
+        if z is None:
+            # 从未拿到过可用深度（目标超出 D405 量程 / 不在工作区内 / 相机安装或
+            # 手眼标定有问题）。不要发布 z_ref 点 —— 那会在幻影位置上假装锁定夹爪。
+            # 此时应保持不动。
+            self.get_logger().warn(
+                f"目标没有可用深度（{note}）—— 不跟踪。"
+                f"请把工件放到相机视野内 {self.min_depth:.2f}-{self.work_z_max:.2f} m 处，"
+                "或检查相机安装 / 手眼标定。",
+                throttle_duration_sec=1.0,
+            )
+            return
+        now = time.monotonic()
+        z = self._adaptive_filter_z(float(z), now)
+        if p_cam is None:
+            p_cam = self._pixel_to_cam(box_uv[0], box_uv[1], float(z))
+        stamp = self.get_clock().now().to_msg()
+
+        self._publish_detection_box(xyxy, float(best.conf), float(best.cls_id), uv, float(z))
+
+        with self._lock:
+            self.kf.advance(now)
+            if self.kf.initialized:
+                pred = self.kf.position
+                dpx = float(np.linalg.norm(box_uv - self._cam_to_pixel(pred)))
+                dz = abs(float(p_cam[2] - pred[2])) if z_ok else 0.0
+                if dpx > self.jump_px or (z_ok and dz > self.jump_z_m):
+                    self.kf.reset(p_cam, now)
+                    self.get_logger().info(
+                        f"重新捕获 conf={best.conf:.2f} dpx={dpx:.0f}",
+                        throttle_duration_sec=0.5,
+                    )
+                else:
+                    self.kf.update(p_cam)
+            else:
+                self.kf.reset(p_cam, now)
+            self._last_uv = box_uv
+            kf_p = self.kf.position
+            blend = self.meas_blend
+            filt = blend * p_cam + (1.0 - blend) * kf_p
+            # 即使卡尔曼里还残留着旧的远距离 Z，也要保证 XY 的尺度真实
+            z_use = float(np.clip(filt[2], self.min_depth, self.work_z_max))
+            if abs(z_use - float(p_cam[2])) > 0.04:
+                filt = p_cam.copy()
+                z_use = float(p_cam[2])
+            filt[2] = z_use
+            self._last_filt = filt.copy()
+
+        self._publish_point(filt, stamp)
+        self.get_logger().info(
+            f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
+            f"z={filt[2]:.3f} z_ok={int(z_ok)} {note}",
+            throttle_duration_sec=0.4,
+        )
+
+    def _depth_fresh(self, msg: Image) -> bool:
+        """拒绝过期的深度 —— 对快速运动的工件，当前检测框处的旧深度会读到背景
+        （约 2.4 m 处的桌面），从而污染三维点。"""
+        try:
+            t = rclpy.time.Time.from_msg(msg.header.stamp)
+            age = (self.get_clock().now() - t).nanoseconds * 1e-9
+        except Exception:  # noqa: BLE001
+            return True
+        return age < 0.25
+
+    def _workpiece_xyz(self, xyxy):
+        msg = self._latest_depth
+        img = self._latest_image
+        if msg is None or img is None:
+            return None
+        if not self._depth_fresh(msg):
+            return None
+        buf = self._decode_depth_m(msg)
+        if buf is None:
+            return None
+        try:
+            return workpiece_xyz_in_box(
+                buf,
+                msg.encoding or "",
+                xyxy,
+                self._fx,
+                self._fy,
+                self._cx,
+                self._cy,
+                (int(img.width), int(img.height)),
+                min_depth=self.min_depth,
+                max_depth=self.max_depth,
+                work_z_max=self.work_z_max,
+                band_m=self.plane_band_m,
+            )
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"inner3d 特征计算失败：{exc}", throttle_duration_sec=2.0)
+            return None
+
+    def _plane_center(self, xyxy):
+        msg = self._latest_depth
+        img = self._latest_image
+        if msg is None or img is None:
+            return None
+        if not self._depth_fresh(msg):
+            return None
+        buf = self._decode_depth_m(msg)
+        if buf is None:
+            return None
+        try:
+            return geometric_center_on_plane(
+                buf,
+                msg.encoding or "",
+                xyxy,
+                self._fx,
+                self._fy,
+                self._cx,
+                self._cy,
+                (int(img.width), int(img.height)),
+                min_depth=self.min_depth,
+                max_depth=self.max_depth,
+                work_z_max=self.work_z_max,
+                inlier_m=self.plane_inlier_m,
+                band_m=self.plane_band_m,
+            )
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"平面特征计算失败：{exc}", throttle_duration_sec=2.0)
+            return None
+
+    def _metric_z(self, z_meas: Optional[float], note: str) -> tuple:
+        """用于像素→米的 Z。绝不能用孔 / 地面的深度（会放大 XY 误差）。
+        当这个目标**从未**有过可用深度时返回 (None, note) —— 那种情况下发布
+        z_ref 点会让伺服假锁到一个幻影 XY 上。"""
+        z_ref = max(0.12, float(self.z_ref))
+        if z_meas is not None and self.min_depth <= z_meas <= self.work_z_max:
+            self._last_good_z = float(z_meas)
+            return float(z_meas), note
+        if self._last_good_z is not None and self._last_good_z <= self.work_z_max:
+            return float(self._last_good_z), f"{note} hold_near"
+        return None, f"{note} no_depth"
+
+    def _publish_point(self, filt: np.ndarray, stamp) -> None:
+        pt = PointStamped()
+        pt.header.stamp = stamp
+        pt.header.frame_id = self.camera_frame
+        pt.point.x = float(filt[0])
+        pt.point.y = float(filt[1])
+        pt.point.z = float(filt[2])
+        self._point_pub.publish(pt)
+        tf = TransformStamped()
+        tf.header.stamp = stamp
+        tf.header.frame_id = self.camera_frame
+        tf.child_frame_id = self.object_frame
+        tf.transform.translation.x = float(filt[0])
+        tf.transform.translation.y = float(filt[1])
+        tf.transform.translation.z = float(filt[2])
+        tf.transform.rotation.w = 1.0
+        self._tf_br.sendTransform(tf)
+
+    def _republish_held_point(self) -> None:
+        if self._last_filt is None:
+            return
+        self._publish_point(self._last_filt, self.get_clock().now().to_msg())
+
+
+def main(args=None) -> None:
+    import multiprocessing as mp
+
+    try:
+        mp.set_start_method("spawn", force=True)
+    except RuntimeError:
+        pass
+    rclpy.init(args=args)
+    node = PerceptionNode()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop.set()
+        node._wake.set()
+        if getattr(node, "_yolo_proc", None) is not None:
+            node._yolo_proc.close()
+            node._yolo_proc = None
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

build_sim/arm_application/build/lib/arm_application/pid_controller.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+"""PID 控制器，带积分限幅、微分作用于测量值（derivative-on-measurement）以及积分分离。"""
+
+from __future__ import annotations
+
+from typing import Optional
+
+
+class PidController:
+    """``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``，然后再做饱和限幅。
+
+    积分分离：当 ``|error| > i_sep_thresh`` 时，积分被强制置 0 且不再累积
+    （只用 P+D，以便快速追赶 / 减少超调）。
+    """
+
+    def __init__(
+        self,
+        kp: float,
+        ki: float,
+        kd: float,
+        *,
+        i_limit: float = 0.05,
+        out_limit: float = 0.20,
+        i_sep_thresh: float = 0.005,
+    ) -> None:
+        self.kp = float(kp)
+        self.ki = float(ki)
+        self.kd = float(kd)
+        self.i_limit = abs(float(i_limit))
+        self.out_limit = abs(float(out_limit))
+        self.i_sep_thresh = abs(float(i_sep_thresh))
+        self._i = 0.0
+        self._meas_prev: Optional[float] = None
+
+    def reset(self) -> None:
+        self._i = 0.0
+        self._meas_prev = None
+
+    @property
+    def integral(self) -> float:
+        return float(self._i)
+
+    def update(self, error: float, meas: float, dt: float) -> float:
+        dt = max(float(dt), 1e-4)
+        err = float(error)
+        x = float(meas)
+        if self._meas_prev is None:
+            dterm = 0.0
+        else:
+            dterm = -(x - self._meas_prev) / dt
+        self._meas_prev = x
+
+        # 积分分离 —— 误差较大：清空 I，不再累积
+        if self.i_sep_thresh > 0.0 and abs(err) > self.i_sep_thresh:
+            self._i = 0.0
+        else:
+            self._i = max(-self.i_limit, min(self.i_limit, self._i + err * dt))
+
+        u = self.kp * err + self.ki * self._i + self.kd * dterm
+        if u > self.out_limit:
+            u = self.out_limit
+        elif u < -self.out_limit:
+            u = -self.out_limit
+        return float(u)

build_sim/arm_application/build/lib/arm_application/pipeline_node.py

@@ -0,0 +1,357 @@
+#!/usr/bin/env python3
+"""三阶段串联：视觉跟踪锁定 → 孔定位 → 搜孔并轻轻按压（默认不拧入）。
+
+流畅性（相对 bash 杀进程再起节点）：
+  - 相机 / 触觉 / 手眼 TF 全程只起一次
+  - 一阶段锁定后 SIGTERM 跟踪节点，立刻 pin 当前关节，不回零、不抬臂
+  - 二阶段已垂直 / 已在悬停高则跳过对应动作；对准完立刻下压搜孔
+  - 找到孔位后力闭环轻压保持；拧入是后续阶段（--depth 才插入）
+"""
+
+from __future__ import annotations
+
+import json
+import os
+import signal
+import subprocess
+import time
+from typing import Optional
+
+import numpy as np
+import rclpy
+from geometry_msgs.msg import PointStamped
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from std_msgs.msg import Bool, String
+
+from arm_application.stage3_search_node import Stage3Search
+
+
+#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
+#:
+#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
+#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
+#: 小幅下降（只需 5cm 以内的行程）。
+#:
+#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
+#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
+#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
+TRACK_CLEARANCE_M = 0.05
+
+
+def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
+    """一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
+
+    这两种情况的排查方向完全相反，而现象长得一模一样（都是"没锁定"）：
+
+    * **一个点都没收到** → 感知侧的事：yolo 后端多半缺 ultralytics/torch、
+      truth 后端真值节点没起来、external 后端没人发。跟标定、跟工件在不在
+      视野里**都没关系**。
+    * **收到了点但没收敛** → 控制/几何侧的事：工件不在视野、手眼不对、
+      目标超出工作空间、增益太小。
+
+    以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
+    因为缺依赖早退了"被读成"算法不收敛"，白查半天。
+    """
+    if points_seen <= 0:
+        return (
+            f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
+            "（visual_servo/filtered_point 上 0 条）—— 这是感知没起来，"
+            "不是标定/视野问题。按 perception.source 对号入座：\n"
+            "        yolo     → 多半缺推理依赖（ultralytics/torch），"
+            f"看感知进程退出前几行：{log_path}\n"
+            "        truth    → 仿真真值节点没起来，或场景/物体名读不出来"
+            "（它退出前会打印原因）\n"
+            "        external → 外部节点没按这个话题/这个类型发"
+        )
+    return (
+        f"一阶段 {timeout_s:.0f}s 没锁定（收到 {points_seen} 个目标点但没收敛）："
+        f"工件不在视野 / 手眼不对 / 目标超出工作空间。看 {log_path}"
+    )
+
+
+class AssemblyPipeline(Stage3Search):
+    def __init__(self, node_name: str = "assembly_pipeline") -> None:
+        # node_name 可覆盖：通止规装配（gauge_assembly_node）继承本类以复用
+        # 一阶段跟踪的拉起/拆分与二阶段定位链，但它是**另一条流程**，
+        # 在图上必须是另一个节点名（否则 ros2 node list 里两个同名的节点，
+        # 出问题时连"是哪条流程在发指令"都分不清）。
+        super().__init__(node_name=node_name)
+        self.declare_parameter("skip_stage1", False)
+        self.declare_parameter("skip_stage2", False)
+        self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth
+        self.declare_parameter("stage1_timeout_sec", 150.0)
+        self.linger3 = 1.2  # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
+
+        self._s1: Optional[subprocess.Popen] = None
+        self._locked = False
+        self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
+        # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
+        # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
+        #
+        # ⚠️ QoS 必须与感知发布端**逐项一致**（BEST_EFFORT + KEEP_LAST(5)，
+        # 见 perception_node / sim_truth_perception）。用默认的 RELIABLE 订阅
+        # BEST_EFFORT 的话题：不报错、话题列表里也在，但**一条都收不到** ——
+        # 于是超时归因会一口咬定"感知没起来"，把人往完全错误的方向带
+        #（真实原因可能只是这里 QoS 不匹配）。这类"诊断自己坏了"最难查，
+        # 所以宁可多写这几行。
+        self.declare_parameter("diag_point_topic", "visual_servo/filtered_point")
+        self._diag_point_topic = str(self.get_parameter("diag_point_topic").value)
+        self._point_msgs = 0
+        self.create_subscription(
+            PointStamped,
+            self._diag_point_topic,
+            self._on_point_seen,
+            QoSProfile(
+                history=HistoryPolicy.KEEP_LAST,
+                depth=5,
+                reliability=ReliabilityPolicy.BEST_EFFORT,
+            ),
+        )
+        self._pipe_pub = self.create_publisher(String, "/pipeline/state", 10)
+        self._pipe_result = self.create_publisher(String, "/pipeline/result", 10)
+
+        skip1 = bool(self.get_parameter("skip_stage1").value)
+        skip2 = bool(self.get_parameter("skip_stage2").value)
+        skip3 = bool(self.get_parameter("skip_stage3").value)
+        self.get_logger().info(
+            f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
+            f"skip1={skip1} skip2={skip2} skip3={skip3} insert={self.insert_depth_mm:.1f}mm"
+        )
+
+    def _state(self, s: str) -> None:
+        self.get_logger().info(f"[pipeline] === {s} ===")
+        msg = String()
+        msg.data = s
+        self._state_pub.publish(msg)
+        self._pipe_pub.publish(msg)
+
+    def _on_lock(self, msg: Bool) -> None:
+        if bool(msg.data):
+            self._locked = True
+
+    def _on_point_seen(self, _msg: PointStamped) -> None:
+        """只记"收到过目标点"，用于超时后的归因（见 stage1_timeout_reason）。"""
+        self._point_msgs += 1
+
+    def _kill_old_stage1(self) -> None:
+        for pat in (
+            "visual_servo.launch",
+            "lib/arm_application/servo_node",
+            "lib/arm_application/perception_node",
+        ):
+            subprocess.call(["pkill", "-f", pat], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+        time.sleep(0.25)
+
+    def _stop_stage1(self) -> None:
+        proc = self._s1
+        self._s1 = None
+        if proc is None:
+            self._kill_old_stage1()
+            return
+        if proc.poll() is None:
+            try:
+                os.killpg(proc.pid, signal.SIGTERM)
+            except Exception:
+                proc.terminate()
+            try:
+                proc.wait(timeout=4.0)
+            except Exception:
+                try:
+                    os.killpg(proc.pid, signal.SIGKILL)
+                except Exception:
+                    proc.kill()
+        self._kill_old_stage1()
+
+    def _track_z(self) -> float:
+        """跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
+        return float(self.hover_z) + TRACK_CLEARANCE_M
+
+    def _ensure_clearance(self) -> None:
+        """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
+        q, Tee = self._wait_state(4.0)
+        if q is None or Tee is None:
+            return
+        tz = self._track_z()
+        z = float(Tee[2, 3])
+        if z >= tz - 0.008:
+            return
+        self._state("pre_lift")
+        self.get_logger().info(
+            f"先离开工件/孔口：Z {z:.3f} → {tz:.3f}m（跟踪高=hover+50mm）"
+        )
+        _, q, Tee = self._goto(
+            q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
+            verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0,
+            tag="pre_lift", v=self.move_speed, max_hops=8, hold_xy=True, settle_s=0.25,
+        )
+        q2 = self._q()
+        self._pin(q2 if q2 is not None else q)
+
+    def _run_stage1(self) -> int:
+        """拉起 perception+servo，等到 /visual_servo/locked 后冻结并拆跟踪。"""
+        self._ensure_clearance()
+        self._state("stage1_track")
+        self._kill_old_stage1()
+        self._locked = False
+        log_path = "/tmp/pipeline_stage1.log"
+        logf = open(log_path, "w", encoding="utf-8")
+        lock_z = self._track_z()
+        # use_sim_time 必须**原样传下去**：本节点在仿真里是 true，子 launch 若是
+        # false，perception/servo 就跑在系统时间上，TF 全是仿真时刻的戳 →
+        # "看得见图、算得出点，就是查不到变换"，一阶段永远不收敛。
+        use_sim = bool(self.get_parameter("use_sim_time").value)
+        cmd = [
+            "ros2", "launch", "arm_application", "visual_servo.launch.py",
+            "enable_control:=true",
+            "publish_handeye_tf:=false",
+            f"ee_z_m:={lock_z:.3f}",
+            "lock_hold_sec:=0.6",
+            f"use_sim_time:={str(use_sim).lower()}",
+        ]
+        self.get_logger().info(
+            f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
+        )
+        self._s1 = subprocess.Popen(
+            cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
+        )
+        timeout = float(self.get_parameter("stage1_timeout_sec").value)
+        self._point_msgs = 0
+        t0 = time.monotonic()
+        while time.monotonic() - t0 < timeout:
+            self._spin(0.1)
+            if self._locked:
+                break
+            if self._s1.poll() is not None:
+                self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
+                return 1
+        else:
+            self._stop_stage1()
+            self.get_logger().error(
+                stage1_timeout_reason(self._point_msgs, timeout, log_path)
+            )
+            return 1
+        self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
+        self._stop_stage1()
+        time.sleep(0.35)
+        q = self._q()
+        if q is not None:
+            self._pin(q)
+        return 0
+
+    def run(self) -> int:
+        skip1 = bool(self.get_parameter("skip_stage1").value)
+        skip2 = bool(self.get_parameter("skip_stage2").value)
+        skip3 = bool(self.get_parameter("skip_stage3").value)
+        if not self._cli.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
+            return 2
+
+        try:
+            if not skip1:
+                code = self._run_stage1()
+                if code != 0:
+                    return code
+            else:
+                self._ensure_clearance()
+                self._state("skip_stage1")
+
+            q, Tee = self._wait_state(10.0)
+            if q is None or Tee is None:
+                self.get_logger().error("拿不到 joint_states/TF")
+                return 2
+
+            final: dict = {}
+            if not skip2:
+                self._state("stage2_locate")
+                code, q, Tee, final = self._run_locate(q, Tee)
+                if code != 0:
+                    self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
+                    return code
+                self.get_logger().info(
+                    f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
+                    + (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
+                )
+            else:
+                self._state("skip_stage2")
+
+            if skip3:
+                self._state("skip_stage3")
+                self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
+                if final:
+                    self._publish_result(final)
+                self._linger_hold(final if final else {"ok": True}, seconds=0.8)
+                payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
+                try:
+                    payload["stage2"] = json.loads(
+                        open("/tmp/stage2_result.json", encoding="utf-8").read()
+                    )
+                except Exception:
+                    pass
+                msg = String()
+                msg.data = json.dumps(payload, ensure_ascii=False)
+                self._pipe_result.publish(msg)
+                return 0
+
+            self._state("stage3_search")
+            code = self._run_search(q, Tee, linger=False, stage2_final=final)
+            payload = {
+                "ok": code == 0,
+                "stage": "pipeline",
+                "exit": code,
+            }
+            try:
+                payload["stage3"] = json.loads(
+                    open("/tmp/stage3_result.json", encoding="utf-8").read()
+                )
+            except Exception:
+                pass
+            msg = String()
+            msg.data = json.dumps(payload, ensure_ascii=False)
+            self._pipe_result.publish(msg)
+            return code
+        finally:
+            self._stop_stage1()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = AssemblyPipeline()
+    code = 2
+    try:
+        code = node.run()
+    except KeyboardInterrupt:
+        node.get_logger().info("用户中断")
+        code = 130
+    except Exception as exc:  # noqa: BLE001
+        node.get_logger().error(f"pipeline 异常终止: {exc}")
+        code = 2
+    finally:
+        try:
+            node._stop_stage1()
+        except Exception:
+            pass
+        if code != 0:
+            try:
+                q = node._q()
+                if q is not None:
+                    node._pin(q)
+            except Exception:
+                pass
+        try:
+            node._shutdown_tf()
+        except Exception:
+            pass
+        try:
+            node.destroy_node()
+        except Exception:
+            pass
+        try:
+            if rclpy.ok():
+                rclpy.shutdown()
+        except Exception:
+            pass
+        os._exit(code)
+
+
+if __name__ == "__main__":
+    main()

build_sim/arm_application/build/lib/arm_application/plane_feature.py

@@ -0,0 +1,295 @@
+#!/usr/bin/env python3
+"""在 YOLO 检测框中心处求立体平面的 Z。
+
+悬停目标是「检测框中心射线」与「拟合出的近平面」的交点。这样既能留在检测结果上，
+又带有真实的米制 Z（30° 相机下，Z 错了会把 base 系的 XY 也带偏）。不使用点云均值
+质心 —— 孔 / 桌面的内点会把它从绿框上拉走。
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional, Tuple
+
+import numpy as np
+
+
+@dataclass
+class PlaneFeature:
+    uv: np.ndarray  # (2,) 三维质心对应的彩色像素坐标
+    z: float  # 三维质心在相机坐标系下的 Z
+    xyz: np.ndarray  # (3,) 相机坐标系下的立体点云质心
+    n_inliers: int
+    note: str
+
+
+def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:
+    enc = (encoding or "").lower()
+    raw = np.asarray(buf)
+    if enc in ("16uc1", "mono16") or raw.dtype == np.uint16:
+        pos = raw.reshape(-1).astype(np.float32)
+        pos = pos[pos > 0]
+        if pos.size >= 8:
+            med = float(np.median(pos))
+            scale = 0.001 if 20.0 <= med <= 8000.0 else (1.0 if med < 20.0 else 0.0001)
+        else:
+            scale = 0.001
+        return raw.astype(np.float32) * scale, scale
+    return raw.astype(np.float32), 1.0
+
+
+def _near_band_mask(
+    meters: np.ndarray,
+    valid: np.ndarray,
+    min_depth: float,
+    work_z_max: float,
+    band_m: float,
+) -> np.ndarray:
+    vals = meters[valid]
+    if vals.size < 8:
+        return valid
+    vals = np.sort(vals)
+    n_near = max(8, int(0.40 * vals.size))
+    near = vals[:n_near]
+    z_p10 = float(np.percentile(near, 10))
+    hi = min(float(work_z_max), z_p10 + float(band_m))
+    return valid & (meters <= hi) & (meters >= float(min_depth))
+
+
+def _ransac_plane(
+    pts: np.ndarray,
+    thresh: float,
+    n_iter: int = 24,
+    rng: Optional[np.random.Generator] = None,
+) -> Optional[Tuple[np.ndarray, np.ndarray]]:
+    n = int(pts.shape[0])
+    if n < 12:
+        return None
+    rng = rng or np.random.default_rng()
+    best_inl: Optional[np.ndarray] = None
+    best_count = 0
+    for _ in range(n_iter):
+        i = rng.choice(n, 3, replace=False)
+        p0, p1, p2 = pts[i]
+        nvec = np.cross(p1 - p0, p2 - p0)
+        nn = float(np.linalg.norm(nvec))
+        if nn < 1e-9:
+            continue
+        nvec = nvec / nn
+        dist = np.abs((pts - p0) @ nvec)
+        inl = dist < thresh
+        c = int(inl.sum())
+        if c > best_count:
+            best_count = c
+            best_inl = inl
+    if best_inl is None or best_count < max(12, int(0.15 * n)):
+        return None
+    subset = pts[best_inl]
+    p0 = subset.mean(axis=0)
+    _, _, vh = np.linalg.svd(subset - p0, full_matrices=False)
+    nvec = vh[-1]
+    nvec = nvec / (float(np.linalg.norm(nvec)) + 1e-12)
+    if float(nvec @ p0) > 0.0:
+        nvec = -nvec
+    return nvec, p0
+
+
+def _plane_z(
+    nvec: np.ndarray,
+    p0: np.ndarray,
+    u: float,
+    v: float,
+    fx: float,
+    fy: float,
+    cx: float,
+    cy: float,
+) -> Optional[float]:
+    direc = np.array([(u - cx) / max(fx, 1.0), (v - cy) / max(fy, 1.0), 1.0], dtype=float)
+    denom = float(nvec @ direc)
+    if abs(denom) < 1e-8:
+        return None
+    z = float(nvec @ p0) / denom
+    return z if np.isfinite(z) else None
+
+
+def workpiece_xyz_in_box(
+    depth_buf: np.ndarray,
+    encoding: str,
+    xyxy,
+    fx: float,
+    fy: float,
+    cx: float,
+    cy: float,
+    img_wh: Tuple[int, int],
+    min_depth: float,
+    max_depth: float,
+    work_z_max: float,
+    inner: float = 0.40,
+    band_m: float = 0.06,
+) -> Optional[PlaneFeature]:
+    """工件上的实体点：内层检测框里近表面深度的三维中位数。
+
+    不是单目射线（孔会穿透到桌面），也不是整个 ROI 的点云均值
+    （桌面 / 单侧深度会把它从工件上拉走）。
+    """
+    meters, _ = _depth_meters(depth_buf, encoding)
+    if meters.ndim != 2:
+        return None
+    img_w, img_h = int(img_wh[0]), int(img_wh[1])
+    dh, dw = int(meters.shape[0]), int(meters.shape[1])
+    x1, y1, x2, y2 = [float(v) for v in xyxy]
+    u_c = 0.5 * (x1 + x2)
+    v_c = 0.5 * (y1 + y2)
+    if dw != img_w or dh != img_h:
+        sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0)
+        x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
+        scale_u, scale_v = 1.0 / sx, 1.0 / sy
+    else:
+        scale_u = scale_v = 1.0
+    bw = max(8.0, x2 - x1)
+    bh = max(8.0, y2 - y1)
+    frac = 0.5 * float(np.clip(inner, 0.2, 0.8))
+    ix0 = int(np.clip(min(x1, x2) + (0.5 - frac) * bw, 0, dw - 1))
+    iy0 = int(np.clip(min(y1, y2) + (0.5 - frac) * bh, 0, dh - 1))
+    ix1 = int(np.clip(max(x1, x2) - (0.5 - frac) * bw, ix0 + 2, dw))
+    iy1 = int(np.clip(max(y1, y2) - (0.5 - frac) * bh, iy0 + 2, dh))
+    roi = meters[iy0:iy1, ix0:ix1]
+    finite = np.isfinite(roi) & (roi > float(min_depth)) & (roi < float(max_depth))
+    if int(finite.sum()) < 12:
+        return None
+    near = _near_band_mask(roi, finite, min_depth, work_z_max, band_m)
+    if int(near.sum()) < 8:
+        near = finite
+    yy, xx = np.nonzero(near)
+    if yy.size < 8:
+        return None
+    zs = roi[yy, xx]
+    us = (xx + ix0) * scale_u
+    vs = (yy + iy0) * scale_v
+    pts = np.stack(
+        [
+            (us - cx) * zs / max(fx, 1.0),
+            (vs - cy) * zs / max(fy, 1.0),
+            zs,
+        ],
+        axis=1,
+    )
+    p_xyz = np.median(pts, axis=0)
+    if not np.all(np.isfinite(p_xyz)):
+        return None
+    z = float(p_xyz[2])
+    if not (min_depth <= z <= work_z_max):
+        return None
+    uv = np.array(
+        [
+            fx * float(p_xyz[0]) / max(z, 1e-6) + cx,
+            fy * float(p_xyz[1]) / max(z, 1e-6) + cy,
+        ],
+        dtype=float,
+    )
+    return PlaneFeature(
+        uv=uv,
+        z=z,
+        xyz=p_xyz.astype(float),
+        n_inliers=int(yy.size),
+        note=f"inner3d n={int(yy.size)} z={z:.3f}",
+    )
+
+
+def geometric_center_on_plane(
+    depth_buf: np.ndarray,
+    encoding: str,
+    xyxy,
+    fx: float,
+    fy: float,
+    cx: float,
+    cy: float,
+    img_wh: Tuple[int, int],
+    min_depth: float,
+    max_depth: float,
+    work_z_max: float,
+    inlier_m: float = 0.008,
+    band_m: float = 0.06,
+) -> Optional[PlaneFeature]:
+    """拟合出的近平面上、位于检测框中心的点，拿不到则返回 None。"""
+    meters, _ = _depth_meters(depth_buf, encoding)
+    if meters.ndim != 2:
+        return None
+    img_w, img_h = int(img_wh[0]), int(img_wh[1])
+    dh, dw = int(meters.shape[0]), int(meters.shape[1])
+    x1, y1, x2, y2 = [float(v) for v in xyxy]
+    u_c = 0.5 * (x1 + x2)
+    v_c = 0.5 * (y1 + y2)
+    if dw != img_w or dh != img_h:
+        sx, sy = dw / max(float(img_w), 1.0), dh / max(float(img_h), 1.0)
+        x1, y1, x2, y2 = x1 * sx, y1 * sy, x2 * sx, y2 * sy
+        # uv 保持用彩色像素坐标：先在深度像素下计算，再缩放回来。
+        scale_u, scale_v = 1.0 / sx, 1.0 / sy
+    else:
+        scale_u = scale_v = 1.0
+
+    x0 = int(np.clip(min(x1, x2), 0, dw - 1))
+    y0 = int(np.clip(min(y1, y2), 0, dh - 1))
+    x1i = int(np.clip(max(x1, x2), x0 + 2, dw))
+    y1i = int(np.clip(max(y1, y2), y0 + 2, dh))
+    roi = meters[y0:y1i, x0:x1i]
+    if roi.size < 16:
+        return None
+
+    finite = np.isfinite(roi) & (roi > float(min_depth)) & (roi < float(max_depth))
+    if int(finite.sum()) < 16:
+        return None
+    near = _near_band_mask(roi, finite, min_depth, work_z_max, band_m)
+    if int(near.sum()) < 12:
+        near = finite
+
+    yy, xx = np.nonzero(near)
+    zs = roi[yy, xx]
+    # 反投影用的彩色等效像素（对齐后的深度使用的是彩色内参 K）。
+    us = (xx + x0) * scale_u
+    vs = (yy + y0) * scale_v
+    pts = np.stack(
+        [
+            (us - cx) * zs / max(fx, 1.0),
+            (vs - cy) * zs / max(fy, 1.0),
+            zs,
+        ],
+        axis=1,
+    )
+    if pts.shape[0] > 800:
+        rng = np.random.default_rng()
+        sel = rng.choice(pts.shape[0], 800, replace=False)
+        fit_pts = pts[sel]
+    else:
+        fit_pts = pts
+
+    fitted = _ransac_plane(fit_pts, thresh=float(inlier_m))
+    if fitted is None:
+        return None
+    nvec, p0 = fitted
+    dist = np.abs((pts - p0) @ nvec)
+    n_inl = int((dist < float(inlier_m)).sum())
+    if n_inl < 12:
+        return None
+    z = _plane_z(nvec, p0, u_c, v_c, fx, fy, cx, cy)
+    if z is None or not (min_depth <= float(z) <= work_z_max):
+        zs_inl = pts[dist < float(inlier_m), 2]
+        z = float(np.median(zs_inl)) if zs_inl.size else None
+        if z is None or not (min_depth <= z <= work_z_max):
+            return None
+    z = float(z)
+    p_xyz = np.array(
+        [
+            (u_c - cx) * z / max(fx, 1.0),
+            (v_c - cy) * z / max(fy, 1.0),
+            z,
+        ],
+        dtype=float,
+    )
+    return PlaneFeature(
+        uv=np.array([u_c, v_c], dtype=float),
+        z=z,
+        xyz=p_xyz,
+        n_inliers=n_inl,
+        note=f"box_on_plane n={n_inl} z={z:.3f}",
+    )

build_sim/arm_application/build/lib/arm_application/realsense_color_pub.py

@@ -0,0 +1,925 @@
+#!/usr/bin/env python3
+"""把 RealSense 彩色流发布到 ROS 话题（不依赖 realsense2_camera 的 apt 包）。
+
+当你没装 ``ros-humble-realsense2-camera``、但 ``pyrealsense2`` 能正常工作时用它。
+
+默认话题（camera_name=d405）：
+  /d405/color/image_raw
+  /d405/color/camera_info
+  /d405/aligned_depth_to_color/image_raw   (16UC1 mm，需开启 enable_depth)
+
+性能说明
+--------
+* 在采集线程里直接发布（没有定时器带来的延迟）。
+* 使用 SensorDataQoS（BEST_EFFORT，depth=1），这样慢的订阅者会主动丢旧帧。
+* 可选的 SDK 预览：``-p show_preview:=true``（沿用 Intel 示例的链路：pipeline → numpy → 窗口）。
+"""
+
+from __future__ import annotations
+
+import array
+import os
+import threading
+import time
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+import rclpy
+from rcl_interfaces.msg import ParameterDescriptor
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image
+from std_msgs.msg import Float32MultiArray
+
+from arm_application.ros_node import TolerantNode, param_as_str
+
+# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"：
+# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。
+# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）：
+#   D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途
+#   `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失）
+#   之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦
+#   接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**，
+#   表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。
+USB_RESEAT_HINT = (
+    '拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— '
+    '只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。'
+    '看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。'
+)
+
+
+def _fill_camera_info(
+    msg: CameraInfo, width: int, height: int, fx, fy, cx, cy, frame_id: str, dist
+) -> None:
+    msg.width = width
+    msg.height = height
+    msg.distortion_model = 'plumb_bob'
+    msg.d = [float(v) for v in list(dist)[:5]]
+    while len(msg.d) < 5:
+        msg.d.append(0.0)
+    msg.k = [
+        float(fx), 0.0, float(cx),
+        0.0, float(fy), float(cy),
+        0.0, 0.0, 1.0,
+    ]
+    msg.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+    msg.p = [
+        float(fx), 0.0, float(cx), 0.0,
+        0.0, float(fy), float(cy), 0.0,
+        0.0, 0.0, 1.0, 0.0,
+    ]
+    msg.header.frame_id = frame_id
+
+
+def _site_yaml_path() -> Path:
+    env = str(os.environ.get("SITE_CONFIG") or "").strip()
+    if env:
+        p = Path(env).expanduser()
+        if p.is_file():
+            return p
+    home = Path.home() / "ros2" / "config" / "site.yaml"
+    if home.is_file():
+        return home
+    return Path("/home/cc/ros2/config/site.yaml")
+
+
+class RealsenseColorPub(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('realsense_color_pub')
+        self.declare_parameter('camera_name', 'd405')  # 话题命名空间
+        self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
+        self._serial = param_as_str(self, 'camera_serial', '')
+        self.declare_parameter('width', 1280)
+        self.declare_parameter('height', 720)
+        self.declare_parameter('fps', 15)
+        self.declare_parameter('show_preview', False)
+        self.declare_parameter('overlay_detection', True)
+        self.declare_parameter('overlay_hold_sec', 1.5)
+        self.declare_parameter('enable_depth', True)
+        self.declare_parameter('enable_infra', True)
+        self.declare_parameter('preview_stereo', True)
+        # Qt 预览每 N 帧才渲染一次 —— 发布始终保持满帧率；
+        # CPU 上色 + 四窗格绘制绝不能拖慢数据流。
+        self.declare_parameter('preview_decimation', 2)
+        self.declare_parameter('auto_exposure', True)
+        self.declare_parameter('ae_limit_us', 3000.0)
+        self.declare_parameter('emitter_enabled', False)
+        self.declare_parameter('laser_power', 0.0)
+        self.declare_parameter('exposure_us', 0.0)
+        self.declare_parameter('gain', 0.0)
+
+        self._width = int(self.get_parameter('width').value)
+        self._height = int(self.get_parameter('height').value)
+        self._fps = int(self.get_parameter('fps').value)
+        self._show_preview = bool(self.get_parameter('show_preview').value)
+        self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
+        self._overlay_hold_sec = float(self.get_parameter('overlay_hold_sec').value)
+        self._enable_depth_param = bool(self.get_parameter('enable_depth').value)
+        self._enable_infra_param = bool(self.get_parameter('enable_infra').value)
+        self._preview_stereo = bool(self.get_parameter('preview_stereo').value)
+        self._preview_every = max(
+            1, int(self.get_parameter('preview_decimation').value)
+        )
+        self._auto_exposure = bool(self.get_parameter('auto_exposure').value)
+        self._ae_limit_us = float(self.get_parameter('ae_limit_us').value)
+        self._emitter_enabled = bool(self.get_parameter('emitter_enabled').value)
+        self._laser_power = float(self.get_parameter('laser_power').value)
+        self._exposure_us = float(self.get_parameter('exposure_us').value)
+        self._gain = float(self.get_parameter('gain').value)
+        # FPS 表：硬件帧号对比实际发布帧数（循环速率）
+        self._fps_t0 = 0.0
+        self._fps_pub_color = 0
+        self._fps_pub_depth = 0
+        self._fps_fn_color = -1
+        self._fps_fn_depth = -1
+        self._t_wait = 0.0
+        self._t_align = 0.0
+        self._t_pub = 0.0
+
+        qos = qos_profile_sensor_data  # BEST_EFFORT，只保留最后 1 帧
+        base = f'/{self._name}/color'
+        self._img_pub = self.create_publisher(
+            Image, f'{base}/image_raw', qos
+        )
+        self._info_pub = self.create_publisher(
+            CameraInfo, f'{base}/camera_info', qos
+        )
+        self._depth_pub = self.create_publisher(
+            Image, f'/{self._name}/aligned_depth_to_color/image_raw', qos
+        )
+        # IR 流也会一起发布，这样预览可以放在独立的查看器进程里
+        # （Qt 不能和采集循环共享 GIL）。
+        self._ir1_pub = None
+        self._ir2_pub = None
+        if self._enable_infra_param and self._preview_stereo:
+            self._ir1_pub = self.create_publisher(
+                Image, f'/{self._name}/infra1/image_rect_raw', qos
+            )
+            self._ir2_pub = self.create_publisher(
+                Image, f'/{self._name}/infra2/image_rect_raw', qos
+            )
+
+        self._pipe = None
+        self._profile = None
+        self._align = None
+        self._have_depth = False
+        self._have_infra = False
+        self._infra_mode = 'none'
+        self._colorizer = None
+        self._logged_depth = False
+        # 设备深度尺度 → 用原始 uint16 乘上它才是真实的 mm。
+        # D405 = 0.1 mm/单位，D435 = 1.0 mm/单位。启动时从传感器读取。
+        self._depth_to_mm = 1.0
+        self._running = True
+        self._thread: Optional[threading.Thread] = None
+        self._fx = self._fy = self._cx = self._cy = 0.0
+        self._dist = [0.0] * 5
+        self._frame_id = f'{self._name}_color_optical_frame'
+        self._info_every = 1
+        self._frame_i = 0
+        self._preview = None
+        self._overlay: Optional[list] = None
+        self._overlay_stamp = 0.0
+        self._capture_started = False
+        self._usb_desc = ''
+        self._last_ok = None
+        self._site_yaml = _site_yaml_path()
+        self._site_mtime = 0.0
+        self._site_wh_warned = False
+        if self._show_preview:
+            from arm_application.realsense_sdk_view import SdkPreviewWindow, ensure_app
+
+            self._qt_app = ensure_app()
+            self._preview = SdkPreviewWindow(
+                title=f'RealSense {self._name}  COLOR|DEPTH|IR-L|IR-R  (pyrealsense2 SDK)'
+            )
+            self._preview.show()
+            if self._overlay_detection:
+                from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+                det_qos = QoSProfile(
+                    history=HistoryPolicy.KEEP_LAST,
+                    depth=5,
+                    reliability=ReliabilityPolicy.RELIABLE,
+                )
+                self.create_subscription(
+                    Float32MultiArray,
+                    'workpiece/detection',
+                    self._on_det,
+                    det_qos,
+                )
+            self.get_logger().info(
+                'SDK 预览已开启 —— 本进程内显示 COLOR|DEPTH|IR-L|IR-R '
+                f'（保留 {self._overlay_hold_sec:.1f}s）。关窗口 ≠ 停相机。'
+            )
+        self._start_pipeline()
+
+    def _pick_device(self, devices, rs):
+        """优先用配置的序列号，其次按名字选 D405（这台机器上还接了 D435）。"""
+        hint = (self._name or 'd405').lower()
+        if self._serial:
+            for d in devices:
+                if d.get_info(rs.camera_info.serial_number) == self._serial:
+                    return d
+            avail = ', '.join(
+                f"{d.get_info(rs.camera_info.name)}:{d.get_info(rs.camera_info.serial_number)}"
+                for d in devices
+            )
+            raise RuntimeError(
+                f'找不到序列号 {self._serial!r}。当前已连接：{avail}'
+            )
+        for d in devices:
+            name = str(d.get_info(rs.camera_info.name) or '').lower()
+            if hint in name or (hint == 'd405' and '405' in name):
+                return d
+        return devices[0]
+
+    def _reload_site_preview(self, stream_w: int = 0, stream_h: int = 0) -> None:
+        """不重启就能读到新的 overlay_hold_sec（宽高变了则给出警告）。"""
+        path = self._site_yaml
+        if not path.is_file():
+            return
+        try:
+            mtime = path.stat().st_mtime
+        except OSError:
+            return
+        if mtime <= self._site_mtime:
+            return
+        self._site_mtime = mtime
+        try:
+            import yaml
+
+            data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+            cam = data.get("camera") or {}
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
+            return
+        if "overlay_hold_sec" in cam:
+            hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
+            if abs(hold - self._overlay_hold_sec) > 1e-6:
+                self.get_logger().info(
+                    f"overlay_hold_sec {self._overlay_hold_sec:.2f}s → {hold:.2f}s (site.yaml)"
+                )
+                self._overlay_hold_sec = hold
+        want_w = int(cam.get("width") or self._width)
+        want_h = int(cam.get("height") or self._height)
+        live_w = stream_w or self._width
+        live_h = stream_h or self._height
+        if (want_w, want_h) != (live_w, live_h) and not self._site_wh_warned:
+            self._site_wh_warned = True
+            self.get_logger().warn(
+                f"预览流是 {live_w}x{live_h}，而 site.yaml 里的 camera 是 "
+                f"{want_w}x{want_h} —— 请重启：bash ~/ros2/scripts/run_camera_d405.sh --preview"
+            )
+        elif (want_w, want_h) == (live_w, live_h):
+            self._site_wh_warned = False
+
+    def _on_det(self, msg: Float32MultiArray) -> None:
+        # 空负载 = 感知漏检 → 立刻清掉绿框
+        if not msg.data or len(msg.data) < 8:
+            self._overlay = None
+            self._overlay_stamp = 0.0
+            if self._preview is not None:
+                self._preview.set_overlay(None)
+            return
+        self._overlay = list(msg.data)
+        self._overlay_stamp = time.monotonic()
+        if self._preview is not None:
+            self._preview.set_overlay(self._overlay)
+
+    def _publish_frame(self, bgr: np.ndarray, depth_u16: Optional[np.ndarray] = None) -> None:
+        h, w = bgr.shape[0], bgr.shape[1]
+        stamp = self.get_clock().now().to_msg()
+
+        img = Image()
+        img.header.stamp = stamp
+        img.header.frame_id = self._frame_id
+        img.height = h
+        img.width = w
+        img.encoding = 'bgr8'
+        img.is_bigendian = 0
+        img.step = w * 3
+        # array.array('B', ...) 会走 setter 的快路径 —— 直接传 bytes 会触发
+        # 逐元素的 debug 检查，在这里每帧要多花约 40 ms。
+        img.data = array.array('B', np.ascontiguousarray(bgr).tobytes())
+        self._img_pub.publish(img)
+
+        info = CameraInfo()
+        info.header.stamp = stamp
+        info.header.frame_id = self._frame_id
+        _fill_camera_info(
+            info, w, h, self._fx, self._fy, self._cx, self._cy,
+            self._frame_id, self._dist,
+        )
+        self._info_pub.publish(info)
+
+        if depth_u16 is not None:
+            dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
+            dmsg = Image()
+            dmsg.header.stamp = stamp
+            dmsg.header.frame_id = self._frame_id
+            dmsg.height = dh
+            dmsg.width = dw
+            dmsg.encoding = '16UC1'
+            dmsg.is_bigendian = 0
+            dmsg.step = dw * 2
+            dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
+            self._depth_pub.publish(dmsg)
+            if not self._logged_depth:
+                self._logged_depth = True
+                self.get_logger().info(
+                    f'首个对齐深度 {dw}x{dh} → '
+                    f'/{self._name}/aligned_depth_to_color/image_raw'
+                )
+
+    def _start_pipeline(self, reconnect: bool = False) -> None:
+        try:
+            import pyrealsense2 as rs
+        except Exception as exc:  # noqa: BLE001
+            raise RuntimeError(
+                'pyrealsense2 不可用。请执行 conda activate ros_flexiv'
+            ) from exc
+
+        # 杀掉上一个发布进程后，USB 需要 1–3s 才能重新枚举。
+        ctx = rs.context()
+        devices = []
+        for attempt in range(8):
+            devices = list(ctx.devices)
+            if devices:
+                break
+            time.sleep(0.4)
+            ctx = rs.context()
+        if not devices:
+            raise RuntimeError(
+                '没有连接 RealSense（多次重试后 USB 仍为空）。'
+                '拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；'
+                + USB_RESEAT_HINT
+            )
+
+        found = self._pick_device(devices, rs)
+        try:
+            self._serial = str(found.get_info(rs.camera_info.serial_number))
+        except Exception:  # noqa: BLE001
+            pass
+        usb_desc = ''
+        try:
+            usb_desc = str(found.get_info(rs.camera_info.usb_type_descriptor))
+        except Exception:  # noqa: BLE001
+            usb_desc = '?'
+        self._usb_desc = usb_desc
+        usb2 = usb_desc.startswith('2')
+        cam_name = ''
+        try:
+            cam_name = str(found.get_info(rs.camera_info.name))
+        except Exception:  # noqa: BLE001
+            cam_name = self._name
+        self.get_logger().info(
+            f'{cam_name} serial={self._serial or "?"} USB={usb_desc}  '
+            f'requested {self._width}x{self._height}@{self._fps}Hz'
+        )
+        if usb2:
+            # 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
+            # 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
+            # （SuperSpeed 没协商上）。
+            self.get_logger().warn(
+                f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 '
+                f'640x480@15Hz。' + USB_RESEAT_HINT
+            )
+            self._width, self._height, self._fps = 640, 480, 15
+
+        want = (self._width, self._height, self._fps)
+        last = self._last_ok
+        if reconnect:
+            candidates = []
+            if last:
+                candidates.append(last)
+            if want not in candidates:
+                candidates.append(want)
+            if (640, 480, 15) not in candidates:
+                candidates.append((640, 480, 15))
+        else:
+            candidates = [
+                want,
+                (1280, 720, 15),
+                (640, 480, 15),
+                (640, 480, 5),
+            ]
+            if usb2:
+                candidates = [(640, 480, 15), (640, 480, 5), (424, 240, 15)]
+        # 丢弃固件里的垃圾模式（848@90 之类），它们会 ioctl 失败并卡死 USB。
+        seen = set()
+        trimmed = []
+        for item in candidates:
+            if item in seen:
+                continue
+            w, h, fps = item
+            if fps not in (5, 15, 30):
+                continue
+            if w * h > 1280 * 720:
+                continue
+            seen.add(item)
+            trimmed.append(item)
+        candidates = trimmed or [(640, 480, 15)]
+
+        pipe = rs.pipeline()
+        profile = None
+        last_err = None
+        want_depth = bool(self._enable_depth_param)
+        want_infra = (
+            bool(self._enable_infra_param)
+            and bool(self._preview_stereo)
+        )
+        have_depth = False
+        have_infra = False
+
+        # USB3：先尝试真正的 IR-L+IR-R（共享立体自动曝光）。彩色+IR-L 在 D405 上
+        # 经常失败 —— 那就退回 IR-R + 把彩色当左目（观感不同，且不共享自动曝光）。
+        def _modes():
+            if want_depth and want_infra and not usb2:
+                yield True, 'both'
+            if want_depth and want_infra:
+                yield True, 'right'
+            if want_depth:
+                yield True, 'none'
+            yield False, 'none'
+
+        for w, h, fps in candidates:
+            for with_depth, infra_mode in _modes():
+                try:
+                    trial = rs.config()
+                    if self._serial:
+                        trial.enable_device(self._serial)
+                    trial.enable_stream(rs.stream.color, w, h, rs.format.bgr8, fps)
+                    if with_depth:
+                        trial.enable_stream(
+                            rs.stream.depth, w, h, rs.format.z16, fps
+                        )
+                    if infra_mode in ('both', 'left'):
+                        trial.enable_stream(
+                            rs.stream.infrared, 1, w, h, rs.format.y8, fps
+                        )
+                    if infra_mode in ('both', 'right'):
+                        trial.enable_stream(
+                            rs.stream.infrared, 2, w, h, rs.format.y8, fps
+                        )
+                    profile = pipe.start(trial)
+                    have_depth = with_depth
+                    have_infra = infra_mode != 'none'
+                    self._infra_mode = infra_mode
+                    if (w, h, fps) != want:
+                        self.get_logger().warn(
+                            f'请求 {want[0]}x{want[1]}@{want[2]}Hz '
+                            f'→ 改用 {w}x{h}@{fps}Hz'
+                        )
+                    if want_depth and not with_depth:
+                        self.get_logger().warn(
+                            f'{w}x{h}@{fps}Hz 彩色+深度失败 —— 只用彩色'
+                        )
+                    elif want_infra and infra_mode == 'none':
+                        self.get_logger().warn(
+                            f'{w}x{h}@{fps}Hz 立体 IR 失败 —— 改用彩色+深度预览'
+                        )
+                    elif infra_mode == 'both':
+                        self.get_logger().info(
+                            f'{w}x{h}@{fps}Hz 立体 IR-L+IR-R（共享自动曝光）+ 深度'
+                        )
+                    elif infra_mode == 'right':
+                        self.get_logger().info(
+                            f'{w}x{h}@{fps}Hz IR-L=彩色灰度，IR-R=原始 IR '
+                            '（明暗不同 —— 这不是两路自动曝光）'
+                        )
+                    self._width, self._height, self._fps = w, h, fps
+                    self._last_ok = (w, h, fps)
+                    break
+                except Exception as exc:  # noqa: BLE001
+                    last_err = exc
+                    kind = f'color+depth+IR-{infra_mode}' if with_depth else 'color'
+                    self.get_logger().warn(f'{w}x{h}@{fps}Hz {kind} 失败：{exc}')
+                    profile = None
+                    have_depth = False
+                    have_infra = False
+                    try:
+                        pipe.stop()
+                    except Exception:  # noqa: BLE001
+                        pass
+                    time.sleep(0.8)
+                    pipe = rs.pipeline()
+            if profile is not None:
+                break
+        if profile is None:
+            raise RuntimeError(
+                f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
+            )
+
+        self._pipe = pipe
+        self._profile = profile
+        self._have_depth = have_depth
+        self._have_infra = have_infra
+        self._logged_depth = False
+        # 深度按**真实毫米**发布。D405 的原始单位是 0.1 mm（D435 是 1 mm）；
+        # 下游（感知/伺服）都按 mm 处理，所以要按设备尺度归一化。
+        self._depth_to_mm = 1.0
+        if have_depth:
+            try:
+                ds = float(profile.get_device().first_depth_sensor().get_depth_scale())
+                self._depth_to_mm = ds * 1000.0  # m/单位 → mm/单位
+                self.get_logger().info(
+                    f'深度尺度 {ds * 1000.0:.4f} mm/单位 → 按真实 mm 发布'
+                )
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().warn(f'读取深度尺度失败，按 mm 处理：{exc}')
+                self._depth_to_mm = 1.0
+        self._align = rs.align(rs.stream.color) if have_depth else None
+        self._colorizer = None
+        if have_depth:
+            try:
+                self._colorizer = rs.colorizer()
+                self._colorizer.set_option(rs.option.color_scheme, 0)
+            except Exception:  # noqa: BLE001
+                self._colorizer = None
+        try:
+            from arm_application.handeye_realsense import apply_realsense_exposure
+            import time as _time
+
+            exp_us = self._exposure_us if self._exposure_us > 0 else None
+            gain = self._gain if self._gain > 0 else None
+            kwargs = dict(
+                auto_exposure=self._auto_exposure,
+                ae_limit_us=self._ae_limit_us,
+                emitter_enabled=self._emitter_enabled,
+                laser_power=self._laser_power,
+                exposure_us=exp_us,
+                gain=gain,
+                log=self.get_logger().info,
+            )
+            apply_realsense_exposure(profile.get_device(), **kwargs)
+            # 固件经常会在最初几帧重置自动曝光；所以再设置一次。
+            _time.sleep(0.25)
+            apply_realsense_exposure(profile.get_device(), **kwargs)
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'应用曝光参数失败：{exc}')
+        if self._preview is not None:
+            if self._infra_mode == 'both':
+                self._preview.set_pane_titles(
+                    'COLOR', 'DEPTH', 'IR-L  (左红外，与右目同一曝光)', 'IR-R  (右红外)'
+                )
+            elif self._infra_mode == 'right':
+                self._preview.set_pane_titles(
+                    'COLOR', 'DEPTH', '左目≈彩图灰度（不是红外曝光）', 'IR-R  (右红外)'
+                )
+            else:
+                self._preview.set_pane_titles(
+                    'COLOR', 'DEPTH', 'IR-L  (无)', 'IR-R  (无)'
+                )
+
+        vs = profile.get_stream(rs.stream.color).as_video_stream_profile()
+        try:
+            intr = vs.get_intrinsics()
+            self._fx = float(intr.fx)
+            self._fy = float(intr.fy)
+            self._cx = float(intr.ppx)
+            self._cy = float(intr.ppy)
+            dist = list(intr.coeffs[:5]) if intr.coeffs else [0.0] * 5
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(
+                f'get_intrinsics 失败（{exc}）；改用 D405 的默认内参，'
+                f'分辨率 {self._width}x{self._height}'
+            )
+            w, h = float(self._width), float(self._height)
+            if w >= 1200:
+                self._fx, self._fy = 654.8, 654.0
+            else:
+                self._fx = 393.9 * (w / 640.0)
+                self._fy = 393.4 * (h / 480.0)
+            self._cx, self._cy = w * 0.5, h * 0.5
+            dist = [0.0] * 5
+        while len(dist) < 5:
+            dist.append(0.0)
+        self._dist = dist
+
+        sn = profile.get_device().get_info(rs.camera_info.serial_number)
+        model = profile.get_device().get_info(rs.camera_info.name)
+        depth_txt = (
+            f' + aligned depth /{self._name}/aligned_depth_to_color/image_raw'
+            if have_depth
+            else ' （仅彩色）'
+        )
+        if have_infra:
+            depth_txt += ' + IR-L/IR-R 预览'
+        self.get_logger().info(
+            f'{model} serial={sn} USB={usb_desc} → /{self._name}/color/image_raw '
+            f'{self._width}x{self._height}@{self._fps}Hz '
+            f'fx={self._fx:.1f} fy={self._fy:.1f}{depth_txt} (SensorDataQoS)'
+        )
+
+        if not getattr(self, '_capture_started', False):
+            self._capture_started = True
+            self._thread = threading.Thread(target=self._capture_loop, daemon=True)
+            self._thread.start()
+
+    def _enable_color_ae_fast(self, profile, rs) -> None:
+        """彩色自动曝光，优先用短曝光（减少运动模糊）。"""
+        try:
+            sensor = profile.get_device().first_color_sensor()
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'彩色 AE：没有彩色传感器（{exc}）')
+            return
+        try:
+            if sensor.supports(rs.option.enable_auto_exposure):
+                sensor.set_option(rs.option.enable_auto_exposure, 1.0)
+            if sensor.supports(rs.option.auto_exposure_priority):
+                # 0 = 保住帧率，场景够亮时缩短曝光时间
+                sensor.set_option(rs.option.auto_exposure_priority, 0.0)
+            limit = getattr(rs.option, "auto_exposure_limit", None)
+            if limit is not None and sensor.supports(limit):
+                try:
+                    sensor.set_option(limit, 5000.0)  # µs；30 Hz 下减少运动模糊
+                except Exception:
+                    pass
+            self.get_logger().info('彩色自动曝光已开启（优先帧率 / 减少模糊）')
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'彩色 AE：{exc}')
+
+    def _enable_stereo_ae(self, profile, rs) -> None:
+        """两路 IR 成像器共用一路自动曝光（立体模块）。"""
+        try:
+            sensor = profile.get_device().first_depth_sensor()
+            if sensor.supports(rs.option.enable_auto_exposure):
+                sensor.set_option(rs.option.enable_auto_exposure, 1.0)
+            limit = getattr(rs.option, "auto_exposure_limit", None)
+            if limit is not None and sensor.supports(limit):
+                try:
+                    # 把 IR 曝光上限压到远低于 33 ms 的帧周期，这样光线暗的场景
+                    # 也不会悄悄把数据流掉到 30 Hz 以下。
+                    sensor.set_option(limit, 20000.0)
+                    self.get_logger().info('立体 IR 曝光上限 20ms')
+                except Exception:
+                    pass
+            self.get_logger().info('立体 IR 自动曝光已开启（左右共享）')
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'立体 AE：{exc}')
+
+    def _colorize_depth(self, depth_frame, depth_u16: np.ndarray) -> np.ndarray:
+        if self._colorizer is not None:
+            try:
+                rgb = np.asanyarray(self._colorizer.colorize(depth_frame).get_data())
+                if rgb.ndim == 3 and rgb.shape[2] >= 3:
+                    return rgb[:, :, ::-1].copy()
+            except Exception:  # noqa: BLE001
+                pass
+        d = np.asarray(depth_u16)
+        vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8)
+        valid = d > 0
+        if not valid.any():
+            return vis
+        x = d.astype(np.float32)
+        lo, hi = np.percentile(x[valid], [5, 95])
+        hi = max(float(hi), float(lo) + 1.0)
+        gray = np.clip((x - lo) / (hi - lo), 0.0, 1.0)
+        gray = (gray * 255.0).astype(np.uint8)
+        gray[~valid] = 0
+        try:
+            import cv2
+
+            vis = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
+            vis[~valid] = 0
+        except Exception:  # noqa: BLE001
+            vis[:, :, 0] = gray
+            vis[:, :, 1] = gray
+            vis[:, :, 2] = gray
+        return vis
+
+    def _reconnect(self) -> bool:
+        try:
+            if self._pipe is not None:
+                self._pipe.stop()
+        except Exception:  # noqa: BLE001
+            pass
+        self._pipe = None
+        time.sleep(2.5)
+        try:
+            self._start_pipeline(reconnect=True)
+            return self._pipe is not None
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().error(
+                f'重连失败：{exc}。请拔插 USB3，并给线缆留点余量。',
+                throttle_duration_sec=5.0,
+            )
+            return False
+
+    def _capture_loop(self) -> None:
+        got = False
+        misses = 0
+        while self._running and rclpy.ok():
+            if self._pipe is None:
+                if not self._reconnect():
+                    time.sleep(2.0)
+                continue
+            try:
+                t_wait0 = time.monotonic()
+                frames = self._pipe.wait_for_frames(timeout_ms=2000)
+                self._t_wait = time.monotonic() - t_wait0
+                ir_l = (
+                    frames.get_infrared_frame(1)
+                    if self._have_infra and self._infra_mode in ('both', 'left')
+                    else None
+                )
+                ir_r = (
+                    frames.get_infrared_frame(2)
+                    if self._have_infra and self._infra_mode in ('both', 'right')
+                    else None
+                )
+                if self._align is not None:
+                    t_al = time.monotonic()
+                    frames = self._align.process(frames)
+                    self._t_align = time.monotonic() - t_al
+                color = frames.get_color_frame()
+            except Exception as exc:  # noqa: BLE001
+                misses += 1
+                text = str(exc)
+                dropped = (
+                    'disconnect' in text.lower()
+                    or 'ioctl' in text.lower()
+                    or 'errno=5' in text.lower()
+                )
+                if dropped or misses >= 4:
+                    usb = getattr(self, '_usb_desc', '?')
+                    self.get_logger().error(
+                        f'相机数据流中断（USB={usb}）：{text[:120]}。'
+                        '正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
+                    )
+                    got = False
+                    misses = 0
+                    self._reconnect()
+                    continue
+                if misses == 1:
+                    self.get_logger().warn(f'wait_for_frames：{exc}')
+                continue
+            if not color:
+                misses += 1
+                continue
+            misses = 0
+            bgr = np.asanyarray(color.get_data())
+            depth = frames.get_depth_frame()
+            depth_u16 = np.asanyarray(depth.get_data()) if depth else None
+            if depth_u16 is not None and self._depth_to_mm != 1.0:
+                # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
+                depth_u16 = np.clip(
+                    depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
+                ).astype(np.uint16)
+            self._frame_i += 1
+            self._fps_meter(color, depth, depth_u16)
+            # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
+            # —— 它绝不能拖慢发布。
+            do_push = (
+                self._preview is not None
+                and (self._frame_i % self._preview_every) == 0
+            )
+            depth_bgr = None
+            ir_l_img = None
+            ir_r_img = None
+            if do_push:
+                if depth and depth_u16 is not None and depth_u16.size:
+                    depth_bgr = self._colorize_depth(depth, depth_u16)
+                ir_l_img = np.asanyarray(ir_l.get_data()).copy() if ir_l else None
+                ir_r_img = np.asanyarray(ir_r.get_data()).copy() if ir_r else None
+                # 只有在拿不到原始左目 IR 流时，才用彩色灰度来顶替。
+                if ir_l_img is None and self._infra_mode != 'both':
+                    ir_l_img = np.mean(bgr, axis=2).astype(np.uint8)
+            if not got:
+                got = True
+                sw, sh = int(bgr.shape[1]), int(bgr.shape[0])
+                self.get_logger().info(f'首帧 {sw}x{sh}')
+                if self._preview is not None:
+                    self._preview.set_pane_titles(
+                        f"COLOR  {sw}x{sh}",
+                        "DEPTH",
+                        "IR-L  (左红外)",
+                        "IR-R  (右红外)",
+                    )
+            try:
+                t_pub = time.monotonic()
+                self._publish_frame(bgr, depth_u16)
+                if self._ir1_pub is not None and ir_l is not None:
+                    self._publish_ir(ir_l, self._ir1_pub)
+                if self._ir2_pub is not None and ir_r is not None:
+                    self._publish_ir(ir_r, self._ir2_pub)
+                self._t_pub = time.monotonic() - t_pub
+                if do_push:
+                    self._reload_site_preview(int(bgr.shape[1]), int(bgr.shape[0]))
+                    hold = max(0.0, self._overlay_hold_sec)
+                    age = time.monotonic() - self._overlay_stamp
+                    if self._overlay_detection and self._overlay is not None and age > hold:
+                        self._overlay = None
+                        self._preview.set_overlay(None)
+                    if self._overlay_detection and (
+                        self._overlay is None or age > hold
+                    ):
+                        self._preview.set_status(
+                            '没有框 —— 检查 perception_node / conf'
+                        )
+                    else:
+                        self._preview.set_status('')
+                    self._preview.push_views(
+                        bgr.copy(), depth_bgr, ir_l_img, ir_r_img
+                    )
+            except Exception as exc:  # noqa: BLE001
+                # 上下文可能正在关闭
+                if not self._running:
+                    break
+                self.get_logger().warn(
+                    f'发布失败：{exc}', throttle_duration_sec=2.0
+                )
+
+    def _publish_ir(self, frame, pub) -> None:
+        """把原始 IR 成像器以 mono8 发布，供外部查看器进程使用。"""
+        arr = np.asanyarray(frame.get_data())
+        h, w = int(arr.shape[0]), int(arr.shape[1])
+        msg = Image()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._frame_id
+        msg.height = h
+        msg.width = w
+        msg.encoding = 'mono8'
+        msg.is_bigendian = 0
+        msg.step = w
+        msg.data = array.array('B', np.ascontiguousarray(arr).tobytes())
+        pub.publish(msg)
+
+    def _fps_meter(self, color, depth, depth_u16) -> None:
+        """记录发布帧率与硬件帧率 —— 用来区分循环太慢
+        （预览/CPU）和 SDK 侧掉帧（自动曝光长曝光 / USB）。"""
+        now = time.monotonic()
+        try:
+            fn_c = int(color.get_frame_number())
+        except Exception:  # noqa: BLE001
+            fn_c = -1
+        try:
+            fn_d = int(depth.get_frame_number()) if depth else -1
+        except Exception:  # noqa: BLE001
+            fn_d = -1
+        if self._fps_t0 <= 0.0:
+            self._fps_t0 = now
+            self._fps_pub_color = 0
+            self._fps_pub_depth = 0
+            self._fps_fn_color = fn_c
+            self._fps_fn_depth = fn_d
+            return
+        self._fps_pub_color += 1
+        if depth_u16 is not None:
+            self._fps_pub_depth += 1
+        dt = now - self._fps_t0
+        if dt < 5.0:
+            return
+        hw_c = (fn_c - self._fps_fn_color) / dt if self._fps_fn_color >= 0 and fn_c >= 0 else -1.0
+        hw_d = (fn_d - self._fps_fn_depth) / dt if self._fps_fn_depth >= 0 and fn_d >= 0 else -1.0
+        self.get_logger().info(
+            f'stream fps color={self._fps_pub_color / dt:.1f} (hw {hw_c:.1f}) '
+            f'depth={self._fps_pub_depth / dt:.1f} (hw {hw_d:.1f}) '
+            f'wait={self._t_wait * 1e3:.0f} align={getattr(self, "_t_align", 0.0) * 1e3:.0f} '
+            f'pub={getattr(self, "_t_pub", 0.0) * 1e3:.0f}ms'
+        )
+        self._fps_t0 = now
+        self._fps_pub_color = 0
+        self._fps_pub_depth = 0
+        self._fps_fn_color = fn_c
+        self._fps_fn_depth = fn_d
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = RealsenseColorPub()
+    try:
+        preview = getattr(node, '_preview', None)
+        if preview is not None:
+            app = node._qt_app
+            # 关闭窗口绝不能停掉相机 —— 跟踪需要这条数据流。
+            while rclpy.ok() and node._running:
+                rclpy.spin_once(node, timeout_sec=0.01)
+                app.processEvents()
+                if node._preview is not None and not node._preview.isVisible():
+                    node.get_logger().warn(
+                        '预览已关闭 —— 相机继续发布'
+                    )
+                    node._preview = None
+        else:
+            rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._running = False
+        if node._pipe is not None:
+            try:
+                node._pipe.stop()
+            except Exception:  # noqa: BLE001
+                pass
+        if node._thread is not None:
+            node._thread.join(timeout=1.0)
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/realsense_sdk_view.py

@@ -0,0 +1,236 @@
+#!/usr/bin/env python3
+"""RealSense SDK 预览：固定的 2x2 Qt 窗格（彩色 / 深度 / IR-L / IR-R）。
+
+把四路画面拼成一张 pixmap 时，一旦缺了深度 / IR 看起来就像单相机视图。
+现在每路画面都有自己带标题的独立窗格。
+"""
+
+from __future__ import annotations
+
+import threading
+from typing import Optional, Sequence, Tuple
+
+import numpy as np
+
+try:
+    from PySide6.QtCore import Qt, QTimer
+    from PySide6.QtGui import QImage, QPixmap
+    from PySide6.QtWidgets import (
+        QApplication,
+        QGridLayout,
+        QLabel,
+        QMainWindow,
+        QSizePolicy,
+        QVBoxLayout,
+        QWidget,
+    )
+except Exception:  # noqa: BLE001
+    QApplication = None  # type: ignore
+
+
+def _to_bgr(img: Optional[np.ndarray]) -> Optional[np.ndarray]:
+    if img is None:
+        return None
+    vis = img
+    if vis.ndim == 2:
+        vis = np.stack([vis, vis, vis], axis=-1)
+    elif vis.ndim == 3 and vis.shape[2] == 4:
+        vis = vis[:, :, :3]
+    if vis.dtype != np.uint8:
+        vis = np.clip(vis, 0, 255).astype(np.uint8)
+    return np.ascontiguousarray(vis)
+
+
+def _bgr_to_pixmap(bgr: np.ndarray) -> "QPixmap":
+    rgb = np.ascontiguousarray(bgr[:, :, ::-1])
+    h, w, _ = rgb.shape
+    qimg = QImage(rgb.data, w, h, w * 3, QImage.Format_RGB888).copy()
+    return QPixmap.fromImage(qimg)
+
+
+def _placeholder(text: str, w: int = 424, h: int = 240) -> np.ndarray:
+    vis = np.zeros((h, w, 3), dtype=np.uint8)
+    vis[:] = (28, 28, 28)
+    try:
+        import cv2
+
+        cv2.putText(
+            vis, text, (16, h // 2), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (180, 180, 180), 2
+        )
+    except Exception:  # noqa: BLE001
+        pass
+    return vis
+
+
+class SdkPreviewWindow(QMainWindow):
+    def __init__(self, title: str = "RealSense") -> None:
+        super().__init__()
+        self.setWindowTitle(title)
+        self._lock = threading.Lock()
+        self._color: Optional[np.ndarray] = None
+        self._depth: Optional[np.ndarray] = None
+        self._ir_l: Optional[np.ndarray] = None
+        self._ir_r: Optional[np.ndarray] = None
+        self._overlay: Optional[Sequence[float]] = None
+        self._status: str = ""
+        self._hud: str = ""
+
+        central = QWidget()
+        grid = QGridLayout(central)
+        grid.setContentsMargins(6, 6, 6, 6)
+        grid.setSpacing(6)
+        self._img_labels: Tuple[QLabel, QLabel, QLabel, QLabel]
+        self._cap_labels: Tuple[QLabel, QLabel, QLabel, QLabel]
+        panes = []
+        caps = []
+        for row, col, name in (
+            (0, 0, "COLOR"),
+            (0, 1, "DEPTH"),
+            (1, 0, "IR-L  (左红外)"),
+            (1, 1, "IR-R  (右红外)"),
+        ):
+            box, lab, cap = self._make_pane(name)
+            grid.addWidget(box, row, col)
+            panes.append(lab)
+            caps.append(cap)
+        self._img_labels = tuple(panes)  # type: ignore[assignment]
+        self._cap_labels = tuple(caps)  # type: ignore[assignment]
+        self.setCentralWidget(central)
+        self.resize(1360, 820)
+        self.setMinimumSize(880, 520)
+
+        self._timer = QTimer(self)
+        self._timer.timeout.connect(self._redraw)
+        self._timer.start(66)
+
+    @staticmethod
+    def _make_pane(title: str) -> Tuple[QWidget, QLabel, QLabel]:
+        box = QWidget()
+        lay = QVBoxLayout(box)
+        lay.setContentsMargins(0, 0, 0, 0)
+        lay.setSpacing(2)
+        cap = QLabel(title)
+        cap.setAlignment(Qt.AlignCenter)
+        cap.setStyleSheet("color:#e8e8e8; font-weight:600; font-size:13px; background:#222;")
+        img = QLabel("等待画面 …")
+        img.setAlignment(Qt.AlignCenter)
+        img.setMinimumSize(400, 220)
+        img.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
+        img.setStyleSheet("background:#111; color:#777;")
+        img.setScaledContents(False)
+        lay.addWidget(cap)
+        lay.addWidget(img, 1)
+        return box, img, cap
+
+    def set_pane_titles(self, color: str, depth: str, ir_l: str, ir_r: str) -> None:
+        for lab, text in zip(self._cap_labels, (color, depth, ir_l, ir_r)):
+            lab.setText(text)
+
+    def push_bgr(self, bgr: np.ndarray) -> None:
+        self.push_views(bgr)
+
+    def push_views(
+        self,
+        color: np.ndarray,
+        depth_bgr: Optional[np.ndarray] = None,
+        ir_left: Optional[np.ndarray] = None,
+        ir_right: Optional[np.ndarray] = None,
+    ) -> None:
+        with self._lock:
+            self._color = color
+            self._depth = depth_bgr
+            self._ir_l = ir_left
+            self._ir_r = ir_right
+
+    def set_overlay(self, data: Optional[Sequence[float]]) -> None:
+        with self._lock:
+            self._overlay = data
+
+    def set_status(self, text: str) -> None:
+        with self._lock:
+            self._status = text or ""
+
+    def _draw_overlay(self, bgr: np.ndarray, ov, status: str) -> np.ndarray:
+        vis = bgr
+        need = (ov is not None and len(ov) >= 8) or bool(status)
+        if not need:
+            return vis
+        try:
+            import cv2
+        except Exception:  # noqa: BLE001
+            return vis
+        vis = bgr.copy()
+        if ov is not None and len(ov) >= 8:
+            x1, y1, x2, y2 = [int(round(v)) for v in ov[:4]]
+            cx, cy = int(round(ov[6])), int(round(ov[7]))
+            cv2.rectangle(vis, (x1, y1), (x2, y2), (0, 255, 0), 2)
+            cv2.drawMarker(
+                vis,
+                (vis.shape[1] // 2, vis.shape[0] // 2),
+                (255, 255, 0),
+                cv2.MARKER_CROSS,
+                20,
+                2,
+            )
+            cv2.circle(vis, (cx, cy), 6, (0, 0, 255), -1)
+            conf = float(ov[4]) if len(ov) > 4 else 0.0
+            z = float(ov[8]) if len(ov) > 8 else 0.0
+            cv2.putText(
+                vis,
+                f"{conf:.2f}  z={z:.3f}m",
+                (x1, max(20, y1 - 8)),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.65,
+                (0, 255, 0),
+                2,
+            )
+        if status:
+            is_right = ("91" in status) or ("右" in status)
+            col = (0, 140, 255) if is_right else (40, 200, 255)
+            cv2.rectangle(vis, (0, 0), (vis.shape[1], 40), (16, 16, 16), -1)
+            cv2.putText(
+                vis,
+                status[:80],
+                (12, 28),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.75,
+                col,
+                2,
+            )
+        return vis
+
+    def _set_pane(self, label: QLabel, bgr: Optional[np.ndarray], empty: str) -> None:
+        vis = _to_bgr(bgr)
+        if vis is None:
+            vis = _placeholder(empty)
+        pix = _bgr_to_pixmap(vis)
+        slot = label.size()
+        if slot.width() > 4 and slot.height() > 4:
+            pix = pix.scaled(slot, Qt.KeepAspectRatio, Qt.FastTransformation)
+        label.setPixmap(pix)
+
+    def _redraw(self) -> None:
+        with self._lock:
+            color = None if self._color is None else self._color
+            depth = self._depth
+            ir_l = self._ir_l
+            ir_r = self._ir_r
+            ov = self._overlay
+            status = self._status
+        if color is None:
+            return
+        color = self._draw_overlay(color, ov, status)
+        labs = self._img_labels
+        self._set_pane(labs[0], color, "无彩色")
+        self._set_pane(labs[1], depth, "无深度")
+        self._set_pane(labs[2], ir_l, "无 IR-L")
+        self._set_pane(labs[3], ir_r, "无 IR-R")
+
+
+def ensure_app() -> "QApplication":
+    if QApplication is None:
+        raise RuntimeError("RealSense 预览窗口缺少 PySide6，无法创建")
+    app = QApplication.instance()
+    if app is None:
+        app = QApplication([])
+    return app

build_sim/arm_application/build/lib/arm_application/realsense_view.py

@@ -0,0 +1,280 @@
+#!/usr/bin/env python3
+"""realsense_view —— D405 四窗格预览（COLOR|DEPTH|IR-L|IR-R），独立进程运行。
+
+相机节点（``realsense_color_pub``）只负责发布话题。所有 Qt 工作都放在这里，
+这样绘制永远不会拖垮相机采集循环（两者同进程时，Python 的 GIL 会让 ``publish``
+的墙钟耗时从 1 ms 跳到约 70 ms）。关闭窗口只是隐藏预览；Ctrl+C 才停止查看器
+（相机仍在持续发布）。
+"""
+
+from __future__ import annotations
+
+import threading
+import time
+from typing import Optional
+
+import numpy as np
+import rclpy
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import (
+    HistoryPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+    qos_profile_sensor_data,
+)
+from sensor_msgs.msg import Image
+from std_msgs.msg import Float32MultiArray
+
+from arm_application.realsense_color_pub import _site_yaml_path
+from arm_application.ros_node import TolerantNode
+
+
+def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
+    """为预览窗格做快速 numpy JET 上色（不使用 SDK 的 colorizer）。"""
+    vis = np.zeros((d.shape[0], d.shape[1], 3), dtype=np.uint8)
+    valid = d > 0
+    if not valid.any():
+        return vis
+    x = d.astype(np.float32)
+    lo, hi = np.percentile(x[valid], [5, 95])
+    hi = max(float(hi), float(lo) + 1.0)
+    gray = (np.clip((x - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
+    gray[~valid] = 0
+    try:
+        import cv2
+
+        vis = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
+        vis[~valid] = 0
+    except Exception:  # noqa: BLE001
+        vis[:, :, 0] = gray
+        vis[:, :, 1] = gray
+        vis[:, :, 2] = gray
+    return vis
+
+
+def _mono_to_bgr(mono: np.ndarray) -> np.ndarray:
+    return np.ascontiguousarray(np.stack([mono, mono, mono], axis=-1))
+
+
+class RealsenseView(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('realsense_view')
+        self.declare_parameter('camera_name', 'd405')
+        self.declare_parameter('overlay_hold_sec', 0.10)
+        self.declare_parameter('view_hz', 20.0)
+        self.declare_parameter('hud_label', '')
+        self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
+        self._hold = max(0.0, float(self.get_parameter('overlay_hold_sec').value))
+        self._hud = str(self.get_parameter('hud_label').value or '').strip()
+
+        self._color: Optional[Image] = None
+        self._depth: Optional[Image] = None
+        self._ir1: Optional[Image] = None
+        self._ir2: Optional[Image] = None
+        self._overlay: Optional[list] = None
+        self._overlay_stamp = 0.0
+        # stage2 孔对准叠图（已是标注好的 BGR 图），新鲜时 COLOR 窗格直接显示它
+        self._stage2_img: Optional[np.ndarray] = None
+        self._stage2_stamp = 0.0
+        self._site_yaml = _site_yaml_path()
+        self._site_mtime = 0.0
+        # RX 速率表 —— 用来直观诊断卡顿（是传输问题还是绘制问题）
+        self._rx_color = 0
+        self._rx_depth = 0
+        self._rx_ir = 0
+        self._rx_t0 = 0.0
+
+        qos = qos_profile_sensor_data  # BEST_EFFORT keep_last(1)
+        base = f'/{self._name}'
+        self.create_subscription(Image, f'{base}/color/image_raw', self._on_color, qos)
+        self.create_subscription(
+            Image, f'{base}/aligned_depth_to_color/image_raw', self._on_depth, qos
+        )
+        self.create_subscription(
+            Image, f'{base}/infra1/image_rect_raw', self._on_ir1, qos
+        )
+        self.create_subscription(
+            Image, f'{base}/infra2/image_rect_raw', self._on_ir2, qos
+        )
+        det_qos = QoSProfile(
+            history=HistoryPolicy.KEEP_LAST,
+            depth=5,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+        self.create_subscription(
+            Float32MultiArray, 'workpiece/detection', self._on_det, det_qos
+        )
+        # stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示
+        self.create_subscription(
+            Image, '/stage2/overlay', self._on_stage2_overlay, qos_profile_sensor_data
+        )
+
+        from arm_application.realsense_sdk_view import SdkPreviewWindow, ensure_app
+
+        self._qt_app = ensure_app()
+        hud = self._hud or self._name
+        self._preview = SdkPreviewWindow(
+            title=f'{hud}  /{self._name}  COLOR|DEPTH|IR  （查看器）'
+        )
+        self._preview.set_pane_titles(
+            'COLOR', 'DEPTH', 'IR-L  (左红外)', 'IR-R  (右红外)'
+        )
+        self._preview.show()
+        hz = max(5.0, min(30.0, float(self.get_parameter('view_hz').value)))
+        self.create_timer(1.0 / hz, self._push)
+        self.get_logger().info(
+            f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 '
+            f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。'
+        )
+
+    # 回调只存最新消息；转换在 _push 里做。
+    def _on_color(self, msg: Image) -> None:
+        self._color = msg
+        self._rx_color += 1
+
+    def _on_depth(self, msg: Image) -> None:
+        self._depth = msg
+        self._rx_depth += 1
+
+    def _on_ir1(self, msg: Image) -> None:
+        self._ir1 = msg
+        self._rx_ir += 1
+
+    def _on_ir2(self, msg: Image) -> None:
+        self._ir2 = msg
+
+    def _on_stage2_overlay(self, msg: Image) -> None:
+        try:
+            arr = np.frombuffer(msg.data, dtype=np.uint8).reshape(
+                int(msg.height), int(msg.width), 3
+            )
+            self._stage2_img = np.ascontiguousarray(arr)
+            self._stage2_stamp = time.monotonic()
+        except Exception:  # noqa: BLE001
+            pass
+
+    def _on_det(self, msg: Float32MultiArray) -> None:
+        data = list(msg.data)
+        if not data:
+            self._overlay = None
+            self._overlay_stamp = 0.0
+            if self._preview is not None:
+                self._preview.set_overlay(None)
+            return
+        self._overlay = data
+        self._overlay_stamp = time.monotonic()
+        if self._preview is not None:
+            self._preview.set_overlay(data)
+
+    def _reload_site(self) -> None:
+        path = self._site_yaml
+        if not path.is_file():
+            return
+        try:
+            mtime = path.stat().st_mtime
+        except OSError:
+            return
+        if mtime <= self._site_mtime:
+            return
+        self._site_mtime = mtime
+        try:
+            import yaml
+
+            data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+            cam = data.get("camera") or {}
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
+            return
+        if "overlay_hold_sec" in cam:
+            hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
+            if abs(hold - self._hold) > 1e-6:
+                self.get_logger().info(
+                    f"overlay_hold_sec {self._hold:.2f}s → {hold:.2f}s (site.yaml)"
+                )
+                self._hold = hold
+
+    @staticmethod
+    def _bgr(msg: Image) -> np.ndarray:
+        return np.frombuffer(msg.data, dtype=np.uint8).reshape(
+            int(msg.height), int(msg.width), 3
+        )
+
+    @staticmethod
+    def _mono(msg: Image) -> np.ndarray:
+        return np.frombuffer(msg.data, dtype=np.uint8).reshape(
+            int(msg.height), int(msg.width)
+        )
+
+    def _push(self) -> None:
+        if self._preview is None:
+            return
+        self._reload_site()
+        now = time.monotonic()
+        if self._rx_t0 <= 0.0:
+            self._rx_t0 = now
+        elif now - self._rx_t0 >= 5.0:
+            dt = now - self._rx_t0
+            self.get_logger().info(
+                f'查看器接收 color={self._rx_color / dt:.1f}/s '
+                f'depth={self._rx_depth / dt:.1f}/s ir={self._rx_ir / dt:.1f}/s'
+            )
+            self._rx_t0 = now
+            self._rx_color = 0
+            self._rx_depth = 0
+            self._rx_ir = 0
+        age = now - self._overlay_stamp
+        if self._overlay is not None and age > self._hold:
+            self._overlay = None
+            self._preview.set_overlay(None)
+        if self._overlay is None or age > self._hold:
+            miss = '没有框 —— 检查 perception_node / conf'
+            self._preview.set_status(f'{self._hud}  {miss}' if self._hud else miss)
+        else:
+            self._preview.set_status(self._hud)
+        if self._color is None:
+            return
+        color = self._bgr(self._color)
+        # stage2 孔对准叠图新鲜时，COLOR 窗格直接显示它（带 k1/k2 标注 + 残差）
+        if self._stage2_img is not None and (now - self._stage2_stamp) < 2.5:
+            color = self._stage2_img
+        depth_bgr = None
+        if self._depth is not None:
+            d = np.frombuffer(self._depth.data, dtype=np.uint16).reshape(
+                int(self._depth.height), int(self._depth.width)
+            )
+            depth_bgr = _colorize_depth_u16(d)
+        # 在这里（执行器线程）做 Mono→BGR，让 Qt 绘制线程保持轻量。
+        ir1 = None if self._ir1 is None else _mono_to_bgr(self._mono(self._ir1))
+        ir2 = None if self._ir2 is None else _mono_to_bgr(self._mono(self._ir2))
+        self._preview.push_views(color, depth_bgr, ir1, ir2)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = RealsenseView()
+    app = node._qt_app
+    # ROS 回调（图像反序列化、深度上色、推送）跑在执行器线程上；
+    # 主线程只负责绘制 Qt —— 不会有共享的 GIL 突发。
+    executor = MultiThreadedExecutor(num_threads=3)
+    executor.add_node(node)
+    spin_t = threading.Thread(target=executor.spin, daemon=True)
+    spin_t.start()
+    try:
+        while rclpy.ok() and spin_t.is_alive():
+            app.processEvents()
+            time.sleep(0.005)
+            if node._preview is not None and not node._preview.isVisible():
+                node.get_logger().warn('预览已关闭 —— 查看器挂起（相机仍在发布）')
+                node._preview = None
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/ros_node.py

@@ -0,0 +1,118 @@
+"""应用层节点的统一基类：把「参数类型的宽窄」问题收在一处。
+
+为什么需要这个基类（2026-09-28 真机落地实测，代价是顶置相机起不来）
+------------------------------------------------------------------
+现场 `config/site.yaml` 写的是:
+
+    cameras:
+      d435:
+        ae_limit_us: 8000        # ← YAML 整数
+
+脚本把它当 `-p ae_limit_us:=8000` 传给节点，而节点里是:
+
+    self.declare_parameter('ae_limit_us', 3000.0)   # ← double
+
+rclpy **严格**比对类型，于是节点**一起就死**:
+
+    InvalidParameterTypeException: Trying to set parameter 'ae_limit_us'
+    to '8000' of type 'INTEGER', expecting type 'DOUBLE'
+
+现场看到的是「顶置 D435 起不来」，而 site.yaml 那一行看起来毫无问题 ——
+`8000` 和 `8000.0` 在运维眼里是同一个数，在 rclpy 眼里是两种类型。
+同一族的雷还有：`width: 848.0`（浮点）喂给 int 参数、`emitter_enabled: 1`
+喂给 bool 参数、`"26610799"` 与 `26610799`（带不带引号）等等。
+
+**为什么不让现场人员"写对一点"**：那等于要求每个人记住「这个数必须带小数点」，
+而记错的表现是节点启动即崩、报错里还完全不提 site.yaml。规则要靠人记住的就
+一定会破（`FRAMEWORK_RULES.md` 的总纲）。
+
+**为什么不在每个参数上加个 cast**：异常发生在**声明阶段**，
+`float(self.get_parameter(...).value)` 根本执行不到 —— 看着宽容，其实没救。
+
+做法
+----
+用 ``ParameterDescriptor(dynamic_typing=True)`` 声明：参数类型由**传进来的值**
+决定，不做匹配检查；取值时由业务代码 cast（`float(...)` / `int(...)`），
+语义仍然由声明处的默认值表达。于是 ``8000`` / ``8000.0`` / ``"8000"`` 都能用。
+
+**代价与边界**：动态类型意味着 rclpy 不再替我们拦「类型写错」。
+本项目接受这个代价，因为这些参数**全部**来自 `config/site.yaml` 这个运维手改的
+文件（不是程序内部传参），宽容比严格更符合现场；而"名字写错"仍然会被
+rclpy 拦下来（参数名是静态的，与类型无关）。
+
+新增节点一律继承 ``TolerantNode``（有守卫测试
+``test_ros_node.py::test_no_node_subclasses_rclpy_node_directly`` 钉住）。
+"""
+
+from __future__ import annotations
+
+from typing import Any, Optional, Sequence, Union
+
+from rcl_interfaces.msg import ParameterDescriptor
+from rclpy.node import Node
+
+__all__ = ["TolerantNode", "tolerant_descriptor", "param_as_str"]
+
+
+def tolerant_descriptor() -> ParameterDescriptor:
+    """声明用的描述符：类型跟着**传进来的值**走，不做宽窄匹配。"""
+    return ParameterDescriptor(dynamic_typing=True)
+
+
+def param_as_str(node: Node, name: str, default: str = "") -> str:
+    """读一个"本质是字符串"的参数，容忍传进来的是数字。
+
+    为什么需要：现场 `site.yaml` 里 `serial_number: 26610799`（不加引号）就是
+    个 YAML 整数，而节点里想要的是字符串。rclpy 同样会因类型不匹配在**声明**
+    阶段抛异常 —— 而字符串参数在运维手上最容易"看着是数字"。
+
+    以前这段逻辑在三个节点里各抄了一份（`dh_pgea_node` / `handeye_teach` /
+    `realsense_color_pub`），相同的东西抄三份迟早歪（`FRAMEWORK_RULES.md` R5.4）。
+    现在只此一处；取不到就返回 `default`。
+    """
+    node.declare_parameter(name, default, tolerant_descriptor())
+    value = node.get_parameter(name).value
+    if value is None:
+        return default
+    return str(value).strip()
+
+
+class TolerantNode(Node):
+    """`rclpy.node.Node` + 「参数类型宽容」。
+
+    只改 `declare_parameter` 一件事：默认描述符换成动态类型。
+    调用方若自己传了 `descriptor`（例如只想放宽某几个），以调用方为准。
+    """
+
+    def declare_parameter(  # type: ignore[override]
+        self,
+        name: str,
+        value: Any = None,
+        descriptor: Optional[ParameterDescriptor] = None,
+        ignore_override: bool = False,
+    ) -> Any:
+        if descriptor is None:
+            descriptor = tolerant_descriptor()
+        return super().declare_parameter(name, value, descriptor, ignore_override)
+
+    # 列表版本是 rclpy 的另一个入口，同样要放宽（`workspace_xyz_min: [1, 2, 3]`
+    # 这种"整数字面量的列表"喂给 DOUBLE_ARRAY 一样会崩）。
+    def declare_parameters(  # type: ignore[override]
+        self,
+        namespace: str,
+        parameters: Sequence[
+            Union[tuple, Any]
+        ],
+        ignore_override: bool = False,
+    ) -> Any:
+        fixed = []
+        for item in parameters:
+            if (
+                isinstance(item, tuple)
+                and len(item) == 2
+                and not isinstance(item[1], ParameterDescriptor)
+            ):
+                fixed.append((item[0], item[1], tolerant_descriptor()))
+            else:
+                fixed.append(item)
+        return super().declare_parameters(namespace, fixed, ignore_override)

build_sim/arm_application/build/lib/arm_application/run_joint_trajectory.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+"""通过 ``/follow_joint_trajectory`` 发送多点关节轨迹（真机 + mock 都适用）。"""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from arm_application.site_io import joint_prefix_default
+from arm_msgs.action import FollowJointTrajectory
+
+from arm_application.ros_node import TolerantNode
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+NUM_JOINTS = 7
+# 扁平化存放的位置（3 个点 x 7 个关节）+ 时间 —— ROS 参数不支持列表套字典
+DEFAULT_TIMES = [3.0, 6.0, 9.0]
+DEFAULT_POSITIONS = [
+    0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0,
+    0.35, -0.698, 0.35, 1.57, 0.0, 0.698, 0.0,
+    0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0,
+]
+
+
+class RunJointTrajectory(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__('run_joint_trajectory')
+        self.declare_parameter('joint_prefix', joint_prefix_default())
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('velocity_scale', 0.15)
+        self.declare_parameter('action_name', 'follow_joint_trajectory')
+        self.declare_parameter('waypoint_times', DEFAULT_TIMES)
+        self.declare_parameter('waypoint_positions', DEFAULT_POSITIONS)
+        # 1 = 只跑一次（默认）；0 = 循环直到 Ctrl+C；N = 重复 N 次
+        self.declare_parameter('loop_count', 1)
+        self.declare_parameter('pause_between_loops', 0.5)
+
+        prefix = str(self.get_parameter('joint_prefix').value or '')
+        base = list(
+            self.get_parameter('joint_names').get_parameter_value().string_array_value
+            or DEFAULT_JOINTS
+        )
+        self.joint_names = [f'{prefix}{n}' for n in base]
+        self.velocity_scale = float(self.get_parameter('velocity_scale').value)
+        action = str(self.get_parameter('action_name').value)
+        self._action_name = action
+        self._client = ActionClient(self, FollowJointTrajectory, action)
+
+    def run(self) -> int:
+        if not self._client.wait_for_server(timeout_sec=15.0):
+            self.get_logger().error(
+                f'Action [{self._action_name}] 不可用。'
+                '请先启动真机栈：bash scripts/run_real.sh'
+            )
+            return 1
+
+        times = list(self.get_parameter('waypoint_times').value)
+        positions = list(self.get_parameter('waypoint_positions').value)
+        n_j = len(self.joint_names)
+        n_pts = len(times)
+        if n_pts < 1:
+            self.get_logger().error('waypoint_times 为空')
+            return 1
+        if len(positions) != n_pts * n_j:
+            self.get_logger().error(
+                f'waypoint_positions 长度 {len(positions)} != '
+                f'{n_pts} 个点 x {n_j} 个关节 = {n_pts * n_j}'
+            )
+            return 1
+
+        goal = FollowJointTrajectory.Goal()
+        goal.joint_names = self.joint_names
+        goal.num_points = n_pts
+        goal.velocity_scale = self.velocity_scale
+        goal.positions = positions
+        goal.time_from_start = [float(t) for t in times]
+
+        loop_count = int(self.get_parameter('loop_count').value)
+        pause = float(self.get_parameter('pause_between_loops').value)
+        duration = goal.time_from_start[-1]
+        if loop_count == 0:
+            self.get_logger().info(
+                f'循环模式：{goal.num_points} 个点，约 {duration:.1f}s/圈 '
+                f'（Ctrl+C 停止）'
+            )
+        elif loop_count > 1:
+            self.get_logger().info(
+                f'循环模式：{loop_count} 圈，{goal.num_points} 个点，'
+                f'约 {duration:.1f}s/圈'
+            )
+        else:
+            self.get_logger().info(
+                f'发送 {goal.num_points} 个点，v_scale={goal.velocity_scale}，'
+                f'时长={duration:.1f}s'
+            )
+
+        iteration = 0
+        while rclpy.ok():
+            iteration += 1
+            if loop_count > 1:
+                self.get_logger().info(f'第 {iteration}/{loop_count} 圈')
+
+            code = self._send_once(goal, duration)
+            if code != 0:
+                return code
+
+            if loop_count == 1:
+                self.get_logger().info('完成：OK')
+                return 0
+            if loop_count > 1 and iteration >= loop_count:
+                self.get_logger().info(f'完成：已跑完 {loop_count} 圈')
+                return 0
+            if pause > 0:
+                time.sleep(pause)
+
+    def _send_once(self, goal: FollowJointTrajectory.Goal, duration: float) -> int:
+        send = self._client.send_goal_async(goal)
+        deadline = time.monotonic() + 10.0
+        while rclpy.ok() and not send.done():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if time.monotonic() > deadline:
+                self.get_logger().error('发送目标超时')
+                return 2
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('目标被拒绝')
+            return 3
+
+        res_fut = handle.get_result_async()
+        deadline = time.monotonic() + max(120.0, duration * 3.0)
+        next_log = time.monotonic() + 10.0
+        while rclpy.ok() and not res_fut.done():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            now = time.monotonic()
+            if now >= next_log:
+                left = max(0.0, deadline - now)
+                self.get_logger().info(
+                    f'正在执行轨迹（计划约 {duration:.0f}s），'
+                    f'等待机器人……（剩余超时 {left:.0f}s）'
+                )
+                next_log = now + 10.0
+            if now > deadline:
+                self.get_logger().error('等待结果超时')
+                return 4
+        result = res_fut.result().result
+        if not result.success:
+            self.get_logger().error(f'失败：{result.message}')
+            return 5
+        self.get_logger().info(f'本圈结束：{result.message or "OK"}')
+        return 0
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = RunJointTrajectory()
+    code = 1
+    try:
+        code = node.run()
+    except KeyboardInterrupt:
+        code = 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+    raise SystemExit(code)
+
+
+if __name__ == '__main__':
+    main()

build_sim/arm_application/build/lib/arm_application/site_io.py

@@ -0,0 +1,82 @@
+"""供应用节点加载 ``~/ros2/config/site.yaml``。"""
+
+from __future__ import annotations
+
+import os
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+
+def ros2_root() -> Path:
+    for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v).expanduser()
+    return Path.home() / "ros2"
+
+
+def site_path() -> Path:
+    env = os.environ.get("SITE_CONFIG", "").strip()
+    if env:
+        return Path(env).expanduser()
+    return ros2_root() / "config" / "site.yaml"
+
+
+def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
+    p = Path(path).expanduser() if path else site_path()
+    if not p.is_file():
+        return {}
+    data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
+    return data if isinstance(data, dict) else {}
+
+
+def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:
+    out = dict(base)
+    for key, val in overlay.items():
+        if isinstance(val, dict) and isinstance(out.get(key), dict):
+            out[key] = deep_merge(out[key], val)
+        else:
+            out[key] = val
+    return out
+
+
+def joint_prefix_default() -> str:
+    """当前臂的关节名前缀（``Rizon4-XXXXXX_``），用作 ROS 参数的默认值。
+
+    来源顺序：
+
+    1. ``SITE_JOINT_PREFIX`` —— ``scripts/env.sh`` → ``load_site_config.sh`` 从
+       site.yaml 导出，这就是**权威值**；
+    2. 由 ``SITE_ROBOT_SN`` 现算（``Rizon4-063989`` → ``Rizon4-063989_``）；
+    3. 都没有 → 空字符串。
+
+    **绝不写死某个具体 SN。** 以前这里的默认值是 ``"Rizon4-063989_"``，
+    于是换台机器人后 ``ros2 run`` 直接起的节点会去订**上一台臂**的关节话题，
+    表现为"节点起来了、什么都不动、也不报错"（R1.1）。
+    """
+    prefix = os.environ.get("SITE_JOINT_PREFIX", "").strip()
+    if prefix:
+        return prefix
+    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
+    return f"{sn}_" if sn else ""
+
+
+def base_frame_default() -> str:
+    """当前臂的基座坐标系（``<SN>_base_link``），用作 ROS 参数默认值。"""
+    frame = os.environ.get("SITE_BASE_FRAME", "").strip()
+    if frame:
+        return frame
+    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
+    return f"{sn}_base_link" if sn else ""
+
+
+def ee_frame_default() -> str:
+    """当前臂的法兰坐标系（``<SN>_flange``），用作 ROS 参数默认值。"""
+    frame = os.environ.get("SITE_EE_FRAME", "").strip()
+    if frame:
+        return frame
+    sn = os.environ.get("SITE_ROBOT_SN", "").strip()
+    return f"{sn}_flange" if sn else ""
+

build_sim/arm_application/build/lib/arm_application/tactile_cli.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+"""Orisys 视触觉命令行工具。端口 / 配置来自 ``site.yaml`` 的 ``tactile`` 段。"""
+
+from __future__ import annotations
+
+import argparse
+import sys
+import time
+
+from arm_application.orisys_tactile import make_tactile, resolve_orisys_config
+from arm_application.site_io import load_site_dict, site_path
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description="Orisys 触觉命令行工具（site.yaml）")
+    parser.add_argument("--duration", type=float, default=10.0)
+    parser.add_argument("--site", default="")
+    args = parser.parse_args()
+    data = load_site_dict(args.site or None)
+    t = data.get("tactile") or {}
+    cfg_name = resolve_orisys_config(str(t.get("config") or "dd03"), str(t.get("config_file") or ""))
+    print(
+        f"site={site_path() if not args.site else args.site} "
+        f"backend={t.get('backend') or 'orisys'} "
+        f"left={t.get('left_video', 0)} right={t.get('right_video', 2)} "
+        f"config={cfg_name}"
+    )
+    tactile = make_tactile(data)
+    try:
+        tactile.connect()
+    except Exception as exc:
+        print(f"错误：{exc}", file=sys.stderr)
+        return 1
+    t0 = time.time()
+    n = 0
+    try:
+        while time.time() - t0 < args.duration:
+            sample = tactile.read_sample()
+            n += 1
+            raw = sample.raw or {}
+            left = raw.get("left") or {}
+            right = raw.get("right") or {}
+            if n % 10 == 0:
+                print(
+                    f"n={n:4d} contact={int(sample.contact)} "
+                    f"L F={float(left.get('fnormal') or 0):8.2f} "
+                    f"R F={float(right.get('fnormal') or 0):8.2f} "
+                    f"Fz={float(sample.force_xyz[0]):8.2f}"
+                )
+            time.sleep(0.01)
+    except KeyboardInterrupt:
+        print("已中断")
+    finally:
+        tactile.disconnect()
+    print(f"完成 frames={n}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

build_sim/arm_application/build/lib/arm_application/tactile_interface.py

@@ -0,0 +1,84 @@
+"""触觉采样 / 指令类型。后端（Orisys / mock）可替换。"""
+
+from __future__ import annotations
+
+import time
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from typing import Any
+
+import numpy as np
+
+
+@dataclass
+class TactileSample:
+    timestamp: float
+    force_xyz: np.ndarray
+    torque_xyz: np.ndarray
+    contact: bool
+    embedding: np.ndarray | None = None
+    raw: dict[str, Any] | None = None
+
+
+@dataclass
+class TactileCommand:
+    delta_xyz: np.ndarray
+    delta_rpy: np.ndarray
+    gripper_pos: float | None = None
+    done: bool = False
+    ok: bool = True
+    message: str = ""
+
+
+class TactileInterface(ABC):
+    name: str = "tactile_base"
+
+    @abstractmethod
+    def connect(self) -> None: ...
+
+    @abstractmethod
+    def disconnect(self) -> None: ...
+
+    @abstractmethod
+    def reset(self, hole_id: int) -> None: ...
+
+    @abstractmethod
+    def step(self, sample: TactileSample) -> TactileCommand: ...
+
+
+class MockTactile(TactileInterface):
+    name = "mock_tactile"
+
+    def __init__(self) -> None:
+        self._i = 0
+        self._hole = 0
+
+    def connect(self) -> None:
+        self._i = 0
+
+    def disconnect(self) -> None:
+        pass
+
+    def reset(self, hole_id: int) -> None:
+        self._hole = hole_id
+        self._i = 0
+
+    def read_sample(self) -> TactileSample:
+        self._i += 1
+        return TactileSample(
+            timestamp=time.time(),
+            force_xyz=np.zeros(3),
+            torque_xyz=np.zeros(3),
+            contact=self._i >= 8,
+        )
+
+    def step(self, sample: TactileSample) -> TactileCommand:
+        self._i += 1
+        done = self._i >= 15
+        return TactileCommand(
+            delta_xyz=np.zeros(3),
+            delta_rpy=np.zeros(3),
+            done=done,
+            ok=True,
+            message=f"mock hole={self._hole} step={self._i}",
+        )

build_sim/arm_application/build/lib/arm_application/tactile_node.py

@@ -0,0 +1,305 @@
+#!/usr/bin/env python3
+"""Publish Orisys dual-finger tactile status. Config from site.yaml tactile:.
+
+架构（厂家示例 two_sensor_pipeline 的推荐）：每个传感器一个独立
+multiprocessing 进程（独立 CUDA 上下文），ROS 主进程只做队列→话题转发。
+原因：实测单进程内双开两路传感器会互相拖垮（30→0.8fps）；独立进程稳定 30fps。
+"""
+
+from __future__ import annotations
+
+import multiprocessing as mp
+import signal
+import time
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+
+from arm_application.image_utils import UnsupportedImageError, ndarray_to_image
+from arm_application.orisys_tactile import load_orisys_from_site, tactile_worker
+from arm_application.site_io import load_site_dict, site_path
+from arm_msgs.msg import TactileStatus
+from sensor_msgs.msg import Image
+
+from arm_application.ros_node import TolerantNode
+
+
+class TactileNode(TolerantNode):
+    def __init__(self) -> None:
+        super().__init__("tactile")
+        self.declare_parameter("publish_hz", 20.0)
+        # 下面两个可以用 ROS 参数覆盖 site.yaml，便于不改站点文件就做验证
+        # （例如 mock 后端 + 打开图像话题来确认链路通）。
+        self.declare_parameter("backend", "")
+        self.declare_parameter("image_hz", -1.0)
+        data = load_site_dict()
+        t = data.get("tactile") or {}
+        hz = float(t.get("publish_hz") or self.get_parameter("publish_hz").value or 20.0)
+        backend_override = str(self.get_parameter("backend").value).strip()
+        self._backend = (
+            backend_override or str(t.get("backend") or "orisys")
+        ).strip().lower()
+        self._mock = self._backend in ("mock", "mock_orisys")
+
+        self._pub = {
+            side: self.create_publisher(TactileStatus, f"/tactile/{side}/status", 10)
+            for side in ("left", "right")
+        }
+        self._latest: dict[str, tuple[float, dict]] = {}
+        self._connected = {"left": False, "right": False}
+        self._mock_n = 0
+        self._workers: list = []
+        self._queues: dict[str, object] = {}
+        self._stop = None
+
+        # --- 视触觉图像 / 力矢量场 ---
+        # 默认关闭（image_hz=0）：图像每帧几百 KB，不该拖累三力的 30fps 时序。
+        # 打开后走一条 maxsize=1 的独立队列，满则丢旧留新 —— 主进程来不及消费
+        # 只会丢图像，三力一条不受影响。
+        self._image_hz = (
+            float(self.get_parameter("image_hz").value)
+            if float(self.get_parameter("image_hz").value) >= 0
+            else float(t.get("image_hz") or 0.0)
+        )
+        self._image_q = None
+        self._image_warned: set[str] = set()
+        self._image_pub: dict[str, object] = {}
+        self._vector_pub: dict[str, object] = {}
+        self._image_count: dict[str, int] = {}
+        self._last_mock_img = 0.0
+
+        # 图像 publisher 与后端无关 —— mock 后端也要能发。
+        # 否则"图像话题到底通不通"只能等到有相机、上真机那天才发现。
+        if self._image_hz > 0:
+            for side in ("left", "right"):
+                self._image_pub[side] = self.create_publisher(
+                    Image, f"/tactile/{side}/image", 5
+                )
+                self._vector_pub[side] = self.create_publisher(
+                    Image, f"/tactile/{side}/vector", 5
+                )
+            self.get_logger().info(
+                f"视触觉图像已开启: {self._image_hz:.1f}Hz "
+                f"→ /tactile/{{left,right}}/image 与 /vector"
+            )
+
+        if self._mock:
+            self._connected = {"left": True, "right": True}
+            self.get_logger().info("触觉使用 mock 后端（假数据，不接真实传感器）")
+        else:
+            cfg = load_orisys_from_site(data)
+            ctx = mp.get_context("spawn")  # spawn：子进程不带 DDS 线程，CUDA 上下文干净
+            self._stop = ctx.Event()
+            if self._image_hz > 0:
+                self._image_q = ctx.Queue(maxsize=1)
+            for side in ("left", "right"):
+                q = ctx.Queue(maxsize=2)
+                self._queues[side] = q
+                p = ctx.Process(
+                    target=tactile_worker,
+                    args=(side, cfg, q, self._stop, self._image_q),
+                    name=f"tactile_{side}", daemon=True,
+                )
+                p.start()
+                self._workers.append(p)
+            self.get_logger().info(
+                f"tactile workers spawned (left pid={self._workers[0].pid} right pid={self._workers[1].pid}) "
+                f"site={site_path()}"
+            )
+        self.create_timer(1.0 / max(1e-3, hz), self._tick)
+
+    def _drain(self, side: str) -> None:
+        q = self._queues.get(side)
+        if q is None:
+            return
+        latest = None
+        while True:
+            try:
+                latest = q.get_nowait()
+            except Exception:
+                break
+        if latest is None:
+            return
+        if latest.get("error"):
+            self.get_logger().error(f"[{side}] {latest['error']}", throttle_duration_sec=3.0)
+            return
+        self._connected[side] = True
+        self._latest[side] = (time.monotonic(), latest)
+
+    def _drain_images(self) -> None:
+        """把 worker 推来的最新图像 / 力矢量场发布出去。
+
+        只取最新一帧：worker 已按 ``image_hz`` 节流、队列 ``maxsize=1``，
+        所以这里读到的永远是最新画面；主进程偶尔卡一下，丢掉的是中间帧
+        而不是最新帧。
+        """
+        if self._image_q is None:
+            return
+        latest = None
+        while True:
+            try:
+                latest = self._image_q.get_nowait()
+            except Exception:
+                break
+        if latest is None:
+            return
+
+        side = str(latest.get("side") or "")
+        if side not in self._image_pub:
+            return
+        stamp = self.get_clock().now().to_msg()
+        frame_id = f"{side}_tactile"
+
+        self._publish_array(
+            self._image_pub[side], latest.get("img"), frame_id, stamp, side, "image"
+        )
+        self._publish_array(
+            self._vector_pub[side], latest.get("vector"), frame_id, stamp, side, "vector"
+        )
+
+    def _publish_array(self, pub, array, frame_id: str, stamp, side: str, tag: str) -> None:
+        """打包并发布一个数组；形状/类型不支持时跳过该话题而不是崩溃。
+
+        ``array is None`` 是正常情况（该侧没有矢量场输出、或本帧没算），
+        不报错也不发空图 —— 发一张全零的假图会让下游以为接触到了东西。
+        """
+        if array is None:
+            return
+        key = f"{side}_{tag}"
+        try:
+            msg = ndarray_to_image(array, frame_id=frame_id, stamp=stamp)
+        except UnsupportedImageError as exc:
+            # 编码不支持是固定的（SDK 给的 dtype 不会变），每侧每种图只提醒一次，
+            # 否则 5Hz 的警告会把现场日志冲掉。
+            if key not in self._image_warned:
+                self._image_warned.add(key)
+                self.get_logger().warn(
+                    f"[{key}] 无法打包成 Image，已跳过该话题: {exc}"
+                )
+            return
+
+        try:
+            pub.publish(msg)
+        except Exception as exc:
+            if f"{key}_pub" not in self._image_warned:
+                self._image_warned.add(f"{key}_pub")
+                self.get_logger().warn(f"[{key}] 发布失败: {exc}")
+            return
+
+        if self._image_count.get(key, 0) == 0:
+            self.get_logger().info(
+                f"[{key}] 首帧已发布 ({msg.width}x{msg.height} {msg.encoding})"
+            )
+        self._image_count[key] = self._image_count.get(key, 0) + 1
+
+    def _publish_mock_images(self, force: float) -> None:
+        """mock 后端下造一张形变图 + 一张力矢量场，用来打通图像发布链路。
+
+        没有传感器时也要能把 ``/tactile/<side>/image`` 与 ``/vector`` 跑起来 ——
+        否则这两个话题是否真的能出图，只能等到上真机那天才知道。
+
+        图像内容模仿视触觉的形变：中间一个高斯隆起（法向深度），
+        连同指向中心的切向流（矢量场），接触越深幅度越大。
+        """
+        if not self._image_pub:
+            return
+        stamp = self.get_clock().now().to_msg()
+        h, w = 48, 64
+        yy, xx = np.mgrid[0:h, 0:w]
+        cx, cy = w / 2.0, h / 2.0
+        bump = np.exp(-(((xx - cx) ** 2 + (yy - cy) ** 2) / (2.0 * 9.0 ** 2)))
+
+        depth = (bump * (0.05 + 0.5 * float(force))).astype(np.float32)
+        vx = ((cx - xx) / w * bump * float(force)).astype(np.float32)
+        vy = ((cy - yy) / h * bump * float(force)).astype(np.float32)
+        vector = np.dstack([vx, vy])
+
+        for side in ("left", "right"):
+            frame = f"{side}_tactile"
+            self._publish_array(self._image_pub[side], depth, frame, stamp, side, "image")
+            self._publish_array(self._vector_pub[side], vector, frame, stamp, side, "vector")
+
+    def _tick(self) -> None:
+        self._drain_images()
+        if self._mock:
+            self._mock_n += 1
+            force = 0.2 if self._mock_n >= 8 else 0.0
+            for side in ("left", "right"):
+                self._publish(side, {
+                    "fnormal": force, "fshearx": 0.0, "fsheary": 0.0,
+                    "fps": 30.0, "is_contact": self._mock_n >= 8,
+                    "centroid": (0.0, 0.0),
+                })
+            # mock 的图像也按 image_hz 节流，避免比真机行为更"勤快"。
+            if self._image_hz > 0:
+                now = time.monotonic()
+                if now - self._last_mock_img >= 1.0 / self._image_hz:
+                    self._last_mock_img = now
+                    self._publish_mock_images(force)
+            return
+        for side in ("left", "right"):
+            self._drain(side)
+            item = self._latest.get(side)
+            if item is None:
+                continue
+            ts, data = item
+            if time.monotonic() - ts > 0.6:
+                continue  # 数据太旧（worker 卡住）不发布
+            self._publish(side, data)
+
+    def _publish(self, side: str, data: dict) -> None:
+        msg = TactileStatus()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = f"{side}_tactile"
+        msg.side = side
+        msg.connected = bool(self._connected.get(side, False))
+        msg.contact = bool(data.get("is_contact", False))
+        msg.fnormal = float(data.get("fnormal") or 0.0)
+        msg.fshearx = float(data.get("fshearx") or 0.0)
+        msg.fsheary = float(data.get("fsheary") or 0.0)
+        msg.fps = float(data.get("fps") or 0.0)
+        c = data.get("centroid") or (0.0, 0.0)
+        msg.centroid_u = float(c[0])
+        msg.centroid_v = float(c[1])
+        msg.error_msg = ""
+        self._pub[side].publish(msg)
+
+    def destroy_node(self) -> bool:
+        try:
+            if self._stop is not None:
+                self._stop.set()
+            for p in self._workers:
+                p.join(timeout=2.0)
+                if p.is_alive():
+                    p.terminate()
+            for p in self._workers:
+                p.join(timeout=1.5)
+                if p.is_alive():
+                    try:
+                        p.kill()  # 还赖着就 SIGKILL，绝不留孤儿占摄像头
+                    except Exception:
+                        pass
+        finally:
+            return super().destroy_node()
+
+
+def main() -> None:
+    rclpy.init()
+    node = TactileNode()
+
+    def _sigterm(_sig, _frm) -> None:
+        raise KeyboardInterrupt
+
+    signal.signal(signal.SIGTERM, _sigterm)  # SIGTERM 也走干净退出，释放相机
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

build_sim/arm_application/build/lib/arm_application/weights_layout.py

@@ -0,0 +1,448 @@
+"""权重模块的目录布局、模型发现与标签归属（纯逻辑）。
+
+这个模块刻意**只依赖标准库 + model_labels**，不 import ROS、不 import
+ultralytics/tensorrt。原因：
+
+1. 可以在没有 GPU、没有 ROS 的环境里跑单元测试；
+2. ``scripts/weights.sh`` 这个命令行工具能直接用上，不必先 ``source`` 一整套环境。
+
+真正干重活的转换（PyTorch → ONNX → TensorRT）在 ``scripts/weights.py`` 里，
+那部分才需要 ``ros_flexiv`` 环境和 GPU。
+
+## 目录约定
+
+    models/
+      README.md                ← 模块规则（要改这个模块先读它）
+      staging/                 ← 非标准格式先丢这里，转换工具有序地消费它
+      <分组>/                  ← 例如 yolo/、hole_insert/
+        <模型名>.onnx
+        <模型名>.engine
+        <模型名>.labels.yaml   ← 标签（旁挂，可手改，进 git）
+
+"分组"只是一个子目录名，用来分门别类，没有语义约束。默认 ``yolo``。
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Dict, List, Optional, Sequence, Tuple, Union
+
+from .model_labels import (
+    SOURCE_MODEL,
+    SOURCE_ONNX,
+    SOURCE_PLACEHOLDER,
+    SOURCE_SIDECAR,
+    LabelSet,
+    label_file_status,
+    labels_path_for,
+    labels_from_model,
+    labels_from_onnx,
+    placeholder_labels,
+    read_label_file,
+)
+
+# 会被当成"权重"的文件后缀（不带点的小写形式）。
+WEIGHT_EXTS: Tuple[str, ...] = (
+    "pt",
+    "pth",
+    "onnx",
+    "engine",
+    "trt",
+    "plan",
+    "tflite",
+)
+
+# 转换工具认识的输入格式。其它格式一律归到 staging 等人工处理。
+# 注意 pth 也收：虽然 Ultralytics 主要用 .pt，但用户手里常是 .pth。
+CONVERTIBLE_INPUT_EXTS: Tuple[str, ...] = ("pt", "pth", "onnx")
+
+# 已经被转换过的产物，重转没有意义。
+FINAL_EXTS: Tuple[str, ...] = ("engine", "trt", "plan")
+
+# 非标准格式/待转换文件放这里。
+STAGING_DIRNAME = "staging"
+
+# 读标签时挑格式的顺序：onnx 有内嵌 metadata 且读取代价极低（毫秒级），
+# 所以排在前面；engine 排最后是因为它很可能什么都没有。
+LABEL_READ_ORDER: Tuple[str, ...] = ("onnx", "pt", "pth", "engine", "trt")
+
+
+# ---------------------------------------------------------------------------
+# 仓库根 / 权重根
+# ---------------------------------------------------------------------------
+
+
+def find_repo_root(start: Optional[Union[str, Path]] = None) -> Optional[Path]:
+    """从 ``start`` 往上找仓库根（以存在 ``config/site.yaml`` 为标志）。
+
+    这样无论代码是从 ``src/`` 直接跑、还是从 ``install/`` 跑，
+    都能定位到同一份 ``models/``，不会出现"两份权重"。
+    """
+    here = Path(start).resolve() if start else Path(__file__).resolve()
+    for cand in [here, *here.parents]:
+        if (cand / "config" / "site.yaml").is_file():
+            return cand
+    return None
+
+
+def default_weights_root() -> Path:
+    """``models/`` 的默认位置。找不到仓库根时退回当前工作目录下的 models/。"""
+    import os
+
+    env = os.environ.get("SITE_WEIGHTS_ROOT")
+    if env:
+        return Path(env).expanduser()
+    root = find_repo_root()
+    if root is not None:
+        return root / "models"
+    return Path.cwd() / "models"
+
+
+# ---------------------------------------------------------------------------
+# 模型条目
+# ---------------------------------------------------------------------------
+
+
+@dataclass
+class ModelEntry:
+    """一个"模型" = 权重目录里同名前缀的一组文件。"""
+
+    group: str
+    name: str
+    files: Dict[str, Path] = field(default_factory=dict)
+
+    @property
+    def dir(self) -> Path:
+        if not self.files:
+            return Path(".")
+        return next(iter(self.files.values())).parent
+
+    @property
+    def labels_path(self) -> Path:
+        return self.dir / (self.name + ".labels.yaml")
+
+    @property
+    def exts(self) -> List[str]:
+        """已存在的格式，按固定顺序返回，保证输出稳定。
+
+        顺序就是转换流水线的顺序：``pt → onnx → engine``（源头 → 中间 → 最终）。
+        这样列表里一眼能看出这个模型走到哪一步了。
+        """
+        order = list(WEIGHT_EXTS)
+        return sorted(self.files, key=lambda e: (order.index(e) if e in order else 99, e))
+
+    def path(self, ext: str) -> Optional[Path]:
+        return self.files.get(ext.lower().lstrip("."))
+
+    def sorted_files(self) -> List[Path]:
+        """按读标签的推荐顺序给出文件列表。"""
+        out: List[Path] = []
+        for ext in LABEL_READ_ORDER:
+            p = self.files.get(ext)
+            if p:
+                out.append(p)
+        for ext in self.exts:
+            p = self.files.get(ext)
+            if p and p not in out:
+                out.append(p)
+        return out
+
+    def missing_of(self, wanted: Sequence[str]) -> List[str]:
+        return [e for e in wanted if e not in self.files]
+
+
+# ---------------------------------------------------------------------------
+# 发现
+# ---------------------------------------------------------------------------
+
+
+def is_staging(path: Path) -> bool:
+    return path.name == STAGING_DIRNAME
+
+
+def groups(root: Union[str, Path]) -> List[str]:
+    """列出所有权重分组目录（跳过 staging 与隐藏目录）。"""
+    r = Path(root)
+    if not r.is_dir():
+        return []
+    return sorted(
+        p.name
+        for p in r.iterdir()
+        if p.is_dir() and not p.name.startswith(".") and not is_staging(p)
+    )
+
+
+def staging_dir(root: Union[str, Path]) -> Path:
+    return Path(root) / STAGING_DIRNAME
+
+
+def infer_group(
+    root: Union[str, Path],
+    source: Union[str, Path],
+    fallback: str = "yolo",
+) -> str:
+    """转换产物该落到**哪个分组**：默认沿用源文件所在的分组。
+
+    为什么不能一律落到 ``yolo``（2026-09-28 真机踩到，代价是一次白转）
+    ----------------------------------------------------------------
+    现场把 ``models/hole_insert/detect_best_1.onnx`` 转 engine，工具默认
+    ``--group yolo``，于是产物落到 ``models/yolo/detect_best_1.engine`` ——
+    而运行时找 engine 的规则是**同级同名**（``yolo_detector.resolve_weights``：
+    ``p.with_suffix(".engine")``，即 site.yaml 指的 ``.onnx`` 旁边）。两边
+    一分家，engine 就等于不存在：现象是"启动即报缺 engine"，而文件明明躺着。
+    这正是本项目最忌讳的**静默失效** —— 工具没报错，位置也"合理"。
+
+    规则：
+      - 源文件已在权重根目录下的某个分组里（例如 ``models/hole_insert/x.onnx``）
+        → 沿用 ``hole_insert``。这样"就地转换"永远落在原地，运行时必然找得到；
+      - 源文件在 ``staging/``、权重根目录之外或根目录正下方 → ``fallback``
+        （默认 ``yolo``）：那才是"新来的模型，还没决定分组"的正常情形。
+
+    Args:
+        root: 权重根目录（``models/``）。
+        source: 输入文件。
+        fallback: 无法沿用时的分组名。
+
+    Returns:
+        分组名（不带路径）。
+    """
+    r = Path(root)
+    try:
+        rel = Path(source).expanduser().resolve().relative_to(r.resolve())
+    except ValueError:
+        # 不在权重根目录下（/tmp、外部磁盘、staging 之类）→ 没有"原地"可言
+        return fallback
+    if len(rel.parts) < 2:
+        # 直接躺在 models/ 正下方：没有分组信息，用默认分组
+        return fallback
+    group = rel.parts[0]
+    if is_staging(r / group) or group.startswith("."):
+        return fallback
+    return group
+
+
+def staging_files(root: Union[str, Path]) -> List[Path]:
+    """staging 里的待处理文件。
+
+    跳过隐藏文件（``.gitkeep`` 等）和 ``.md`` 说明文档 —— 那些是给人和 git 看的，
+    不是待转换的模型。否则 ``doctor`` 会把 README 报成"格式不支持"。
+    """
+    d = staging_dir(root)
+    if not d.is_dir():
+        return []
+    return sorted(
+        p
+        for p in d.iterdir()
+        if p.is_file() and not p.name.startswith(".") and p.suffix.lower() != ".md"
+    )
+
+
+def discover(root: Optional[Union[str, Path]] = None) -> List[ModelEntry]:
+    """扫描权重根，按"分组 + 模型名"归并出所有模型。
+
+    同名不同格式（m.onnx / m.engine / m.pt）会被合成一个条目 —— 因为它们
+    共用一个模型名、一份标签。``.labels.yaml`` 本身不算权重。
+    """
+    r = Path(root) if root is not None else default_weights_root()
+    out: List[ModelEntry] = []
+    if not r.is_dir():
+        return out
+    for g in groups(r):
+        gdir = r / g
+        by_name: Dict[str, ModelEntry] = {}
+        for p in sorted(gdir.iterdir()):
+            if not p.is_file() or p.name.startswith("."):
+                continue
+            ext = p.suffix.lower().lstrip(".")
+            if ext not in WEIGHT_EXTS:
+                continue
+            # m.labels.yaml 的后缀是 .yaml，不会落进 WEIGHT_EXTS，天然被排除。
+            stem = p.name[: -(len(ext) + 1)]
+            entry = by_name.get(stem)
+            if entry is None:
+                entry = ModelEntry(group=g, name=stem, files={})
+                by_name[stem] = entry
+            entry.files[ext] = p
+        out.extend(by_name.values())
+    return out
+
+
+def find_model(
+    root: Optional[Union[str, Path]] = None,
+    name: str = "",
+    group: Optional[str] = None,
+) -> Optional[ModelEntry]:
+    """按模型名找模型。
+
+    - ``name`` 是完整路径 → 直接按路径构造条目（不要求它在权重目录里）
+    - ``name`` 是 ``分组/模型名`` → 按分组+名字精确查找
+    - ``name`` 是裸名字 → 在所有分组里找同名模型；``group`` 可缩小范围
+
+    裸名字跨分组同名时会优先返回 ``group`` 指定的那个；都没指定就返回找到的第一个，
+    并由调用方提示存在歧义。
+    """
+    p = Path(name).expanduser()
+    if p.suffix and (p.is_file() or p.parent.is_dir() and str(name).count("/")):
+        ext = p.suffix.lower().lstrip(".")
+        if ext in WEIGHT_EXTS:
+            entry = ModelEntry(group=group or p.parent.name, name=p.stem, files={})
+            entry.files[ext] = p
+            return entry
+
+    for e in discover(root):
+        if e.name != name:
+            continue
+        if group and e.group != group:
+            continue
+        return e
+
+    # 裸名字没匹配上，再试 ``分组/模型名``。
+    # ``list`` 的输出里模型是带分组一起显示的，用户照着敲 ``yolo/detect_best``
+    # 非常自然；以前这里会直接报"找不到模型"，而报错又只列出裸名字，很别扭。
+    # 放在裸名字查找之后，是为了不把真正的相对路径（``a/b.onnx``）误判成分组名。
+    if group is None and "/" in name:
+        head, _, tail = name.rpartition("/")
+        if head and tail:
+            return find_model(root, tail, head)
+    return None
+
+
+# ---------------------------------------------------------------------------
+# 标签归属
+# ---------------------------------------------------------------------------
+
+
+def model_labels(entry: ModelEntry, *, deep: bool = False) -> LabelSet:
+    """给出某个模型当前生效的标签，附带来源。
+
+    优先级：旁挂文件 > ONNX 内嵌 > 模型内嵌 > 占位符。
+
+    ``deep=False``（默认）只读旁挂文件和 ONNX metadata —— 都快。
+    ``deep=True`` 才会去加载模型本身（``.pt`` / ``.engine``），
+    代价是要起 Ultralytics、加载权重，秒级，所以默认不开。
+    这样 ``weights.sh list`` 能秒出结果，不因为几个 .pt 卡十几秒。
+    """
+    files = entry.sorted_files()
+    if not files:
+        return LabelSet(names={}, source=SOURCE_PLACEHOLDER, path=None)
+
+    side = label_file_status(files[0])[1]
+    names = read_label_file(side) if side else None
+    if names:
+        return LabelSet(names=names, source=SOURCE_SIDECAR, path=side)
+
+    for p in files:
+        ext = p.suffix.lower().lstrip(".")
+        if ext == "onnx":
+            got = labels_from_onnx(p)
+            if got:
+                return LabelSet(names=got[0], source=SOURCE_ONNX, path=p)
+        elif deep and ext in ("pt", "pth", "engine", "trt", "plan"):
+            # engine 在当前的 TensorRT 下读不到（没有 metadata API），
+            # 但万一用户是拿 Ultralytics 导出的，还是值得试一次。
+            got = labels_from_model(p)
+            if got:
+                return LabelSet(names=got[0], source=SOURCE_MODEL, path=p)
+
+    return LabelSet(names=placeholder_labels(0), source=SOURCE_PLACEHOLDER, path=None)
+
+
+# ---------------------------------------------------------------------------
+# 转换规划
+# ---------------------------------------------------------------------------
+
+
+@dataclass(frozen=True)
+class ConversionChain:
+    """从某个输入格式到目标格式，需要依次产出哪些文件。"""
+
+    source_ext: str
+    target: str
+    produce: Tuple[str, ...]
+
+    @property
+    def is_noop(self) -> bool:
+        return not self.produce
+
+    def describe(self) -> str:
+        if self.is_noop:
+            return f"{self.source_ext} → 无需转换"
+        return f"{self.source_ext} → " + " → ".join(self.produce)
+
+
+def conversion_chain(source_ext: str, target: str = "all") -> ConversionChain:
+    """算出转换链。
+
+    TensorRT 的 engine 只能由 ONNX 构建，所以任何通往 engine 的路都必须
+    先得到 ONNX。这点在这里集中表达，避免调用方各写一遍。
+
+    - ``target="all"``：能产出多少产出多少（.pt → ONNX + engine）
+    - ``target="onnx"``：只要 ONNX（engine 无法反向拆回 ONNX，返回空）
+    - ``target="engine"``：只要 engine（.pt 会顺带产出 ONNX 作为中间产物）
+    """
+    src = source_ext.lower().lstrip(".")
+    tgt = (target or "all").lower().strip()
+    if tgt not in ("all", "onnx", "engine"):
+        raise ValueError(f"未知转换目标: {target}（可选 all/onnx/engine）")
+
+    if src not in CONVERTIBLE_INPUT_EXTS:
+        # 已经是最终产物，或干脆不认识 —— 都交给调用方去报错/跳过。
+        return ConversionChain(src, tgt, ())
+
+    if src in ("pt", "pth"):
+        if tgt == "onnx":
+            return ConversionChain(src, tgt, ("onnx",))
+        # engine 需要 ONNX 做中间产物，两条路都带上它。
+        return ConversionChain(src, tgt, ("onnx", "engine"))
+
+    # src == "onnx"
+    if tgt == "onnx":
+        return ConversionChain(src, tgt, ())
+    return ConversionChain(src, tgt, ("engine",))
+
+
+def canonical_path(root: Union[str, Path], group: str, name: str, ext: str) -> Path:
+    """转换产物该落在哪：``models/<group>/<name>.<ext>``。
+
+    这是"自动改名并归位"的落点。无论输入文件叫什么、放在哪
+    （哪怕在 /tmp 或 staging 里叫 ``best_v3_final(1).pt``），
+    产出都用调用方给的 ``name`` 和 ``group``。
+    """
+    return Path(root) / group / f"{name}.{ext.lstrip('.')}"
+
+
+def validate_name(name: str) -> Optional[str]:
+    """检查模型名是否安全可用作文件名。返回错误信息或 None。"""
+    n = (name or "").strip()
+    if not n:
+        return "模型名不能为空"
+    if n != name:
+        return f"模型名首尾不能有空格: {name!r}"
+    if any(c.isspace() for c in n):
+        # 名字里有空格会让命令行、site.yaml、ROS 参数到处都要加引号，
+        # 早晚有人忘。直接禁掉，改用下划线或短横线。
+        return "模型名不能含有空格（建议用 _ 或 - 代替）"
+    bad = set('/\\:*?"<>|')
+    hit = sorted(bad & set(n))
+    if hit:
+        return f"模型名不能含有这些字符: {''.join(hit)}"
+    if n.startswith("."):
+        return "模型名不能以点开头"
+    if n.endswith(".labels"):
+        return "不要带 .labels 后缀，标签文件会自动生成"
+    return None
+
+
+def plan_conversion(
+    root: Union[str, Path],
+    source: Union[str, Path],
+    *,
+    name: str,
+    group: str = "yolo",
+    target: str = "all",
+) -> Tuple[ConversionChain, List[Path]]:
+    """算出一条完整的转换计划：产出链 + 每个产物的最终落点。"""
+    src = Path(source).expanduser()
+    chain = conversion_chain(src.suffix, target)
+    outs = [canonical_path(root, group, name, ext) for ext in chain.produce]
+    return chain, outs

build_sim/arm_application/build/lib/arm_application/yolo_detector.py

@@ -0,0 +1,560 @@
+#!/usr/bin/env python3
+"""Ultralytics YOLO / TensorRT 封装。
+
+对外接口：``resolve_weights``、``YoloDetector``、``Detection``、``pick_best``、
+``MpYoloDetector``。当 ``backend='tensorrt'`` 时，TensorRT 会加载与
+``.onnx`` / ``.pt`` 路径同级的 ``.engine`` 文件。
+
+类别名（标签）由 ``model_labels`` 统一提供：优先读权重旁边的
+``<模型名>.labels.yaml``（用户可随时手改），其次读模型内嵌 metadata。
+这样做是因为 TensorRT engine 在当前的 TensorRT Python API 下**无法携带标签**，
+只能靠旁挂文件；详见 ``model_labels`` 模块头。
+"""
+
+from __future__ import annotations
+
+import logging
+import multiprocessing as mp
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import List, Optional, Sequence, Union
+
+import numpy as np
+
+from .model_labels import (
+    SOURCE_PLACEHOLDER,
+    LabelSet,
+    describe,
+    parse_names_field,
+    placeholder_labels,
+    read_labels,
+)
+
+logger = logging.getLogger(__name__)
+
+# 哨兵值：worker 还停在上一次的那帧 —— 调用方不能把它算作漏检。
+INFER_BUSY = object()
+
+
+@dataclass
+class Detection:
+    xyxy: np.ndarray  # (4,) float：x1,y1,x2,y2
+    conf: float
+    cls_id: int
+    cls_name: str
+
+    @property
+    def cx(self) -> float:
+        return float(0.5 * (self.xyxy[0] + self.xyxy[2]))
+
+    @property
+    def cy(self) -> float:
+        return float(0.5 * (self.xyxy[1] + self.xyxy[3]))
+
+    @property
+    def area(self) -> float:
+        return float(max(0.0, self.xyxy[2] - self.xyxy[0]) * max(0.0, self.xyxy[3] - self.xyxy[1]))
+
+
+def _resolve_device(device: str) -> str:
+    try:
+        import torch
+    except Exception:  # noqa: BLE001
+        return "cpu"
+    d = (device or "").strip().lower()
+    if d in ("", "auto", "cuda", "cuda:0", "0", "gpu"):
+        return "0" if torch.cuda.is_available() else "cpu"
+    return device.strip() or "cpu"
+
+
+def resolve_weights(weights: Union[str, Path], backend: str = "auto") -> Path:
+    """当 backend=auto/tensorrt 时，优先使用与 .pt 同级的 TensorRT .engine。"""
+    p = Path(weights).expanduser()
+    b = (backend or "auto").strip().lower()
+    engine = p.with_suffix(".engine")
+    onnx = p.with_suffix(".onnx")
+    if b in ("tensorrt", "engine"):
+        if engine.is_file():
+            return engine
+        raise FileNotFoundError(
+            f"缺少 TensorRT engine：{engine}。"
+            f"导出命令：bash ~/ros2/scripts/weights.sh convert {p} --name {p.stem}"
+        )
+    if b == "onnx":
+        if onnx.is_file():
+            return onnx
+        raise FileNotFoundError(f"缺少 ONNX 文件：{onnx}")
+    if b in ("", "auto"):
+        if engine.is_file():
+            return engine
+        if p.is_file():
+            return p
+        if onnx.is_file():
+            return onnx
+    return p
+
+
+def resolve_class_labels(path: Union[str, Path], model=None) -> LabelSet:
+    """给出一个权重该用的类别标签（旁挂文件优先）。
+
+    ``model`` 是已经加载好的 Ultralytics 模型对象（可为 None）。顺序：
+
+    1. ``<模型名>.labels.yaml`` —— 用户可编辑的权威来源
+    2. 传入的 ``model.names`` —— 模型内嵌 metadata
+    3. 占位符 ``class_0 / class_1 …``
+
+    第 3 步刻意不用 ``str(cid)``。原先读不到名字时会退化成类名 `"0"`，
+    和"真的有个叫 0 的类"无法区分，标签丢了都看不出来。
+    占位符 + 一条 warning 让这件事在日志里可见。
+    """
+    p = Path(path).expanduser()
+
+    # 先只看旁挂文件（快，不加载模型）：它必须能覆盖模型内嵌的旧值，
+    # 否则"我改了标签为什么不生效"会变成很难查的问题。
+    labels = read_labels(p, allow_model_metadata=False)
+    if not labels.is_placeholder:
+        return labels
+
+    raw = getattr(model, "names", None) if model is not None else None
+    names = parse_names_field(raw or {})
+    if names:
+        from .model_labels import SOURCE_MODEL
+
+        return LabelSet(names=names, source=SOURCE_MODEL, path=p)
+
+    nc = 0
+    try:
+        nc = int(getattr(model, "nc", 0) or 0)
+    except (TypeError, ValueError):
+        nc = 0
+    labels = LabelSet(names=placeholder_labels(nc), source=SOURCE_PLACEHOLDER, path=None)
+    logger.warning(
+        "模型 %s 没有类别标签，暂时用占位名显示。补一个 %s 即可（改完重跑节点生效）。"
+        "命令行: bash ~/ros2/scripts/weights.sh labels %s --from-model",
+        p.name,
+        p.stem + ".labels.yaml",
+        p.stem,
+    )
+    return labels
+
+
+class YoloDetector:
+    def __init__(
+        self,
+        weights: Union[str, Path],
+        *,
+        conf: float = 0.5,
+        iou: float = 0.45,
+        device: str = "",
+        imgsz: int = 640,
+        class_ids: Optional[Sequence[int]] = None,
+        class_names: Optional[Sequence[str]] = None,
+    ) -> None:
+        try:
+            from ultralytics import YOLO
+        except Exception as exc:  # noqa: BLE001
+            raise RuntimeError(
+                "未安装 ultralytics。在 ros_flexiv 环境里执行：pip install ultralytics"
+            ) from exc
+
+        path = Path(weights).expanduser()
+        if not path.is_file():
+            raise FileNotFoundError(
+                f"找不到 YOLO 权重：{path}。"
+                f"请把你的 .pt 放到 ~/ros2/models/yolo/workpiece.pt"
+            )
+        try:
+            import torch
+
+            # 别让 PyTorch 占满所有 CPU 核心（相机 + ROS 都要用）。
+            torch.set_num_threads(1)
+            torch.set_num_interop_threads(1)
+            if torch.cuda.is_available():
+                torch.backends.cudnn.benchmark = True
+        except Exception:
+            pass
+
+        self.device = _resolve_device(device)
+        self.conf = float(conf)
+        self.iou = float(iou)
+        self.imgsz = int(imgsz)
+        self._half_ok = False
+        self._is_engine = path.suffix.lower() in {".engine", ".trt"}
+        self._model = YOLO(str(path), task="detect")
+        if not self._is_engine:
+            try:
+                self._model.fuse()
+            except Exception:
+                pass
+        self.class_ids = set(int(v) for v in class_ids) if class_ids else None
+        self.class_names = (
+            {str(n).strip().lower() for n in class_names if str(n).strip()}
+            if class_names
+            else None
+        )
+        self._labels = resolve_class_labels(path, self._model)
+        self._id_to_name = dict(self._labels.names)
+        self._warmed = False
+        self._pending = None
+
+    def warmup(self, *, half: bool = False) -> None:
+        if self._warmed:
+            return
+        if half and (not self._is_engine) and self.device != "cpu":
+            try:
+                self._model.to("cuda")
+                self._model.model.half()
+                self._half_ok = True
+            except Exception:
+                self._half_ok = False
+        dummy = np.zeros((self.imgsz, self.imgsz, 3), dtype=np.uint8)
+        self._predict(dummy)
+        self._warmed = True
+
+    def _predict(self, image_bgr: np.ndarray):
+        kwargs = dict(
+            source=image_bgr,
+            conf=self.conf,
+            iou=self.iou,
+            imgsz=self.imgsz,
+            device=self.device,
+            verbose=False,
+            max_det=20,
+        )
+        if self._half_ok and not self._is_engine:
+            kwargs["quantize"] = 16
+        try:
+            import torch
+
+            ctx = torch.inference_mode()
+        except Exception:
+            from contextlib import nullcontext
+
+            ctx = nullcontext()
+        with ctx:
+            return self._model.predict(**kwargs)
+
+    def detect(
+        self,
+        image_bgr: np.ndarray,
+        conf: float | None = None,
+        iou: float | None = None,
+        imgsz: int | None = None,
+        **_kwargs,
+    ) -> List[Detection]:
+        """给孔检测器用的别名接口（xyxy + conf）。"""
+        if conf is not None:
+            self.conf = float(conf)
+        if iou is not None:
+            self.iou = float(iou)
+        if imgsz is not None:
+            self.imgsz = int(imgsz)
+        return self.infer(image_bgr)
+
+    def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0) -> List[Detection]:
+        del seq
+        if not self._warmed:
+            self.warmup(half=half)
+        results = self._predict(image_bgr)
+        if not results:
+            return []
+        r0 = results[0]
+        boxes = getattr(r0, "boxes", None)
+        if boxes is None or len(boxes) == 0:
+            return []
+
+        out: List[Detection] = []
+        xyxy = boxes.xyxy.detach().cpu().numpy()
+        confs = boxes.conf.detach().cpu().numpy()
+        clss = boxes.cls.detach().cpu().numpy().astype(int)
+        for i in range(len(xyxy)):
+            cid = int(clss[i])
+            # 用 LabelSet.get：读不到名字时给 class_N 占位，而不是退化成裸数字 "0"。
+            name = self._labels.get(cid)
+            if self.class_ids is not None and cid not in self.class_ids:
+                continue
+            if self.class_names is not None and name.lower() not in self.class_names:
+                continue
+            out.append(
+                Detection(
+                    xyxy=xyxy[i].astype(float),
+                    conf=float(confs[i]),
+                    cls_id=cid,
+                    cls_name=name,
+                )
+            )
+        out.sort(key=lambda d: d.conf, reverse=True)
+        return out
+
+    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
+        self._pending = (int(seq), image_bgr)
+
+    def wait_result(self, timeout: float = 0.045):
+        del timeout
+        if self._pending is None:
+            return None
+        seq, img = self._pending
+        self._pending = None
+        t0 = time.monotonic()
+        dets = self.infer(img, seq=seq)
+        return seq, dets, (time.monotonic() - t0) * 1000.0
+
+
+def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
+    """spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。"""
+    try:
+        path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
+        det = YoloDetector(
+            path,
+            conf=float(cfg.get("conf") or 0.25),
+            iou=float(cfg.get("iou") or 0.45),
+            device=str(cfg.get("device") or "0"),
+            imgsz=int(cfg.get("imgsz") or 416),
+        )
+        det.warmup(half=bool(cfg.get("half", True)))
+        q_out.put(("ready", str(path)))
+    except Exception as exc:  # noqa: BLE001
+        q_out.put(("error", str(exc)))
+        return
+    while True:
+        item = q_in.get()
+        if item is None:
+            return
+        # 一直排空到队列里最新的帧 —— 绝不把 GPU 浪费在过期图像上。
+        try:
+            while True:
+                newer = q_in.get_nowait()
+                if newer is None:
+                    return
+                item = newer
+        except Exception:
+            pass
+        seq, image = item
+        try:
+            t0 = time.monotonic()
+            dets = det.infer(image, half=bool(cfg.get("half", True)))
+            gpu_ms = (time.monotonic() - t0) * 1000.0
+            packed = [
+                (d.xyxy.astype(np.float32), float(d.conf), int(d.cls_id), str(d.cls_name))
+                for d in dets
+            ]
+            _put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
+        except Exception as exc:  # noqa: BLE001
+            _put_drop_old(q_out, ("error", str(exc)))
+
+
+def _put_drop_old(q, item) -> None:
+    """绝不让 GPU 进程因为结果队列满了而阻塞。"""
+    try:
+        q.put_nowait(item)
+        return
+    except Exception:
+        pass
+    try:
+        q.get_nowait()
+    except Exception:
+        pass
+    try:
+        q.put_nowait(item)
+    except Exception:
+        pass
+
+
+class YoloInferProcess:
+    """把 YOLO 放进 spawn() 子进程，让 ROS 回调永远不用和 PyTorch 抢 GIL。"""
+
+    def __init__(self, cfg: dict) -> None:
+        self._cfg = dict(cfg)
+        self._p = None
+        self._start()
+
+    def _start(self) -> None:
+        ctx = mp.get_context("spawn")
+        # maxsize=2 + worker 排空到最新：父进程永远不需要主动丢帧
+        # （之前 Queue put_nowait/get_nowait 的竞争，正是 worker 明明活着
+        # 却出现数秒“静默”冻结的根源）。
+        self._q_in = ctx.Queue(maxsize=2)
+        self._q_out = ctx.Queue(maxsize=4)
+        self._p = ctx.Process(
+            target=_yolo_worker_main,
+            args=(self._q_in, self._q_out, self._cfg),
+            name="yolo_infer",
+            daemon=True,
+        )
+        self._p.start()
+        msg = self._q_out.get(timeout=90.0)
+        if not msg or msg[0] != "ready":
+            err = msg[1] if msg and len(msg) > 1 else "worker failed"
+            raise RuntimeError(f"YOLO 工作进程：{err}")
+        self.weights_path = str(msg[1])
+        self.imgsz = int(self._cfg.get("imgsz") or 416)
+
+    def restart(self) -> None:
+        """杀掉卡死 / 挂掉的 worker，再起一个新的（重新加载 engine 约 2-5 s）。"""
+        self.close()
+        self._start()
+
+    @property
+    def alive(self) -> bool:
+        return bool(self._p is not None and self._p.is_alive())
+
+    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
+        """把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
+        排空到队列里最新的帧，所以不会跑任何过期数据。"""
+        if not self.alive:
+            raise RuntimeError("YOLO 工作进程已死亡")
+        try:
+            self._q_in.put((int(seq), image_bgr), timeout=0.010)
+        except Exception:
+            pass
+
+    def wait_result(self, timeout: float = 0.045):
+        """返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
+        if not self.alive:
+            raise RuntimeError("YOLO 工作进程已死亡")
+        deadline = time.monotonic() + max(0.004, float(timeout))
+        latest = None
+        while time.monotonic() < deadline:
+            try:
+                msg = self._q_out.get(
+                    timeout=max(0.001, deadline - time.monotonic())
+                )
+            except Exception:
+                break
+            latest = self._parse_out(msg)
+            while True:
+                try:
+                    extra = self._q_out.get_nowait()
+                except Exception:
+                    break
+                parsed = self._parse_out(extra)
+                if parsed is not None:
+                    latest = parsed
+            if latest is not None:
+                break
+        return latest
+
+    def _parse_out(self, msg):
+        if not msg:
+            return None
+        if msg[0] == "error":
+            raise RuntimeError(str(msg[1]))
+        if msg[0] != "ok":
+            return None
+        seq, packed = int(msg[1]), msg[2]
+        gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
+        dets = [
+            Detection(
+                xyxy=np.asarray(xyxy, dtype=float),
+                conf=float(conf),
+                cls_id=int(cid),
+                cls_name=str(name),
+            )
+            for xyxy, conf, cid, name in packed
+        ]
+        return seq, dets, gpu_ms
+
+    def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):
+        """兼容接口：提交 + 等待。要做流水线的话优先用 submit/wait_result。"""
+        del half
+        self.submit(image_bgr, seq)
+        got = self.wait_result(timeout=0.08)
+        if got is None:
+            return INFER_BUSY
+        return got[1]
+
+    def close(self) -> None:
+        if self._p is None:
+            return
+        try:
+            self._q_in.put_nowait(None)
+        except Exception:
+            pass
+        self._p.join(timeout=2.0)
+        if self._p.is_alive():
+            self._p.terminate()
+            self._p.join(timeout=1.0)
+        if self._p.is_alive():
+            self._p.kill()  # 卡在 CUDA 调用里出不来 —— 直接 SIGKILL
+            self._p.join(timeout=1.0)
+        self._p = None
+
+
+def _iou_xyxy(a: np.ndarray, b: np.ndarray) -> float:
+    ax1, ay1, ax2, ay2 = [float(v) for v in a]
+    bx1, by1, bx2, by2 = [float(v) for v in b]
+    ix1, iy1 = max(ax1, bx1), max(ay1, by1)
+    ix2, iy2 = min(ax2, bx2), min(ay2, by2)
+    inter = max(0.0, ix2 - ix1) * max(0.0, iy2 - iy1)
+    ua = (
+        max(0.0, ax2 - ax1) * max(0.0, ay2 - ay1)
+        + max(0.0, bx2 - bx1) * max(0.0, by2 - by1)
+        - inter
+    )
+    return float(inter / ua) if ua > 1e-6 else 0.0
+
+
+def pick_best(
+    dets: Sequence[Detection],
+    *,
+    prefer_center: Optional[tuple] = None,
+    prev_uv: Optional[np.ndarray] = None,
+    prev_xyxy: Optional[np.ndarray] = None,
+    max_track_px: float = 320.0,
+    reacquire_conf: float = 0.18,
+    lock_iou: float = 0.08,
+    min_conf: float = 0.12,
+    min_area: float = 4000.0,
+) -> Optional[Detection]:
+    """让当前工件在快速运动下也能被持续跟踪。
+
+    用一个较宽的匹配门限去和**预测出的**上一帧检测框做匹配。如果 IoU 不达标、
+    但 YOLO 仍以高于 min_conf 的置信度报出了目标，就保留该检测 —— 不要返回
+    None（那正是运动过程中目标丢失的主要来源）。
+    过小的框（logo / 通止规尖）会被拒掉，避免机械臂锁到垫子上。
+    """
+    if not dets:
+        return None
+    # 已锁定的跟踪在运动（模糊）时可能掉到 min_conf 以下；但新锁定不允许这样。
+    floor = max(0.06, 0.50 * float(min_conf))
+    usable = [d for d in dets if float(d.conf) >= floor and float(d.area) >= float(min_area)]
+    if not usable:
+        return None
+
+    locked: List[Detection] = []
+    if prev_xyxy is not None:
+        pw = max(8.0, float(prev_xyxy[2] - prev_xyxy[0]))
+        ph = max(8.0, float(prev_xyxy[3] - prev_xyxy[1]))
+        gate = max(0.90 * float(max_track_px), 1.25 * float(np.hypot(pw, ph)))
+        pcx = 0.5 * (float(prev_xyxy[0]) + float(prev_xyxy[2]))
+        pcy = 0.5 * (float(prev_xyxy[1]) + float(prev_xyxy[3]))
+        for d in usable:
+            iou = _iou_xyxy(d.xyxy, prev_xyxy)
+            dpx = float(np.hypot(d.cx - pcx, d.cy - pcy))
+            if iou >= float(lock_iou) or dpx <= gate:
+                locked.append(d)
+
+    if locked:
+        return max(
+            locked,
+            key=lambda d: (float(d.conf) * (float(d.area) ** 0.25), float(d.conf)),
+        )
+
+    def score_new(d: Detection) -> float:
+        s = float(d.conf) * 10.0 + 0.015 * np.sqrt(max(float(d.area), 1.0))
+        if prefer_center is not None:
+            cx0, cy0 = float(prefer_center[0]), float(prefer_center[1])
+            s -= 1e-6 * ((d.cx - cx0) ** 2 + (d.cy - cy0) ** 2)
+        if prev_uv is not None:
+            dpx = float(np.hypot(d.cx - float(prev_uv[0]), d.cy - float(prev_uv[1])))
+            if dpx <= max_track_px:
+                s += 2.0 * (1.0 - dpx / max(max_track_px, 1.0))
+        return s
+
+    best = max(usable, key=score_new)
+    # 快速移动：即使 YOLO 报出的框离上一帧很远，也保留这个实时检测。
+    need = max(float(min_conf), float(reacquire_conf))
+    if float(best.conf) >= need:
+        return best
+    return None

build_sim/arm_application/colcon_build.rc

@@ -0,0 +1 @@
+0

build_sim/arm_application/colcon_command_prefix_setup_py.sh

@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em
+. "/home/cc/ros2/install_sim/arm_kinematics/share/arm_kinematics/package.sh"
+. "/home/cc/ros2/install_sim/arm_msgs/share/arm_msgs/package.sh"

build_sim/arm_application/colcon_test.rc

@@ -0,0 +1 @@
+0

build_sim/arm_application/install.log

@@ -0,0 +1,147 @@
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/geom_ik.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/realsense_view.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/servo_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/dual_gripper_cli.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/stage3_search_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/pipeline_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/goto_ee_plane.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/box_tracker.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/handeye_calibrate.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/dual_gripper.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/onnx_workpiece.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/run_joint_trajectory.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/gauge_config.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/realsense_sdk_view.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/gauge_assembly_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/demo_named_poses.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/hole_detect_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/tactile_interface.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/handeye_lib.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/yolo_detector.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/pid_controller.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/tactile_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/kalman_filter.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/model_labels.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/gauge_gripper.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/perception_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/handeye_waypoints.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/handeye_teach.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/plane_feature.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/dh_pgea_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/weights_layout.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/move_to_pose_client.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/site_io.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/image_utils.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/dh_official.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/handeye_realsense.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/hole_detector.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/stage2_locate_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/dual_gripper_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/realsense_color_pub.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/dh_pgea.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/gauge_boot.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__init__.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/orisys_tactile.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/hole_detect_hough.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/gauge_kin.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/gauge_fsm.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/tactile_cli.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/ros_node.py
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/geom_ik.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/realsense_view.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/servo_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/dual_gripper_cli.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/stage3_search_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/pipeline_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/goto_ee_plane.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/box_tracker.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/handeye_calibrate.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/dual_gripper.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/onnx_workpiece.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/run_joint_trajectory.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/gauge_config.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/realsense_sdk_view.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/gauge_assembly_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/demo_named_poses.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/hole_detect_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/tactile_interface.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/handeye_lib.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/yolo_detector.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/pid_controller.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/tactile_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/kalman_filter.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/model_labels.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/gauge_gripper.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/perception_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/handeye_waypoints.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/handeye_teach.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/plane_feature.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/dh_pgea_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/weights_layout.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/move_to_pose_client.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/site_io.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/image_utils.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/dh_official.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/handeye_realsense.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/hole_detector.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/stage2_locate_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/dual_gripper_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/realsense_color_pub.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/dh_pgea.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/gauge_boot.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/__init__.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/orisys_tactile.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/hole_detect_hough.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/gauge_kin.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/gauge_fsm.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/tactile_cli.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application/__pycache__/ros_node.cpython-311.pyc
+/home/cc/ros2/install_sim/arm_application/share/ament_index/resource_index/packages/arm_application
+/home/cc/ros2/install_sim/arm_application/share/arm_application/package.xml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/hole_detect.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/visual_servo.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/demo.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/dh_pgea.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/stage2_locate.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/stage3_search.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/gauge_assembly.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/dual_gripper.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/tactile.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/launch/pipeline.launch.py
+/home/cc/ros2/install_sim/arm_application/share/arm_application/trajectories/xunhuan.traj
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/servo.yaml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/perception.yaml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/xunhuan_trajectory.yaml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/hole_detect.yaml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/demo.yaml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/hole_insert.yaml
+/home/cc/ros2/install_sim/arm_application/share/arm_application/config/dual_grippers.yaml
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/SOURCES.txt
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/zip-safe
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/entry_points.txt
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/top_level.txt
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/dependency_links.txt
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/PKG-INFO
+/home/cc/ros2/install_sim/arm_application/lib/python3.11/site-packages/arm_application-0.1.0-py3.11.egg-info/requires.txt
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/assembly_pipeline
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/demo_named_poses
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/dh_pgea
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/dh_pgea_cli
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/dual_gripper
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/dual_gripper_cli
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/gauge_assembly
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/goto_ee_plane
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/handeye_calibrate
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/handeye_teach
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/handeye_waypoints
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/hole_detect
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/move_to_pose_client
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/perception_node
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/realsense_color_pub
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/realsense_view
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/run_joint_trajectory
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/servo_node
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/stage2_locate
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/stage3_search
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/tactile
+/home/cc/ros2/install_sim/arm_application/lib/arm_application/tactile_cli

build_sim/arm_application/prefix_override/sitecustomize.py

@@ -0,0 +1,4 @@
+import sys
+if sys.prefix == '/home/cc/anaconda3/envs/ros_gz_sim':
+    sys.real_prefix = sys.prefix
+    sys.prefix = sys.exec_prefix = '/home/cc/ros2/install_sim/arm_application'