chore: 框架科学优化前的完整基线

在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态，
使后续任何删除/重构都可回退。

本基线包含：
- 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等）
- 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接）
- 设备契约层 src/arm_interfaces（新增，46 项单测通过）
- devices/ 设备选型与品牌清单（新增）
- scripts/gen_site_example.py 与由它生成的 config/site.example.yaml

按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。

src/README.md

@@ -0,0 +1,9 @@
+# src/ — ROS 2 包
+
+详细职责见 [docs/WORKSPACE.md](../docs/WORKSPACE.md)。
+
+**跟踪/手眼相关（本仓库维护）：** `arm_application` `arm_bringup` `arm_control` `arm_msgs`
+
+**Flexiv 官方驱动（尽量不改）：** `flexiv_hardware` `flexiv_bringup` `flexiv_msgs` `flexiv_description` 以及 `flexiv_ros2/` 拷贝。
+
+编译：`./scripts/build_all.sh`（先 `source scripts/env.sh`）。

src/arm_application/README.md

@@ -0,0 +1,39 @@
+# arm_application
+
+现场业务包。操作员用 `~/ros2/scripts/*.sh`，不要直接改这里的默认 yaml。
+
+## 节点（console_scripts）
+
+| 可执行文件 | 源文件 | 作用 |
+|------------|--------|------|
+| `realsense_color_pub` | `realsense_color_pub.py` | D405 发布 + 预览 |
+| `perception_node` | `perception_node.py` | YOLO + 平面中心 → 点 |
+| `servo_node` | `servo_node.py` | 100 Hz hover / 锁高 |
+| `handeye_teach` | `handeye_teach.py` | 拖拽记点 |
+| `handeye_calibrate` | `handeye_calibrate.py` | 走点 + 解 T |
+| `handeye_waypoints` | `handeye_waypoints.py` | 旧巡航记点（可选） |
+| `dh_pgea` / `dh_pgea_cli` | `dh_pgea_node.py` / `dh_pgea.py` | 左臂 PGEA |
+| `dual_gripper` / `dual_gripper_cli` | `dual_gripper_node.py` / `dual_gripper_cli.py` | PGEA+RGI |
+| `hole_detect` | `hole_detect_node.py` | 入孔检测 |
+| `tactile` / `tactile_cli` | `tactile_node.py` / `tactile_cli.py` | Orisys 视触觉 |
+| `goto_ee_plane` | `goto_ee_plane.py` | 可选：抬高再回 home（默认启动不再调用） |
+| `demo_named_poses` | `demo_named_poses.py` | mock/实机点位示例 |
+| `run_joint_trajectory` | `run_joint_trajectory.py` | 轨迹回放 |
+
+库模块见包内 `__init__.py`。接口总表：[docs/INTERFACES.md](../../docs/INTERFACES.md)。
+
+## 配置
+
+| 文件 | 角色 |
+|------|------|
+| `config/perception.yaml` `servo.yaml` | 默认值，被 `site.yaml` 覆盖 |
+| `config/hole_detect.yaml` | `two_stage` 缺省；被 `site.yaml hole_detect` 覆盖 |
+| `config/hole_insert.yaml` | `yolo`/`hough` 缺省 |
+| `config/orisys/dd03.json` | Orisys 厂家配置，路径写在 `site.yaml tactile.config_file` |
+| `config/handeye_result_89.yaml` / `_91.yaml` | 该臂手眼 T（标定写入；禁止混用） |
+| `config/handeye_poses_89.yaml` / `_91.yaml` | 该臂示教关节 |
+| `launch/visual_servo.launch.py` | 感知 + 伺服 + 手眼 TF |
+| `launch/dh_pgea.launch.py` | 左臂 PGEA |
+| `launch/dual_gripper.launch.py` | 双爪 |
+| `launch/hole_detect.launch.py` | 入孔检测 |
+| `launch/tactile.launch.py` | 视触觉 |

src/arm_application/arm_application/__init__.py

@@ -0,0 +1,32 @@
+"""arm_application — field nodes for Flexiv Rizon + D405 + DH grippers.
+
+Public ROS executables (see setup.py console_scripts)
+----------------------------------------------------
+Camera:      realsense_color_pub
+Perception:  perception_node
+Servo:       servo_node
+Hand-eye:    handeye_teach, handeye_waypoints, handeye_calibrate
+Gripper:     dh_pgea, dh_pgea_cli, dual_gripper, dual_gripper_cli
+Hole detect: hole_detect
+Tactile:     tactile, tactile_cli
+Helpers:     goto_ee_plane, demo_named_poses, run_joint_trajectory
+
+Reusable Python modules (import from your own nodes)
+----------------------------------------------------
+yolo_detector     YOLO / TensorRT inference + pick_best
+box_tracker       pixel-space constant-velocity box coast
+plane_feature     stereo plane centroid inside a YOLO box
+dh_pgea           official Modbus RTU client for DH PGEA
+dh_official       DH Modbus client used by dual_gripper
+dual_gripper      PGEA left + RGI right
+hole_detector     yolo / hough / two_stage hole centers
+orisys_tactile    Orisys visuo-tactile adapter
+geom_ik           Rizon geometric Jacobian / DLS / hand-eye load
+handeye_lib       chessboard + OpenCV calibrateHandEye
+handeye_realsense topic / serial helpers for D405
+pid_controller    PID with integral separation
+kalman_filter     3D constant-velocity KF
+
+Operators should use ``scripts/*.sh`` and ``config/site.yaml``, not these
+imports. See ``docs/INTERFACES.md``.
+"""

src/arm_application/arm_application/box_tracker.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+"""Constant-velocity tracker for a YOLO xyxy box (pixel space).
+
+Used so fast workpiece motion does not drop the lock: the next-frame gate
+is the *predicted* box, and brief YOLO misses coast the overlay.
+"""
+
+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)  # center 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)

src/arm_application/arm_application/demo_named_poses.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+"""Demo: cycle named poses via /set_named_pose (works for any backend)."""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from rclpy.node import Node
+
+from arm_msgs.srv import SetNamedPose
+
+
+class DemoNamedPoses(Node):
+    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 not available')
+            return 1
+        for loop in range(max(self.loops, 1)):
+            self.get_logger().info(f'loop {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'timeout on {pose}')
+                        return 2
+                res = fut.result()
+                if res is None or not res.success:
+                    self.get_logger().error(
+                        f'{pose} failed: {res.message if res else "no response"}'
+                    )
+                    return 3
+                self.get_logger().info(f'{pose}: {res.message}')
+                time.sleep(self.dwell_sec)
+        self.get_logger().info('demo done')
+        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()

src/arm_application/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 register map
+# =============================================================================
+
+# Basic control registers (holding registers, function code 03/06/10)
+REG_INIT = 0x0100                 # 0x01 initialize, 0xA5 fully initialize
+REG_FORCE = 0x0101                # gripper closing force, 20~100 %
+REG_POSITION = 0x0103             # finger reference position, 0~1000 per-mille
+REG_SPEED = 0x0104                # finger open/close speed, 1~100 %
+
+# RGI rotation control registers
+REG_ROTATION_ANGLE = 0x0105       # target rotation angle, signed degree value
+REG_ROTATION_SPEED = 0x0107       # rotation speed, 1~100 %
+REG_ROTATION_FORCE = 0x0108       # rotation force/torque, 20~100 %
+
+# Feedback registers
+REG_INIT_STATE = 0x0200           # 0 uninitialized, 1 initialized, 2 initializing
+REG_GRIPPER_STATE = 0x0201        # 0 moving, 1 reached, 2 object caught, 3 object dropped
+REG_CURRENT_POSITION = 0x0202     # current finger position, per-mille
+REG_ROTATION_ANGLE_FB = 0x0208    # current rotation angle, signed degree value
+REG_ROTATION_INIT_STATE = 0x020A  # 0 uninitialized, 1 initialized, 2 initializing
+REG_ROTATION_STATE = 0x020B       # 0 moving, 1 reached, 2 blocking, 3 had been blocked
+
+# Configuration registers
+REG_SAVE_PARAMS = 0x0300          # write 1 to save all parameters to flash
+
+
+class DHModbusError(RuntimeError):
+    """DH Modbus communication or protocol error."""
+
+
+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:
+    """Encode signed degree value as DH one's-complement register value.
+
+    RGI manual example: +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:
+    """Decode DH one's-complement register value to signed integer."""
+    raw = int(raw) & 0xFFFF
+    if raw & 0x8000:
+        mag = (~raw) & 0x7FFF
+        return -mag
+    return raw & 0x7FFF
+
+
+class DHModbusClient:
+    """Minimal thread-safe Modbus RTU client for DH-Robotics grippers."""
+
+    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 - depends on runtime env
+            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
+
+    # ------------------------------------------------------------------
+    # Low-level register access
+    # ------------------------------------------------------------------
+    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))
+
+    # ------------------------------------------------------------------
+    # Common PGE/PGEA/RGI control
+    # ------------------------------------------------------------------
+    def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
+        """Initialize/homing the gripper and wait until ready."""
+        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:
+        """Best-effort stop/hold for DH grippers without a dedicated stop register."""
+        self.set_position_per_mille(self.get_position_per_mille())
+
+    # ------------------------------------------------------------------
+    # RGI rotation control
+    # ------------------------------------------------------------------
+    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:
+        """Stop rotation by re-commanding the current feedback angle."""
+        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()

src/arm_application/arm_application/dh_pgea.py

@@ -0,0 +1,584 @@
+#!/usr/bin/env python3
+"""DH Robotics PGE / PGEA Modbus RTU client.
+
+Port of the official C++ driver:
+  DH-Robotics/dh_gripper_ros  →  DH_Modbus_Gripper
+  (factory maps model names containing ``PGE`` / ``PGEA`` onto this class)
+
+Flexiv RDK ``Gripper`` cannot drive this claw: it is not a Studio Device.
+Talk RS485 (USB–UART) with the same registers the DH ROS driver uses.
+
+Registers (holding, FC03 read / FC06 write, CRC-16/Modbus LE):
+  0x0100  init write 0xA5
+  0x0101  force 20–100 %
+  0x0103  position 0–1000 ‰
+  0x0104  speed 1–100 %
+  0x0200  init state  0=need  1=done  2=doing
+  0x0201  grip state  0=moving  1=arrived  2=caught  3=dropped
+  0x0202  current position
+"""
+
+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 missing. In conda ros_flexiv:  pip install pyserial"
+    ) from exc
+
+
+# Official CRC-16/Modbus (same table as 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:
+    """Official DH CRC16 (Modbus, little-endian on the wire)."""
+    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:
+    """Thread-safe Modbus RTU client for PGE / PGEA (official register map)."""
+
+    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 must be '', tx, or tx_inv, got {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"cannot open {self.port}: {exc}. "
+                "Need 24 V DC + RS485, and user in group 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 write, wait for echo (official 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 read one holding register (official 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"Modbus read 0x{index:04X} failed on {self.port} "
+            f"(no valid reply{hint}). baud={self.baudrate} slave={self.slave_id} "
+            f"rts={self.rs485_rts or 'off'}"
+        )
+
+    def probe(self) -> int:
+        """One short read of init-state. Raises if the claw is silent."""
+        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("serial not open")
+        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:
+        """Write 0xA5 then wait until init state == finished (official bringup)."""
+        state = self.get_init_state()
+        if state == S_INIT_FINISHED:
+            return True
+        if not self.initialization():
+            raise DhPgeaError("init write 0x0100=0xA5 failed (no Modbus echo)")
+        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"init timeout after {timeout_s:.0f}s (state={state}). "
+            "Fingers should home once on first power-up."
+        )
+
+    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:
+    """Official short-manual example: set position 500."""
+    frame = _frame(1, 0x06, 0x0103, 500)
+    expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
+    if frame != expect:
+        raise AssertionError(f"CRC mismatch: {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:
+    """Try baud × slave × RTS until one combination answers."""
+    bauds = (115200, 57600, 38400, 19200, 9600)
+    slaves = (1, 2, 3)
+    rts_modes = ("", "tx", "tx_inv")
+    print(f"scan {port}  (stop the ROS node first — port is exclusive)")
+    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("no reply on any combination.", 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 CLI (official register map, not 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: empty | tx | tx_inv")
+    ap.add_argument(
+        "cmd",
+        choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"],
+    )
+    ap.add_argument("value", nargs="?", default="", help="position 0–1000 for 'pos'")
+    args = ap.parse_args(argv)
+
+    if args.cmd == "selfcheck":
+        _selfcheck_crc()
+        print("CRC self-check OK (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"ERROR: {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("initializing (fingers home)…")
+            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("open write failed")
+            print(grip.wait_settle(1000).summary())
+            return 0
+        if args.cmd == "close":
+            if not grip.close_jaw(args.force, args.speed):
+                raise DhPgeaError("close write failed")
+            print(grip.wait_settle(0).summary())
+            return 0
+        if args.cmd == "pos":
+            if args.value == "":
+                print("usage: pos <0-1000>", file=sys.stderr)
+                return 2
+            pos = int(float(args.value))
+            if not grip.move(pos, args.force, args.speed):
+                raise DhPgeaError("position write failed")
+            print(grip.wait_settle(pos).summary())
+            return 0
+    except DhPgeaError as exc:
+        print(f"ERROR: {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())

src/arm_application/arm_application/dh_pgea_node.py

@@ -0,0 +1,254 @@
+#!/usr/bin/env python3
+"""ROS 2 node for DH PGEA (official Modbus RTU, not Flexiv gripper).
+
+Topics (layout matches DH-Robotics/dh_gripper_ros):
+  /gripper/ctrl     std_msgs/Float32MultiArray
+                    data: [initialize, position, force, speed]
+                    initialize>0.5 → write 0xA5; else set force/speed/position
+  /gripper/states   std_msgs/Float32MultiArray
+                    data: [initialized, grip_state, position, target_pos, target_force]
+  /gripper/joint_states  sensor_msgs/JointState
+
+Services (std_srvs/Trigger):
+  /gripper/initialize  /gripper/open  /gripper/close
+
+Do not set Flexiv bringup load_gripper:=true — that is Flexiv-GN01 only.
+"""
+
+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,
+)
+
+
+def _param_as_str(node: Node, name: str, default: str = "") -> str:
+    """String param that also accepts numeric CLI overrides (e.g. SN 26610799)."""
+    node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
+    val = node.get_parameter(name).value
+    if val is None or val == "":
+        return ""
+    return str(val).strip()
+
+
+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(Node):
+    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"(official Modbus RTU; Flexiv RDK gripper API unused)"
+        )
+        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("will keep retrying; open/close will fail until the bus answers")
+
+        if self._auto_init and self._online:
+            try:
+                self._grip.initialize()
+                self.get_logger().info("gripper initialized")
+            except DhPgeaError as exc:
+                self.get_logger().error(f"auto_init failed: {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("move write failed")
+        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("initialize via /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("gripper bus recovered")
+        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"]
+        # Official ROS driver maps 0–1000 → ~0–0.637 rad; we publish mm as well.
+        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()

src/arm_application/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: str = "Rizon4-063989"
+    right_arm_sn: str = "Rizon4-063991"
+    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_arm_sn=left.get("robot_sn", "Rizon4-063989"),
+        right_arm_sn=right.get("robot_sn", "Rizon4-063991"),
+        left_gripper=left_cfg,
+        right_gripper=right_cfg,
+    )
+
+
+def load_from_site(data: dict) -> DualArmGripperConfig:
+    """Build DualArmGripperConfig from ~/ros2/config/site.yaml."""
+    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"), {})
+    if not left:
+        left = next((r for r in robots if "063989" in str(r.get("sn") or "")), {})
+    if not right:
+        right = next((r for r in robots if "063991" in str(r.get("sn") or "")), {})
+    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_arm_sn=str(left.get("sn") or "Rizon4-063989"),
+        right_arm_sn=str(right.get("sn") or "Rizon4-063991"),
+        left_gripper=left_cfg,
+        right_gripper=right_cfg,
+    )

src/arm_application/arm_application/dual_gripper_cli.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python3
+"""CLI for DH PGEA (left) and RGI (right). Loads ports from 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 _normalize_arm(raw: str) -> str:
+    t = (raw or "").strip().lower()
+    if t in ("89", "left", "l", "063989"):
+        return "left"
+    if t in ("91", "right", "r", "063991"):
+        return "right"
+    if t in ("both", "dual", "all"):
+        return "both"
+    raise SystemExit(f"unknown --arm {raw!r} (use 89|91|left|right|both)")
+
+
+def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
+    gripper = manager.left
+    print(f"=== PGEA parallel ({gripper.cfg.port}) ===")
+    gripper.connect()
+    _print_status("connected", gripper.get_status())
+    if args.test in ("init", "probe", "all"):
+        if args.test != "probe":
+            gripper.initialize(full=args.init_full)
+            _print_status("init", gripper.get_status())
+    if args.test in ("open", "all"):
+        _print_status("open", gripper.open(force=args.force, speed=args.speed))
+    if args.test in ("close", "all"):
+        _print_status("close", gripper.close(force=args.force, speed=args.speed))
+    if args.test in ("move", "pos") and args.pos is not None:
+        _print_status("move", gripper.move_to(args.pos, force=args.force, speed=args.speed))
+    if args.test in ("status", "probe"):
+        _print_status("status", gripper.get_status())
+
+
+def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
+    gripper = manager.right
+    print(f"=== RGI rotary ({gripper.cfg.port}) ===")
+    gripper.connect()
+    _print_status("connected", gripper.get_status())
+    if args.test in ("init", "all"):
+        gripper.initialize(full=args.init_full)
+        _print_status("init", gripper.get_status())
+    if args.test in ("open", "all"):
+        _print_status("open", gripper.open_fingers(force=args.force, speed=args.speed))
+    if args.test in ("close", "all"):
+        _print_status("close", gripper.close_fingers(force=args.force, speed=args.speed))
+    if args.test in ("move", "pos") and args.pos is not None:
+        _print_status("fingers", 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(
+                "revolutions",
+                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("rotate", 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("rotate_by", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
+    if args.test in ("status", "probe"):
+        _print_status("status", gripper.get_status())
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description="DH PGEA / RGI CLI (site.yaml ports)")
+    parser.add_argument("--arm", default=os.environ.get("ROBOT_ARM") or os.environ.get("ROBOT_SN") or "89")
+    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 or RGI finger 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"ERROR: site.yaml missing: {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))
+    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("interrupt — stop")
+        manager.emergency_stop()
+        return 130
+    except Exception as exc:
+        print(f"ERROR: {exc}", file=sys.stderr)
+        return 1
+    finally:
+        manager.disconnect_all()
+        time.sleep(0.05)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/arm_application/arm_application/dual_gripper_node.py

@@ -0,0 +1,318 @@
+"""ROS 2 dual DH gripper node (PGEA left + RGI right). Uses arm_msgs, not 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
+
+
+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(Node):
+    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"gripper config from 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 connected: {gripper.cfg.port}")
+            except Exception as exc:
+                self._connect_errors[side] = str(exc)
+                self.get_logger().error(f"{side} gripper connect failed: {exc}")
+        if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
+            raise RuntimeError(f"gripper connect failed: {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"unsupported command {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} command {msg.command!r} failed: {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()

src/arm_application/arm_application/geom_ik.py

@@ -0,0 +1,135 @@
+#!/usr/bin/env python3
+"""TF-based geometric Jacobian + damped least-squares position IK step."""
+
+from __future__ import annotations
+
+from typing import List, Optional, Sequence, Tuple
+
+import numpy as np
+
+# URDF axes expressed in each child link frame (Rizon4).
+RIZON_LINK_AXES = [
+    (0.0, 0.0, 1.0),  # joint1 -> link1
+    (0.0, 1.0, 0.0),  # joint2 -> link2
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+]
+
+
+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 geometric_jacobian(
+    link_R: Sequence[np.ndarray],
+    link_p: Sequence[np.ndarray],
+    ee_p: np.ndarray,
+    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+) -> np.ndarray:
+    """Return 6x7 geometric Jacobian in base frame (linear then angular)."""
+    n = len(axes_local)
+    J = np.zeros((6, n), dtype=float)
+    pee = np.asarray(ee_p, dtype=float).reshape(3)
+    for i in range(n):
+        R = np.asarray(link_R[i], dtype=float).reshape(3, 3)
+        p = np.asarray(link_p[i], dtype=float).reshape(3)
+        z = R @ np.asarray(axes_local[i], dtype=float).reshape(3)
+        J[:3, i] = np.cross(z, pee - p)
+        J[3:, i] = z
+    return J
+
+
+def position_jacobian(
+    link_R: Sequence[np.ndarray],
+    link_p: Sequence[np.ndarray],
+    ee_p: np.ndarray,
+    axes_local: Sequence[Tuple[float, float, float]] = RIZON_LINK_AXES,
+) -> np.ndarray:
+    """Return 3x7 translational Jacobian in base frame."""
+    return geometric_jacobian(link_R, link_p, ee_p, axes_local)[:3, :]
+
+
+def dls_position_step(
+    J: np.ndarray,
+    dp_base: np.ndarray,
+    *,
+    damping: float = 0.05,
+    max_dq: float = 0.05,
+) -> np.ndarray:
+    """Solve J dq = dp with damped least squares; clip |dq|."""
+    dp = np.asarray(dp_base, dtype=float).reshape(3)
+    JT = J.T
+    A = J @ JT + (damping ** 2) * np.eye(3)
+    dq = JT @ np.linalg.solve(A, dp)
+    dq = np.clip(dq, -abs(max_dq), abs(max_dq))
+    return dq
+
+
+def dls_twist(
+    J6: np.ndarray,
+    v_base: np.ndarray,
+    *,
+    w_base: Optional[np.ndarray] = None,
+    damping: float = 0.08,
+    max_qdot: float = 0.80,
+    ori_weight: float = 1.2,
+) -> np.ndarray:
+    """Solve J qdot = [v; w] with orientation held (w≈0 by default)."""
+    dx = np.zeros(6, dtype=float)
+    dx[:3] = np.asarray(v_base, dtype=float).reshape(3)
+    if w_base is not None:
+        dx[3:] = np.asarray(w_base, dtype=float).reshape(3)
+    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
+    Jw = J6 * w[:, None]
+    dxw = dx * w
+    JT = Jw.T
+    A = Jw @ JT + (damping ** 2) * np.eye(6)
+    qdot = JT @ np.linalg.solve(A, dxw)
+    return np.clip(qdot, -abs(max_qdot), abs(max_qdot))
+
+
+def dls_translate_hold_ori(
+    J6: np.ndarray,
+    dp_base: np.ndarray,
+    *,
+    damping: float = 0.08,
+    max_dq: float = 0.05,
+    ori_weight: float = 1.2,
+) -> np.ndarray:
+    """Translate EE by dp while keeping orientation (wx,wy,wz ≈ 0)."""
+    dx = np.zeros(6, dtype=float)
+    dx[:3] = np.asarray(dp_base, dtype=float).reshape(3)
+    w = np.array([1.0, 1.0, 1.0, ori_weight, ori_weight, ori_weight], dtype=float)
+    Jw = J6 * w[:, None]
+    dxw = dx * w
+    JT = Jw.T
+    A = Jw @ JT + (damping ** 2) * np.eye(6)
+    dq = JT @ np.linalg.solve(A, dxw)
+    return np.clip(dq, -abs(max_dq), abs(max_dq))
+
+
+def load_handeye_T(path: str) -> np.ndarray:
+    """Load 4x4 from handeye_result.yaml (T_4x4 list)."""
+    import yaml
+
+    with open(path, 'r', encoding='utf-8') as f:
+        data = yaml.safe_load(f)
+    if not isinstance(data, dict) or 'T_4x4' not in data:
+        raise ValueError(f'No T_4x4 in hand-eye file: {path}')
+    T = np.array(data['T_4x4'], dtype=float)
+    if T.shape != (4, 4):
+        raise ValueError(f'T_4x4 shape {T.shape}, expected (4,4)')
+    return T

src/arm_application/arm_application/goto_ee_plane.py

@@ -0,0 +1,451 @@
+#!/usr/bin/env python3
+"""Startup return-to-initial before perception/servo starts.
+
+Sequence:
+  [A] if the flange is below servo.safe_z_m, lift it STRAIGHT UP first
+      (closed-loop Z hops) so the home move can't sweep walls/fixtures;
+  [B] call /set_named_pose (Flexiv named pose, default "home") — the arm
+      returns to the Flexiv initial pose with Flexiv's own planner;
+  [C] hold (pin) so the servo takes over from a clean, stationary pose.
+"""
+
+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,
+)
+
+
+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 "Rizon4-063989_"),
+        "base": str(robot.get("base_frame") or "Rizon4-063989_base_link"),
+        "ee": str(robot.get("ee_frame") or "Rizon4-063989_flange"),
+        "z": ee_z,
+        # Lift straight up to this Z before the home move (avoid hitting
+        # walls/fixtures); then return to the Flexiv named initial pose.
+        "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),
+        # only used by the (currently unused) verticalize planner path
+        "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(Node):
+    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("  ERROR: JTC rejected the height trajectory", 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"  moving  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("  WARN: JTC result timeout — checking TF anyway", 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:
+        """Plan and execute ONE short cartesian segment (≤ max_seg) toward
+        target. Long moves are handled by the caller in closed-loop hops,
+        because the Jacobian is frozen at the current pose — over >10 cm the
+        linearization error makes single-shot plans land far off target."""
+        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:  # clamp this hop; caller loops with fresh TF
+            dp = dp * (max_seg / dist)
+            dist = max_seg
+        print(
+            f"  plan {tag} Δxyz=({dp[0]*1000:+.0f},{dp[1]*1000:+.0f},{dp[2]*1000:+.0f})mm  "
+            f"tilt={ang * 180 / np.pi:.1f}deg",
+            flush=True,
+        )
+        J = self._jacobian(p)
+        if J is None:
+            print("ERROR: missing link TF for Jacobian", 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"  WARN {tag}: planner stalled "
+                f"(planned {planned*1000:.0f}/{dist*1000:.0f}mm) — skip hop",
+                flush=True,
+            )
+            return False
+        # joint-excursion sanity gate: refuse wild plans
+        if float(np.max(np.abs(waypoints[-1] - q0))) > 1.0:
+            print(f"  ERROR {tag}: joint excursion >1 rad — abort hop", 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))
+        # force zero velocity at the last point
+        traj.points[-1].velocities = [0.0] * 7
+        print(
+            f"  send JTC action {tag} {n} steps  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]:
+        """Closed-loop point-to-point: plan ≤8 cm hop → execute → re-measure
+        TF → repeat, until within tolerance. Robust to Jacobian linearization
+        error over long distances."""
+        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{hop + 1}  remain={dist*1000:.0f}mm  "
+                f"tilt={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("ERROR: lost state during move", 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:
+        """Call Flexiv /set_named_pose and wait until the arm is stationary."""
+        cli = self.create_client(SetNamedPose, "/set_named_pose")
+        if not cli.wait_for_server(timeout_sec=8.0):
+            print("ERROR: /set_named_pose unavailable — start 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("ERROR: set_named_pose timeout", flush=True)
+            return False
+        resp = fut.result()
+        print(
+            f"  [B] resp success={resp.success}  msg={resp.message}",
+            flush=True,
+        )
+        if not resp.success:
+            return False
+        # wait until the flange stops moving (Δp < 1 mm over ~0.8 s)
+        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("  WARN: still-moving wait timeout — continue anyway", 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("ERROR: no joint_states / TF — start run_real.sh", flush=True)
+            return 2
+        if not self._cli.wait_for_server(timeout_sec=8.0):
+            print("ERROR: JTC action unavailable — start run_real.sh", flush=True)
+            return 3
+
+        p = Tee[:3, 3]
+        safe_z = float(cfg["safe_z"])
+        print(
+            f"=== return to Flexiv initial pose '{cfg['home_pose']}'  "
+            f"safe_z={safe_z*1000:.0f}mm ===",
+            flush=True,
+        )
+        print(
+            f"  now ee=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})",
+            flush=True,
+        )
+
+        # --- Phase A: pure-Z lift to the safe height before the home move ---
+        if p[2] < safe_z - 0.01:
+            lift_tgt = np.array([p[0], p[1], safe_z], dtype=float)
+            print(f"  [A] lift 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("ERROR: lift phase failed — pin here", flush=True)
+                self._pin(q)
+                return 4
+
+        # --- Phase B: Flexiv named pose → real initial pose, good posture ---
+        if not self._named_pose(cfg["home_pose"], cfg["home_speed"]):
+            print("ERROR: named-pose return failed — pin here", flush=True)
+            self._pin(q)
+            return 4
+
+        # --- Phase C: hold so the servo takes over from a stationary pose ---
+        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"  at initial pose  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())

src/arm_application/arm_application/handeye_calibrate.py

@@ -0,0 +1,594 @@
+#!/usr/bin/env python3
+"""Full hand-eye pipeline: move -> stabilize chessboard -> capture -> solve -> cleanup.
+
+Supports eye_in_hand / eye_to_hand.
+
+Camera intrinsics priority:
+  1) ROS CameraInfo topic (if publishing)
+  2) RealSense D405/D435 EEPROM via pyrealsense2 (camera_source)
+  3) Manual camera_fx/fy/cx/cy in YAML
+
+Chessboard size (board_cols / board_rows / square_size_m) is user-configurable.
+"""
+
+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_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import GetArmState
+
+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'Unsupported image encoding: {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(Node):
+    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', 'Rizon4-063989_')
+        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)
+
+        # Camera: 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])
+
+        # Chessboard (customizable)
+        self.declare_parameter('board_cols', 9)  # inner corners
+        self.declare_parameter('board_rows', 6)
+        self.declare_parameter('square_size_m', 0.025)
+
+        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)
+        self.declare_parameter('skip_unstable_poses', True)
+
+        self.declare_parameter('output_dir', '/tmp/handeye_calib')
+        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.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 empty. Set camera_source:=d405|d435 or fill image_topic.'
+            )
+        else:
+            self.create_subscription(
+                Image, img_topic, self._on_image, qos_profile_sensor_data
+            )
+            self.get_logger().info(f'Subscribed image: {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'Subscribed 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'Unknown mode={self.mode!r}; use eye_in_hand or eye_to_hand'
+            )
+            return 1
+
+        poses = self._load_poses()
+        if len(poses) < self.min_poses:
+            self.get_logger().error(
+                f'Only {len(poses)} filled poses (need >= {self.min_poses}). '
+                'Teach with: 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} >= filled poses {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}] unavailable. '
+                'Start: 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'Hand-eye [{self.mode}] camera_source={self.camera_source}: '
+            f'{len(poses)} poses, board={self.board.cols}x{self.board.rows} '
+            f'square={self.board.square_size}m, v={self.velocity_scale}'
+        )
+        self.get_logger().warn('Clear workspace; E-Stop ready. Ctrl+C aborts.')
+
+        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.get_logger().info(f'[{i + 1}/{len(poses)}] moving...')
+                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}] arm did not reach target joints '
+                        '(controller reported OK but joints unchanged). '
+                        'Restart run_real.sh; pendant AUTO Remote; clear fault. '
+                        'Do NOT trust this calibration.'
+                    )
+                    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)
+
+                ok, std, corners, pattern, bgr = self._wait_stable()
+                if not ok or corners is None or bgr is None:
+                    msg = f'[{i + 1}] chessboard not stable (std={std:.3f})'
+                    if self.skip_unstable:
+                        self.get_logger().warn(f'{msg} — skip')
+                        continue
+                    self.get_logger().error(f'{msg}. Abort.')
+                    return 7
+
+                ee = self._read_ee_pose()
+                if ee is None:
+                    self.get_logger().error('Failed to read 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)}] CAPTURED '
+                    f'(corner_std={std:.3f}px) -> {img_path.name}'
+                )
+        except KeyboardInterrupt:
+            self.get_logger().info('Interrupted')
+            return 0
+
+        if len(Rg) < self.min_poses:
+            self.get_logger().error(
+                f'Only {len(Rg)} captures; need >= {self.min_poses} to solve'
+            )
+            return 9
+
+        self.get_logger().info(f'Solving hand-eye with {len(Rg)} views...')
+        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'# Hand-eye result\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('======== Hand-eye result ========')
+        for line in (header + yaml_txt).strip().splitlines():
+            self.get_logger().info(line)
+        self.get_logger().info(f'Saved -> {self.result_file}')
+
+        if self.cleanup_images:
+            if self.images_dir.exists():
+                shutil.rmtree(self.images_dir, ignore_errors=True)
+            self.get_logger().info('Calibration images deleted')
+        else:
+            self.get_logger().info(f'Images kept in {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 length {len(flat)} < num_poses*{n_j}={need}; '
+                'padding with zeros'
+            )
+            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'Pose slot {i} empty (all-zero) — skipped')
+        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'Intrinsics from 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 intrinsics query failed: {exc}')
+            else:
+                self._intrinsics = CameraIntrinsics(K=intr.as_K(), dist=intr.as_dist())
+                self.get_logger().info(
+                    f'Intrinsics from 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'Requested {self.rs_width}x{self.rs_height}, '
+                        f'got {intr.width}x{intr.height} (closest 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(
+                'No intrinsics. Set camera_source:=d405|d435, '
+                'or provide camera_info_topic, or fill 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'Intrinsics from YAML: fx={fx:.2f} fy={fy:.2f} cx={cx:.2f} cy={cy:.2f}'
+        )
+        return True
+
+    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'Image convert failed: {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 = 'yes' if corners is not None else 'NO'
+                self.get_logger().info(
+                    f'  board={seen}  corner_std='
+                    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:
+        """Confirm joint_states reached target (detect fake trajectory success)."""
+        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'  joint RMSE vs target: {err:.4f} rad (tol={tol})'
+            )
+            return err <= tol
+        self.get_logger().error('No joint_states while verifying motion')
+        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, 'send goal'):
+            return 3
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('Goal rejected')
+            return 4
+        res_fut = handle.get_result_async()
+        if not self._wait_future(res_fut, max(60.0, duration * 3.0), 'result'):
+            return 5
+        result = res_fut.result().result
+        if not result.success:
+            self.get_logger().error(f'Move failed: {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'Timeout waiting for {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 failed: {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()

src/arm_application/arm_application/handeye_lib.py

@@ -0,0 +1,266 @@
+#!/usr/bin/env python3
+"""Hand-eye helpers: chessboard stabilize + AX=XB solve.
+
+Uses ``cv2.calibrateHandEye`` when present; otherwise Tsai-Lenz (NumPy).
+Chessboard detection always needs OpenCV.
+"""
+
+from __future__ import annotations
+
+from collections import deque
+from dataclasses import dataclass
+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  # inner corners (width)
+    rows: int  # inner corners (height)
+    square_size: float  # meters
+
+
+@dataclass
+class CameraIntrinsics:
+    K: np.ndarray
+    dist: np.ndarray
+
+
+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]]]:
+    """Return (corners, pattern_size) or None. pattern is (cols, rows) used."""
+    if cv2 is None:
+        raise RuntimeError('OpenCV (cv2) is required for chessboard detection')
+    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('OpenCV (cv2) is required for solvePnP')
+    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 failed')
+    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. Returns R_cam2gripper, t_cam2gripper."""
+    n = len(R_gripper2base)
+    if n < 3:
+        raise ValueError('Need >= 3 poses')
+
+    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 (mounted on flange)
+    eye_to_hand  -> X = cam -> base   (fixed camera)
+    """
+    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  # fall back to 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:
+    """False if all zeros / NaN placeholder."""
+    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

src/arm_application/arm_application/handeye_realsense.py

@@ -0,0 +1,251 @@
+#!/usr/bin/env python3
+"""Query RealSense (D405 / D435) factory color intrinsics via pyrealsense2."""
+
+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 order, length 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)
+
+
+# Default ROS topic namespaces when camera_source is d405/d435.
+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]]:
+    """Return [(name, serial), ...]."""
+    try:
+        import pyrealsense2 as rs
+    except Exception as exc:  # noqa: BLE001
+        raise RuntimeError(
+            'pyrealsense2 not installed. '
+            'Install in 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:
+    """
+    Read on-device color intrinsics for D405/D435 at the given resolution.
+
+    Does not start a streaming pipeline (uses profile metadata).
+    """
+    try:
+        import pyrealsense2 as rs
+    except Exception as exc:  # noqa: BLE001
+        raise RuntimeError(
+            'pyrealsense2 not installed. '
+            'Install in ros_flexiv: pip/conda install pyrealsense2'
+        ) from exc
+
+    want = _norm_model(model)
+    if want not in ('d405', 'd435'):
+        raise ValueError(f'Unsupported model {model!r}; use d405 or d435')
+
+    hint = want.upper()  # D405 / D435
+    serial = (serial or '').strip()
+    devices = list(rs.context().devices)
+    if not devices:
+        raise RuntimeError('No RealSense device connected')
+
+    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'No RealSense matching model={want!r} serial={serial!r}. '
+            f'Connected: {avail or "none"}'
+        )
+
+    name = chosen.get_info(rs.camera_info.name)
+    sn = chosen.get_info(rs.camera_info.serial_number)
+
+    best = None  # (score, width, height, fx, fy, cx, cy, coeffs)
+    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'No color stream profile on {name} ({sn})')
+
+    _, 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:
+    """True for d405 / d435 and namespaced aliases (d405_left, d405_right)."""
+    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]:
+    """Fill default image/camera_info topics for d405/d435 if empty.
+
+    ``model`` may be a topic namespace such as ``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]:
+    """Cap AE / kill projector. Wrist D405 overexposure is usually long AE + emitter."""
+    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))
+        # Must disable AE *before* writing exposure; D405 firmware ignores exposure while AE is on.
+        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 exposure: " + ", ".join(dict.fromkeys(notes)))
+    return notes

src/arm_application/arm_application/handeye_teach.py

@@ -0,0 +1,626 @@
+#!/usr/bin/env python3
+"""Hand-eye teach: live preview + Free Drive + Enter to photo/record joints.
+
+Two backends
+------------
+ros (default, stack running):
+  - Joints from /get_arm_state
+  - Drag: teach pendant Free Drive + hold enable
+  Keep: bash ~/ros2/scripts/run_real.sh
+
+rdk (software Free Drive):
+  - Stop run_real.sh first (RDK exclusive)
+  - Press 'd' to enable FloatingJoint drag via RDK
+  - Joints from RDK robot.states
+
+  source ~/ros2/scripts/env.sh
+  ros2 run arm_application handeye_teach --ros-args -p backend:=rdk
+
+UI keys (preview window)
+  Enter / Space  photo + record joints
+  d             enable Free Drive (rdk) / show pendant tip (ros)
+  s             stop Free Drive (rdk) / hold
+  q / Esc       finish
+"""
+
+from __future__ import annotations
+
+import os
+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 rcl_interfaces.msg import ParameterDescriptor
+from rclpy.node import Node
+
+HELP = """
+================================================
+ Hand-eye TEACH: preview -> drag -> photo+pose
+================================================
+ Window keys:
+   Enter / Space  = take photo + save joints
+   d              = Free Drive ON
+   s              = Free Drive OFF / hold
+   q / Esc        = finish
+ Tip: make board clear & fill ~30-70% of image
+ Prefer 8-12 poses with varied angles
+================================================
+"""
+
+
+def _setup_matplotlib_font():
+    """Avoid Chinese mojibake: prefer CJK font if present."""
+    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:
+        # try by file path
+        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
+
+
+def _param_as_str(node: Node, name: str, default: str = '') -> str:
+    node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
+    val = node.get_parameter(name).value
+    if val is None or val == '':
+        return ''
+    return str(val).strip()
+
+
+class RdkBridge:
+    """Talk to Flexiv RDK via rdk_py310 subprocess (Python 3.10 ABI)."""
+
+    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 not found: {bridge}')
+
+        # Only Flexiv libs — do not inherit conda LD_LIBRARY_PATH (can hang 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',
+        }
+        # Do NOT inherit conda PYTHONPATH.
+        # IMPORTANT: do not use PIPE for stderr — RDK logs fill the pipe and
+        # deadlock the bridge (stdout never replies). Log to a file instead.
+        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 bridge started; log → {log_path}')
+        pong = self._ask({'cmd': 'ping'}, timeout=8.0)
+        if not pong.get('ok'):
+            raise RuntimeError(
+                f'RDK bridge ping failed: {pong}. See {log_path}'
+            )
+        if logger:
+            logger.info('RDK bridge ping OK, connecting to robot...')
+        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 connect failed: {conn.get("error")}. '
+                f'Stop run_real.sh first; AUTO Remote + clear fault. See {log_path}'
+            )
+        if logger:
+            logger.info(f'RDK bridge OK (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
+
+        # One in-flight command: camera/GUI must never share this pipe.
+        if not self._io.acquire(timeout=max(timeout, 0.2)):
+            return {'ok': False, 'error': 'bridge busy'}
+        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'bridge exited rc={self._proc.returncode}; see {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'bad reply: {exc} {line!r}'}
+            return {'ok': False, 'error': 'bridge timeout'}
+        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(Node):
+    def __init__(self) -> None:
+        super().__init__('handeye_teach')
+        self.declare_parameter('output_dir', '/tmp/handeye_calib')
+        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', 'Rizon4-063989')
+        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)
+
+        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] Press d = soft-drag (works in AUTO Remote, no Floating license)')
+            print('              run_real.sh must be STOPPED.')
+            print('              拖着就能按 Enter 拍照，不必先按 s')
+        else:
+            print('[backend=ros] Drag with pendant Free Drive + hold enable.')
+            print('             Or stop run_real.sh and use: -p backend:=rdk')
+        if font:
+            self.get_logger().info(f'UI font: {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 unavailable. Start run_real.sh, '
+                'or use: -p backend:=rdk (after stopping the stack)'
+            )
+
+    def _init_rdk(self) -> None:
+        self.get_logger().info(f'Connecting RDK bridge to {self.robot_sn} ...')
+        self._rdk = RdkBridge(self.robot_sn, logger=self.get_logger())
+        self._robot = True  # marker: rdk backend ready
+        self.get_logger().info('RDK connected. Press d for 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'Camera {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'Cannot open D405 — stop realsense_color_pub first. ({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'exposure apply: {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=photo  d=drag  s=stop  q=quit'
+        )
+        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 = 'DRAG ON' if self._drag_on else 'drag off'
+        if self.backend == 'rdk':
+            tip = 'd=soft-drag  s=hold'
+        else:
+            tip = 'Pendant FreeDrive+Enable'
+        return (
+            f'captured:{len(self._captures)}  [{drag}]  '
+            f'{tip}  |  Enter=photo  q=quit'
+        )
+
+    def _on_key(self, event) -> None:
+        key = (event.key or '').lower()
+        if key in ('q', 'escape'):
+            self._quit = True
+            self.get_logger().info('Quit requested')
+            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 {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 backend: use teach pendant Free Drive + hold enable button. '
+                'For software drag: stop run_real.sh, then '
+                'ros2 run arm_application handeye_teach --ros-args -p backend:=rdk'
+            )
+            self._status.set_text(self._status_text())
+            return
+        self.get_logger().info('starting soft-drag (preview stays live) ...')
+        # soft = joint impedance drag under AUTO Remote (no Floating license)
+        res = self._rdk.drag_on(self.drag_primitive, mode='soft')
+        if res.get('ok'):
+            self._drag_on = True
+            self.get_logger().info(
+                f'Drag ON mode={res.get("mode")} — push the arm, Enter=photo (s=hold optional)'
+            )
+            if res.get('hint'):
+                self.get_logger().info(str(res.get('hint')))
+        else:
+            self._drag_on = False
+            self.get_logger().error(f'Drag failed: {res.get("error")}')
+        # status text is refreshed by _tick on the GUI thread
+
+    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'Stop drag: {res.get("error")}')
+        self._drag_on = False
+        self.get_logger().info('Free Drive OFF / hold')
+
+    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 joints failed')
+            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('No camera frame yet')
+                return
+            q = self._read_joints()
+            if q is None:
+                self.get_logger().error('Failed to read joints')
+                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)
+            self.get_logger().info(f'[{idx + 1}] saved photo+joints')
+            self._write_yaml_snippet()
+        finally:
+            self._busy = False
+
+    def _write_yaml_snippet(self) -> None:
+        lines = [
+            '# Paste into 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========== TEACH DONE ==========')
+        print(f'poses: {n}')
+        print(f'joints: {self.poses_file}')
+        print(f'yaml:   {self.yaml_file}')
+        print(f'images: {self.images_dir}')
+        if n >= 1:
+            print('\n--- paste into handeye_calibrate.yaml ---\n')
+            print(self.yaml_file.read_text(encoding='utf-8'))
+            print('Also set num_poses:', n)
+        if n < 5:
+            self.get_logger().warn(f'Only {n} poses; need >=5 (prefer 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()

src/arm_application/arm_application/handeye_waypoints.py

@@ -0,0 +1,522 @@
+#!/usr/bin/env python3
+"""Hand-eye calibration waypoint runner.
+
+Moves to each joint waypoint one-by-one, waits for arrival, then dwells so a
+camera (or operator) can capture a calibration image.
+
+capture_mode:
+  auto      — dwell_sec then next pose
+  keypress  — terminal Enter to continue
+  preview   — OpenCV live camera + Enter/Space confirm (recommended for teaching)
+
+During ``preview`` mode, use the teach pendant **Free Drive** to drag-adjust
+while the preview window is open (ros2_control keeps AUTO Remote; drag is
+via pendant, not 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_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import GetArmState
+
+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'Unsupported image encoding: {msg.encoding!r}')
+
+
+class HandeyeWaypoints(Node):
+    def __init__(self) -> None:
+        super().__init__('handeye_waypoints')
+        self.declare_parameter('joint_prefix', 'Rizon4-063989_')
+        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 has no GUI (headless). Preview on /handeye/preview_image; '
+                'confirm with Enter in this terminal.'
+            )
+        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'Preview image: {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 mode needs OpenCV (cv2)')
+                return 1
+            if not self.image_topic:
+                self.get_logger().error('preview mode needs image_topic or 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 is empty')
+            return 1
+        if len(positions) != n_pts * n_j:
+            self.get_logger().error(
+                f'waypoint_positions length {len(positions)} != {n_pts}*{n_j}'
+            )
+            return 1
+        if self.start_index >= n_pts:
+            self.get_logger().error(
+                f'start_index {self.start_index} >= num waypoints {n_pts}'
+            )
+            return 1
+
+        if not self._client.wait_for_server(timeout_sec=15.0):
+            self.get_logger().error(
+                f'Action [{self.action_name}] not available. '
+                'Start stack first: 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'Logging captures → {path}')
+
+        if self.capture_mode == 'preview':
+            print(PREVIEW_HELP_GUI if self._cv_gui else PREVIEW_HELP_HEADLESS)
+
+        self.get_logger().info(
+            f'Hand-eye: {n_pts} poses, start={self.start_index}, '
+            f'v_scale={self.velocity_scale}, dwell={self.dwell_sec}s, '
+            f'mode={self.capture_mode}'
+        )
+        self.get_logger().warn(
+            'Clear workspace; keep board in FOV. Ctrl+C aborts.'
+        )
+
+        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}] moving (~{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('Aborted by user')
+                    return 0
+
+                # After confirm in preview mode, save taught joints
+                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('Hand-eye waypoint sequence done')
+        if self.append_poses_file and self.capture_mode == 'preview':
+            self.get_logger().info(
+                f'Taught poses saved → {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'Saved pose {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, 'send goal'):
+            return 3
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('Goal rejected')
+            return 4
+
+        res_fut = handle.get_result_async()
+        if not self._wait_future(res_fut, max(60.0, duration * 3.0), 'result'):
+            return 5
+        result = res_fut.result().result
+        if not result.success:
+            self.get_logger().error(f'Move failed: {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'Timeout waiting for {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}] ARRIVED — Free Drive adjust, then Enter/Space'
+            )
+        else:
+            self.get_logger().info(
+                f'[{index + 1}/{n_pts}] ARRIVED — CAPTURE NOW  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(
+                'Press Enter after capturing (q+Enter to quit)...'
+            )
+            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'Pose {index + 1}/{n_pts}',
+            (12, 28),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.7,
+            (0, 255, 0),
+            2,
+        )
+        cv2.putText(
+            vis,
+            'Free Drive drag | Enter=next q=quit',
+            (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())  # fast path; bytes costs ~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}] Adjust with Free Drive, then press Enter '
+            '(q+Enter to quit)'
+        )
+        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'Waiting for {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'Waiting for {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()

src/arm_application/arm_application/hole_detect_hough.py

@@ -0,0 +1,694 @@
+"""工件二阶段检测：YOLO 定位工件 + HoughCircles 检测左右螺纹孔中心。"""
+
+from __future__ import annotations
+
+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
+
+
+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)) -> None:
+    x1, y1, x2, y2 = bbox
+    cv2.rectangle(img, (x1, y1), (x2, y2), color, 2)
+    cv2.putText(img, "gongjian", (x1, max(20, y1 - 8)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2)
+
+
+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,
+) -> 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)
+    _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,
+) -> dict[str, Any]:
+    """
+    二阶段检测螺纹孔中心。
+
+    detector: 需提供 detect(image_bgr, conf=..., iou=..., imgsz=...) -> list[DetBox]
+              （YOLODetector / FasterRCNNDetector 均可）
+
+    返回:
+      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:
+            return _fail("未检测到工件", draw_img)
+
+        _draw_bbox(draw_img, bbox)
+
+        # 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)
+                return {
+                    "status": True,
+                    "msg": f"螺纹嵌件对，两孔距离 {dist:.1f}px",
+                    "hole_points": hole_points,
+                    "hole_radii": hole_radii,
+                    "bbox": list(bbox),
+                    "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)
+        return {
+            "status": True,
+            "msg": f"检测成功，两孔距离 {dist:.1f}px",
+            "hole_points": hole_points,
+            "hole_radii": hole_radii,
+            "bbox": list(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)
+
+
+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)
+
+            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)

src/arm_application/arm_application/hole_detect_node.py

@@ -0,0 +1,188 @@
+#!/usr/bin/env python3
+"""Hole detection on the current-arm D405 color topic. Detect-only (no insert motion)."""
+
+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
+
+
+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(Node):
+    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()

src/arm_application/arm_application/hole_detector.py

@@ -0,0 +1,349 @@
+"""孔定位模块：加载已训练模型，输出孔中心像素坐标与置信度。
+
+与工件定位（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"
+DEFAULT_MODEL_DIR = Path.home() / "ros2" / "models" / "hole_insert"
+
+
+@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]:
+    """Package yaml defaults, then overlay site.yaml hole_detect: (operator source of truth)."""
+    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() / "models" / "hole_insert" / "best.pt")
+    )
+
+
+def _default_model_path() -> Path:
+    candidates = [
+        DEFAULT_MODEL_DIR / "best.pt",
+        DEFAULT_MODEL_DIR / "hole_best.pt",
+        Path.home() / "ros2" / "models" / "yolo" / "detect_best.pt",
+    ]
+    for p in candidates:
+        if p.exists():
+            return p
+    return candidates[0]
+
+
+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)

src/arm_application/arm_application/kalman_filter.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+"""Constant-velocity Kalman filter for a 3D point (camera frame)."""
+
+from __future__ import annotations
+
+from typing import Optional
+
+import numpy as np
+
+
+class KalmanFilter3D:
+    """State ``[x, y, z, vx, vy, vz]``. Measurement is position only."""
+
+    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:
+        """Predict the state forward to time ``t`` (seconds, monotonic)."""
+        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:
+            # Long gap — keep position, zero velocity.
+            self.x[3:] = 0.0
+            self._t = t
+            return
+        self._predict(dt)
+        self._t = t
+
+    def update(self, z: np.ndarray) -> None:
+        """Measurement update with a 3D position."""
+        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

src/arm_application/arm_application/move_to_pose_client.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+"""Example client for arm_msgs/action/MoveToPose (Cartesian goal).
+
+Wire this to MoveIt or your IK solver action server when available.
+"""
+
+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
+
+
+class MoveToPoseClient(Node):
+    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 not available. '
+                'Start a Cartesian planner (e.g. MoveIt) that implements it.'
+            )
+            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
+        # Identity orientation
+        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'Sending Cartesian goal: ({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('Goal rejected')
+            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()
+    # Default demo pose; override via CLI: 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()

src/arm_application/arm_application/onnx_workpiece.py

@@ -0,0 +1,112 @@
+#!/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
+
+from pathlib import Path
+from typing import List, Optional, Union
+
+import numpy as np
+
+from arm_application.yolo_detector import Detection
+
+
+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)
+        self._names = class_names or {0: "workpiece"}
+
+    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._names.get(cid, str(cid)),
+                )
+            )
+        dets.sort(key=lambda d: d.conf, reverse=True)
+        return dets

src/arm_application/arm_application/orisys_tactile.py

@@ -0,0 +1,491 @@
+"""Orisys visuo-tactile adapter. SDK is pip-installed; site.yaml holds device ids."""
+
+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
+
+
+def resolve_orisys_config(config_name: str, config_file: str = "") -> str:
+    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)
+    pkg = ros2_root() / "src" / "arm_application" / "config" / "orisys" / f"{raw}.json"
+    if pkg.is_file():
+        return str(pkg)
+    try:
+        import orisys
+
+        root = Path(orisys.__file__).resolve().parent.parent
+        for cand in (root / "configs" / f"{raw}.json", 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 "")
+    cfg_name = str(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"),
+    )
+
+
+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 missing. pip install -e /path/to/orisys-sdk  "
+                "(git.orisys-tech.cn/orisys/test). Config still comes from 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) -> dict[str, Any]:
+        """读指定侧（单侧进程模式）；未连接/未知侧返回全零。"""
+        if not self._connected:
+            return self._empty_sample(side)
+        for item in self._sensors:
+            if item["side"] == side:
+                return self._read_single(item)
+        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) -> 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,
+        }
+
+    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,
+        }
+
+    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) -> None:
+    """独立进程：开一侧传感器，按 target_fps 把最新样本推进队列（图像不过队列）。
+
+    q: mp.Queue（调用方给 maxsize≈2，满了丢旧留新）
+    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
+
+    period = 1.0 / max(1.0, float(getattr(cfg.left if side == "left" else cfg.right, "target_fps", 30.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
+        t0 = time.perf_counter()
+        try:
+            d = tac.read_side(side)
+            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:
+            d = {k: v for k, v in d.items() if k != "img"}
+            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

src/arm_application/arm_application/perception_node.py

@@ -0,0 +1,916 @@
+#!/usr/bin/env python3
+"""perception_node — YOLO + depth + KF (own process).
+
+Hard rules:
+- Image callbacks ONLY store latest frames (never run YOLO in the callback)
+- YOLO runs on a dedicated worker thread so MutuallyExclusive I/O is never blocked
+- SensorDataQoS BEST_EFFORT keep_last(5)
+- Publish filtered 3D as geometry_msgs/PointStamped (no Services)
+- Dynamic object TF at vision rate only
+- High-confidence detections always reacquire (no freeze on fast motion)
+"""
+
+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,
+)
+
+
+def _bgr_copy(msg: Image) -> np.ndarray:
+    """BGR uint8 owned by caller (safe for async YOLO; ROS may drop the msg)."""
+    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}")
+
+
+class PerceptionNode(Node):
+    def __init__(self) -> None:
+        super().__init__("perception_node")
+        tid = threading.get_ident()
+        self.get_logger().warn(
+            f"perception_node started 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_hc90.pt")
+        self.declare_parameter("conf", 0.12)
+        self.declare_parameter("iou", 0.45)
+        self.declare_parameter("imgsz", 416)
+        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)  # above this = hole/background
+        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=raw meas, 0=pure KF
+        self.declare_parameter("half_precision", True)
+        self.declare_parameter("backend", "auto")  # auto|torch|tensorrt
+        self.declare_parameter("mp_infer", True)  # YOLO in a spawn() process
+        # Adaptive Z LPF: 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)
+        # Faster YOLO: downscale before infer; boxes scaled back to full res
+        self.declare_parameter("infer_max_width", 0)
+        # Box EMA (1=raw YOLO every frame — tight follow; lower = smoother/lag)
+        self.declare_parameter("box_ema_alpha", 0.85)
+        # Brief YOLO misses: keep last box on preview; clear only after N frames
+        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 = stereo surface centroid (recommended); box = YOLO box center
+        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 group: only lightweight store-latest (never 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  # duck-typed 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 warmup failed: {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 empty (camera_source={cam_src!r} ns={ns!r}); "
+                "use 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): camera preview tolerates brief DDS gaps
+        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 ready 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)
+        # Do NOT pickle the image into the YOLO queue here — that stalls the
+        # camera callback and looks like inference freeze. Infer thread submits.
+        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:
+        """Copy the newest camera frame and hand it to the GPU process."""
+        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 submit: {exc}", throttle_duration_sec=2.0)
+
+    def _restart_yolo(self, why: str) -> None:
+        """Self-heal a hung/dead GPU worker (engine reload takes a few sec)."""
+        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 worker {why} — restarting (box coasts)")
+        try:
+            self.detector.restart()
+            self._submitted_seq = -1
+            self._last_gpu_t = time.monotonic()
+            self.get_logger().info("YOLO worker restarted")
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().error(f"YOLO restart failed: {exc}")
+
+    def _infer_loop(self) -> None:
+        """Submit latest frame, collect GPU result, then stereo.
+
+        GPU must never wait on stereo: the worker drops old results if we are
+        briefly busy. We also submit *before* waiting so the next infer is
+        already queued.
+        """
+        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"infer: {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 {silent*1000:.0f}ms "
+                        f"(worker={'up' if getattr(self.detector, 'alive', True) else 'DEAD'})",
+                        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"hung {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"apply: {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, "no depth"
+        if not self._depth_fresh(msg):
+            return None, "stale depth"
+        enc = (msg.encoding or "").lower()
+        buf = self._decode_depth_m(msg)
+        if buf is None:
+            return None, "bad depth"
+        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)
+        # Inner ROI — avoid box edges; still keep enough pixels on a holey workpiece
+        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:
+            # Inner ROI is often holes; retry the full box.
+            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}"
+        # Near-surface: holes / table / far background must NOT pull Z out.
+        # Take closest 40%, then keep a 6 cm band around the 10th percentile.
+        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:
+        """Item 4: adaptive LPF on depth — quiet when still, responsive when moving."""
+        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:
+        """Empty detection → preview clears green box (must NOT reuse last 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:
+        """Optional downscale for YOLO; return (infer_bgr, scale) full = infer * 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:
+        """Keep the green box moving with last velocity; do not drive 3D."""
+        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"box coast {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:
+        """Match YOLO, publish a *now*-predicted box, then stereo on that box."""
+        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(
+                    "detection miss — cleared overlay",
+                    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)
+        # Compensate infer delay: draw where the part is *now*, not where YOLO saw it
+        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
+        )
+
+        # Hover target = stereo 3D on the workpiece inside the box (not a ray
+        # through a hole / table, and not z_ref). Red dot = that 3D point.
+        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:
+            # Never had usable depth (target beyond D405 range / not in the work
+            # area / camera mount or handeye off). Do NOT publish a z_ref point —
+            # that would fake-lock the gripper over a phantom. Hold instead.
+            self.get_logger().warn(
+                f"target has NO usable depth ({note}) — NOT tracking. "
+                f"Put the workpiece in view at {self.min_depth:.2f}-{self.work_z_max:.2f} m "
+                "from the camera, or check camera mount / handeye calibration.",
+                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"reacquire 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
+            # Keep XY scale honest even if KF still has an old far-Z
+            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"track {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:
+        """Reject stale depth — on a fast-moving part, old depth at the current
+        box reads the background (table at ~2.4 m) and poisons the 3D point."""
+        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 feature: {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"plane feature: {exc}", throttle_duration_sec=2.0)
+            return None
+
+    def _metric_z(self, z_meas: Optional[float], note: str) -> tuple:
+        """Z used for pixel→metre. Never use hole/floor depth (inflates XY error).
+        Returns (None, note) when this target NEVER had usable depth — publishing
+        a z_ref point then would fake-lock the servo onto a phantom 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()

src/arm_application/arm_application/pid_controller.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+"""PID with integral clamp, derivative-on-measurement, and integral separation."""
+
+from __future__ import annotations
+
+from typing import Optional
+
+
+class PidController:
+    """``u = Kp e + Ki ∫e + Kd (−dmeas/dt)``, then saturate.
+
+    Integral separation: when ``|error| > i_sep_thresh``, integral is forced to 0
+    and does not accumulate (P+D only for fast chase / less overshoot).
+    """
+
+    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
+
+        # Integral separation — large error: clear I, do not accumulate
+        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)

src/arm_application/arm_application/pipeline_node.py

@@ -0,0 +1,276 @@
+#!/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 std_msgs.msg import Bool, String
+
+from arm_application.stage3_search_node import Stage3Search
+
+
+class AssemblyPipeline(Stage3Search):
+    def __init__(self) -> None:
+        super().__init__(node_name="assembly_pipeline")
+        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)
+        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 _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:
+        """跟踪高度 = 孔悬停高 + 5cm，通止规离开工件，二阶段只需小幅下降。"""
+        return float(self.hover_z) + 0.05
+
+    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()
+        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",
+        ]
+        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)
+        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(
+                f"一阶段 {timeout:.0f}s 没锁定（视野里没工件？看 {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()

src/arm_application/arm_application/plane_feature.py

@@ -0,0 +1,296 @@
+#!/usr/bin/env python3
+"""Stereo plane Z at the YOLO box center.
+
+The hover target is the box-center ray intersected with the fitted near
+plane. That stays on the detection, with metric Z (30° camera: wrong Z
+would shift base XY). The cloud-mean centroid is not used — holes / table
+inliers pull it off the green box.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional, Tuple
+
+import numpy as np
+
+
+@dataclass
+class PlaneFeature:
+    uv: np.ndarray  # (2,) color-pixel coords of the 3D centroid
+    z: float  # camera-frame Z of the 3D centroid
+    xyz: np.ndarray  # (3,) stereo-cloud centroid in camera frame
+    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]:
+    """Physical workpiece point: median 3D of the near surface in the inner box.
+
+    Not a monocular ray (holes punch through to the table) and not the
+    whole-ROI cloud mean (table / one-sided depth pulls it off the part).
+    """
+    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]:
+    """Box-center point on the fitted near plane, or 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
+        # Keep uv in color pixels: work in depth pixels then scale back.
+        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]
+    # Color-equivalent pixel for back-projection (aligned depth uses color 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}",
+    )

src/arm_application/arm_application/realsense_color_pub.py

@@ -0,0 +1,917 @@
+#!/usr/bin/env python3
+"""Publish RealSense color stream to ROS topics (no realsense2_camera apt pkg).
+
+Use when ``ros-humble-realsense2-camera`` is not installed but ``pyrealsense2`` works.
+
+Default topics (camera_name=d405):
+  /d405/color/image_raw
+  /d405/color/camera_info
+  /d405/aligned_depth_to_color/image_raw   (16UC1 mm, if enable_depth)
+
+Performance notes
+-----------------
+* Publishes from the capture thread (no timer lag).
+* Uses SensorDataQoS (BEST_EFFORT, depth=1) so slow subscribers drop old frames.
+* Optional SDK preview: ``-p show_preview:=true`` (Intel sample path: pipeline → numpy → window).
+"""
+
+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
+
+
+def _param_as_str(node: Node, name: str, default: str = '') -> str:
+    """String param that also accepts numeric CLI overrides (e.g. serial numbers)."""
+    node.declare_parameter(
+        name, default, ParameterDescriptor(dynamic_typing=True)
+    )
+    val = node.get_parameter(name).value
+    if val is None or val == '':
+        return ''
+    return str(val).strip()
+
+
+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(Node):
+    def __init__(self) -> None:
+        super().__init__('realsense_color_pub')
+        self.declare_parameter('camera_name', 'd405')  # topic namespace
+        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)
+        # Render the Qt preview only every Nth frame — publishing stays at full
+        # rate; the CPU colorizer + 4-pane paint must not slow the stream.
+        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 meter: hardware frame numbers vs actually published (loop rate)
+        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, keep last 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 streams are published too so the preview can live in a separate
+        # viewer process (Qt must not share the GIL with the capture loop).
+        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
+        # Device depth scale → multiply raw uint16 by this to get true mm.
+        # D405 = 0.1 mm/unit, D435 = 1.0 mm/unit. Read from the sensor at startup.
+        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 preview ON — COLOR|DEPTH|IR-L|IR-R in this process '
+                f'(hold {self._overlay_hold_sec:.1f}s). Close window ≠ stop camera.'
+            )
+        self._start_pipeline()
+
+    def _pick_device(self, devices, rs):
+        """Prefer configured serial, then D405 by name (this PC also has a 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'Serial {self._serial!r} not found. Connected: {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:
+        """Pick up overlay_hold_sec (and warn if width/height changed) without restart."""
+        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 reload failed: {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"preview stream is {live_w}x{live_h}, site.yaml camera is "
+                f"{want_w}x{want_h} — restart: 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:
+        # Empty payload = perception miss → clear green box immediately
+        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', ...) hits the setter's fast path — plain bytes gets
+        # an element-wise debug check that costs ~40 ms per frame here.
+        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'first aligned depth {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 not available. conda activate ros_flexiv'
+            ) from exc
+
+        # After killing a previous publisher, USB may take 1–3s to re-enumerate.
+        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(
+                'No RealSense connected (USB empty after retries). '
+                'Plug D405 into THIS machine on a USB3 (blue) port, unplug/replug, '
+                'and close realsense-viewer / the 192.168.0.108 capture site if it holds the camera.'
+            )
+
+        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:
+            self.get_logger().warn(
+                f'{cam_name} is on USB {usb_desc} — keeping depth/IR at '
+                '640x480@15. Plug D405 into a USB3 (blue) port for 848@30.'
+            )
+            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)]
+        # Drop firmware junk (848@90 etc.) that ioctl-fails and wedges 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: try real IR-L+IR-R first (same stereo AE). Color+IR-L often
+        # fails on D405 — then IR-R + color-as-left (different look, not AE).
+        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'Requested {want[0]}x{want[1]}@{want[2]}Hz '
+                            f'→ using {w}x{h}@{fps}Hz'
+                        )
+                    if want_depth and not with_depth:
+                        self.get_logger().warn(
+                            f'{w}x{h}@{fps}Hz color+depth failed — color only'
+                        )
+                    elif want_infra and infra_mode == 'none':
+                        self.get_logger().warn(
+                            f'{w}x{h}@{fps}Hz stereo IR failed — color+depth preview'
+                        )
+                    elif infra_mode == 'both':
+                        self.get_logger().info(
+                            f'{w}x{h}@{fps}Hz stereo IR-L+IR-R (shared AE) + depth'
+                        )
+                    elif infra_mode == 'right':
+                        self.get_logger().info(
+                            f'{w}x{h}@{fps}Hz IR-L=color gray, IR-R=raw IR '
+                            '(looks brighter/darker — not two AE loops)'
+                        )
+                    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} failed: {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'No supported color profile for D405. Last error: {last_err}'
+            )
+
+        self._pipe = pipe
+        self._profile = profile
+        self._have_depth = have_depth
+        self._have_infra = have_infra
+        self._logged_depth = False
+        # Publish depth in TRUE millimetres. D405 raw units are 0.1 mm (D435 1 mm);
+        # downstream (perception/servo) assumes mm, so normalise by the device scale.
+        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/unit → mm/unit
+                self.get_logger().info(
+                    f'depth scale {ds * 1000.0:.4f} mm/unit → publishing true mm'
+                )
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().warn(f'depth scale read failed, assume 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)
+            # Firmware often resets AE on the first frames; stamp it again.
+            _time.sleep(0.25)
+            apply_realsense_exposure(profile.get_device(), **kwargs)
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'exposure apply: {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 failed ({exc}); using D405 defaults for '
+                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 ' (color only)'
+        )
+        if have_infra:
+            depth_txt += ' + IR-L/IR-R preview'
+        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:
+        """Color auto-exposure that prefers short exposure (less motion blur)."""
+        try:
+            sensor = profile.get_device().first_color_sensor()
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'color AE: no color sensor ({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 = keep FPS, shorten exposure when the scene is bright enough
+                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; less motion blur at 30 Hz
+                except Exception:
+                    pass
+            self.get_logger().info('color auto-exposure ON (prefer FPS / less blur)')
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'color AE: {exc}')
+
+    def _enable_stereo_ae(self, profile, rs) -> None:
+        """One auto-exposure for both IR imagers (stereo module)."""
+        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:
+                    # Cap IR exposure well under the 33 ms frame period so a
+                    # dim scene cannot silently drop the stream below 30 Hz.
+                    sensor.set_option(limit, 20000.0)
+                    self.get_logger().info('stereo IR exposure limit 20ms')
+                except Exception:
+                    pass
+            self.get_logger().info('stereo IR auto-exposure ON (shared L/R)')
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'stereo 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'reconnect failed: {exc}. Unplug/replug USB3, slack the cable.',
+                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'Camera stream lost (USB={usb}): {text[:120]}. '
+                        'Reconnecting — slack the USB cable so the arm does not yank it.'
+                    )
+                    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:
+                # raw device units → true mm (D405 raw is 0.1 mm/unit)
+                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)
+            # Preview work (CPU colorizer, IR copies, 4-pane paint) only every
+            # Nth frame — it must not slow down publishing.
+            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
+                # Only substitute color-gray when the raw left IR stream is unavailable.
+                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'first frame {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(
+                            'NO box — check 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
+                # Context may be shutting down
+                if not self._running:
+                    break
+                self.get_logger().warn(
+                    f'publish failed: {exc}', throttle_duration_sec=2.0
+                )
+
+    def _publish_ir(self, frame, pub) -> None:
+        """Raw IR imager as mono8 for the external viewer process."""
+        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:
+        """Log published vs hardware frame rate — separates loop slowness
+        (preview/CPU) from SDK-side drops (AE long exposure / 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
+            # Closing the window must NOT stop the camera — track needs the stream.
+            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(
+                        'Preview closed — camera keeps publishing'
+                    )
+                    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()

src/arm_application/arm_application/realsense_sdk_view.py

@@ -0,0 +1,236 @@
+#!/usr/bin/env python3
+"""RealSense SDK preview: fixed 2x2 Qt panes (color / depth / IR-L / IR-R).
+
+Stitching four streams into one pixmap looked like a single camera view when
+depth/IR were missing. Each stream now has its own labeled pane.
+"""
+
+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, "NO COLOR")
+        self._set_pane(labs[1], depth, "NO DEPTH")
+        self._set_pane(labs[2], ir_l, "NO IR-L")
+        self._set_pane(labs[3], ir_r, "NO IR-R")
+
+
+def ensure_app() -> "QApplication":
+    if QApplication is None:
+        raise RuntimeError("PySide6 not available for RealSense preview window")
+    app = QApplication.instance()
+    if app is None:
+        app = QApplication([])
+    return app

src/arm_application/arm_application/realsense_view.py

@@ -0,0 +1,280 @@
+#!/usr/bin/env python3
+"""realsense_view — D405 4-pane preview (COLOR|DEPTH|IR-L|IR-R), own process.
+
+The camera node (``realsense_color_pub``) only publishes topics. All Qt work
+runs here so painting can never starve the camera capture loop (Python GIL
+made ``publish`` wall-time jump from 1 ms to ~70 ms when they shared a
+process). Closing the window only hides the preview; Ctrl+C stops the viewer
+(the camera keeps publishing).
+"""
+
+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
+
+
+def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
+    """Fast numpy JET colorize for the preview pane (no 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(Node):
+    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 rate meter — visible stutter diagnosis (transport vs paint)
+        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  (viewer)'
+        )
+        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'viewer ON — subscribing /{self._name}/color|depth|infra1|infra2 '
+            f'(hold {self._hold:.2f}s). Close window = hide only.'
+        )
+
+    # Callbacks only store the newest message; conversion happens in _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 reload failed: {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'viewer rx 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 = 'NO box — check 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 here (executor thread) so the Qt paint thread stays cheap.
+        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 callbacks (image deserialization, depth colorize, push) run on
+    # executor threads; the main thread only paints Qt — no shared GIL bursts.
+    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('Preview closed — viewer idles (camera keeps publishing)')
+                node._preview = None
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/arm_application/arm_application/run_joint_trajectory.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+"""Send a multi-point joint trajectory via /follow_joint_trajectory (real + mock)."""
+
+from __future__ import annotations
+
+import time
+from typing import List
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+NUM_JOINTS = 7
+# flat positions (3 pts x 7 joints) + times — ROS params do not support dict lists
+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(Node):
+    def __init__(self) -> None:
+        super().__init__('run_joint_trajectory')
+        self.declare_parameter('joint_prefix', 'Rizon4-063989_')
+        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 = run once (default); 0 = loop until Ctrl+C; N = repeat N times
+        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}] not available. '
+                'Start real stack first: 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 is empty')
+            return 1
+        if len(positions) != n_pts * n_j:
+            self.get_logger().error(
+                f'waypoint_positions length {len(positions)} != '
+                f'{n_pts} points x {n_j} joints = {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'Loop mode: {goal.num_points} points, ~{duration:.1f}s/cycle '
+                f'(Ctrl+C to stop)'
+            )
+        elif loop_count > 1:
+            self.get_logger().info(
+                f'Loop mode: {loop_count} cycles, {goal.num_points} points, '
+                f'~{duration:.1f}s/cycle'
+            )
+        else:
+            self.get_logger().info(
+                f'Sending {goal.num_points} points, v_scale={goal.velocity_scale}, '
+                f'duration={duration:.1f}s'
+            )
+
+        iteration = 0
+        while rclpy.ok():
+            iteration += 1
+            if loop_count > 1:
+                self.get_logger().info(f'Cycle {iteration}/{loop_count}')
+
+            code = self._send_once(goal, duration)
+            if code != 0:
+                return code
+
+            if loop_count == 1:
+                self.get_logger().info('Done: OK')
+                return 0
+            if loop_count > 1 and iteration >= loop_count:
+                self.get_logger().info(f'Done: completed {loop_count} cycles')
+                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('Timed out sending goal')
+                return 2
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            self.get_logger().error('Goal rejected')
+            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'Executing trajectory (~{duration:.0f}s planned), '
+                    f'waiting for robot... ({left:.0f}s timeout left)'
+                )
+                next_log = now + 10.0
+            if now > deadline:
+                self.get_logger().error('Timed out waiting for result')
+                return 4
+        result = res_fut.result().result
+        if not result.success:
+            self.get_logger().error(f'Failed: {result.message}')
+            return 5
+        self.get_logger().info(f'Cycle finished: {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()

src/arm_application/arm_application/site_io.py

@@ -0,0 +1,42 @@
+"""Load ~/ros2/config/site.yaml from application nodes."""
+
+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

src/arm_application/arm_application/tactile_cli.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+"""Orisys visuo-tactile CLI. Ports / config from 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 tactile CLI (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"ERROR: {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("interrupt")
+    finally:
+        tactile.disconnect()
+    print(f"done frames={n}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/arm_application/arm_application/tactile_interface.py

@@ -0,0 +1,84 @@
+"""Tactile sample / command types. Backend (Orisys / mock) is swappable."""
+
+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}",
+        )

src/arm_application/arm_application/tactile_node.py

@@ -0,0 +1,159 @@
+#!/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 rclpy
+from rclpy.node import Node
+
+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
+
+
+class TactileNode(Node):
+    def __init__(self) -> None:
+        super().__init__("tactile")
+        self.declare_parameter("publish_hz", 20.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)
+        self._backend = 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
+
+        if self._mock:
+            self._connected = {"left": True, "right": True}
+            self.get_logger().info("tactile mock backend")
+        else:
+            cfg = load_orisys_from_site(data)
+            ctx = mp.get_context("spawn")  # spawn：子进程不带 DDS 线程，CUDA 上下文干净
+            self._stop = ctx.Event()
+            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),
+                    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 _tick(self) -> None:
+        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),
+                })
+            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()

src/arm_application/arm_application/yolo_detector.py

@@ -0,0 +1,498 @@
+#!/usr/bin/env python3
+"""Ultralytics YOLO / TensorRT wrapper.
+
+Public: ``resolve_weights``, ``YoloDetector``, ``Detection``, ``pick_best``,
+``MpYoloDetector``. TensorRT loads the sibling ``.engine`` next to the
+``.onnx`` / ``.pt`` path when ``backend='tensorrt'``.
+"""
+
+from __future__ import annotations
+
+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
+
+# Sentinel: worker still on the previous frame — caller must NOT count a miss.
+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:
+    """Prefer TensorRT .engine next to the .pt when backend=auto/tensorrt."""
+    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 missing: {engine}. "
+            f"Export: bash ~/ros2/scripts/export_yolo_trt.sh"
+        )
+    if b == "onnx":
+        if onnx.is_file():
+            return onnx
+        raise FileNotFoundError(f"ONNX missing: {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
+
+
+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 not installed. In ros_flexiv: pip install ultralytics"
+            ) from exc
+
+        path = Path(weights).expanduser()
+        if not path.is_file():
+            raise FileNotFoundError(
+                f"YOLO weights not found: {path}. "
+                f"Place your .pt at ~/ros2/models/yolo/workpiece.pt"
+            )
+        try:
+            import torch
+
+            # Do not let PyTorch occupy all CPU cores (camera + ROS need them).
+            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
+        )
+        names = getattr(self._model, "names", None) or {}
+        self._id_to_name = {int(k): str(v) for k, v in dict(names).items()}
+        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]:
+        """Alias used by hole detectors (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])
+            name = self._id_to_name.get(cid, str(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:
+    """Spawned process: GPU YOLO only. Never import rclpy here."""
+    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
+        # Drain to the newest queued frame — never spend GPU on stale images.
+        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:
+    """Never block the GPU process on a full result queue."""
+    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 in a spawn() child so ROS callbacks never share the 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 drains to newest: parent never has to drop frames
+        # (Queue put_nowait/get_nowait races were the source of multi-second
+        # "silent" freezes with the worker alive).
+        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 worker: {err}")
+        self.weights_path = str(msg[1])
+        self.imgsz = int(self._cfg.get("imgsz") or 416)
+
+    def restart(self) -> None:
+        """Kill a hung/dead worker and spawn a fresh one (engine reload ~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:
+        """Queue the newest frame. If the worker is busy, skip — it drains to
+        the newest queued frame after every infer, so nothing stale runs."""
+        if not self.alive:
+            raise RuntimeError("YOLO worker died")
+        try:
+            self._q_in.put((int(seq), image_bgr), timeout=0.010)
+        except Exception:
+            pass
+
+    def wait_result(self, timeout: float = 0.045):
+        """Return ``(seq, list[Detection], gpu_ms)`` or ``None``."""
+        if not self.alive:
+            raise RuntimeError("YOLO worker died")
+        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):
+        """Compat: submit + wait. Prefer submit/wait_result for pipelining."""
+        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()  # stuck in a CUDA call — 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]:
+    """Keep the current workpiece under fast motion.
+
+    Match against a *predicted* previous box with a wide gate. If IoU fails
+    but YOLO still fired above min_conf, keep that detection — do not return
+    None (that was the main source of dropouts while moving).
+    Tiny boxes (logo / 通止规尖) are rejected so the arm does not lock on the mat.
+    """
+    if not dets:
+        return None
+    # Locked track may dip below min_conf while moving (blur). New locks cannot.
+    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)
+    # Fast move: keep a live YOLO even if it jumped far from the last box.
+    need = max(float(min_conf), float(reacquire_conf))
+    if float(best.conf) >= need:
+        return best
+    return None

src/arm_application/config/demo.yaml

@@ -0,0 +1,5 @@
+# Application-level waypoints / task parameters
+demo:
+  poses: [home, ready, inspect, home]
+  velocity_scale: 0.5
+  dwell_sec: 1.0

src/arm_application/config/dual_grippers.yaml

@@ -0,0 +1,25 @@
+# Optional fallback for DualArmGripperManager(load_dual_arm_gripper_config(path)).
+# Daily use reads ~/ros2/config/site.yaml via load_from_site().
+left_arm:
+  robot_sn: "Rizon4-063989"
+  gripper_port: "/dev/ttyUSB0"
+  gripper_slave_id: 1
+  gripper_baudrate: 115200
+right_arm:
+  robot_sn: "Rizon4-063991"
+  gripper_port: "/dev/ttyUSB1"
+  gripper_slave_id: 1
+  gripper_baudrate: 115200
+gripper:
+  parallel:
+    stroke_min: 0.0
+    stroke_max: 20.0
+    default_speed: 50
+    default_force: 20
+    auto_initialize: true
+  rotary:
+    auto_initialize: false
+    default_speed: 50
+    default_torque: 50
+    finger_open_per_mille: 1000
+    finger_close_per_mille: 0

src/arm_application/config/handeye_calibrate.yaml

@@ -0,0 +1,80 @@
+# Hand-eye auto calibration config.
+#
+# Workspace: ~/ros2
+#
+# Teach poses, then solve (prefer the wrapper):
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#   bash ~/ros2/scripts/run_handeye.sh calibrate
+
+handeye_calibrate:
+  ros__parameters:
+    mode: "eye_in_hand"
+
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    velocity_scale: 0.10
+    action_name: "follow_joint_trajectory"
+    move_time_sec: 4.0
+    start_index: 0
+    min_poses: 5
+
+    # Camera on flange (eye-in-hand): D405
+    camera_source: "d405"            # d405 | d435 | manual
+    camera_serial: ""
+    camera_namespace: ""
+    realsense_width: 1280
+    realsense_height: 720
+    image_topic: ""
+    camera_info_topic: ""
+
+    camera_fx: 0.0
+    camera_fy: 0.0
+    camera_cx: 0.0
+    camera_cy: 0.0
+    camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
+
+    # Physical board verified on teach images: inner corners 11x16 (not 10x15)
+    board_cols: 11
+    board_rows: 16
+    square_size_m: 0.011
+
+    settle_sec: 0.4
+    stable_window: 8
+    stable_pixel_std: 0.45
+    stable_timeout_sec: 25.0
+
+    output_dir: "/tmp/handeye_calib"
+    result_file: ""
+    cleanup_images: true
+
+    num_poses: 13
+    # Taught 2026-08-28 via handeye_teach (backend:=rdk)
+    waypoint_positions: [
+      # pose 0
+      -1.107107, -0.744489, 0.982447, 1.856279, -0.735517, 0.946910, -0.118163,
+      # pose 1
+      -0.982764, -1.163516, 1.128527, 1.406787, -0.502416, 0.571857, 0.224024,
+      # pose 2
+      -1.012149, -1.180698, 1.184095, 1.390763, -0.559587, 0.407246, 0.249883,
+      # pose 3
+      -0.984199, -1.175286, 1.175012, 1.397197, -0.796608, 0.459682, 0.247304,
+      # pose 4
+      -0.982355, -1.196864, 1.076502, 1.590187, -1.106339, 0.636800, 0.481410,
+      # pose 5
+      -1.121428, -1.173892, 1.152070, 1.762215, -1.147604, 0.641314, 0.434178,
+      # pose 6
+      -1.121716, -1.174258, 1.203286, 1.643879, -1.200144, 0.459669, 0.315685,
+      # pose 7
+      -1.100360, -1.337467, 1.235742, 1.377341, -1.211692, 0.569701, 0.220985,
+      # pose 8
+      -1.160070, -1.473164, 1.364572, 1.369112, -0.784077, 0.489467, 0.378945,
+      # pose 9
+      -0.575560, -1.470411, 1.348577, 0.982228, -1.163608, -0.208091, 0.410659,
+      # pose 10
+      -0.716065, -1.411332, 1.272539, 1.753754, -1.856130, -0.098214, 0.341287,
+      # pose 11
+      -0.870910, -1.412227, 1.243149, 1.900543, -1.886769, 0.153908, -0.399394,
+      # pose 12
+      -0.656925, -1.412314, 1.216671, 1.820324, -1.850224, -0.103640, -0.399964,
+    ]

src/arm_application/config/handeye_poses.yaml

@@ -0,0 +1,38 @@
+# Legacy alias of 89 taught joints. Prefer handeye_poses_89.yaml / _91.yaml.
+#
+# Edit here, or:
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#
+# /** so any node name receives these params.
+
+/**:
+  ros__parameters:
+    num_poses: 12
+    # Taught 2026-09-02 via handeye_teach (RDK free-drive)
+    waypoint_positions: [
+      # pose 0
+      -0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
+      # pose 1
+      -0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
+      # pose 2
+      -0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
+      # pose 3
+      -0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
+      # pose 4
+      -0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
+      # pose 5
+      -0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
+      # pose 6
+      -0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
+      # pose 7
+      -0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
+      # pose 8
+      -0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
+      # pose 9
+      -0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
+      # pose 10
+      -0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
+      # pose 11
+      -0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
+    ]

src/arm_application/config/handeye_poses_89.yaml

@@ -0,0 +1,38 @@
+# Hand-eye taught joint poses (7 joints × N). Shared by calibrate + waypoints.
+#
+# Edit here, or:
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#
+# /** so any node name receives these params.
+
+/**:
+  ros__parameters:
+    num_poses: 12
+    # Taught 2026-09-02 via handeye_teach (RDK free-drive)
+    waypoint_positions: [
+      # pose 0
+      -0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
+      # pose 1
+      -0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
+      # pose 2
+      -0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
+      # pose 3
+      -0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
+      # pose 4
+      -0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
+      # pose 5
+      -0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
+      # pose 6
+      -0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
+      # pose 7
+      -0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
+      # pose 8
+      -0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
+      # pose 9
+      -0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
+      # pose 10
+      -0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
+      # pose 11
+      -0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
+    ]

src/arm_application/config/handeye_poses_91.yaml

@@ -0,0 +1,36 @@
+# Right arm 91 (Rizon4-063991) taught joints for hand-eye.
+# Fill with:
+#   bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
+#   bash ~/ros2/scripts/run_handeye.sh --arm 91 merge
+#
+# Do not reuse 89's waypoint_positions.
+
+/**:
+  ros__parameters:
+    num_poses: 12
+    waypoint_positions: [
+      # pose 0
+      0.387144, -0.514770, -0.072695, 1.399918, 0.326501, 0.308501, -0.298212,
+      # pose 1
+      0.459205, -0.747691, 0.042380, 0.908430, 0.436933, -0.099413, -0.398336,
+      # pose 2
+      0.113162, -0.929267, -0.132969, 0.776759, 0.579499, -0.081271, -0.588715,
+      # pose 3
+      0.823056, -1.181803, -0.157161, 0.344860, 0.073450, -0.436449, -0.333484,
+      # pose 4
+      0.561660, 0.126479, -0.217063, 2.400061, 1.116363, 1.205544, -0.955972,
+      # pose 5
+      0.749011, 0.058548, -0.125615, 1.951037, 0.399065, 0.388437, -1.057397,
+      # pose 6
+      0.707611, -0.557010, -0.302672, 1.263980, 0.333093, 0.208229, -0.938372,
+      # pose 7
+      0.197846, -0.400530, 0.263064, 1.879643, 0.062586, 1.133715, -0.791285,
+      # pose 8
+      0.804027, -0.518162, 0.353947, 1.985246, -0.941195, 0.481238, -1.551113,
+      # pose 9
+      0.296161, -0.030159, 0.317117, 2.335652, 1.072158, 1.524893, -0.591372,
+      # pose 10
+      0.346492, -0.906418, -0.118014, 0.810448, 0.420788, -0.122104, -0.197859,
+      # pose 11
+      1.031802, -0.422436, -0.640939, 1.656629, 0.375163, 0.470365, -0.328041,
+    ]

src/arm_application/config/handeye_result.yaml

@@ -0,0 +1,14 @@
+# Legacy alias of 89 (Rizon4-063989). Calibrate into handeye_result_89.yaml / _91.yaml.
+# Hand-eye result
+# mode: eye_in_hand
+# camera_source: d405
+# views: 12
+# utc: 2026-09-04T10:15:27.480727+00:00
+# board: 8x11 square_m=0.015
+# T_cam_to_gripper (eye_in_hand)
+translation_m: [-0.06085181, -0.00864320, 0.12364093]
+T_4x4:
+  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
+  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
+  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

src/arm_application/config/handeye_result_89.yaml

@@ -0,0 +1,14 @@
+# Left arm 89 (Rizon4-063989) eye-in-hand. Do not use for 91.
+# Hand-eye result
+# mode: eye_in_hand
+# camera_source: d405
+# views: 12
+# utc: 2026-09-04T10:15:27.480727+00:00
+# board: 8x11 square_m=0.015
+# T_cam_to_gripper (eye_in_hand)
+translation_m: [-0.06085181, -0.00864320, 0.12364093]
+T_4x4:
+  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
+  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
+  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

src/arm_application/config/handeye_result_91.yaml

@@ -0,0 +1,13 @@
+# Hand-eye result
+# mode: eye_in_hand
+# camera_source: d405
+# views: 11
+# utc: 2026-09-15T08:44:59.380472+00:00
+# board: 8x11 square_m=0.015
+# T_cam_to_gripper (eye_in_hand)
+translation_m: [-0.06856274, -0.00768546, 0.13773073]
+T_4x4:
+  - [0.00611182, -0.87098536, 0.49127096, -0.06856274]
+  - [0.99997742, 0.00395004, -0.00543744, -0.00768546]
+  - [0.00279539, 0.49129310, 0.87098983, 0.13773073]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

src/arm_application/config/handeye_waypoints.yaml

@@ -0,0 +1,25 @@
+# Hand-eye interactive waypoint tour (preview + Free Drive confirm).
+# Poses: sibling handeye_poses.yaml
+#
+# Run:
+#   bash ~/ros2/scripts/run_handeye.sh waypoints
+
+handeye_waypoints:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    velocity_scale: 0.05
+    action_name: "follow_joint_trajectory"
+    dwell_sec: 2.5
+    capture_mode: "preview"       # auto | keypress | preview
+    start_index: 0
+    log_csv: ""
+    capture_topic: "handeye/capture"
+
+    camera_source: "d405"
+    camera_namespace: ""
+    image_topic: ""
+    preview_window: true
+    append_poses_file: "/tmp/handeye_calib/taught_poses.txt"
+
+    waypoint_times: [4.0, 4.0, 4.0, 4.0, 4.0, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.0, 4.0]

src/arm_application/config/hole_detect.yaml

@@ -0,0 +1,47 @@
+# 工件二阶段检测配置：YOLO 定位 + HoughCircles 螺纹孔
+# 供 hole_detect_hough.py / hole_detect_node（backend=two_stage）使用
+# 操作入口：bash ~/ros2/scripts/run_hole_detect.sh --arm 89|91 --preview
+
+# ----- YOLO -----
+conf: 0.25
+iou: 0.7
+imgsz: 640
+
+# ----- 居中门控：bbox 中心接近画面中心才做二阶圆孔检测 -----
+# 精对准 k1/k2 时 stage2 会 skip 此门控（对准目标孔后工件必然离开画面中心）
+center_gate_enable: true
+center_max_offset_ratio_x: 0.15   # |bbox_cx - 图像中心x| <= 该比例 * 宽
+center_max_offset_ratio_y: 0.18   # |bbox_cy - 图像中心y| <= 该比例 * 高
+center_gate_draw: true            # 可视化画出允许居中区域
+refine_hole_centers: true         # Hough 后再用暗孔矩细化亚像素圆心
+
+# ----- ROI：bbox 向外膨胀像素，裁剪时做边界保护 -----
+bbox_padding: 25
+
+# ----- ROI 预处理 -----
+gaussian_ksize: 5          # 必须为奇数
+gaussian_sigma: 1.5
+
+# ----- HoughCircles（全部由此读取）-----
+hough_dp: 1.2
+hough_min_dist: 50
+hough_param1: 100
+hough_param2: 26
+hough_min_radius: 10
+hough_max_radius: 30
+
+# ----- 工业校验 -----
+min_radius: 10             # 有效圆孔半径下限（滤掉噪点）
+max_radius: 30             # 有效圆孔半径上限（滤掉中间大孔）
+hole_distance_min: 150     # 两孔像素距离下限
+hole_distance_max: 700     # 两孔像素距离上限
+
+# ----- 时序稳帧（抑制闪烁）-----
+stabilize_hold_frames: 15  # 连续失败多少帧后才清空结果
+stabilize_ema_alpha: 0.35  # 中心点/半径 EMA，越小越稳
+stabilize_max_jump: 60     # 单帧跳变超过该像素则忽略（防闪）
+
+# ----- 螺纹嵌件对（亮环签名 + 关于中心对称，深度自适应）-----
+insert_pair_enable: true
+insert_percentile: 96
+insert_bolt_radius_mm: 29.0   # 两目标孔物理间距~58mm → 螺栓圆半径29mm；按深度换算像素，尺度不变

src/arm_application/config/hole_insert.yaml

@@ -0,0 +1,35 @@
+# 孔定位检测配置（改 conf / 内参 / 双孔间距即可，不必改代码）
+# 权重默认：~/ros2/models/hole_insert/best.pt
+# backend: yolo | hough | two_stage（two_stage 请用 hole_detect.yaml）
+
+model_path: ""
+backend: yolo                 # yolo | faster_rcnn | hough
+num_classes: 1                # 单类孔=1；左右孔分两类=2
+expected_holes: 2
+min_hole_sep_px: 20
+max_hole_sep_px: 2000
+
+conf: 0.25
+iou: 0.7
+imgsz: 640
+
+gaussian_ksize: 5
+gaussian_sigma: 1.5
+hough_dp: 1.2
+hough_min_dist: 40
+hough_param1: 100
+hough_param2: 28
+hough_min_radius: 6
+hough_max_radius: 45
+
+stabilize_window: 7
+stabilize_ema: 0.35
+stabilize_max_jump_px: 60
+
+camera_intrinsics:
+  fx: 425.0
+  fy: 425.0
+  cx: 640.0
+  cy: 360.0
+
+default_depth_m: 0.10

src/arm_application/config/orisys/README.md

@@ -0,0 +1,10 @@
+# Orisys 传感器配置
+
+`dd03.json` 是厂家相机/跟踪参数，由 `site.yaml` → `tactile.config_file` 指向。  
+换型号时把新 json 放到本目录，并改 `tactile.config` / `config_file`。
+
+Orisys Python SDK **不进本仓库**，在工控机：
+
+```bash
+pip install -e /path/to/orisys-sdk
+```

src/arm_application/config/orisys/dd03.json

@@ -0,0 +1,35 @@
+{
+  "name": "dd03",
+  "type": "single_camera",
+  "version": "1.0",
+  "camera": {
+    "resolution_raw": [1280, 720],
+    "fps": 30,
+    "resolution_stitch": [420, 450],
+    "source_scale": 0.4,
+    "old": [672, 728, 1, 0.5],
+    "work": [504, 546, 1],
+    "work2": [420, 450, 0.8],
+    "work3": [350, 375, 4, 0.7],
+    "work4": [280, 300, 4, 0.4, 0.2]
+  },
+  "tracker": {
+    "adaptive": true,
+    "lower_threshold": 12.0,
+    "upper_threshold": 18.0,
+    "loop_closure_interval": 1,
+    "algorithm": "dis",
+    "dis_preset": "medium",
+    "dis_mode": 3,
+    "algorithm_list": ["raft", "dis"],
+    "hhd_downsample": 3
+  },
+  "contact": {
+    "min_contour_area": 3000,
+    "flow_threshold": 0.2,
+    "div_threshold": 0.022,
+    "expansion_threshold": 0.01,
+    "kernel_size": [5, 5],
+    "denoise_sigma": 12
+  }
+}

src/arm_application/config/perception.yaml

@@ -0,0 +1,42 @@
+# perception_node defaults. Launch overlays ~/ros2/config/site.yaml → yolo: / paths:
+# Do not edit this file for day-to-day tuning.
+
+perception_node:
+  ros__parameters:
+    camera_source: "d405"
+    camera_namespace: ""
+    image_topic: ""
+    camera_info_topic: ""
+    depth_topic: "/d405/aligned_depth_to_color/image_raw"
+    weights: "/home/cc/ros2/models/yolo/detect_yolo26_gongjian.pt"
+    conf: 0.35
+    iou: 0.45
+    imgsz: 416
+    device: "0"
+    half_precision: true
+    min_conf: 0.35
+    vision_hz: 20.0
+    z_ref_m: 0.30
+    # Large gate + reacquire: fast motion must NOT freeze track
+    jump_px: 280.0
+    jump_z_m: 0.20
+    min_depth_m: 0.05
+    max_depth_m: 2.50
+    object_frame: "tracked_object"
+    camera_frame: "d405_color_optical_frame"
+    # Snappy KF + blend toward raw measurement
+    kf_q_pos: 0.002
+    kf_q_vel: 0.05
+    kf_r_pos: 0.0008
+    meas_blend: 0.65
+    # Faster infer + tight box follow
+    infer_max_width: 640
+    box_ema_alpha: 0.85
+    miss_clear_frames: 2
+    # Item 4 — adaptive Z depth LPF
+    z_alpha_min: 0.10
+    z_alpha_max: 0.60
+    z_alpha_gain: 2.0
+    feature_mode: "plane"  # stereo 3D centroid; box = monocular box-center ray
+    plane_inlier_m: 0.008
+    plane_band_m: 0.06

src/arm_application/config/servo.yaml

@@ -0,0 +1,69 @@
+# servo_node defaults. Launch overlays ~/ros2/config/site.yaml → servo:
+# Do not edit this file for day-to-day tuning.
+
+servo_node:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    base_frame: "Rizon4-063989_base_link"
+    ee_frame: "Rizon4-063989_flange"
+    camera_frame: "d405_color_optical_frame"
+    traj_topic: "rizon_arm_controller/joint_trajectory"
+    point_topic: "visual_servo/filtered_point"
+    handeye_file: "/home/cc/ros2/src/arm_application/config/handeye_result.yaml"
+    use_static_handeye_tf: false
+    handeye_child_frame: "d405_color_optical_frame"
+
+    control_hz: 100.0
+    z_ref_m: 0.30
+    enable_control: false
+    max_extrapolate_sec: 0.08
+
+    # Item 1 — lost-target hard stop
+    watchdog_timeout_sec: 0.3
+    lost_timeout_sec: 0.3
+
+    kp_xy: 2.00
+    ki_xy: 0.02
+    kd_xy: 0.50
+    kp_z: 1.20
+    ki_z: 0.02
+    kd_z: 0.45
+    i_limit_xy: 0.03
+    i_limit_z: 0.025
+    # Item 2 — integral separation
+    i_sep_pos_m: 0.005
+    i_sep_ang_rad: 0.02
+
+    deadzone_m: 0.0015
+    deadzone_z_m: 0.006
+    v_max_xy: 0.15
+    v_max_z: 0.07
+    a_max: 0.70
+
+    # Item 3 — velocity LPF
+    vel_lpf_alpha: 0.40
+    wz_lpf_alpha: 0.40
+
+    # Item 5 — dq sliding window
+    dq_window_size: 5
+
+    min_ee_z_m: 0.20
+    workspace_xyz_min: [0.08, -0.55, 0.18]
+    workspace_xyz_max: [0.85, 0.55, 0.65]
+    ee_x_m: 0.25
+    ee_y_m: 0.00
+    ee_z_m: 0.25
+    ee_hold_xy: false
+    ee_vertical_down: true
+    ee_hold_kp: 2.0
+    ee_w_max: 0.60
+    damping: 0.08
+    max_qdot: 0.85
+    exec_horizon_sec: 0.20
+    traj_stream_hz: 15.0
+    min_cmd_delta_rad: 0.004
+    camera_fx: 390.0
+    camera_fy: 390.0
+    camera_cx: 320.0
+    camera_cy: 240.0

src/arm_application/config/xunhuan_trajectory.yaml

@@ -0,0 +1,15 @@
+# Auto-converted from Flexiv Elements .traj (joints: deg -> rad)
+# ROS params: flat waypoint_positions + waypoint_times (no nested dicts)
+run_joint_trajectory:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    velocity_scale: 0.2
+    waypoint_times: [3.0, 6.0, 9.92, 12.92, 20.54]
+    waypoint_positions: [
+      -0.121649, 0.522726, 0.030020, 1.610764, -0.113097, -0.218864, -0.006458,  # WPT0
+      -0.006889, 0.050452, 0.128620, 1.823614, -0.044784, 0.338795, -0.091060,  # WPT1
+      -0.009020, -0.311032, 0.106302, 2.548074, -0.066892, 1.318222, -0.091348,  # WPT2
+      0.014012, -0.847726, 0.074948, 2.395150, -0.251875, 1.738244, -0.091738,  # WPT3
+      0.089637, 0.502090, 0.299444, 1.749088, 0.070761, -0.168363, -0.091845,  # WPT4
+    ]

src/arm_application/launch/demo.launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='arm_application',
+            executable='demo_named_poses',
+            name='demo_named_poses',
+            output='screen',
+            parameters=[{
+                'poses': ['home', 'ready', 'inspect', 'home'],
+                'velocity_scale': 0.5,
+                'dwell_sec': 1.0,
+                'loops': 1,
+            }],
+        ),
+    ])

src/arm_application/launch/dh_pgea.launch.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+"""Launch DH PGEA Modbus node. Config from ~/ros2/config/site.yaml gripper:."""
+
+from __future__ import annotations
+
+import os
+import sys
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import LogInfo, OpaqueFunction
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    here = Path(__file__).resolve()
+    for p in [here.parents[i] for i in range(2, 8)]:
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    return Path.home() / "ros2"
+
+
+def _setup(context, *args, **kwargs):
+    root = _ws_root()
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import gripper_overrides, load_site, site_path  # type: ignore
+
+    cfg = os.environ.get("SITE_CONFIG", "").strip()
+    path = Path(cfg).expanduser() if cfg else site_path()
+    if not path.is_file():
+        path = root / "config" / "site.yaml"
+    data = load_site(path)
+    params = gripper_overrides(data)
+    return [
+        LogInfo(
+            msg=(
+                f"[dh_pgea] {params.get('model')} SN={params.get('serial_number')} "
+                f"{params.get('port')} @{params.get('baud')}  site={path}"
+            )
+        ),
+        LogInfo(
+            msg=(
+                "[dh_pgea] Flexiv load_gripper stays false — this is DH Modbus, "
+                "not a Flexiv Studio gripper."
+            )
+        ),
+        Node(
+            package="arm_application",
+            executable="dh_pgea",
+            name="dh_pgea",
+            output="screen",
+            parameters=[params],
+        ),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription([OpaqueFunction(function=_setup)])

src/arm_application/launch/dual_gripper.launch.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""Launch dual DH grippers. Ports come from site.yaml robots[].gripper."""
+
+from __future__ import annotations
+
+import os
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    sides = LaunchConfiguration("sides")
+    allow_offline = LaunchConfiguration("allow_offline")
+    site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("sides", default_value="left,right"),
+            DeclareLaunchArgument("allow_offline", default_value="true"),
+            LogInfo(msg=f"[dual_gripper] site={site} (arm_msgs, IPC Modbus, not Flexiv gripper)"),
+            Node(
+                package="arm_application",
+                executable="dual_gripper",
+                name="dual_gripper_driver",
+                output="screen",
+                parameters=[
+                    {
+                        "config_file": "",
+                        "auto_connect": True,
+                        "allow_offline": allow_offline,
+                        "publish_hz": 10.0,
+                        "sides": sides,
+                    }
+                ],
+            ),
+        ]
+    )

src/arm_application/launch/hole_detect.launch.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+"""Launch hole_detect on the current-arm color topic."""
+
+from __future__ import annotations
+
+import os
+from pathlib import Path
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    return Path.home() / "ros2"
+
+
+def _setup(context, *args, **kwargs):
+    root = _ws_root()
+    backend = LaunchConfiguration("backend").perform(context) or os.environ.get("SITE_HOLE_BACKEND") or "two_stage"
+    weights = LaunchConfiguration("weights").perform(context) or os.environ.get("SITE_HOLE_WEIGHTS") or ""
+    cfg = LaunchConfiguration("config_file").perform(context) or os.environ.get("SITE_HOLE_CONFIG") or ""
+    cam = os.environ.get("SITE_CAM_NAME") or "d405"
+    preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
+    if not cfg:
+        if backend in ("two_stage", "hough_roi"):
+            cfg = str(root / "src" / "arm_application" / "config" / "hole_detect.yaml")
+        else:
+            cfg = str(root / "src" / "arm_application" / "config" / "hole_insert.yaml")
+    if not weights:
+        if backend in ("two_stage", "hough_roi"):
+            weights = os.environ.get("SITE_HOLE_WORKPIECE_WEIGHTS") or os.environ.get("SITE_YOLO_WEIGHTS") or ""
+        else:
+            weights = os.environ.get("SITE_HOLE_WEIGHTS") or str(root / "models" / "hole_insert" / "best.pt")
+    return [
+        LogInfo(msg=f"[hole_detect] cam={cam} backend={backend} weights={weights} cfg={cfg}"),
+        Node(
+            package="arm_application",
+            executable="hole_detect",
+            name="hole_detect",
+            output="screen",
+            parameters=[
+                {
+                    "camera_source": cam,
+                    "weights": weights,
+                    "config_file": cfg,
+                    "backend": backend,
+                    "device": os.environ.get("SITE_YOLO_DEVICE") or "0",
+                    "preview": preview,
+                }
+            ],
+        ),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("backend", default_value=""),
+            DeclareLaunchArgument("weights", default_value=""),
+            DeclareLaunchArgument("config_file", default_value=""),
+            DeclareLaunchArgument("preview", default_value="false"),
+            OpaqueFunction(function=_setup),
+        ]
+    )

src/arm_application/launch/pipeline.launch.py

@@ -0,0 +1,177 @@
+#!/usr/bin/env python3
+"""Launch the three-stage assembly pipeline + static hand-eye TF.
+
+相机 / 触觉由 scripts/run_all.sh 负责拉起一次。本 launch 只起手眼 TF 和串联节点。
+一阶段跟踪由 pipeline 节点在进程内再拉 visual_servo.launch（不重复发手眼 TF）。
+"""
+
+from __future__ import annotations
+
+import math
+import os
+import sys
+from pathlib import Path
+
+import yaml
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.actions import EmitEvent, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    here = Path(__file__).resolve()
+    for p in [here.parents[i] for i in range(2, 8)]:
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    return Path.home() / "ros2"
+
+
+def _quat_from_R(R):
+    tr = float(R[0][0] + R[1][1] + R[2][2])
+    if tr > 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][1]) / 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][1]) / 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][2] - R[0][1]) / s
+        x = (R[0][2] - R[0][1]) / s
+        y = (R[1][2] - R[2][0]) / s
+        z = 0.25 * s
+    return x, y, z, w
+
+
+def _handeye_xyz_xyzw(path: str):
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f)
+    T = data["T_4x4"]
+    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    qx, qy, qz, qw = _quat_from_R(R)
+    return tx, ty, tz, qx, qy, qz, qw
+
+
+def _as_bool(s: str) -> bool:
+    return s.strip().lower() in ("1", "true", "yes")
+
+
+def _launch_setup(context, *args, **kwargs):
+    root = _ws_root()
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import apply_active_arm, load_site, site_path  # type: ignore
+
+    cfg = os.environ.get("SITE_CONFIG", "").strip()
+    path = Path(cfg).expanduser() if cfg else site_path()
+    if not path.is_file():
+        path = root / "config" / "site.yaml"
+    data = load_site(path)
+    merged, robot = apply_active_arm(data)
+
+    ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
+    paths = merged.get("paths") or {}
+    handeye = str(paths.get("handeye_file") or "").strip()
+    cam_cfg = merged.get("camera") or {}
+    cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
+    cam_frame = f"{cam}_color_optical_frame"
+
+    skip1 = _as_bool(LaunchConfiguration("skip_stage1").perform(context))
+    skip2 = _as_bool(LaunchConfiguration("skip_stage2").perform(context))
+    skip3 = _as_bool(LaunchConfiguration("skip_stage3").perform(context))
+    preview = _as_bool(LaunchConfiguration("preview").perform(context))
+    resnap = _as_bool(LaunchConfiguration("resnap").perform(context))
+    target_hole = LaunchConfiguration("target_hole").perform(context).strip()
+    depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
+    pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
+
+    nodes = []
+    if handeye and Path(handeye).is_file():
+        tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
+        nodes.append(
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                name="handeye_static_tf",
+                arguments=[
+                    "--x", str(tx), "--y", str(ty), "--z", str(tz),
+                    "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
+                    "--frame-id", ee_frame,
+                    "--child-frame-id", cam_frame,
+                ],
+                output="screen",
+            )
+        )
+
+    node = Node(
+        package="arm_application",
+        executable="assembly_pipeline",
+        name="assembly_pipeline",
+        output="screen",
+        parameters=[
+            {
+                "robot_sn": str(robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""),
+                "preview": preview,
+                "resnap": resnap,
+                "target_hole": target_hole,
+                "insert_depth_mm": float(depth) if depth else -1.0,
+                "screw_pitch_mm": float(pitch) if pitch else -999.0,
+                "skip_stage1": skip1,
+                "skip_stage2": skip2,
+                "skip_stage3": skip3,
+            }
+        ],
+    )
+
+    return [
+        LogInfo(msg=f"[pipeline] site={path}"),
+        LogInfo(msg=f"[pipeline] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
+        LogInfo(
+            msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
+            f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
+        ),
+        *nodes,
+        node,
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=node,
+                on_exit=[EmitEvent(event=Shutdown(reason="pipeline 完成"))],
+            )
+        ),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("skip_stage1", default_value="false"),
+            DeclareLaunchArgument("skip_stage2", default_value="false"),
+            DeclareLaunchArgument("skip_stage3", default_value="false"),
+            DeclareLaunchArgument("target_hole", default_value=""),
+            DeclareLaunchArgument("insert_depth_mm", default_value=""),
+            DeclareLaunchArgument("screw_pitch_mm", default_value=""),
+            DeclareLaunchArgument("resnap", default_value="false"),
+            DeclareLaunchArgument("preview", default_value="true"),
+            OpaqueFunction(function=_launch_setup),
+        ]
+    )

src/arm_application/launch/stage2_locate.launch.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python3
+"""Launch stage-2 one-shot hole localization (k1/k2) + static hand-eye TF.
+
+所有参数来自 ~/ros2/config/site.yaml（launch 参数仅做少量覆盖）。
+相机与触觉服务由 scripts/run_stage2_locate.sh 负责拉起/检查。
+"""
+
+from __future__ import annotations
+
+import math
+import os
+import sys
+from pathlib import Path
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    here = Path(__file__).resolve()
+    for p in [here.parents[i] for i in range(2, 8)]:
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    return Path.home() / "ros2"
+
+
+def _quat_from_R(R):
+    tr = float(R[0][0] + R[1][1] + R[2][2])
+    if tr > 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 = (R[0][1] + R[1][0]) / s
+        y = (R[1][0] + R[0][1]) / s
+        z = (R[0][2] - R[2][1]) / 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][1]) / s
+        x = (R[0][1] + R[1][0]) / s
+        y = (R[1][2] + R[2][1]) / 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][2] - R[0][1]) / s
+        x = (R[0][2] - R[0][1]) / s
+        y = (R[1][2] - R[2][0]) / s
+        z = (R[1][2] - R[2][1]) / s
+    return x, y, z, w
+
+
+def _handeye_xyz_xyzw(path: str):
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f)
+    T = data["T_4x4"]
+    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    qx, qy, qz, qw = _quat_from_R(R)
+    return tx, ty, tz, qx, qy, qz, qw
+
+
+def _launch_setup(context, *args, **kwargs):
+    root = _ws_root()
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import apply_active_arm, load_site, site_path  # type: ignore
+
+    cfg = os.environ.get("SITE_CONFIG", "").strip()
+    path = Path(cfg).expanduser() if cfg else site_path()
+    if not path.is_file():
+        path = root / "config" / "site.yaml"
+    data = load_site(path)
+    merged, robot = apply_active_arm(data)
+
+    ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
+    paths = merged.get("paths") or {}
+    handeye = str(paths.get("handeye_file") or "").strip()
+    if not handeye or not Path(handeye).is_file():
+        raise FileNotFoundError(
+            f"hand-eye result missing: {handeye}. "
+            "先标定: bash ~/ros2/scripts/run_handeye.sh calibrate"
+        )
+    cam_cfg = merged.get("camera") or {}
+    cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
+    cam_frame = f"{cam}_color_optical_frame"
+
+    hover_only = LaunchConfiguration("hover_only").perform(context).lower() in ("1", "true", "yes")
+    preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
+    target_hole = LaunchConfiguration("target_hole").perform(context).strip()
+    hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
+
+    tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="handeye_static_tf",
+        arguments=[
+            "--x", str(tx), "--y", str(ty), "--z", str(tz),
+            "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
+            "--frame-id", ee_frame,
+            "--child-frame-id", cam_frame,
+        ],
+        output="screen",
+    )
+
+    node = Node(
+        package="arm_application",
+        executable="stage2_locate",
+        name="stage2_locate",
+        output="screen",
+        parameters=[
+            {
+                "robot_sn": str(robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""),
+                "hover_only": hover_only,
+                "target_hole": target_hole,
+                "hover_z_m": float(hover_z) if hover_z else -1.0,
+                "preview": preview,
+            }
+        ],
+    )
+
+    return [
+        LogInfo(msg=f"[stage2] site={path}"),
+        LogInfo(
+            msg=f"[stage2] arm={robot.get('label') or robot.get('sn')} cam={cam} handeye={handeye}"
+        ),
+        LogInfo(msg=f"[stage2] hover_only={hover_only} target={target_hole or 'site'}"),
+        static_tf,
+        node,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("hover_only", default_value="false"),
+            DeclareLaunchArgument("target_hole", default_value=""),
+            DeclareLaunchArgument("hover_z_m", default_value=""),
+            DeclareLaunchArgument("preview", default_value="true"),
+            OpaqueFunction(function=_launch_setup),
+        ]
+    )

src/arm_application/launch/stage3_search.launch.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+"""Launch stage-3 compliant hole search (Orisys tactile + optional D405 resnap).
+
+所有阈值来自 ~/ros2/config/site.yaml → hole_detect.stage3（launch 参数仅做少量覆盖）。
+触觉服务由 scripts/run_stage3_search.sh 负责拉起/检查。
+前置：二阶段已完成（臂悬停在目标孔上方、垂直、夹持通止规）。
+"""
+
+from __future__ import annotations
+
+import math
+import os
+import sys
+from pathlib import Path
+
+import yaml
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    here = Path(__file__).resolve()
+    for p in [here.parents[i] for i in range(2, 8)]:
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    return Path.home() / "ros2"
+
+
+def _quat_from_R(R):
+    tr = float(R[0][0] + R[1][1] + R[2][2])
+    if tr > 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 = (R[0][1] + R[1][0]) / s
+        y = (R[1][0] + R[0][1]) / s
+        z = (R[1][2] - R[2][1]) / 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][1]) / s
+        x = (R[1][0] + R[0][1]) / s
+        y = (R[1][2] + R[2][1]) / 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][2] - R[0][1]) / s
+        x = (R[0][2] - R[0][1]) / s
+        y = (R[1][2] + R[2][1]) / s
+        z = (R[1][2] - R[2][1]) / s
+    return x, y, z, w
+
+
+def _handeye_xyz_xyzw(path: str):
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f)
+    T = data["T_4x4"]
+    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    qx, qy, qz, qw = _quat_from_R(R)
+    return tx, ty, tz, qx, qy, qz, qw
+
+
+def _launch_setup(context, *args, **kwargs):
+    root = _ws_root()
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import apply_active_arm, load_site, site_path  # type: ignore
+
+    cfg = os.environ.get("SITE_CONFIG", "").strip()
+    path = Path(cfg).expanduser() if cfg else site_path()
+    if not path.is_file():
+        path = root / "config" / "site.yaml"
+    data = load_site(path)
+    merged, robot = apply_active_arm(data)
+
+    ee_frame = str(robot.get("ee_frame") or f"{robot.get('sn')}_flange")
+    paths = merged.get("paths") or {}
+    handeye = str(paths.get("handeye_file") or "").strip()
+    cam_cfg = merged.get("camera") or {}
+    cam = str(cam_cfg.get("name") or "d405").strip().strip("/")
+    cam_frame = f"{cam}_color_optical_frame"
+
+    preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
+    resnap = LaunchConfiguration("resnap").perform(context).lower() in ("1", "true", "yes")
+    target_hole = LaunchConfiguration("target_hole").perform(context).strip()
+    depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
+    pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
+    rmax = LaunchConfiguration("spiral_r_max_mm").perform(context).strip()
+
+    nodes = []
+    if handeye and Path(handeye).is_file():
+        tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
+        nodes.append(
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                name="handeye_static_tf",
+                arguments=[
+                    "--x", str(tx), "--y", str(ty), "--z", str(tz),
+                    "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
+                    "--frame-id", ee_frame,
+                    "--child-frame-id", cam_frame,
+                ],
+                output="screen",
+            )
+        )
+
+    node = Node(
+        package="arm_application",
+        executable="stage3_search",
+        name="stage3_search",
+        output="screen",
+        parameters=[
+            {
+                "robot_sn": str(robot.get("sn") or os.environ.get("SITE_ROBOT_SN") or ""),
+                "preview": preview,
+                "resnap": resnap,
+                "target_hole": target_hole,
+                "insert_depth_mm": float(depth) if depth else -1.0,
+                "screw_pitch_mm": float(pitch) if pitch else -999.0,
+                "spiral_r_max_mm": float(rmax) if rmax else -1.0,
+            }
+        ],
+    )
+
+    return [
+        LogInfo(msg=f"[stage3] site={path}"),
+        LogInfo(msg=f"[stage3] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
+        LogInfo(
+            msg=f"[stage3] target={target_hole or 'site'} depth={depth or 'site'} "
+            f"pitch={pitch or 'site'} r_max={rmax or 'site'} resnap={resnap}"
+        ),
+        *nodes,
+        node,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("target_hole", default_value=""),
+            DeclareLaunchArgument("insert_depth_mm", default_value=""),
+            DeclareLaunchArgument("screw_pitch_mm", default_value=""),
+            DeclareLaunchArgument("spiral_r_max_mm", default_value=""),
+            DeclareLaunchArgument("resnap", default_value="false"),
+            DeclareLaunchArgument("preview", default_value="true"),
+            OpaqueFunction(function=_launch_setup),
+        ]
+    )

src/arm_application/launch/tactile.launch.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python3
+"""Launch Orisys tactile node（双指，多进程架构）。
+
+tactile 节点内部为每侧手指各起一个独立进程（厂家推荐：独立 CUDA 上下文），
+发布 /tactile/left|right/status。Device ids come from site.yaml tactile:.
+"""
+
+from pathlib import Path
+import os
+
+from launch import LaunchDescription
+from launch.actions import LogInfo
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    site = os.environ.get("SITE_CONFIG") or str(Path.home() / "ros2" / "config" / "site.yaml")
+    return LaunchDescription(
+        [
+            LogInfo(msg=f"[tactile] site={site} (mp workers: left + right)"),
+            Node(
+                package="arm_application",
+                executable="tactile",
+                name="tactile",
+                output="screen",
+            ),
+        ]
+    )

src/arm_application/launch/visual_servo.launch.py

@@ -0,0 +1,249 @@
+#!/usr/bin/env python3
+"""Launch: perception_node + servo_node + static hand-eye TF.
+
+All tunables come from ~/ros2/config/site.yaml (override via launch args if needed).
+"""
+
+from __future__ import annotations
+
+import math
+import os
+import sys
+from pathlib import Path
+
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def _ws_root() -> Path:
+    # Prefer env from env.sh / load_site_config
+    for key in ("ROS2_WS", "SITE_ROS2_ROOT"):
+        v = os.environ.get(key, "").strip()
+        if v:
+            return Path(v)
+    # launch file lives in install/.../share/arm_application/launch or src/...
+    here = Path(__file__).resolve()
+    for p in [here.parents[i] for i in range(2, 8)]:
+        if (p / "config" / "site.yaml").is_file():
+            return p
+    return Path.home() / "ros2"
+
+
+def _load_site():
+    root = _ws_root()
+    sys.path.insert(0, str(root / "scripts" / "lib"))
+    from export_site_env import (  # type: ignore
+        apply_active_arm,
+        load_site,
+        perception_overrides,
+        servo_overrides,
+        site_path,
+    )
+
+    cfg = os.environ.get("SITE_CONFIG", "").strip()
+    path = Path(cfg).expanduser() if cfg else site_path()
+    # site_path() uses script location; force workspace root file
+    if not path.is_file():
+        path = root / "config" / "site.yaml"
+    data = load_site(path)
+    merged, robot = apply_active_arm(data)
+    return merged, path, perception_overrides(data), servo_overrides(data), robot
+
+
+def _quat_from_R(R):
+    tr = float(R[0][0] + R[1][1] + R[2][2])
+    if tr > 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
+    return x, y, z, w
+
+
+def _handeye_xyz_xyzw(path: str):
+    with open(path, "r", encoding="utf-8") as f:
+        data = yaml.safe_load(f)
+    T = data["T_4x4"]
+    tx, ty, tz = float(T[0][3]), float(T[1][3]), float(T[2][3])
+    R = [[float(T[i][j]) for j in range(3)] for i in range(3)]
+    qx, qy, qz, qw = _quat_from_R(R)
+    return tx, ty, tz, qx, qy, qz, qw
+
+
+def _launch_setup(context, *args, **kwargs):
+    share = Path(get_package_share_directory("arm_application"))
+    site, site_path, perc_over, servo_over, robot = _load_site()
+
+    # Launch args override site when explicitly passed (non-empty / non-default handled below)
+    handeye = LaunchConfiguration("handeye_file").perform(context).strip()
+    site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "")
+    if not handeye or handeye in (
+        "/tmp/handeye_calib/handeye_result.yaml",
+        "/home/cc/ros2/src/arm_application/config/handeye_result.yaml",
+    ):
+        handeye = site_he or handeye
+    if not Path(handeye).is_file():
+        raise FileNotFoundError(
+            f"hand-eye result missing: {handeye}. "
+            "Re-run: bash ~/ros2/scripts/run_handeye.sh calibrate"
+        )
+
+    enable = LaunchConfiguration("enable_control").perform(context).lower() in (
+        "1", "true", "yes",
+    )
+    ee_frame = LaunchConfiguration("ee_frame").perform(context).strip()
+    cam_frame = LaunchConfiguration("camera_frame").perform(context).strip()
+    yolo = site.get("yolo") or {}
+    if not ee_frame or ee_frame == "Rizon4-063989_flange":
+        ee_frame = str(robot.get("ee_frame") or ee_frame)
+    if not cam_frame or cam_frame == "d405_color_optical_frame":
+        cam_frame = str(
+            perc_over.get("camera_frame")
+            or yolo.get("camera_frame")
+            or cam_frame
+        )
+
+    detect_yaml = str(share / "config" / "perception.yaml")
+    servo_yaml = str(share / "config" / "servo.yaml")
+
+    tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
+
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="handeye_static_tf",
+        arguments=[
+            "--x", str(tx), "--y", str(ty), "--z", str(tz),
+            "--qx", str(qx), "--qy", str(qy), "--qz", str(qz), "--qw", str(qw),
+            "--frame-id", ee_frame,
+            "--child-frame-id", cam_frame,
+        ],
+        output="screen",
+    )
+
+    perc_params = dict(perc_over)
+    perc_params["camera_frame"] = cam_frame
+
+    servo_params = dict(servo_over)
+    servo_params.update(
+        {
+            "enable_control": enable,
+            "ee_frame": ee_frame,
+            "camera_frame": cam_frame,
+            "handeye_child_frame": cam_frame,
+            "handeye_file": handeye,
+        }
+    )
+    ee_z_arg = LaunchConfiguration("ee_z_m").perform(context).strip()
+    if ee_z_arg:
+        try:
+            z = float(ee_z_arg)
+            if z > 0.05:
+                servo_params["ee_z_m"] = z
+        except ValueError:
+            pass
+    lock_hold_arg = LaunchConfiguration("lock_hold_sec").perform(context).strip()
+    if lock_hold_arg:
+        try:
+            servo_params["lock_hold_sec"] = float(lock_hold_arg)
+        except ValueError:
+            pass
+
+    perception = Node(
+        package="arm_application",
+        executable="perception_node",
+        name="perception_node",
+        output="screen",
+        parameters=[detect_yaml, perc_params],
+    )
+
+    servo = Node(
+        package="arm_application",
+        executable="servo_node",
+        name="servo_node",
+        output="screen",
+        parameters=[servo_yaml, servo_params],
+    )
+
+    weights = perc_params.get("weights", "")
+    nodes = [
+        LogInfo(msg=f"[site] {site_path}"),
+        LogInfo(
+            msg=f"[arm] {robot.get('label') or robot.get('sn')}  "
+            f"handeye={handeye}"
+        ),
+        LogInfo(msg=f"[site] YOLO weights: {weights}"),
+        LogInfo(
+            msg="nodes: perception_node (YOLO) | servo_node (100Hz) — "
+            "separate processes, topic /visual_servo/filtered_point"
+        ),
+    ]
+    pub_tf = LaunchConfiguration("publish_handeye_tf").perform(context).lower() in (
+        "1", "true", "yes",
+    )
+    if pub_tf:
+        nodes.append(
+            LogInfo(msg=f"static hand-eye {ee_frame} → {cam_frame} t=[{tx:.3f},{ty:.3f},{tz:.3f}]")
+        )
+        nodes.append(static_tf)
+    nodes.extend([perception, servo])
+    return nodes
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "handeye_file",
+                default_value="/home/cc/ros2/src/arm_application/config/handeye_result.yaml",
+            ),
+            DeclareLaunchArgument(
+                "ee_frame",
+                default_value="Rizon4-063989_flange",
+            ),
+            DeclareLaunchArgument(
+                "camera_frame",
+                default_value="d405_color_optical_frame",
+            ),
+            DeclareLaunchArgument(
+                "enable_control",
+                default_value="false",
+            ),
+            DeclareLaunchArgument(
+                "publish_handeye_tf",
+                default_value="true",
+            ),
+            DeclareLaunchArgument(
+                "ee_z_m",
+                default_value="",
+            ),
+            DeclareLaunchArgument(
+                "lock_hold_sec",
+                default_value="",
+            ),
+            OpaqueFunction(function=_launch_setup),
+        ]
+    )

src/arm_application/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_application</name>
+  <version>0.1.0</version>
+  <description>Field nodes: D405, visual servo, hand-eye, DH grippers, hole detect.</description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>arm_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>trajectory_msgs</depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>python3-serial</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/arm_application/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/arm_application
+[install]
+install_scripts=$base/lib/arm_application

src/arm_application/setup.py

@@ -0,0 +1,69 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'arm_application'
+
+
+def _data_files():
+    entries = [
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'trajectories'), glob('trajectories/*')),
+    ]
+    for root, _dirs, files in os.walk('config'):
+        if not files:
+            continue
+        dest = os.path.join('share', package_name, root)
+        srcs = [os.path.join(root, name) for name in files]
+        entries.append((dest, srcs))
+    return entries
+
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=_data_files(),
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='arm_dev',
+    maintainer_email='dev@example.com',
+    description='High-level arm application nodes.',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            # Bringup helpers
+            'demo_named_poses = arm_application.demo_named_poses:main',
+            'move_to_pose_client = arm_application.move_to_pose_client:main',
+            'run_joint_trajectory = arm_application.run_joint_trajectory:main',
+            'goto_ee_plane = arm_application.goto_ee_plane:main',
+            # Camera
+            'realsense_color_pub = arm_application.realsense_color_pub:main',
+            'realsense_view = arm_application.realsense_view:main',
+            # Hand-eye
+            'handeye_teach = arm_application.handeye_teach:main',
+            'handeye_waypoints = arm_application.handeye_waypoints:main',
+            'handeye_calibrate = arm_application.handeye_calibrate:main',
+            # Visual servo (current stack)
+            'perception_node = arm_application.perception_node:main',
+            'servo_node = arm_application.servo_node:main',
+            # Gripper
+            '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',
+            # Hole detect (no insert motion)
+            'hole_detect = arm_application.hole_detect_node:main',
+            # Stage-2 one-shot hole localization (k1/k2) + gauge alignment
+            'stage2_locate = arm_application.stage2_locate_node:main',
+            # Stage-3 compliant hole search (Orisys tactile spiral + insert)
+            'stage3_search = arm_application.stage3_search_node:main',
+            # Three-stage assembly pipeline (track → locate → search, one process)
+            'assembly_pipeline = arm_application.pipeline_node:main',
+            'tactile = arm_application.tactile_node:main',
+            'tactile_cli = arm_application.tactile_cli:main',
+        ],
+    },
+)

src/arm_application/trajectories/xunhuan.traj

@@ -0,0 +1,247 @@
+traj_coord_type: ""
+traj_coord_ref: ""
+pattern_params {
+  pattern_name: "WPT0"
+  pattern_type: "Scatter"
+  scatterpoint_pattern {
+    pose_params {
+      x: "0.26072"
+      y: "-0.14604"
+      z: "0.85707"
+      rx: "6.58"
+      ry: "165.63"
+      rz: "-6.63"
+      coord_type: "WORLD"
+      coord_ref: "WORLD_ORIGIN"
+      use_quaternion: false
+      preferJntPos {
+        joint1: "-6.97"
+        joint2: "29.95"
+        joint3: "1.72"
+        joint4: "92.29"
+        joint5: "-6.48"
+        joint6: "-12.54"
+        joint7: "-0.37"
+      }
+      externalAxes {
+        axis1: "0"
+        axis2: "0"
+        axis3: "0"
+        axis4: "0"
+        axis5: "0"
+        axis6: "0"
+      }
+    }
+    force_params {
+      fx: "0"
+      fy: "0"
+      fz: "0"
+      mx: "0"
+      my: "0"
+      mz: "0"
+    }
+    path_profile: "line"
+    blend_zone: "Z10"
+    velocity: "0.2"
+    acceleration: "1"
+    vel_var_name: ""
+    wrench_var_name: ""
+  }
+}
+pattern_params {
+  pattern_name: "WPT1"
+  pattern_type: "Scatter"
+  scatterpoint_pattern {
+    pose_params {
+      x: "0.50244"
+      y: "-0.05458"
+      z: "0.62368"
+      rx: "3.30681"
+      ry: "173.28739"
+      rz: "12.76833"
+      coord_type: "WORLD"
+      coord_ref: "WORLD_ORIGIN"
+      use_quaternion: false
+      preferJntPos {
+        joint1: "-0.39471"
+        joint2: "2.89069"
+        joint3: "7.36936"
+        joint4: "104.4854"
+        joint5: "-2.56594"
+        joint6: "19.41153"
+        joint7: "-5.21738"
+      }
+      externalAxes {
+        axis1: "0"
+        axis2: "0"
+        axis3: "0"
+        axis4: "0"
+        axis5: "0"
+        axis6: "0"
+      }
+    }
+    force_params {
+      fx: "0"
+      fy: "0"
+      fz: "0"
+      mx: "0"
+      my: "0"
+      mz: "0"
+    }
+    path_profile: "line"
+    blend_zone: "Z10"
+    velocity: "0.2"
+    acceleration: "1"
+    vel_var_name: ""
+    wrench_var_name: ""
+  }
+}
+pattern_params {
+  pattern_name: "WPT2"
+  pattern_type: "Scatter"
+  scatterpoint_pattern {
+    pose_params {
+      x: "0.38216"
+      y: "-0.07015"
+      z: "0.2772"
+      rx: "-0.76189"
+      ry: "178.83063"
+      rz: "13.77851"
+      coord_type: "WORLD"
+      coord_ref: "WORLD_ORIGIN"
+      use_quaternion: false
+      preferJntPos {
+        joint1: "-0.51679"
+        joint2: "-17.82083"
+        joint3: "6.09068"
+        joint4: "145.99388"
+        joint5: "-3.83264"
+        joint6: "75.52854"
+        joint7: "-5.23384"
+      }
+      externalAxes {
+        axis1: "0"
+        axis2: "0"
+        axis3: "0"
+        axis4: "0"
+        axis5: "0"
+        axis6: "0"
+      }
+    }
+    force_params {
+      fx: "0"
+      fy: "0"
+      fz: "0"
+      mx: "0"
+      my: "0"
+      mz: "0"
+    }
+    path_profile: "line"
+    blend_zone: "Z10"
+    velocity: "0.2"
+    acceleration: "1"
+    vel_var_name: ""
+    wrench_var_name: ""
+  }
+}
+pattern_params {
+  pattern_name: "WPT3"
+  pattern_type: "Scatter"
+  scatterpoint_pattern {
+    pose_params {
+      x: "0.42395"
+      y: "-0.04059"
+      z: "0.135"
+      rx: "-4.09647"
+      ry: "175.59924"
+      rz: "22.59976"
+      coord_type: "WORLD"
+      coord_ref: "WORLD_ORIGIN"
+      use_quaternion: false
+      preferJntPos {
+        joint1: "0.80285"
+        joint2: "-48.57114"
+        joint3: "4.29418"
+        joint4: "137.232"
+        joint5: "-14.43138"
+        joint6: "99.59405"
+        joint7: "-5.25622"
+      }
+      externalAxes {
+        axis1: "0"
+        axis2: "0"
+        axis3: "0"
+        axis4: "0"
+        axis5: "0"
+        axis6: "0"
+      }
+    }
+    force_params {
+      fx: "0"
+      fy: "0"
+      fz: "0"
+      mx: "0"
+      my: "0"
+      mz: "0"
+    }
+    path_profile: "line"
+    blend_zone: "Z10"
+    velocity: "0.2"
+    acceleration: "1"
+    vel_var_name: ""
+    wrench_var_name: ""
+  }
+}
+pattern_params {
+  pattern_name: "WPT4"
+  pattern_type: "Scatter"
+  scatterpoint_pattern {
+    pose_params {
+      x: "0.2971"
+      y: "0.06042"
+      z: "0.76959"
+      rx: "4.64468"
+      ry: "173.46657"
+      rz: "27.06819"
+      coord_type: "WORLD"
+      coord_ref: "WORLD_ORIGIN"
+      use_quaternion: false
+      preferJntPos {
+        joint1: "5.13584"
+        joint2: "28.76765"
+        joint3: "17.15688"
+        joint4: "100.21538"
+        joint5: "4.05433"
+        joint6: "-9.64651"
+        joint7: "-5.26232"
+      }
+      externalAxes {
+        axis1: "0"
+        axis2: "0"
+        axis3: "0"
+        axis4: "0"
+        axis5: "0"
+        axis6: "0"
+      }
+    }
+    force_params {
+      fx: "0"
+      fy: "0"
+      fz: "0"
+      mx: "0"
+      my: "0"
+      mz: "0"
+    }
+    path_profile: "line"
+    blend_zone: "Z10"
+    velocity: "0.2"
+    acceleration: "1"
+    vel_var_name: ""
+    wrench_var_name: ""
+  }
+}
+group_params {
+  group_name: "RecordGroup"
+  pattern_start_idx: 0
+  pattern_end_idx: 4
+}

src/arm_bringup/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.8)
+project(arm_bringup)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/arm_bringup/config/bringup.yaml

@@ -0,0 +1,14 @@
+# Site defaults for Flexiv Rizon 4 (SN: Rizon4-063989)
+robot_sn: Rizon4-063989
+rizon_type: Rizon4
+# Flexiv Studio gripper (GN01) — keep false. DH PGEA uses run_gripper.sh.
+load_gripper: false
+use_fake_hardware: false
+start_rviz: true
+
+# Official Flexiv joint naming: Rizon4-063989_joint1 ... joint7
+joint_prefix: Rizon4-063989_
+
+base_frame: base_link
+ee_frame: tcp
+flange_frame: flange

src/arm_bringup/launch/real.launch.py

@@ -0,0 +1,77 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory, PackageNotFoundError
+
+import os
+
+# Site robot — DH PGEA is a separate Modbus node (run_gripper.sh).
+# Keep Flexiv load_gripper false (that flag is Flexiv-GN01 in Studio).
+SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
+
+
+def _launch_setup(context, *args, **kwargs):
+    robot_sn = LaunchConfiguration('robot_sn').perform(context)
+    rizon_type = LaunchConfiguration('rizon_type').perform(context)
+    use_fake = LaunchConfiguration('use_fake_hardware').perform(context)
+    load_gripper = LaunchConfiguration('load_gripper').perform(context)
+    start_rviz = LaunchConfiguration('start_rviz').perform(context)
+
+    if not robot_sn:
+        return [
+            LogInfo(msg=f'[arm_bringup] ERROR: robot_sn is required, e.g. robot_sn:={SITE_ROBOT_SN}')
+        ]
+
+    try:
+        get_package_share_directory('flexiv_bringup')
+    except PackageNotFoundError:
+        return [
+            LogInfo(
+                msg=(
+                    '[arm_bringup] flexiv_bringup not found. '
+                    'Run scripts/fetch_flexiv.sh and build, or use sim.launch.py for mock.'
+                )
+            )
+        ]
+
+    return [
+        LogInfo(
+            msg=(
+                f'[arm_bringup] Starting Flexiv {rizon_type} ({robot_sn}), '
+                f'load_gripper={load_gripper}'
+            )
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution([
+                    FindPackageShare('flexiv_bringup'), 'launch', 'rizon.launch.py'
+                ])
+            ),
+            launch_arguments={
+                'robot_sn': robot_sn,
+                'rizon_type': rizon_type,
+                'use_fake_hardware': use_fake,
+                'load_gripper': load_gripper,
+                'start_rviz': start_rviz,
+            }.items(),
+        ),
+    ]
+
+
+def generate_launch_description():
+    """Real Rizon4 bringup via official flexiv_bringup (RDK v1 / humble-v1)."""
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'robot_sn',
+            default_value=SITE_ROBOT_SN,
+            description='Site robot serial number (no spaces)',
+        ),
+        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
+        DeclareLaunchArgument('use_fake_hardware', default_value='false'),
+        # Flexiv-GN01 only. DH PGEA: bash ~/ros2/scripts/run_gripper.sh
+        DeclareLaunchArgument('load_gripper', default_value='false'),
+        DeclareLaunchArgument('start_rviz', default_value='true'),
+        OpaqueFunction(function=_launch_setup),
+    ])

src/arm_bringup/launch/sim.launch.py

@@ -0,0 +1,46 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    """Offline mock bringup for Flexiv Rizon 4 (7-DOF), no real robot required."""
+    use_rviz = LaunchConfiguration('use_rviz')
+    use_moveit = LaunchConfiguration('use_moveit')
+    robot_sn = LaunchConfiguration('robot_sn')
+
+    mock = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('arm_control'), 'launch', 'mock_control.launch.py'
+            ])
+        ),
+        launch_arguments={
+            'use_rviz': use_rviz,
+            'robot_sn': robot_sn,
+        }.items(),
+    )
+
+    moveit = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('arm_moveit_config'), 'launch', 'move_group.launch.py'
+            ])
+        ),
+        condition=IfCondition(use_moveit),
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('use_moveit', default_value='false'),
+        DeclareLaunchArgument(
+            'robot_sn',
+            default_value='Rizon4-063989',
+            description='Site SN; joints become Rizon4-063989_joint1..joint7',
+        ),
+        mock,
+        moveit,
+    ])

src/arm_bringup/launch/stack.launch.py

@@ -0,0 +1,208 @@
+"""Unified stack launch: backend:=mock|moveit|real|isaac
+
+Application interfaces stay the same across backends:
+  /set_named_pose, /get_arm_state, /arm_status, /follow_joint_trajectory
+"""
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo, OpaqueFunction
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import PackageNotFoundError, get_package_share_directory
+
+
+SITE_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
+SITE_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_SN}_')
+SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
+
+
+def _setup(context, *args, **kwargs):
+    backend = LaunchConfiguration('backend').perform(context).strip().lower()
+    use_rviz = LaunchConfiguration('use_rviz')
+    robot_sn = LaunchConfiguration('robot_sn')
+    load_gripper = LaunchConfiguration('load_gripper')
+    sn = robot_sn.perform(context).strip() or SITE_SN
+
+    actions = [
+        LogInfo(msg=f'[arm_bringup] backend={backend} sn={sn}'),
+    ]
+
+    if backend in ('mock', 'moveit', 'isaac'):
+        actions.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    PathJoinSubstitution([
+                        FindPackageShare('arm_control'),
+                        'launch',
+                        'mock_control.launch.py',
+                    ])
+                ),
+                launch_arguments={
+                    'use_rviz': use_rviz,
+                    'robot_sn': robot_sn,
+                    'load_gripper': load_gripper,
+                }.items(),
+            )
+        )
+
+    if backend == 'moveit':
+        actions.append(
+            Node(
+                package='arm_control',
+                executable='trajectory_bridge',
+                name='trajectory_bridge',
+                parameters=[{
+                    'control_action': 'rizon_arm_controller/follow_joint_trajectory',
+                    'arm_action': 'follow_joint_trajectory',
+                }],
+                output='screen',
+            )
+        )
+        actions.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    PathJoinSubstitution([
+                        FindPackageShare('arm_moveit_config'),
+                        'launch',
+                        'move_group.launch.py',
+                    ])
+                ),
+                launch_arguments={
+                    'use_fake_hardware': 'true',
+                    'robot_sn': robot_sn,
+                    'start_rviz': 'false',
+                }.items(),
+            )
+        )
+        actions.append(
+            LogInfo(
+                msg=(
+                    '[arm_bringup] MoveIt on top of mock. '
+                    'Named poses still via /set_named_pose; '
+                    'MoveIt plans execute through trajectory_bridge.'
+                )
+            )
+        )
+
+    if backend == 'real':
+        try:
+            get_package_share_directory('flexiv_bringup')
+        except PackageNotFoundError:
+            actions.append(
+                LogInfo(
+                    msg=(
+                        '[arm_bringup] flexiv_bringup missing. '
+                        'Run: ./scripts/fetch_flexiv.sh && rebuild. '
+                        'Falling back tip: use backend:=mock for now.'
+                    )
+                )
+            )
+            return actions
+        actions.append(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    PathJoinSubstitution([
+                        FindPackageShare('arm_bringup'),
+                        'launch',
+                        'real.launch.py',
+                    ])
+                ),
+                launch_arguments={
+                    'robot_sn': robot_sn,
+                    'load_gripper': load_gripper,
+                    'start_rviz': use_rviz,
+                }.items(),
+            )
+        )
+        # Keep application services on top of real HW when possible
+        actions.append(
+            Node(
+                package='arm_control',
+                executable='real_trajectory_adapter',
+                name='real_trajectory_adapter',
+                parameters=[{
+                    'control_action': 'rizon_arm_controller/follow_joint_trajectory',
+                    'arm_action': 'follow_joint_trajectory',
+                }],
+                output='screen',
+            )
+        )
+        poses_params = []
+        if SITE_NAMED_POSES and os.path.isfile(SITE_NAMED_POSES):
+            poses_params.append(SITE_NAMED_POSES)
+        else:
+            poses_params.append(
+                PathJoinSubstitution([
+                    FindPackageShare('arm_control'), 'config', 'named_poses.yaml'
+                ])
+            )
+        poses_params.append({'joint_prefix': f'{sn}_'})
+        actions.append(
+            Node(
+                package='arm_control',
+                executable='named_pose_server',
+                name='named_pose_server',
+                parameters=poses_params,
+                output='screen',
+            )
+        )
+        actions.append(
+            Node(
+                package='arm_control',
+                executable='arm_status_node',
+                name='arm_status_node',
+                parameters=[{
+                    'base_frame': f'{sn}_base_link',
+                    'ee_frame': f'{sn}_flange',
+                    'tcp_pose_topic': f'/{sn.replace("-", "_")}/tcp_pose',
+                }],
+                output='screen',
+            )
+        )
+        actions.append(
+            LogInfo(
+                msg=(
+                    '[arm_bringup] REAL mode: flexiv_bringup + named_pose_server '
+                    '+ arm_status_node. '
+                    'Ensure RDK is enabled and network reaches the robot.'
+                )
+            )
+        )
+
+    if backend == 'isaac':
+        actions.append(
+            LogInfo(
+                msg=(
+                    '[arm_bringup] ISAAC mode: mock ROS stack is up for bridge testing. '
+                    'Start Isaac Sim + flexiv_sim_plugin separately '
+                    '(see docs/INTERFACES.md). '
+                    'Sync joints via /joint_states or RDK bridge topics.'
+                )
+            )
+        )
+
+    if backend not in ('mock', 'moveit', 'real', 'isaac'):
+        actions.append(
+            LogInfo(msg=f'[arm_bringup] Unknown backend "{backend}". Use mock|moveit|real|isaac')
+        )
+
+    return actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'backend',
+            default_value='mock',
+            description='mock | moveit | real | isaac',
+        ),
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('robot_sn', default_value=SITE_SN),
+        DeclareLaunchArgument('load_gripper', default_value='false'),
+        OpaqueFunction(function=_setup),
+    ])

src/arm_bringup/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_bringup</name>
+  <version>0.1.0</version>
+  <description>Bringup for Flexiv Rizon 4 simulation and real robot.</description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>arm_description</exec_depend>
+  <exec_depend>arm_control</exec_depend>
+  <exec_depend>arm_moveit_config</exec_depend>
+  <exec_depend>arm_application</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/arm_control/arm_control/__init__.py

@@ -0,0 +1 @@
+"""Package init for arm_control."""

src/arm_control/arm_control/arm_status_node.py

@@ -0,0 +1,124 @@
+#!/usr/bin/env python3
+"""Publish ArmStatus + GetArmState from joint_states, TF, and Flexiv tcp_pose."""
+
+from __future__ import annotations
+
+from typing import Optional
+
+import rclpy
+from geometry_msgs.msg import PoseStamped
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from tf2_ros import Buffer, TransformListener
+
+from arm_msgs.msg import ArmStatus
+from arm_msgs.srv import GetArmState
+
+
+class ArmStatusNode(Node):
+    def __init__(self) -> None:
+        super().__init__('arm_status_node')
+        self.declare_parameter('base_frame', 'base_link')
+        self.declare_parameter('ee_frame', 'tcp')
+        self.declare_parameter('tcp_pose_topic', '')
+        self.declare_parameter('publish_rate', 20.0)
+
+        self.base_frame = str(self.get_parameter('base_frame').value)
+        self.ee_frame = str(self.get_parameter('ee_frame').value)
+        self.tcp_pose_topic = str(self.get_parameter('tcp_pose_topic').value).strip()
+        self._last_js: Optional[JointState] = None
+        self._last_tcp: Optional[PoseStamped] = None
+        self._mode = 'idle'
+
+        self._tf = Buffer()
+        TransformListener(self._tf, self)
+        self._pub = self.create_publisher(ArmStatus, 'arm_status', 10)
+        self.create_subscription(JointState, 'joint_states', self._on_js, 10)
+        if self.tcp_pose_topic:
+            self.create_subscription(
+                PoseStamped, self.tcp_pose_topic, self._on_tcp, 10
+            )
+        self.create_service(GetArmState, 'get_arm_state', self._on_get)
+        rate = float(self.get_parameter('publish_rate').value)
+        self.create_timer(1.0 / rate, self._tick)
+        self.get_logger().info(
+            f'ArmStatus: base={self.base_frame} ee={self.ee_frame} '
+            f'tcp_topic={self.tcp_pose_topic or "(none)"}'
+        )
+
+    def _on_js(self, msg: JointState) -> None:
+        self._last_js = msg
+        moving = any(abs(v) > 1e-3 for v in msg.velocity) if msg.velocity else False
+        self._mode = 'moving' if moving else 'idle'
+
+    def _on_tcp(self, msg: PoseStamped) -> None:
+        self._last_tcp = msg
+
+    def _ee(self) -> Optional[PoseStamped]:
+        # Prefer Flexiv-published TCP pose (reliable on real stack)
+        if self._last_tcp is not None:
+            return self._last_tcp
+        try:
+            tf = self._tf.lookup_transform(
+                self.base_frame, self.ee_frame, rclpy.time.Time()
+            )
+        except Exception:  # noqa: BLE001
+            return None
+        pose = PoseStamped()
+        pose.header = tf.header
+        pose.pose.position.x = tf.transform.translation.x
+        pose.pose.position.y = tf.transform.translation.y
+        pose.pose.position.z = tf.transform.translation.z
+        pose.pose.orientation = tf.transform.rotation
+        return pose
+
+    def _build(self) -> ArmStatus:
+        s = ArmStatus()
+        s.header.stamp = self.get_clock().now().to_msg()
+        s.header.frame_id = self.base_frame
+        s.mode = self._mode
+        s.is_moving = self._mode == 'moving'
+        s.is_homed = True
+        if self._last_js is not None:
+            s.joint_positions = list(self._last_js.position)
+            s.joint_velocities = list(self._last_js.velocity)
+        ee = self._ee()
+        if ee is not None:
+            s.ee_pose = ee
+        return s
+
+    def _tick(self) -> None:
+        self._pub.publish(self._build())
+
+    def _on_get(self, _req, response):
+        s = self._build()
+        response.success = True
+        response.mode = s.mode
+        response.joint_positions = s.joint_positions
+        response.ee_pose = s.ee_pose
+        if (
+            abs(s.ee_pose.pose.position.x) < 1e-9
+            and abs(s.ee_pose.pose.position.y) < 1e-9
+            and abs(s.ee_pose.pose.position.z) < 1e-9
+        ):
+            response.message = 'ok (ee pose unavailable — check TF/tcp_pose)'
+        else:
+            response.message = 'ok'
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ArmStatusNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/arm_control/arm_control/mock_arm_driver.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+"""Mock 7-DOF Rizon4 driver: joint_states + FollowJointTrajectory + JointCommand."""
+
+from __future__ import annotations
+
+import time
+from typing import List
+
+import rclpy
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.msg import JointCommand
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+
+
+class MockArmDriver(Node):
+    def __init__(self) -> None:
+        super().__init__('mock_arm_driver')
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('joint_prefix', '')
+        self.declare_parameter('publish_rate', 50.0)
+        self.declare_parameter(
+            'default_positions', [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+        )
+
+        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: List[str] = [f'{prefix}{n}' for n in base]
+        rate = float(self.get_parameter('publish_rate').value)
+        defaults = list(
+            self.get_parameter('default_positions').get_parameter_value().double_array_value
+            or [0.0] * len(self.joint_names)
+        )
+        while len(defaults) < len(self.joint_names):
+            defaults.append(0.0)
+
+        self.positions = defaults[: len(self.joint_names)]
+        self.velocities = [0.0] * len(self.joint_names)
+        self._busy = False
+        self._cb = ReentrantCallbackGroup()
+
+        self._js_pub = self.create_publisher(JointState, 'joint_states', 10)
+        self.create_subscription(JointCommand, 'joint_command', self._on_cmd, 10)
+        self.create_timer(1.0 / rate, self._publish)
+
+        ActionServer(
+            self,
+            FollowJointTrajectory,
+            'follow_joint_trajectory',
+            execute_callback=self._execute,
+            goal_callback=self._goal_cb,
+            cancel_callback=self._cancel_cb,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'Mock Rizon4 ready: {self.joint_names}')
+
+    def _goal_cb(self, _req):
+        if self._busy:
+            self.get_logger().warn('Reject: already executing')
+            return GoalResponse.REJECT
+        return GoalResponse.ACCEPT
+
+    def _cancel_cb(self, _gh):
+        return CancelResponse.ACCEPT
+
+    def _on_cmd(self, msg: JointCommand) -> None:
+        if self._busy:
+            return
+        for i, name in enumerate(msg.joint_names):
+            if name in self.joint_names and i < len(msg.positions):
+                idx = self.joint_names.index(name)
+                self.positions[idx] = float(msg.positions[i])
+                self.velocities[idx] = 0.0
+
+    def _publish(self) -> None:
+        msg = JointState()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.name = self.joint_names
+        msg.position = list(self.positions)
+        msg.velocity = list(self.velocities)
+        self._js_pub.publish(msg)
+
+    def _execute(self, goal_handle):
+        self._busy = True
+        goal = goal_handle.request
+        result = FollowJointTrajectory.Result()
+        feedback = FollowJointTrajectory.Feedback()
+
+        n_j = len(goal.joint_names)
+        n_p = int(goal.num_points)
+        if n_j == 0 or n_p == 0 or len(goal.positions) < n_j * n_p:
+            result.success = False
+            result.message = 'Invalid / empty trajectory'
+            goal_handle.abort()
+            self._busy = False
+            return result
+
+        start = time.monotonic()
+        times = list(goal.time_from_start) if goal.time_from_start else [
+            (i + 1) * 0.5 for i in range(n_p)
+        ]
+        try:
+            for p in range(n_p):
+                if goal_handle.is_cancel_requested:
+                    goal_handle.canceled()
+                    result.success = False
+                    result.message = 'Canceled'
+                    result.final_positions = list(self.positions)
+                    self._busy = False
+                    return result
+                target = goal.positions[p * n_j:(p + 1) * n_j]
+                deadline = start + times[p]
+                while time.monotonic() < deadline:
+                    for name, tval in zip(goal.joint_names, target):
+                        if name not in self.joint_names:
+                            continue
+                        idx = self.joint_names.index(name)
+                        err = tval - self.positions[idx]
+                        self.velocities[idx] = err * 5.0
+                        self.positions[idx] += 0.15 * err
+                    feedback.current_point = p
+                    feedback.progress = (p + 1) / n_p
+                    feedback.status = 'moving'
+                    goal_handle.publish_feedback(feedback)
+                    time.sleep(0.02)
+                for name, tval in zip(goal.joint_names, target):
+                    if name in self.joint_names:
+                        self.positions[self.joint_names.index(name)] = tval
+                self.velocities = [0.0] * len(self.joint_names)
+
+            goal_handle.succeed()
+            result.success = True
+            result.message = 'Trajectory completed'
+            result.final_positions = list(self.positions)
+        except Exception as exc:  # noqa: BLE001
+            result.success = False
+            result.message = str(exc)
+            goal_handle.abort()
+        finally:
+            self._busy = False
+            self.velocities = [0.0] * len(self.joint_names)
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = MockArmDriver()
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/arm_control/arm_control/named_pose_server.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+"""Service server: SetNamedPose -> FollowJointTrajectory (backend-agnostic)."""
+
+from __future__ import annotations
+
+import time
+from typing import Dict, List
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory
+from arm_msgs.srv import SetNamedPose
+
+DEFAULT_JOINTS = [
+    'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
+]
+DEFAULT_POSES: Dict[str, List[float]] = {
+    'home': [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
+    'ready': [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0],
+    'inspect': [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
+    'pos_left': [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698],
+}
+
+
+def _wait_future(future, timeout_sec: float) -> bool:
+    deadline = time.monotonic() + timeout_sec
+    while not future.done():
+        if time.monotonic() > deadline:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class NamedPoseServer(Node):
+    def __init__(self) -> None:
+        super().__init__('named_pose_server')
+        self.declare_parameter('joint_names', DEFAULT_JOINTS)
+        self.declare_parameter('joint_prefix', '')
+        for name, vals in DEFAULT_POSES.items():
+            self.declare_parameter(f'named_poses.{name}', vals)
+
+        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.poses = {
+            name: list(self.get_parameter(f'named_poses.{name}').value)
+            for name in DEFAULT_POSES
+        }
+
+        self._cb = ReentrantCallbackGroup()
+        self._client = ActionClient(
+            self,
+            FollowJointTrajectory,
+            'follow_joint_trajectory',
+            callback_group=self._cb,
+        )
+        self.create_service(
+            SetNamedPose, 'set_named_pose', self._on_set, callback_group=self._cb
+        )
+        self.get_logger().info(
+            f'Named poses {list(self.poses)} joints={self.joint_names}'
+        )
+
+    def _on_set(self, request: SetNamedPose.Request, response: SetNamedPose.Response):
+        name = request.pose_name
+        if name not in self.poses:
+            response.success = False
+            response.message = f'Unknown pose "{name}". Available: {list(self.poses)}'
+            return response
+        if not self._client.wait_for_server(timeout_sec=3.0):
+            response.success = False
+            response.message = 'follow_joint_trajectory action not available'
+            return response
+
+        goal = FollowJointTrajectory.Goal()
+        goal.joint_names = self.joint_names
+        goal.num_points = 1
+        goal.positions = list(self.poses[name])
+        duration = max(0.5, 1.0 / max(float(request.velocity_scale), 0.1))
+        goal.time_from_start = [duration]
+        goal.velocity_scale = float(request.velocity_scale)
+
+        self.get_logger().info(f'-> pose [{name}] T={duration:.2f}s')
+        send_fut = self._client.send_goal_async(goal)
+        if not _wait_future(send_fut, 5.0):
+            response.success = False
+            response.message = 'Timed out waiting for goal accept'
+            return response
+        handle = send_fut.result()
+        if handle is None or not handle.accepted:
+            response.success = False
+            response.message = 'Goal rejected'
+            return response
+        res_fut = handle.get_result_async()
+        if not _wait_future(res_fut, 60.0):
+            response.success = False
+            response.message = 'Timed out waiting for trajectory result'
+            return response
+        wrapped = res_fut.result()
+        if wrapped is None:
+            response.success = False
+            response.message = 'Empty result'
+            return response
+        result = wrapped.result
+        response.success = bool(result.success)
+        response.message = result.message or f'Moved to {name}'
+        return response
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = NamedPoseServer()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        executor.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/arm_control/arm_control/real_trajectory_adapter.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+"""Expose arm_msgs/FollowJointTrajectory for apps; drive flexiv JTC (control_msgs)."""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory as CtrlFJT
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+
+from arm_msgs.action import FollowJointTrajectory as ArmFJT
+
+
+def _wait(fut, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while not fut.done():
+        if time.monotonic() > end:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class RealTrajectoryAdapter(Node):
+    def __init__(self) -> None:
+        super().__init__('real_trajectory_adapter')
+        self.declare_parameter('arm_action', 'follow_joint_trajectory')
+        self.declare_parameter(
+            'control_action', 'rizon_arm_controller/follow_joint_trajectory'
+        )
+        arm = str(self.get_parameter('arm_action').value)
+        ctrl = str(self.get_parameter('control_action').value)
+        self._cb = ReentrantCallbackGroup()
+        self._ctrl = ActionClient(self, CtrlFJT, ctrl, callback_group=self._cb)
+        ActionServer(
+            self,
+            ArmFJT,
+            arm,
+            execute_callback=self._execute,
+            goal_callback=lambda _g: GoalResponse.ACCEPT,
+            cancel_callback=lambda _g: CancelResponse.ACCEPT,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'Real adapter {arm} -> {ctrl}')
+
+    def _execute(self, goal_handle):
+        result = ArmFJT.Result()
+        req = goal_handle.request
+        if not req.joint_names or req.num_points < 1:
+            result.success = False
+            result.message = 'Invalid goal'
+            goal_handle.abort()
+            return result
+        if not self._ctrl.wait_for_server(timeout_sec=5.0):
+            result.success = False
+            result.message = f'Controller action not available'
+            goal_handle.abort()
+            return result
+
+        n_j = len(req.joint_names)
+        traj = JointTrajectory()
+        traj.joint_names = list(req.joint_names)
+        pos = list(req.positions)
+        times = list(req.time_from_start) or [1.0]
+        idx = 0
+        n_pts = min(int(req.num_points), len(times))
+        for i, t in enumerate(times[:n_pts]):
+            pt = JointTrajectoryPoint()
+            pt.positions = pos[idx : idx + n_j]
+            idx += n_j
+            # Point-to-point named poses must end at rest — nonzero end vel
+            # + open_loop JTC makes the arm coast / shake after arrival.
+            pt.velocities = [0.0] * n_j
+            pt.accelerations = [0.0] * n_j
+            sec = int(t)
+            pt.time_from_start.sec = sec
+            pt.time_from_start.nanosec = int((t - sec) * 1e9)
+            traj.points.append(pt)
+
+        ctrl_goal = CtrlFJT.Goal()
+        ctrl_goal.trajectory = traj
+        send = self._ctrl.send_goal_async(ctrl_goal)
+        if not _wait(send, 5.0):
+            result.success = False
+            result.message = 'Timed out sending to controller'
+            goal_handle.abort()
+            return result
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            result.success = False
+            result.message = 'Controller rejected goal'
+            goal_handle.abort()
+            return result
+        traj_duration = 0.0
+        if traj.points:
+            last = traj.points[-1].time_from_start
+            traj_duration = last.sec + last.nanosec * 1e-9
+        res_fut = handle.get_result_async()
+        if not _wait(res_fut, max(120.0, traj_duration * 3.0)):
+            result.success = False
+            result.message = 'Timed out waiting for controller'
+            goal_handle.abort()
+            return result
+        ctrl_res = res_fut.result().result
+        ok = ctrl_res.error_code == CtrlFJT.Result.SUCCESSFUL
+        result.success = ok
+        result.message = 'OK' if ok else f'controller error_code={ctrl_res.error_code}'
+        if ok:
+            goal_handle.succeed()
+        else:
+            goal_handle.abort()
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = RealTrajectoryAdapter()
+    ex = MultiThreadedExecutor(num_threads=4)
+    ex.add_node(node)
+    try:
+        ex.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ex.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/arm_control/arm_control/trajectory_bridge.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+"""Bridge control_msgs/FollowJointTrajectory -> arm_msgs/FollowJointTrajectory."""
+
+from __future__ import annotations
+
+import time
+
+import rclpy
+from control_msgs.action import FollowJointTrajectory as CtrlFJT
+from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from arm_msgs.action import FollowJointTrajectory as ArmFJT
+
+
+def _wait(fut, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while not fut.done():
+        if time.monotonic() > end:
+            return False
+        time.sleep(0.01)
+    return True
+
+
+class TrajectoryBridge(Node):
+    def __init__(self) -> None:
+        super().__init__('trajectory_bridge')
+        self.declare_parameter(
+            'control_action', 'rizon_arm_controller/follow_joint_trajectory'
+        )
+        self.declare_parameter('arm_action', 'follow_joint_trajectory')
+        ctrl = str(self.get_parameter('control_action').value)
+        arm = str(self.get_parameter('arm_action').value)
+        self._cb = ReentrantCallbackGroup()
+        self._arm = ActionClient(self, ArmFJT, arm, callback_group=self._cb)
+        ActionServer(
+            self,
+            CtrlFJT,
+            ctrl,
+            execute_callback=self._execute,
+            goal_callback=lambda _g: GoalResponse.ACCEPT,
+            cancel_callback=lambda _g: CancelResponse.ACCEPT,
+            callback_group=self._cb,
+        )
+        self.get_logger().info(f'Bridge {ctrl} -> {arm}')
+
+    def _execute(self, goal_handle):
+        result = CtrlFJT.Result()
+        traj = goal_handle.request.trajectory
+        if not traj.joint_names or not traj.points:
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+        if not self._arm.wait_for_server(timeout_sec=3.0):
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+
+        n_j = len(traj.joint_names)
+        goal = ArmFJT.Goal()
+        goal.joint_names = list(traj.joint_names)
+        goal.num_points = len(traj.points)
+        goal.velocity_scale = 1.0
+        positions = []
+        times = []
+        for pt in traj.points:
+            positions.extend(list(pt.positions)[:n_j])
+            t = pt.time_from_start.sec + pt.time_from_start.nanosec * 1e-9
+            times.append(max(t, 0.05))
+        goal.positions = positions
+        goal.time_from_start = times
+
+        send = self._arm.send_goal_async(goal)
+        if not _wait(send, 5.0):
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+            return result
+        handle = send.result()
+        if handle is None or not handle.accepted:
+            result.error_code = CtrlFJT.Result.INVALID_GOAL
+            goal_handle.abort()
+            return result
+        res_fut = handle.get_result_async()
+        if not _wait(res_fut, 120.0):
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+            return result
+        if res_fut.result().result.success:
+            result.error_code = CtrlFJT.Result.SUCCESSFUL
+            goal_handle.succeed()
+        else:
+            result.error_code = CtrlFJT.Result.GOAL_TOLERANCE_VIOLATED
+            goal_handle.abort()
+        return result
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = TrajectoryBridge()
+    ex = MultiThreadedExecutor(num_threads=4)
+    ex.add_node(node)
+    try:
+        ex.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        ex.shutdown()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/arm_control/config/controllers.yaml

@@ -0,0 +1,34 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    # Name aligned with Flexiv official bringup
+    rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+rizon_arm_controller:
+  ros__parameters:
+    # Site robot Rizon4-063989 (official flexiv_description keeps hyphen)
+    joints:
+      - Rizon4-063989_joint1
+      - Rizon4-063989_joint2
+      - Rizon4-063989_joint3
+      - Rizon4-063989_joint4
+      - Rizon4-063989_joint5
+      - Rizon4-063989_joint6
+      - Rizon4-063989_joint7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    allow_partial_joints_goal: false
+    # Must be false: true interpolates from the last *command* (already at target
+    # after a "success" with no hardware motion) so the arm never moves.
+    open_loop_control: false
+    constraints:
+      stopped_velocity_tolerance: 0.05
+      goal_time: 2.0

src/arm_control/config/named_poses.yaml

@@ -0,0 +1,10 @@
+# Named poses — site robot Rizon4-063989 (7-DOF), radians
+# Joint base names match flexiv_description: joint1..joint7
+named_pose_server:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    named_poses.home: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.ready: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.inspect: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
+    named_poses.pos_left: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]

src/arm_control/config/named_poses_89.yaml

@@ -0,0 +1,9 @@
+# Left arm 89 (Rizon4-063989) named poses (radians).
+named_pose_server:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    named_poses.home: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.ready: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.inspect: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
+    named_poses.pos_left: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]

src/arm_control/config/named_poses_91.yaml

@@ -0,0 +1,10 @@
+# Right arm 91 (Rizon4-063991) named poses (radians).
+# Starting values match the generic home; teach cell-specific poses on 91.
+named_pose_server:
+  ros__parameters:
+    joint_prefix: "Rizon4-063991_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    named_poses.home: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.ready: [0.0, -0.698, 0.0, 1.57, 0.0, 0.698, 0.0]
+    named_poses.inspect: [0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
+    named_poses.pos_left: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]

src/arm_control/launch/mock_control.launch.py

@@ -0,0 +1,99 @@
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+# Site robot. Official flexiv_description keeps the hyphen in the SN prefix.
+SITE_ROBOT_SN = os.environ.get('SITE_ROBOT_SN', 'Rizon4-063989')
+SITE_JOINT_PREFIX = os.environ.get('SITE_JOINT_PREFIX', f'{SITE_ROBOT_SN}_')
+SITE_NAMED_POSES = os.environ.get('SITE_NAMED_POSES_FILE', '').strip()
+
+
+def generate_launch_description():
+    use_rviz = LaunchConfiguration('use_rviz')
+    robot_sn = LaunchConfiguration('robot_sn')
+    rizon_type = LaunchConfiguration('rizon_type')
+    load_gripper = LaunchConfiguration('load_gripper')
+
+    urdf_path = PathJoinSubstitution(
+        [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
+    )
+    named_poses = SITE_NAMED_POSES if SITE_NAMED_POSES and os.path.isfile(SITE_NAMED_POSES) else None
+    if not named_poses:
+        named_poses = PathJoinSubstitution(
+            [FindPackageShare('arm_control'), 'config', 'named_poses.yaml']
+        )
+    rviz_config = PathJoinSubstitution(
+        [FindPackageShare('flexiv_description'), 'rviz', 'view_rizon.rviz']
+    )
+
+    robot_description = ParameterValue(
+        Command([
+            'xacro ', urdf_path,
+            ' robot_sn:=', robot_sn,
+            ' rizon_type:=', rizon_type,
+            ' load_gripper:=', load_gripper,
+            ' use_fake_hardware:=true',
+            ' ros2_control:=false',
+        ]),
+        value_type=str,
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument(
+            'robot_sn',
+            default_value=SITE_ROBOT_SN,
+            description='Site SN; joints become {SN}_joint1..joint7 (hyphen kept)',
+        ),
+        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
+        DeclareLaunchArgument(
+            'load_gripper',
+            default_value='false',
+            description='Site has no gripper yet',
+        ),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[{'robot_description': robot_description}],
+            output='screen',
+        ),
+        Node(
+            package='arm_control',
+            executable='mock_arm_driver',
+            name='mock_arm_driver',
+            parameters=[{'joint_prefix': SITE_JOINT_PREFIX}],
+            output='screen',
+        ),
+        Node(
+            package='arm_control',
+            executable='arm_status_node',
+            name='arm_status_node',
+            parameters=[{
+                'ee_frame': f'{SITE_JOINT_PREFIX}tcp',
+                'base_frame': f'{SITE_JOINT_PREFIX}base_link',
+            }],
+            output='screen',
+        ),
+        Node(
+            package='arm_control',
+            executable='named_pose_server',
+            name='named_pose_server',
+            parameters=[named_poses, {'joint_prefix': SITE_JOINT_PREFIX}],
+            output='screen',
+        ),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])

src/arm_control/launch/ros2_control.launch.py

@@ -0,0 +1,51 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    use_fake = LaunchConfiguration('use_fake_hardware')
+
+    urdf_path = PathJoinSubstitution(
+        [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
+    )
+    controllers = PathJoinSubstitution(
+        [FindPackageShare('arm_control'), 'config', 'controllers.yaml']
+    )
+
+    robot_description = ParameterValue(
+        Command(['xacro ', urdf_path, ' use_fake_hardware:=', use_fake]),
+        value_type=str,
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_fake_hardware', default_value='true'),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[{'robot_description': robot_description}],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='ros2_control_node',
+            parameters=[{'robot_description': robot_description}, controllers],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+            output='screen',
+        ),
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['rizon_arm_controller', '--controller-manager', '/controller_manager'],
+            output='screen',
+        ),
+    ])

src/arm_control/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_control</name>
+  <version>0.1.0</version>
+  <description>Control nodes for Flexiv Rizon 4 mock and ros2_control.</description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>control_msgs</depend>
+  <depend>arm_msgs</depend>
+  <depend>tf2_ros</depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2_control</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/arm_control/resource/arm_control

@@ -0,0 +1 @@
+

src/arm_control/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/arm_control
+[install]
+install_scripts=$base/lib/arm_control

src/arm_control/setup.py

@@ -0,0 +1,33 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'arm_control'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+        (os.path.join('share', package_name, 'config'), glob('config/*')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='arm_dev',
+    maintainer_email='dev@example.com',
+    description='Arm control nodes for mock and real hardware.',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'mock_arm_driver = arm_control.mock_arm_driver:main',
+            'arm_status_node = arm_control.arm_status_node:main',
+            'named_pose_server = arm_control.named_pose_server:main',
+            'trajectory_bridge = arm_control.trajectory_bridge:main',
+            'real_trajectory_adapter = arm_control.real_trajectory_adapter:main',
+        ],
+    },
+)

src/arm_description/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.8)
+project(arm_description)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY urdf meshes rviz launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/arm_description/config/joint_names.yaml

@@ -0,0 +1,21 @@
+# Flexiv Rizon 4 — site robot Rizon4-063989
+robot_model: Rizon4
+robot_sn: Rizon4-063989
+dof: 7
+load_gripper: false
+
+# Official flexiv_description: SN hyphen is kept in the prefix
+joint_prefix: "Rizon4-063989_"
+
+joint_names:
+  - joint1
+  - joint2
+  - joint3
+  - joint4
+  - joint5
+  - joint6
+  - joint7
+
+base_frame: base_link
+ee_frame: tcp
+flange_frame: flange

src/arm_description/launch/display.launch.py

@@ -0,0 +1,70 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    use_gui = LaunchConfiguration('use_gui')
+    use_rviz = LaunchConfiguration('use_rviz')
+    robot_sn = LaunchConfiguration('robot_sn')
+    rizon_type = LaunchConfiguration('rizon_type')
+    load_gripper = LaunchConfiguration('load_gripper')
+
+    urdf_path = PathJoinSubstitution(
+        [FindPackageShare('arm_description'), 'urdf', 'arm.urdf.xacro']
+    )
+    rviz_config = PathJoinSubstitution(
+        [FindPackageShare('flexiv_description'), 'rviz', 'view_rizon.rviz']
+    )
+
+    robot_description = ParameterValue(
+        Command([
+            'xacro ', urdf_path,
+            ' robot_sn:=', robot_sn,
+            ' rizon_type:=', rizon_type,
+            ' load_gripper:=', load_gripper,
+        ]),
+        value_type=str,
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('use_gui', default_value='true'),
+        DeclareLaunchArgument('use_rviz', default_value='true'),
+        DeclareLaunchArgument('robot_sn', default_value='Rizon4-063989'),
+        DeclareLaunchArgument('rizon_type', default_value='Rizon4'),
+        DeclareLaunchArgument('load_gripper', default_value='false'),
+
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            parameters=[{'robot_description': robot_description}],
+            output='screen',
+        ),
+        Node(
+            package='joint_state_publisher_gui',
+            executable='joint_state_publisher_gui',
+            name='joint_state_publisher_gui',
+            condition=IfCondition(use_gui),
+            output='screen',
+        ),
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher',
+            condition=UnlessCondition(use_gui),
+            output='screen',
+        ),
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config],
+            condition=IfCondition(use_rviz),
+            output='screen',
+        ),
+    ])

src/arm_description/meshes/.gitkeep

@@ -0,0 +1,2 @@
+# Placeholder: put STL/DAE meshes here and reference them from URDF, e.g.:
+#   package://arm_description/meshes/link1.stl

src/arm_description/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>arm_description</name>
+  <version>0.1.0</version>
+  <description>URDF/Xacro wrapper for Flexiv Rizon 4 via flexiv_description.</description>
+  <maintainer email="dev@example.com">arm_dev</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>flexiv_description</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/arm_description/rviz/arm.rviz

@@ -0,0 +1,58 @@
+Panels:
+  Displays:
+    class: rviz_common/Displays
+    name: Displays
+    displays:
+      - class: rviz_default_plugins/Grid
+        name: Grid
+        enabled: true
+        Cell Size: 0.25
+        Reference Frame: <Fixed Frame>
+      - class: rviz_default_plugins/RobotModel
+        name: RobotModel
+        enabled: true
+        Description Source: Topic
+        Description Topic:
+          Value: /robot_description
+        Visual Enabled: true
+        Collision Enabled: false
+        Alpha: 1
+      - class: rviz_default_plugins/TF
+        name: TF
+        enabled: true
+        Show Names: true
+        Show Axes: true
+        Frame Timeout: 15
+        Marker Scale: 0.15
+  Views:
+    class: rviz_common/Views
+    name: Views
+    Current:
+      class: rviz_default_plugins/Orbit
+      name: Current View
+      Distance: 2.2
+      Focal Point:
+        X: 0.25
+        Y: 0.0
+        Z: 0.35
+      Pitch: 0.45
+      Yaw: 0.85
+Visualization Manager:
+  class: ""
+  displays: ~
+  tools:
+    - class: rviz_default_plugins/Interact
+    - class: rviz_default_plugins/MoveCamera
+    - class: rviz_default_plugins/Select
+    - class: rviz_default_plugins/FocusCamera
+  views:
+    current:
+      class: rviz_default_plugins/Orbit
+Global Options:
+  Background Color: 48; 48; 48
+  # Official flexiv_description Fixed Frame (or Rizon4-063989_base_link)
+  Fixed Frame: world
+  Frame Rate: 30
+Window Geometry:
+  Height: 800
+  Width: 1200