首轮顺时针进孔改回 10°/s，空回固定做 4 轮。

首轮咬合的顺时针进孔又走了接口上限，螺纹还没咬住就转完了。这一段改回读
extreme_speed_deg_s（现场 10°/s）。首轮逆时针出孔，以及空回每一轮的正反转，
仍走 0.90 rad/s 上限。

空回原先只做一轮。首轮咬合结束后，张开、微抬、逆时针、按本轮实测上抬降回、
合爪、顺时针这一整段固定重复 UNWIND_ROUNDS=4。抬和降留在轮内，下降量用这一轮
记住的上抬，不用上一轮的数。日志打空回第 i/4 轮；某一轮没走完就停，后面不再做。

10:40 合爪命令已经发到话题，节点被 Serial.flush/tcdrain 堵住 120s 才执行，
流水线 8s 时手指还停在 pos=400，第 2–4 轮没开始。发送改成等发送缓冲排空，
0.25s 排不空就失败返回，不再调用 tcdrain。同一次开合状态没跟上再发一次。

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

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

@@ -40,7 +40,19 @@ arm_application/kalman_filter.py
 arm_application/model_labels.py
 arm_application/move_to_pose_client.py
 arm_application/onnx_workpiece.py
+arm_application/operator_app.py
+arm_application/operator_arm_mesh.py
+arm_application/operator_arm_state.py
+arm_application/operator_camera_view.py
+arm_application/operator_console.py
+arm_application/operator_gripper_mesh.py
+arm_application/operator_gripper_state.py
+arm_application/operator_handeye_board.py
+arm_application/operator_tactile_view.py
+arm_application/operator_teach.py
 arm_application/orisys_tactile.py
+arm_application/overhead_oneshot.py
+arm_application/path_profile.py
 arm_application/perception_node.py
 arm_application/pid_controller.py
 arm_application/pipeline_node.py

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

@@ -10,8 +10,10 @@ from rclpy.node import Node
 
 from arm_msgs.srv import SetNamedPose
 
+from arm_application.ros_node import TolerantNode
 
-class DemoNamedPoses(Node):
+
+class DemoNamedPoses(TolerantNode):
     def __init__(self) -> None:
         super().__init__('demo_named_poses')
         self.declare_parameter(

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

@@ -201,21 +201,36 @@ class DHModbusClient:
 
     def set_position_per_mille(self, position: float | int) -> int:
         value = _clamp_int(position, 0, 1000)
+        # 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
+        # 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
+        from arm_application.dh_pgea import latched_position_break
+
         try:
             cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
         except Exception:
             cmd = value
-        if cmd == value:
+        try:
-            try:
+            cur = self.get_position_per_mille()
-                cur = self.get_position_per_mille()
+        except Exception:
-            except Exception:
+            cur = value
-                cur = value
+        bridge = latched_position_break(cmd, cur, value)
-            nudge = cur + (40 if value >= cur else -40)
+        if bridge is not None:
-            nudge = _clamp_int(nudge, 0, 1000)
+            self.write_register(REG_POSITION, bridge)
-            if nudge == value:
+            deadline = time.monotonic() + 0.6
-                nudge = 0 if value != 0 else 40
+            echoed = False
-            self.write_register(REG_POSITION, nudge)
+            while time.monotonic() < deadline:
-            time.sleep(0.06)
+                try:
+                    if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
+                        echoed = True
+                        break
+                except Exception:
+                    pass
+                time.sleep(0.05)
+            if not echoed:
+                raise DHModbusError(
+                    f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
+                )
+            time.sleep(0.15)
         self.write_register(REG_POSITION, value)
         return value
 

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

@@ -104,6 +104,44 @@ INIT_STATE_NAME = {
 }
 
 
+def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
+    """命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
+
+    大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
+    开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
+    先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
+    固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
+    来不及看到中间值，最终仍是旧命令，手指不动。
+    """
+    cmd = int(max(0, min(1000, command)))
+    cur = int(max(0, min(1000, current)))
+    pos = int(max(0, min(1000, target)))
+    if cmd != pos or abs(cur - pos) <= 25:
+        return None
+    if cur != cmd:
+        return cur
+    return 0 if cmd != 0 else 40
+
+
+def grip_motion_settled(
+    grip_state: int, position: int, target: int, seen_motion: bool
+) -> bool:
+    """这一拍能不能算「这句位置命令已经结束」。
+
+    张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
+    闭合夹住物体：动过之后停在比目标更开的位置，算完成。
+    """
+    near = abs(int(position) - int(target)) <= 25
+    if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
+        return True
+    if grip_state == S_GRIP_CAUGHT and seen_motion:
+        if int(target) + 25 < int(position):
+            return True
+        if near:
+            return True
+    return False
+
+
 def crc16_modbus(data: bytes) -> int:
     """官方 DH CRC16（Modbus，线路上按小端发送）。"""
     crc = 0xFFFF
@@ -175,6 +213,34 @@ WIRING_HINT = (
 )
 
 
+def open_failure_text(port: str, exc: BaseException) -> str:
+    """串口打不开时把「被占用」和「没应答」分开。
+
+    独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
+    界面上看起来像缺自动上电，实际是别的进程还握着这个口。
+    """
+    text = str(exc).lower()
+    errno = getattr(exc, "errno", None)
+    busy = errno in (11, 16) or any(
+        token in text
+        for token in (
+            "temporarily unavailable",
+            "device or resource busy",
+            "could not exclusively lock",
+            "resource busy",
+        )
+    )
+    if busy:
+        return (
+            f"无法独占打开 {port}：{exc}。"
+            "串口已被别的进程占用（夹爪驱动或状态读取）。"
+        )
+    return (
+        f"无法打开 {port}：{exc}。"
+        "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
+    )
+
+
 class DhPgeaGripper:
     """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
 
@@ -219,10 +285,7 @@ class DhPgeaGripper:
                 exclusive=True,
             )
         except Exception as exc:
-            raise DhPgeaError(
+            raise DhPgeaError(open_failure_text(self.port, exc)) from exc
-                f"无法打开 {self.port}：{exc}。"
-                "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
-            ) from exc
         try:
             self._ser.dtr = True
         except Exception:
@@ -319,17 +382,28 @@ class DhPgeaGripper:
     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:
+    def initialize(
-        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
+        self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
+    ) -> bool:
+        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
+
+        ``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
+        命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
+        手指根本不会回零，看起来像初始化失效。
+        """
         state = self.get_init_state()
-        if state == S_INIT_FINISHED:
+        if state == S_INIT_FINISHED and not force:
             return True
         if not self.initialization():
             raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
         deadline = time.monotonic() + float(timeout_s)
+        # force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
+        saw_busy = not force
         while time.monotonic() < deadline:
             state = self.get_init_state()
-            if state == S_INIT_FINISHED:
+            if state != S_INIT_FINISHED:
+                saw_busy = True
+            elif saw_busy:
                 return True
             time.sleep(poll_s)
         raise DhPgeaError(
@@ -345,42 +419,58 @@ class DhPgeaGripper:
         speed_pct = int(max(1, min(100, speed_pct)))
         return self.write_register(REG_SPEED, speed_pct)
 
+    def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
+        """等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
+        deadline = time.monotonic() + float(timeout_s)
+        while time.monotonic() < deadline:
+            try:
+                if self.get_target_position(retries=1) == int(value):
+                    return True
+            except Exception:
+                pass
+            time.sleep(0.05)
+        return False
+
     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:
-            try:
+            cur = self.get_current_position(retries=2)
-                cur = self.get_current_position(retries=2)
+        except Exception:
-            except Exception:
+            cur = pos
-                cur = pos
+        bridge = latched_position_break(cmd, cur, pos)
-            nudge = cur + (40 if pos >= cur else -40)
+        if bridge is not None:
-            nudge = int(max(0, min(1000, nudge)))
+            if not self.write_register(REG_POSITION, bridge):
-            if nudge == pos:
+                return False
-                nudge = 0 if pos != 0 else 40
+            if not self._wait_command_echo(bridge):
-            self.write_register(REG_POSITION, nudge)
+                return False
-            time.sleep(0.06)
+            # 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
+            time.sleep(0.15)
         return self.write_register(REG_POSITION, pos)
 
     def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
-        """等到到位/夹住/松开，或超时返回最后一帧。"""
+        """等到到位或夹住，或超时返回最后一帧。
+
+        张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
+        手指还停在夹持位。
+        """
         t0 = time.monotonic()
         last = None
+        seen_motion = False
         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
+            if last is not None and st.position != last:
+                seen_motion = True
             last = st.position
-            if st.grip_state == S_GRIP_MOVING or moved:
+            if st.grip_state == S_GRIP_MOVING:
+                seen_motion = True
                 time.sleep(0.08)
                 continue
-            if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
+            if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
-                return st
-            if st.grip_state == S_GRIP_CAUGHT:
                 return st
             time.sleep(0.08)
         return st
@@ -497,6 +587,36 @@ def _scan_bus(port: str) -> int:
     return 1
 
 
+def ctrl_values(
+    cmd: str,
+    value: str = "",
+    force: int = 50,
+    speed: int = 50,
+) -> Optional[tuple[float, float, float, float]]:
+    """节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
+
+    顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
+    """
+    name = str(cmd or "").strip().lower()
+    if name in ("probe", "status"):
+        return None
+    if name == "init":
+        return (1.0, 0.0, float(force), float(speed))
+    if name == "open":
+        pos = 1000
+    elif name == "close":
+        pos = 0
+    elif name in ("pos", "move"):
+        if str(value).strip() == "":
+            raise ValueError("pos 需要 0–1000")
+        pos = int(round(float(value)))
+        if pos < 0 or pos > 1000:
+            raise ValueError("位置只接受 0–1000")
+    else:
+        raise ValueError(f"节点在线时不支持 {name}")
+    return (0.0, float(pos), float(force), float(speed))
+
+
 def _cli(argv: Optional[list[str]] = None) -> int:
     site = _site_defaults()
     ap = argparse.ArgumentParser(
@@ -541,8 +661,8 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             print(st.summary())
             return 0
         if args.cmd == "init":
-            print("正在初始化（手指回零）…")
+            print("正在初始化（手指回零，已初始化也会再做一次）…")
-            grip.initialize()
+            grip.initialize(force=True)
             print(grip.get_run_states().summary())
             return 0
         if args.cmd == "status":
@@ -551,12 +671,25 @@ def _cli(argv: Optional[list[str]] = None) -> int:
         if args.cmd == "open":
             if not grip.open_jaw(args.force, args.speed):
                 raise DhPgeaError("张开写入失败")
-            print(grip.wait_settle(1000).summary())
+            st = grip.wait_settle(1000)
+            print(st.summary())
+            if abs(st.position - 1000) > 25:
+                print(
+                    "张开失败：手指没有离开夹持位。"
+                    "若仍是 grip=caught，先取下工件再执行 init。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
         if args.cmd == "close":
             if not grip.close_jaw(args.force, args.speed):
                 raise DhPgeaError("闭合写入失败")
-            print(grip.wait_settle(0).summary())
+            st = grip.wait_settle(0)
+            print(st.summary())
+            # 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
+            if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
+                print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
+                return 1
             return 0
         if args.cmd == "pos":
             if args.value == "":
@@ -565,7 +698,15 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             pos = int(float(args.value))
             if not grip.move(pos, args.force, args.speed):
                 raise DhPgeaError("位置写入失败")
-            print(grip.wait_settle(pos).summary())
+            st = grip.wait_settle(pos)
+            print(st.summary())
+            grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
+            if abs(st.position - pos) > 25 and not grasped_short:
+                print(
+                    f"位置 {pos} 没有到位（仍在 {st.position}）。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
     except DhPgeaError as exc:
         print(f"错误：{exc}", file=sys.stderr)

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

@@ -33,15 +33,7 @@ from arm_application.dh_pgea import (
     GripperState,
     WIRING_HINT,
 )
-
+from arm_application.ros_node import TolerantNode, param_as_str
-
-def _param_as_str(node: Node, name: str, default: str = "") -> str:
-    """字符串参数，同时允许命令行用数字覆盖（例如 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]:
@@ -51,7 +43,7 @@ def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
     return float(data[0]), float(data[1]), float(data[2]), float(data[3])
 
 
-class DhPgeaNode(Node):
+class DhPgeaNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("dh_pgea")
         self.declare_parameter("port", "/dev/ttyUSB0")
@@ -64,9 +56,9 @@ class DhPgeaNode(Node):
         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._model = param_as_str(self, "model", "PGEA-100-20-W")
-        self._sn = _param_as_str(self, "serial_number", "")
+        self._sn = param_as_str(self, "serial_number", "")
-        self._rts = _param_as_str(self, "rs485_rts", "")
+        self._rts = param_as_str(self, "rs485_rts", "")
 
         self._port = str(self.get_parameter("port").value)
         self._baud = int(self.get_parameter("baud").value)

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

@@ -22,6 +22,8 @@ from arm_msgs.msg import GripperCommand as GripperCommandMsg
 from arm_msgs.msg import GripperStatus as GripperStatusMsg
 from arm_msgs.srv import GripperExecute
 
+from arm_application.ros_node import TolerantNode
+
 
 def _site_path() -> Path:
     env = os.environ.get("SITE_CONFIG", "").strip()
@@ -38,7 +40,7 @@ def _as_bool(val, default: bool = True) -> bool:
     return bool(val)
 
 
-class DualGripperNode(Node):
+class DualGripperNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("dual_gripper_driver")
         self.declare_parameter("config_file", "")

build/arm_application/build/lib/arm_application/gauge_assembly_node.py

@@ -94,6 +94,10 @@ from arm_application.gauge_config import (
 )
 from arm_application.gauge_fsm import (
     DESCEND_STALL_STEPS,
+    ENTRY_HIGH,
+    ROT_AXIS,
+    ROT_LIMIT,
+    ROT_SHORT,
     Event,
     FsmError,
     GaugeContext,
@@ -104,12 +108,16 @@ from arm_application.gauge_fsm import (
     descend_iter_budget,
     descend_needed_m,
     descend_progress,
+    hole_entry_verdict,
     descend_tol_m,
     is_joint_limit_error,
     pixel_stable,
     points_stable,
     ratchet_cycles,
+    ratchet_delta_rad,
+    ratchet_travel_ok,
     relief_tol_m,
+    rotation_verdict,
     screw_stop,
     tangential_jump,
     tool_clearance_m,
@@ -135,16 +143,41 @@ from arm_application.gauge_kin import (
 )
 from arm_application.pipeline_node import AssemblyPipeline
 from arm_application.handeye_realsense import resolve_realsense_topics
-from arm_application.site_io import deep_merge, load_site_dict, ros2_root
+from arm_application.site_io import (
+    deep_merge,
+    load_site_dict,
+    overhead_camera,
+    resolve_eye_to_hand_file,
+    ros2_root,
+)
 # 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2）
 from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point
 
 #: 允许的"绕轴验证"误差 [deg]。超过就说明这根关节轴不是工具 Z 轴，
 #: 后面所有以"转多少度"为单位的动作都不成立 —— 必须报错而不是继续。
 AXIS_VERIFY_TOL_DEG = 5.0
+#: 棘轮转腕时距离关节限位保留的余量 [deg]。
+#: 3° 不够用：实测轨迹会**自己**少走 ~5°（比如命令 180° 只到位 175.1°，
+#: 而关节7 的限位正好是 175.06° —— 两者叠在一起，判断"是被限位截住还是
+#: 没走完"就分不清了）。留 8° 让这两种情况都还没有贴住。
+RATCHET_LIMIT_MARGIN_DEG = 8.0
 #: 限制"跟踪丢失 → 粗定位 → 跟踪"的来回次数，防死循环。
 MAX_TRACK_RETRIES = 3
 
+#: ``stage2_locate`` 返回码 → 人话。为什么要这张表：日志里只有 ``code=6`` 时，
+#: 现场只能去翻源码；而每个码对应的**下一步动作完全不同**（垂直化/粗定位/
+#: 检测/对准/工件被推走），写在这里就不用来回翻了。
+_LOCATE_CODE_HINT: Dict[int, str] = {
+    2: "（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）",
+    3: "（粗定位失败：视野里没框到工件 —— 看相机/光照/权重）",
+    4: "（k1/k2 解不出来：孔被挡、曝光不对、或孔根本没在视野里）",
+    5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）",
+    6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件"
+       " —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）",
+    8: "（规尖偏移一直没量到：残差看着达标、但用的是**法兰轴线**不是规尖 —— "
+       "对的可能是法兰、不是规；摆到能看清规尖再跑。看日志『规尖视觉量取失败』）",
+}
+
 
 class GaugeAbort(RuntimeError):
     """阶段内部判定要中止。带上 exit code，方便脚本区分原因。"""
@@ -194,12 +227,11 @@ class GripperClient:
         if math.isfinite(pos):
             self._last_aperture = pos
         self._last_gripped = bool(msg.is_gripped)
-        if self._last_gripped:
+        # ⚠️ 这里**故意不**记"夹住时的开口"。曾经记过，代价是一个假 E1：
-            if not self._ever_gripped:
+        #    本后端的 is_gripped 是**推断值**，夹爪**全开**时也会闪成 True
-                # 记住"夹住时的开口"：之后的掉件判据是**相对它的塌陷量**，
+        #    （实测 99.29mm，见 dropped 的说明），于是基准被记成 99.29mm、
-                # 换一把规、换一个夹持开口都不用改阈值。
+        #    塌陷量算成 77mm ⇒ "通止规掉了(E1)"，而规好好地在爪里。
-                self._grip_aperture = self._last_aperture
+        #    基准只由 :meth:`adopt_current_grip` 在**明确知道夹住了**的时候记。
-            self._ever_gripped = True
 
     def _on_legacy(self, msg: Float32MultiArray) -> None:
         fields = legacy_states_to_fields(list(msg.data))
@@ -210,6 +242,8 @@ class GripperClient:
         if got is not None:
             self._last_gripped = got
             if got:
+                # 老后端只有状态字、没有开口，掉件判据本来就吃状态字（见 dropped），
+                # 所以这里记"曾经夹住过"是对的。
                 self._ever_gripped = True
 
     # -- 查询 -------------------------------------------------------------
@@ -224,6 +258,75 @@ class GripperClient:
         """``True``/``False``/``None``（后端给不出明确答案）。"""
         return self._last_gripped
 
+    def aperture(self) -> Optional[float]:
+        """最近一帧的**开口** [mm]（只有契约后端的 ``position`` 有；没有返回 None）。"""
+        return self._last_aperture
+
+    def grip_aperture(self) -> Optional[float]:
+        """第一次判到"夹住"时的开口 [mm]（掉件判据的基准，见 :meth:`dropped`）。"""
+        return self._grip_aperture
+
+    def collapse_mm(self) -> Optional[float]:
+        """相对夹住时的**开口塌陷量** [mm]（正 = 比夹住时更小）。拿不到返回 None。
+
+        S08/S09 每轮都要"松爪 → 回转 → 夹紧"，日志里必须看得到这个数：
+        它一路变大就说明每轮夹紧都比上一轮更往里挤，或者规在往下滑。
+        """
+        if self._grip_aperture is None or self._last_aperture is None:
+            return None
+        return float(self._grip_aperture) - float(self._last_aperture)
+
+    # -- 开口的"等稳定"与"记基准" ------------------------------------------
+    def settle(self, *, settle_s: float = 1.0, tol_mm: float = 0.05) -> Optional[float]:
+        """等开口**稳定**下来，返回稳定后的开口 [mm]（拿不到读数返回 None）。
+
+        为什么必须有这个（2026-09-24 用日志抓到的）：
+            ``close``/``open`` 服务**返回时动作还没做完**（JTC 轨迹是异步的）。
+            立刻读开口会读到**松手前的旧值**——实测 S08 夹紧后立刻读，读到的还是
+            全开 99.93mm，于是日志与基准全错（"夹住时 99.29mm"）。谁要在动作之后
+            判断开口，都必须先过这里。
+        """
+        spin = getattr(self._node, "_spin", None)
+        deadline = time.monotonic() + max(0.0, settle_s)
+        prev = self._last_aperture
+        stable = 0
+        while True:
+            if spin is not None:
+                spin(0.05)
+            else:  # pragma: no cover - 只有测试桩走这里
+                time.sleep(0.05)
+            now = self._last_aperture
+            if now is not None and prev is not None and abs(float(now) - float(prev)) <= tol_mm:
+                stable += 1
+                if stable >= 2:
+                    return float(now)
+            else:
+                stable = 0
+            prev = now
+            if time.monotonic() >= deadline:
+                return None if now is None else float(now)
+
+    def adopt_current_grip(self, why: str, *, settle_s: float = 1.0) -> Optional[float]:
+        """把**当前**开口记为"夹住时的开口"（掉件判据的基准），并记"曾经夹住过"。
+
+        只在**明确知道爪里夹着规**的时刻调用（S00 自检通过后、以及每一次
+        确认夹紧之后）——它是掉件判据唯一的参照，随手记会让判据失去意义
+        （见 :meth:`dropped` 与 :meth:`_on_contract` 的说明）。
+        """
+        ap = self.settle(settle_s=settle_s)
+        if ap is None:
+            # 拿不到开口（老固件/别的夹爪）也要记"曾经夹住过"：否则 dropped()
+            # 会永远返回 False，那比"会抖的旧判据"更糟（真掉件也看不见）。
+            self._ever_gripped = True
+            self._node.get_logger().warn(
+                f"拿不到开口读数（{why}）—— 掉件判据退回状态字，会抖但不会漏"
+            )
+            return None
+        self._grip_aperture = float(ap)
+        self._ever_gripped = True
+        self._node.get_logger().info(f"夹持基准：开口 {ap:.2f}mm（{why}）")
+        return float(ap)
+
     def dropped(self) -> bool:
         """E1 掉件：**曾经夹住过**，现在报"掉落"。
 
@@ -238,6 +341,9 @@ class GripperClient:
             的指令落到与实际开口同一档，``is_gripped`` 就翻成 False。于是
             **S08 第 2/12 轮直接报 E1**（爪里其实好好的，是判据在抖），
             然后整条流程去等人工"放好通止规"，而仿真里根本没人能做这件事。
+            更坏的是这个推断值在**全开**时也会闪成 True（实测 99.29mm）——
+            拿它当基准就会得到"塌陷 77mm"的假 E1。所以基准只由
+            :meth:`adopt_current_grip` 明确记录（见 :meth:`_on_contract`）。
 
             真正"掉了"的物理表现是**两指合到底**：开口从"工件宽度"塌到几毫米，
             是几十毫米的变化，不会被指令切换带偏。所以判据用
@@ -349,6 +455,12 @@ class GaugeAssembly(AssemblyPipeline):
         # `$?` 判不出成败 —— 自动化里会把"流程失败"记成"成功"。结论文件是唯一
         # 能被脚本/CI 读到的东西，由 run_gauge_assembly.sh 传入并读回。
         self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json")
+        # 眼在手外标定产物的**确切路径**，由 launch 按 site.yaml 派生后传入
+        # （`<臂号>_<相机>_eye_to_hand.yaml`）。为什么不让节点自己拼：
+        # "臂号 = SN 末两位"这条口径只有 export_site_env 一处实现，节点再写一遍
+        # 必然漂移；而漂移的症状是"标定做完了、S03 却找不到文件"，回退到 S04 跟踪，
+        # 看起来像跟踪的问题。留空则退回下面那串候选名（兼容旧命名/仿真）。
+        self.declare_parameter("coarse_eye_to_hand_file", "")
 
         site = load_site_dict()
         # robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。
@@ -369,6 +481,13 @@ class GaugeAssembly(AssemblyPipeline):
         holes = int(self.get_parameter("holes_total").value)
         if holes > 0:
             overrides["holes_total"] = holes
+        # 工具长：仿真里由启动脚本传**实测值**（上料脚本刚量的），真机不传（走 site）。
+        # 为什么必须能覆盖：这个值决定悬停高度与 S07 下探预算，而仿真里规会在指间
+        # 下滑到落座位置、下滑量随装夹变化（实测见过 243 / 254mm，差 11mm）。
+        # 差 11mm 的后果见 gauge_fsm.hover_z_guard / drag_verdict 的说明。
+        tool_len = float(self.get_parameter("tool_tip_below_flange_m").value)
+        if tool_len > 0.0:
+            overrides["tool_tip_below_flange_m"] = tool_len
         try:
             self.cfg: GaugeConfig = from_site(site, overrides=overrides or None)
         except GaugeConfigError as exc:
@@ -496,49 +615,30 @@ class GaugeAssembly(AssemblyPipeline):
     def _d435_ns(self, site: Dict[str, Any]) -> str:
         """手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。
 
-        为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``），
+        规则（三级收敛）的**唯一实现**在 ``site_io.overhead_camera``——手眼标定
-        只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 ——
+        脚本也用它取相机名。这里只负责把"配置没区分开"这件事**出声**：静默取第一台
-        同一份配置在仿真/真机之间**错而不报**地互串。
+        会让"标定用的相机"和"推理用的相机"悄悄不是同一台。
-
-        规则（确定性的三级）：
-          1. ``arm`` 与当前臂相同的优先（仿真：sim_d435 / 真机若两台各有相机也适用）；
-          2. 都没有 ``arm``（= 双臂共用的那台老相机）时回落到它；
-          3. 还是多个 → 出声告警并取第一个，让人去配置里区分。
         """
-        cams = [
+        cfg, others = overhead_camera(site, self._robot_dict(site))
-            cfg
+        if others:
-            for cfg in (site.get("cameras") or {}).values()
+            names = ", ".join([str(cfg.get("name") or "?")] + others)
-            if isinstance(cfg, dict)
-            and str(cfg.get("role") or "").startswith("overhead")
-        ]
-        if not cams:
-            return ""
-        arm = str(self._robot_dict(site).get("arm") or "").strip()
-        mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
-        hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
-        if len(hits) > 1:
-            names = ", ".join(str(c.get("name") or "?") for c in hits)
             self.get_logger().warn(
-                f"有 {len(hits)} 台手外相机都符合当前臂（{names}）—— 取第一个，"
+                f"有 {len(others) + 1} 台手外相机都符合当前臂（{names}）—— 取第一个，"
                 "但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气"
             )
-        return str(hits[0].get("name") or "").strip("/")
+        return str(cfg.get("name") or "").strip("/")
 
     def _eye_to_hand_file(self) -> Optional[Path]:
         """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。
 
-        路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录
+        规则（候选顺序 + 按仓库根解析）只有一份，在 ``site_io.resolve_eye_to_hand_file``。
-        启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
+        这里只负责把 launch 传入的确切路径和当前手外相机名交给它。
         """
-        root = ros2_root()
+        return resolve_eye_to_hand_file(
-        for rel in (
+            ros2_root(),
-            f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml",
+            explicit=str(self.get_parameter("coarse_eye_to_hand_file").value or ""),
-            "handeye/results/eye_to_hand.yaml",
+            camera_name=self.cfg.coarse_camera,
-        ):
+        )
-            cand = root / rel
-            if cand.is_file():
-                return cand
-        return None
 
     def _on_robot_description(self, msg: String) -> None:
         self._robot_description = str(msg.data)
@@ -959,6 +1059,13 @@ class GaugeAssembly(AssemblyPipeline):
                 return Event.BOOT_OK
             self._publish_gauge_result({"ok": False, "state": "S00", "failures": blockers})
             return Event.BOOT_FAIL
+        # 记"夹住时的开口"（掉件判据的基准）。**放在这里而不是等到 S08**：
+        #   · 此刻爪里确实夹着规（仿真上料脚本刚装好，真机是人工装好才开始跑）；
+        #   · 基准必须是**明确知道夹住**时记的 —— 本后端的 is_gripped 是全开时
+        #     都会闪 True 的推断值（实测 99.29mm），拿它当基准会造出假 E1
+        #     （见 GripperClient.dropped）。
+        if self.grip is not None:
+            self.grip.adopt_current_grip("S00 自检通过：爪里已装好通止规")
         return Event.BOOT_OK
 
     # =====================================================================
@@ -1140,20 +1247,25 @@ class GaugeAssembly(AssemblyPipeline):
             f"{tag}: 目标 {delta_deg:+.1f}°，实测绕工具 Z {got:+.1f}°"
             f"（转轴偏差 {mis:.1f}°，阈值 {AXIS_VERIFY_TOL_DEG:.1f}°）"
         )
-        if abs(got) < 3.0 and abs(delta_deg) > 10.0:
+        verdict = rotation_verdict(delta_deg, got, mis, AXIS_VERIFY_TOL_DEG)
-            # 完全没动：最可能是撞上关节极限（用户要求的"转到极限角"就是这种情形）
+        if verdict == ROT_LIMIT:
+            # 完全没动、或只走了一部分：都是**关节限位**把这段截住了
+            # （2026-09-24 实测：棘轮回转每轮顶到上限 ±175°，于是下一步反向
+            #  只剩 ~16° 余量）。交给调用方让余量重试（见 _ratchet_step）；
+            #  调用方不处理时 FSM 会按未定义转移报错，不会静默走下去。
             self.get_logger().warn(
-                f"{tag}: 指令 {delta_deg:+.1f}° 但几乎没动 —— 判为已贴关节极限"
+                f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°"
+                f"（差 {abs(delta_deg) - abs(got):.1f}°）—— 判为**贴到关节限位**"
             )
             return Event.JOINT_LIMIT
-        if mis > AXIS_VERIFY_TOL_DEG:
+        if verdict == ROT_AXIS:
             self.get_logger().error(
                 f"{tag}: 转轴偏差 {mis:.1f}° 超过阈值 —— 关节7 轴与工具 Z 不共线，"
                 "以『度』为单位的动作（S01/S08/S09）都不可信，中止。"
                 "先确认工具系是否垂直向下、有没有装转接件"
             )
             return Event.ABORT
-        if abs(abs(got) - abs(delta_deg)) > max(AXIS_VERIFY_TOL_DEG, 0.1 * abs(delta_deg)):
+        if verdict == ROT_SHORT:
             self.get_logger().error(
                 f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°，偏差过大，中止"
             )
@@ -1161,6 +1273,146 @@ class GaugeAssembly(AssemblyPipeline):
         self._axis_verified = True
         return fsm_event
 
+    def _ratchet_step(
+        self, target_rad: float, *, tag: str, fsm_event: Event, speed_deg_s: Optional[float],
+        direction: int = 0,
+    ) -> Event:
+        """棘轮的一步"转腕"：行程不够时**张开爪让出余量**再转。
+
+        :param target_rad: 关节 7 的**目标角（绝对值）**，与
+            :meth:`_rotate_joint_and_verify` 同一口径（那边算的就是
+            ``target_rad - q_now``）。**不是增量。**
+        :param direction: 这一步的**工艺转向**：``+1`` 正转、``-1`` 反转、
+            ``0`` 交给"最短路径"。棘轮的四步一律显式给：步长接近 180° 时
+            "最短路径"会被 0.1° 的读数差翻方向（详见
+            :func:`arm_application.gauge_fsm.ratchet_delta_rad`）。
+
+        ⚠️ 为什么要把口径写在这里（2026-09-24 栽过一次，S09 直接 ABORT）：
+            调用方（S08/S09）给的本来就是绝对值（``q[-1] ± screw_step_rad``），
+            而本函数此前把它当**增量**用（``prewind(want_deg=abs(deg(target)))``、
+            让余量时又按 ``q - target`` 算新目标）。于是 S09 第一步被算成
+            "要转 338.9°"（其实是目标 338.9°、增量只有 180°）⇒ 误判"行程不够"
+            ⇒ 让余量那一步算到限位**外面**（``158.9 − 338.9 = −180°``，比
+            −175° 的限位还低 5°）⇒ 报"期望 −338.9°、实测 +26.0°，偏差过大"
+            ⇒ 整条流程在**最不该失败的一步** ABORT（规在爪里好好的、什么坏事都没发生）。
+            两个函数各用一种口径，编译器不会说话，日志还会把"目标"和"增量"都印成
+            一个数 —— 所以口径写进参数名和 docstring。
+
+        为什么需要"让余量"这一步：
+            棘轮要求"一正一反各 180°"，而关节 7 的行程只有 ±175°（3.0543 rad）。
+            拧入（S08）的**回转**那一步每轮都正好顶到上限（实测 +175.1°），
+            于是 S09 要做的**反方向**第一步只剩 ~16° 余量，被限位截住。
+            而这一轮其实什么坏事都没发生。
+
+        为什么"让余量"是免费的：
+            指间**张开**时规在打滑（这正是棘轮成立的原理），所以手腕空转既不会
+            把已经拧进去的螺纹转回去，也不会漏掉进度 —— 只是把行程重新分配。
+
+        让多少：整步（不去算亏欠了多少）。反正张开状态下这一步不产生任何装配
+        效果，整步最省事也最不容易算错；重试时的那一步才是"用力"的那一步。
+
+        两条路：**先算**（限位已知时，提前让余量，干净）＋ **兜底**（真被限位
+        截住时再让，覆盖"限位没读到 / 轨迹自己没走完"这两种情况）。
+
+        两条路**最后都汇到同一次让余量**：只有一次，避免"先算发现不够、
+        让完还不够、再让一次"这种把行程越绕越少的做法。
+        """
+        limits = self._wait_joint_limits()
+        q = None
+        if limits is not None:
+            q, _Tee = self._wait_state(3.0)
+        if q is None:
+            return Event.TIMEOUT
+        # 目标角可能落在限位**外面**（这正是棘轮要解决的问题），先折成"最近一圈"
+        # 的增量（gauge_fsm.ratchet_delta_rad，含完整说明与单测）：
+        # 关节在 +158.9°、目标 +338.9° ⇒ 增量 +180°（而不是 +338.9°）。
+        # **方向按工艺意图给**：步长贴着 180° 时"最短路径"会被读数差翻向（R2.10）。
+        delta_rad = ratchet_delta_rad(target_rad, float(q[-1]), direction)
+        why = ""
+        if limits is not None and abs(math.degrees(delta_rad)) > 1e-6:
+            plan = prewind(
+                q_now_rad=float(q[-1]), want_deg=abs(math.degrees(delta_rad)),
+                direction=1 if delta_rad > 0 else -1,
+                limits=limits, margin_deg=RATCHET_LIMIT_MARGIN_DEG,
+            )
+            if plan.clamped:
+                why = (
+                    f"行程不够（关节7 在 {math.degrees(float(q[-1])):+.1f}°，"
+                    f"这一步要转 {math.degrees(delta_rad):+.1f}°"
+                    f"（目标 {math.degrees(float(target_rad)):+.1f}°），限位 "
+                    f"{math.degrees(limits[0]):+.0f}~{math.degrees(limits[1]):+.0f}°）"
+                )
+        if not why:
+            ev = self._rotate_joint_and_verify(
+                target_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s
+            )
+            if ev is not Event.JOINT_LIMIT:
+                return ev
+            why = "轨迹被关节限位截住（算着够、实际没走完）"
+        return self._ratchet_rewind_then(
+            delta_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s, why=why,
+        )
+
+    def _ratchet_rewind_then(
+        self, delta_rad: float, *, tag: str, fsm_event: Event,
+        speed_deg_s: Optional[float], why: str,
+    ) -> Event:
+        """张开爪 → 反向让一步 → 夹紧 → 转原来那一步。
+
+        :param delta_rad: **要转的增量**（不是目标角，见 :meth:`_ratchet_step`）。
+            让余量时按它反着退一步，重试时再按它前进 —— 两次都以**当时的**
+            关节角为基准重新算目标，所以让完余量后关节会回到原处、工具净转
+            ``delta_rad``。把"目标"固定成一开始那个绝对值是错的：让完余量之后
+            那个目标已经在另一圈上了（差 360°）。
+        """
+        if self.grip is None:
+            self.get_logger().error(f"{tag}: {why}，但没有夹爪客户端，无法让余量 → 中止")
+            return Event.ABORT
+        self.get_logger().info(
+            f"{tag}: {why} —— 张开爪让出一步（此刻规在打滑，不会动到螺纹）再转"
+        )
+        if not self.grip.open():
+            self.get_logger().error(f"{tag}: 需要张爪让余量，但开爪失败 → 中止")
+            return Event.ABORT
+        time.sleep(self.cfg.screw_release_wait_s)
+        q, _Tee = self._wait_state(3.0)
+        if q is None:
+            return Event.TIMEOUT
+        ev = self._rotate_joint_and_verify(
+            float(q[-1]) - delta_rad, tag=f"{tag}_rewind",
+            fsm_event=fsm_event, speed_deg_s=speed_deg_s,
+        )
+        if ev is not fsm_event:
+            self.get_logger().error(
+                f"{tag}: 让余量那一步也没走开（{ev}）—— 关节7 的行程真用尽了，中止。\n"
+                f"        排查：site.yaml 的 gauge_assembly.screw_step_deg 太接近关节行程了"
+                f"（Rizon4 的关节7 是 ±175°），棘轮一步要 {math.degrees(abs(delta_rad)):.0f}°，"
+                f"留给'回转'的余量本来就不够。\n"
+                f"        改法：把 screw_step_deg 调小（例如 120°），并保证 "
+                f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍** —— "
+                f"累计必须转整数圈，否则规会停在螺纹中间（S09 拧出也就少转几分之一圈）。"
+            )
+            return Event.ABORT
+        if not self.grip.close():
+            self.get_logger().error(f"{tag}: 让完余量后夹紧失败 → 中止")
+            return Event.ABORT
+        # 重试目标**按让完余量后的位置重算**：让余量退了一步，原来那个绝对目标
+        # 现在差了 360°（正好一整圈），照原样下发会让关节反向绕一圈回去。
+        q2, _Tee2 = self._wait_state(3.0)
+        if q2 is None:
+            return Event.TIMEOUT
+        ev = self._rotate_joint_and_verify(
+            float(q2[-1]) + delta_rad, tag=f"{tag}_retry",
+            fsm_event=fsm_event, speed_deg_s=speed_deg_s,
+        )
+        if ev is not fsm_event:
+            self.get_logger().error(
+                f"{tag}: 让完余量重试仍然没走到位（{ev}）—— 中止"
+                f"（不是限位问题，先查是不是有东西卡住/螺纹咬死）"
+            )
+            return Event.ABORT
+        return fsm_event
+
     # =====================================================================
     # S02 夹持确认（预留）
     # =====================================================================
@@ -1200,11 +1452,15 @@ class GaugeAssembly(AssemblyPipeline):
         ns = self.cfg.coarse_camera
         he = self._eye_to_hand_file()
         if he is None:
+            expect = str(
+                self.get_parameter("coarse_eye_to_hand_file").value or ""
+            ).strip() or f"handeye/results/{ns}_eye_to_hand.yaml"
             self.get_logger().warn(
                 "S03 需要手外相机的 eye-to-hand 标定文件，现在没有 —— "
                 f"按兜底逻辑回 S04 跟踪。\n"
-                f"  相机 {ns} 的标定产物应放在 handeye/results/<名>_eye_to_hand.yaml；\n"
+                f"  应该生成：{expect}\n"
-                f"  做法：bash scripts/run_handeye.sh --mode eye_to_hand（标完再跑本流程）"
+                f"  做法：bash scripts/run_handeye.sh --mode eye_to_hand "
+                f"teach/merge/calibrate（标完再跑本流程）"
             )
             return Event.COARSE_FAIL
         if not self._wait_coarse_frame(ns):
@@ -1354,7 +1610,9 @@ class GaugeAssembly(AssemblyPipeline):
             return Event.TIMEOUT
         code, q, Tee, final = self._run_locate(q, Tee)
         if code != 0 or not final:
-            self.get_logger().error(f"S05 精拍定位失败 code={code}")
+            self.get_logger().error(
+                f"S05 精拍定位失败 code={code}{_LOCATE_CODE_HINT.get(code, '')}"
+            )
             return Event.TIMEOUT
         self._snap_final = dict(final)
         self._snap_pose = Tee.copy()
@@ -1535,6 +1793,9 @@ class GaugeAssembly(AssemblyPipeline):
         z0 = float(Tee[2, 3])
         step_m = self.cfg.contact_step_mm / 1000.0
         max_depth_m = self.cfg.contact_max_depth_mm / 1000.0
+        # 台肩落面所需的下压量。在下面几何自检里算出来；**要留到接触判据那里用**
+        # （看 hole_entry_verdict：下压深度远小于它 ⇒ 规没进孔）。
+        needed_m: Optional[float] = None
 
         # ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------
         # 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面
@@ -1663,6 +1924,11 @@ class GaugeAssembly(AssemblyPipeline):
                         f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N"
                         f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
                     )
+                    # 下压这么浅就"接触" ⇒ 尖端顶在工件上表面，没进孔。必须在这里
+                    # 拦下：放它进 S08 只会干转几轮、憋住，最后报一句"转轴偏差"
+                    # 把所有人引去查工具系。见 hole_entry_verdict 的说明。
+                    if self._hole_lost_guard(z0, Tee, needed_m, q, "腕部 FT"):
+                        return Event.HOLE_LOST
                     # 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限**
                     # 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限
                     # 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。
@@ -1709,6 +1975,9 @@ class GaugeAssembly(AssemblyPipeline):
                     f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
                 )
                 # 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。
+                # 同样先过"是不是根本没进孔"这道闸（见 hole_entry_verdict）。
+                if self._hole_lost_guard(z0, Tee, needed_m, q, "触觉切向"):
+                    return Event.HOLE_LOST
                 self._relieve_after_contact(q, Tee, "触觉切向", ft_ref)
                 return Event.CONTACT_OK
             if fn - self._fn_rest > self.cfg.contact_safe_delta:
@@ -1745,6 +2014,47 @@ class GaugeAssembly(AssemblyPipeline):
         self._retract(q, Tee, dz_m=0.01)
         return Event.TIMEOUT
 
+    def _hole_lost_guard(
+        self,
+        z0: float,
+        Tee: np.ndarray,
+        needed_m: Optional[float],
+        q: np.ndarray,
+        why: str,
+    ) -> bool:
+        """S07 判到接触时：下压深度是不是**远小于**几何预期（= 规没进孔）。
+
+        返回 True 表示"确实没进孔"，调用方要抬臂并返回 ``Event.HOLE_LOST``。
+
+        完整来龙去脉在 ``gauge_fsm.hole_entry_verdict`` 的 docstring 里，这里只
+        留现场要看的那几句：工件在 S08/S09/S10 会被工具推走，而 k2 的孔位是
+        S05 一次性定的 ⇒ 工件一动孔位就失效 ⇒ 规落在孔旁边的平面上。
+        不拦的话会干转 5 轮，最后报一句"转轴偏差…先确认工具系有没有装转接件"，
+        把所有人引去查工具系（实测 2026-09-28）。
+        """
+        if needed_m is None:
+            # 没算出几何预期（工件上表面 z 没标定且 S05 也没给）—— 没有判据就别猜。
+            return False
+        descended_m = abs(float(Tee[2, 3]) - z0)
+        if hole_entry_verdict(descended_m, needed_m) != ENTRY_HIGH:
+            return False
+        self.get_logger().error(
+            f"S07 接触（{why}）发生在下压 {descended_m * 1000:.2f}mm 处，"
+            f"而按几何「台肩落面」需要 {needed_m * 1000:.1f}mm —— 只走了 "
+            f"{descended_m / needed_m * 100.0:.0f}%。\n"
+            "  **规没有进孔，尖端顶在工件上表面**（正常入孔要压到台肩落面那一下）。\n"
+            "  最可能的原因：工件被工具推走了，而本孔的坐标是 **S05 一次性定下**的 ——\n"
+            "  工件一动，那个坐标就失效（S06 的「实测残差」发现不了：它比的是\n"
+            "  「命令位置 vs 实际位置」，不是「规尖 vs 真实孔位」）。\n"
+            "  所以**不要**去查工具系/转接件/力阈值（那几个都是好的）。\n"
+            "  正确处置：把臂退开、确认工件的实际位置；能复位就复位工件，然后\n"
+            "  从 S05 重新拍照定位（重跑流程即会重新定位）。\n"
+            "  若要治根：换孔前复核工件没动（当前只有 S05 阶段有 drag 看门狗）。\n"
+            "  抬起并中止，不带着失效的孔位进 S08。"
+        )
+        self._retract(q, Tee, dz_m=0.01)
+        return True
+
     def _relieve_after_contact(
         self,
         q: np.ndarray,
@@ -1879,6 +2189,29 @@ class GaugeAssembly(AssemblyPipeline):
     # =====================================================================
     # S08 旋转装配循环（棘轮）
     # =====================================================================
+    def _log_regrip(self, tag: str) -> None:
+        """把"这一轮夹紧后的开口"打出来（含相对夹住时的塌陷量）。
+
+        为什么每轮都要打（2026-09-24 实测）：
+            S08/S09 每轮都是"慢转 → 松爪 → 回转 → 夹紧"，而 `close` 在仿真里
+            是"位置控制走到全合 0mm" —— **真机走到力上限就停，仿真会一路挤进规里**。
+            只报一句"通止规掉了(E1)"时，人分不出"真掉了"和"被挤小了"。
+            把开口与塌陷量打出来一眼就能分辨：
+                · 塌陷量逐轮变大、开口还在十几毫米 → 是**被挤**（规还在指间）
+                · 开口塌到接近 0（两指合到底）        → 是**真掉了**
+        """
+        if self.grip is None:
+            return
+        ap = self.grip.settle(settle_s=1.0)   # 服务返回时动作还没做完，等它停
+        if ap is None:
+            return
+        gp = self.grip.grip_aperture()
+        col = self.grip.collapse_mm()
+        detail = ""
+        if gp is not None and col is not None:
+            detail = f"（夹住时 {gp:.2f}mm，塌陷 {col:+.2f}mm）"
+        self.get_logger().info(f"{tag}：开口 {ap:.2f}mm{detail}")
+
     def _step_screw_in(self) -> Event:
         hole = self._hole_name()
         self._state(f"S08_screw_in_{hole}")
@@ -1895,6 +2228,20 @@ class GaugeAssembly(AssemblyPipeline):
             f"停止判据「切向 >+{self.cfg.screw_shear_delta:.0f} "
             f"且法向 >+{self.cfg.screw_normal_delta:.0f}」"
         )
+        # 步长 vs 关节行程：**在这里算一次**，不要等第 3 轮贴限位再炸。
+        # 2026-09-24 实测：180° 的步长配 ±175° 的关节 7，第 3 轮 ABORT，
+        # 日志是一长串"贴限位 → 让余量 → 还是贴限位"，真因（配置）藏在最后。
+        travel = ratchet_travel_ok(
+            self.cfg.screw_step_deg, self._wait_joint_limits(), RATCHET_LIMIT_MARGIN_DEG
+        )
+        self.get_logger().info(
+            f"S08 行程核算：一步棘轮要 {travel.need_deg:.0f}°，"
+            f"关节7 行程 {travel.travel_deg:.0f}°"
+            f"（最大可用步长 {travel.max_step_deg:.0f}°）—— {'放得下' if travel.ok else '放不下'}"
+        )
+        if not travel.ok:
+            self.get_logger().error(f"S08 配置自检不通过（先改配置，别硬跑）：{travel.why}")
+            return Event.ABORT
         for cycle in range(self.cfg.screw_max_cycles):
             q, Tee = self._wait_state(5.0)
             if q is None or Tee is None:
@@ -1909,9 +2256,10 @@ class GaugeAssembly(AssemblyPipeline):
             # --- 四步原子动作，顺序固定（慢转 → 松爪 → 快回转 → 夹紧）---
             # 慢转是**顺时针**：与 S01 的逆时针预绕相反，这样预绕行程就是
             # 这一整段拧入的回转空间（S01 不预绕，转两圈就贴限位）。
-            ev = self._rotate_joint_and_verify(
+            ev = self._ratchet_step(
                 float(q[-1]) - self.cfg.screw_step_rad, tag=f"S08_slow_{cycle}",
                 fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
+                direction=-1,   # 顺时针拧入（与 S01 的预绕相反）
             )
             if ev is not Event.SCREW_DONE:
                 return ev
@@ -1924,15 +2272,17 @@ class GaugeAssembly(AssemblyPipeline):
                 q, Tee = self._wait_state(3.0)
                 if q is None:
                     return Event.TIMEOUT
-                ev = self._rotate_joint_and_verify(
+                ev = self._ratchet_step(
                     float(q[-1]) + self.cfg.screw_step_rad, tag=f"S08_back_{cycle}",
                     fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
+                    direction=+1,   # 回转：与慢转正好相反
                 )
                 if ev is not Event.SCREW_DONE:
                     return ev
                 if not self.grip.close():
                     self.get_logger().error("S08 夹紧失败 —— 下一轮慢转会打滑磨螺纹")
                     return Event.ABORT
+                self._log_regrip(f"S08 第 {cycle + 1} 轮夹紧后")
             else:
                 self.get_logger().error("S08 没有夹爪客户端，无法松爪/夹紧，中止")
                 return Event.ABORT
@@ -1961,9 +2311,17 @@ class GaugeAssembly(AssemblyPipeline):
                 self._progress(f"{hole} 拧入到位（{cycle + 1} 轮）")
                 return Event.SCREW_STOP
             if self.grip is not None and self.grip.dropped():
+                ap = self.grip.aperture()
+                gp = self.grip.grip_aperture()
                 self.get_logger().error(
                     "S08 通止规掉了（E1）—— 回 S01 流程：张开爪、提示人工放好，"
-                    "确认后继续（按看板：该孔记为完成，换下一个孔）"
+                    "确认后继续（按看板：该孔记为完成，换下一个孔）\n"
+                    f"        开口：夹住时 {gp if gp is None else f'{gp:.2f}mm'}"
+                    f" → 现在 {ap if ap is None else f'{ap:.2f}mm'}"
+                    f"（判据：塌陷 ≥ {GripperClient.DROP_COLLAPSE_MM:.1f}mm）\n"
+                    "        分辨真掉还是被挤：开口塌到接近 0 = 两指合到底（真掉）；\n"
+                    "        开口还有十几毫米、只是比夹住时小了几毫米 = 被**挤**"
+                    "（仿真位置控制的「全合」不知道「挡住就停」，见 GripperClient.dropped）"
                 )
                 self._e1_pending = True
                 return Event.GAUGE_DROPPED
@@ -1994,9 +2352,10 @@ class GaugeAssembly(AssemblyPipeline):
             if q is None:
                 return Event.TIMEOUT
             # 反向：慢转是逆时针（与 S08 相反），空手回转是顺时针
-            ev = self._rotate_joint_and_verify(
+            ev = self._ratchet_step(
                 float(q[-1]) + self.cfg.screw_step_rad, tag=f"S09_slow_{cycle}",
                 fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
+                direction=+1,   # 逆时针拧出
             )
             if ev is not Event.UNSCREW_DONE:
                 return ev
@@ -2008,17 +2367,64 @@ class GaugeAssembly(AssemblyPipeline):
             q, Tee = self._wait_state(3.0)
             if q is None:
                 return Event.TIMEOUT
-            ev = self._rotate_joint_and_verify(
+            ev = self._ratchet_step(
                 float(q[-1]) - self.cfg.screw_step_rad, tag=f"S09_back_{cycle}",
                 fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
+                direction=-1,   # 回转：与慢转相反
             )
             if ev is not Event.UNSCREW_DONE:
                 return ev
             if not self.grip.close():
                 return Event.ABORT
+            self._log_regrip(f"S09 第 {cycle + 1} 轮夹紧后")
+        self._lift_clear_of_hole(hole)
         self._progress(f"{hole} 已拧出")
         return Event.UNSCREW_DONE
 
+    def _lift_clear_of_hole(self, hole: str) -> None:
+        """拧出后**先竖直把规提出孔**，之后才允许任何横向动作。
+
+        **为什么必须有这一步**（2026-09-24 仿真实测，两次栽在同一个地方）：
+          1. 仿真里**没有螺纹几何**：S09 的"拧出"只是让规在孔里转，规**仍然插在
+             孔里**（真机会沿螺纹退出来几个毫米，也没退出多少）。
+          2. 于是紧跟着的横向动作（S10 换孔 / S11 回 home）会把规从孔里**拽**
+             出来 —— 实测抓取锁被拽断、规掉在台面上，而下游只表现为
+             "下一孔 S07 下探 35mm 判不到接触"（工具上已经没规了），
+             日志完全指不到真因。
+          3. 真机也怕这个动作：螺纹刚脱开时横向别住最容易啃伤螺纹。标准做法就是
+             **先竖直退出来，再走 XY**。
+
+        高度提到**跟踪高**（``_track_z()`` = 孔悬停高 + 50mm）而不是写死毫米数：
+        跟踪高是这条流程里"工具已离开工件"的既有定义（``_ensure_clearance``
+        也用它），拿它当判据不用再维护第二套口径。实测：不提出时规尖比工件面
+        **低 30mm**，而 S10 的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和
+        "干净退出"的分界，所以必须是**先后有序**的两个动作，不能靠斜线一起走。
+
+        失败只告警（不失败）：这一刻孔里已经没有规要保护了，硬失败会把一次成功
+        的装配变成失败；后面的 S10/S11 仍会走到安全位。
+        """
+        try:
+            q, Tee = self._wait_state(3.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.get_logger().info(
+                f"S09 {hole} 拧出后竖直提出：Z {z:.3f} → {tz:.3f}m"
+                "（先离开孔口，再做横向动作）"
+            )
+            _, q2, _Tee2 = 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=f"S09_exit_{hole}", v=self.move_speed, max_hops=8,
+                hold_xy=True, settle_s=0.25,
+            )
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"S09 {hole} 提出孔口失败（继续走 S10/S11）：{exc}")
+
     # =====================================================================
     # S10 换孔 / S11 复位
     # =====================================================================

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

@@ -254,7 +254,9 @@ DEFAULTS: Dict[str, Any] = {
     "contact_hold_n": 5.0,
     "contact_safe_delta": 2000.0,
     # S08
-    "screw_step_deg": 180.0,
+    # 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
+    # 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
+    "screw_step_deg": 120.0,
     "screw_speed_deg_s": 30.0,
     "screw_back_speed_deg_s": 120.0,
     "screw_max_cycles": 12,

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

@@ -79,6 +79,10 @@ class Event(str, Enum):
 
     CONTACT_OK = "contact_ok"
     FORCE_OVER_LIMIT = "force_over_limit"  # 力超安全上限 → 抬起
+    #: S07 判到接触、但**下压深度远小于几何预期** ⇒ 规没进孔，顶在工件上表面。
+    #: 单列一个事件是为了让结论与日志都是实话：复用 FORCE_OVER_LIMIT 会把这种
+    #: 情况报成"力超安全上限"，而它的力根本没超限 —— 又是一句误导。
+    HOLE_LOST = "hole_lost"
 
     SCREW_DONE = "screw_done"
     SCREW_STOP = "screw_stop"            # 力判据满足：切向力>阈值 且 法向力突增
@@ -158,6 +162,7 @@ TRANSITIONS: Dict[Tuple[State, Event], State] = {
     # --- S07 柔顺接触 ---
     (State.CONTACT, Event.CONTACT_OK): State.SCREW_IN,
     (State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT,
+    (State.CONTACT, Event.HOLE_LOST): State.ABORT,
     # --- S08 旋转装配循环 ---
     (State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW,
     (State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW,
@@ -406,6 +411,40 @@ def wrench_live(age_s: Optional[float]) -> bool:
     return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S
 
 
+#: 判定"六个分量全为 0"用的容差 [N]/[Nm]。真机 FT 的噪声量级是 1e-2~1e-1，
+#: 所以 1e-6 足够区分"真的是 0（数据没出来）"与"很小但活的读数"。
+WRENCH_ZERO_EPS: float = 1.0e-6
+
+
+def wrench_all_zero(
+    fx: float,
+    fy: float,
+    fz: float,
+    tx: float,
+    ty: float,
+    tz: float,
+    eps: float = WRENCH_ZERO_EPS,
+) -> bool:
+    """六个分量**同时**为 0（容差 ``eps``）⇒ 这是"数据没出来"的指纹，不是"安静"。
+
+    与 :func:`wrench_live` 的分工（两个都要有，缺一不可）：
+
+    * ``wrench_live`` 只看"够不够新"，**故意不看数值** —— 静止时轴向力本来就接近 0，
+      拿数值判活会把"安静的传感器"误判成掉线（2026-09-24 触觉上踩过）。
+    * 但一个**带着负载**的腕部 FT 不可能六个分量**同时**恰好为 0：重力、装配预紧、
+      传感器零漂至少让某个分量不为 0。所以"全零"是另一种失效 —— **数据源没出数**
+      （话题名对、但内容是空的；或 RDK 侧没启用该传感）。
+
+    2026-10-08 真机实测：应用在用的 ``/<sn>/ft_sensor_wrench`` 就是恒 (0,0,0)，
+    于是"``abs(fz−基线)>2.5N`` 判接触"的兜底**永远触发不了** —— 而日志里一切正常
+    （话题收得到、回调在跑、``wrench_live`` 报"活"），属于最危险的静默失效。
+    改用 ``/<sn>/external_wrench_in_tcp`` 后有数（≈负载 54N）即可恢复兜底。
+
+    所以：``wrench_live`` 通过 **且** ``wrench_all_zero`` 为真 ⇒ 判为"数据源是空的"。
+    """
+    return all(abs(float(v)) <= eps for v in (fx, fy, fz, tx, ty, tz))
+
+
 #: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
 #: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
 #: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
@@ -513,6 +552,209 @@ def tool_clearance_m(
     return tip_z - float(surface_z_m)
 
 
+def hover_z_for_tool_clearance(
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    clearance_m: float,
+) -> float:
+    """让工具尖端停在工件面上方 ``clearance_m`` 所需的**法兰**高度 [m]。
+
+    就是 ``tool_clearance_m`` 的逆运算 —— 写成一个函数是为了让"该用多高"这个
+    结论只有一个来源（下面 ``hover_z_guard`` 与调用方都用它，不各推一遍）。
+    """
+    return float(surface_z_m) + float(tool_tip_below_flange_m) + float(clearance_m)
+
+
+def hover_z_guard(
+    hover_z_m: float,
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    want_clearance_m: float,
+) -> Tuple[float, str]:
+    """二阶段悬停高度的自检 → ``(该用的悬停高度, 说明; 不用改时说明是空串)``。
+
+    **为什么必须有这条**（2026-09-24 实测栽在这上面）：
+
+        悬停高度是**按相机**定的（腕部相机要离工件面 ~0.17m 才看得清孔），
+        而工具（夹爪 + 通止规）可能比相机长得多 —— 仿真里实测
+        ``tool_tip_below_flange_m=0.245``，而悬停算的是
+        ``0.412 + 0.17 + 0.05 = 0.632`` ⇒ 工具尖端在工件上表面**下面 25mm**。
+
+        后果不在 S07（S07 有自己的几何自检），而在**更早的 S05**：
+        "悬停"这一步本身就把工具捅进工件里了，于是 S05 的横向对准等于
+        拿规尖去拨工件 —— 实测把工件从 x=0.520 拖到 0.4284（**92mm**）。
+        而真值孔位是按工件的**实测**位姿算的（``sim_live_pose``，这是对的），
+        工件一走，孔位就跟着工具走 ⇒ 残差恒定在 13mm、迭代 9 次永不收敛
+        （日志：``[fine 3..8] 残差=13.25/13.19/…/11.79mm``）。
+        现象看起来像"手眼标定坏了 / 检测不认孔"。
+
+    真机同参数同结果：这不是仿真专有，只要"悬停按相机定、工具比相机长"就会这样。
+
+    判据取"明显低于目标间隙"而不是"一毫米都不许低"：配置的悬停与实测的
+    工件面 z 都有几毫米的不确定度，拿 0 当界会把正常配置判成错的。
+    """
+    clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
+    if clear_m >= float(want_clearance_m) - 0.001:
+        return float(hover_z_m), ""
+    need_z = hover_z_for_tool_clearance(surface_z_m, tool_tip_below_flange_m, want_clearance_m)
+    why = (
+        f"悬停高度会让工具尖端钻进工件：法兰 {float(hover_z_m):.4f} − 工具长 "
+        f"{float(tool_tip_below_flange_m) * 1000:.1f}mm = 尖端 "
+        f"{float(hover_z_m) - float(tool_tip_below_flange_m):.4f}，工件上表面 "
+        f"{float(surface_z_m):.4f} → 尖端在面{'下' if clear_m < 0 else '上仅'} "
+        f"{abs(clear_m) * 1000:.1f}mm（要求 ≥ {float(want_clearance_m) * 1000:.1f}mm）。\n"
+        f"        悬停高度通常是按**相机**视距定的，而工具比相机长 —— 两者冲突时必须"
+        f"先保证不碰工件。\n"
+        f"        已按几何抬到 {need_z:.4f}（= {float(surface_z_m):.3f} 上表面 + "
+        f"{float(tool_tip_below_flange_m) * 1000:.0f}mm 工具 + "
+        f"{float(want_clearance_m) * 1000:.1f}mm 间隙）。\n"
+        f"        要固定这个值：改 site.yaml 的 robots[<sn>].servo.hole_hover_z_m = "
+        f"{need_z:.3f}（以及同组件的 ee_z_m ≥ 它 +0.05）。"
+    )
+    return need_z, why
+
+
+def resolve_hover_z(
+    hover_z_m: float,
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    want_clearance_m: float,
+    user_set: bool,
+) -> Tuple[float, str]:
+    """返回 ``(要用的法兰悬停高, 拒绝原因)``。
+
+    启动参数写了高度：不安全就拒绝，原因非空，高度原样返回，调用方不许改成别的数。
+    没写（用 site）：沿用 :func:`hover_z_guard` 的几何抬高，拒绝原因留空。
+    """
+    fixed, why = hover_z_guard(
+        hover_z_m, surface_z_m, tool_tip_below_flange_m, want_clearance_m
+    )
+    if not why:
+        return float(hover_z_m), ""
+    if not user_set:
+        return float(fixed), ""
+    clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
+    return float(hover_z_m), (
+        f"悬停高度会让工具尖端进入工件：法兰 {float(hover_z_m):.4f} − 工具长 "
+        f"{float(tool_tip_below_flange_m) * 1000:.1f}mm，工件面 {float(surface_z_m):.4f}，"
+        f"间隙 {clear_m * 1000:.1f}mm，要求 ≥ {float(want_clearance_m) * 1000:.1f}mm。"
+        "这个值是启动参数指定的，不会改成别的高度。"
+    )
+
+
+#: S05 精对位时判"工件被工具推走"的两级阈值 [m]。
+#
+#   为什么需要这条判据（2026-09-24 又栽了一次，见下面 drag_verdict 的说明）：
+#       S05 的对准是"看着孔走"的闭环，而孔位是**工件上**的。工具一旦碰到工件，
+#       闭环追的就不再是孔，而是"自己推着走的工件" —— 残差恒定（本轮 11mm）、
+#       迭代用满，日志报的是"检查手眼/相机安装"，与真因（工具比相机长、
+#       悬停时已经插进工件）差着十万八千里。
+#   取 3mm：工件是刚体，两拍之间它整体挪动 >3mm 只可能是被推/被撞（正常加工
+#   过程里它一动不动）。同一对孔的两拍测量噪声在 0.2mm 量级，离阈值很远。
+SNAP_DRAG_TOL_M: float = 0.003
+#: 单拍就这么大 ⇒ 不必等第二拍确认（刚体不会自己"挪回去"）。
+SNAP_DRAG_HARD_M: float = 0.006
+
+
+#: "已经到位"的容差：避免 ±1e-9 的浮点噪声变成"再转一整圈"。
+_RATCHET_FULL_TURN_EPS: float = 1e-9
+
+
+def ratchet_delta_rad(target_rad: float, q_now_rad: float, direction: int = 0) -> float:
+    """"目标角 − 当前角"，折成**一次**能转到的增量 [rad]。
+
+    **为什么要有这个函数**（2026-09-24 实测：棘轮把整条流程 ABORT 在 S09 第一步）：
+
+        S08/S09 给棘轮的是**关节 7 的目标角（绝对值）**（``q[-1] ± step``），
+        而棘轮此前把它当**增量**用。于是"关节在 +158.9°、目标 +338.9°"被算成
+        "要转 338.9°"（实际增量只有 180°）⇒ 误判行程不够 ⇒ 让余量那一步算到
+        限位外面（``158.9 − 338.9 = −180°``，比 −175° 还低）⇒ 报
+        "期望 −338.9°、实测 +26.0°，偏差过大" ⇒ ABORT。现场什么坏事都没发生。
+
+        差值必须折一圈：目标常常**故意**超出限位（这正是要"让余量"的原因），
+        ``+338.9°`` 与 ``−21.1°`` 是同一个关节位置。
+
+    **为什么还要 direction**（2026-09-24 又一次实测：S08 第 3 轮 ABORT）：
+
+        不给方向时这里取"最短的那条路"，而名义步长**正好是 180°** 时，
+        哪条路更短取决于 0.1° 级的读数差：调用方按自己读到的 ``q`` 算出目标
+        ``−18.0°``，棘轮再用**稍晚一点**读到的 ``q=+162.1°`` 去算差值，
+        ``−180.1°`` 折一圈就成了 ``+179.9°`` —— 方向**翻了个个儿**。
+        于是棘轮朝螺纹的反方向走，接着贴限位、让余量、重试，净位移 0 ⇒ ABORT。
+
+        棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给
+        "谁更短"这种数值巧合决定，所以由调用方明确给出 ``direction``。
+
+        注：``direction=0``（默认）保留旧的"最短路径"语义，给"目标本来就明确
+        在某一侧"的调用方用；棘轮的四个原子步一律显式给方向。
+
+    :param target_rad: 目标角（绝对值，可以是限位外的角度）。
+    :param q_now_rad: 当前关节角。
+    :param direction: 想要的转向：``+1`` 正转、``-1`` 反转、``0`` 取最短路径。
+    :returns: 增量 [rad]。``direction=0`` 时落在 ``(−π, π]``；给了方向时落在
+        ``[0, 2π)``（正转）或 ``(−2π, 0]``（反转）——**一定**沿指定方向，
+        且是能落到目标角的最小那一步。**同一个目标、同一个当前位置**只会有一个
+        增量 —— 棘轮的"让余量"与"重试"都按它算，两次不会互相矛盾。
+    """
+    base = float(target_rad) - float(q_now_rad)
+    two_pi = 2.0 * math.pi
+    if direction == 0:
+        return math.remainder(base, two_pi)
+    # 先折到 [0, 2π)，再按想要的转向取那一侧（与目标角差正好一整圈，是同一个位置）。
+    pos = math.fmod(base, two_pi)
+    if pos < 0.0:
+        pos += two_pi
+    if pos >= two_pi - _RATCHET_FULL_TURN_EPS:
+        pos = 0.0  # 已经（几乎）在目标上：不要为了凑方向白转一整圈
+    if direction > 0:
+        return pos
+    return pos - two_pi if pos > 0.0 else 0.0
+
+
+
+def drag_verdict(
+    mid_now: Tuple[float, float],
+    mid_prev: Optional[Tuple[float, float]],
+    mid_prev2: Optional[Tuple[float, float]] = None,
+    *,
+    tol_m: float = SNAP_DRAG_TOL_M,
+    hard_m: float = SNAP_DRAG_HARD_M,
+) -> Tuple[bool, float]:
+    """两孔中点是否在两拍之间"整体挪动" → ``(是否判定为被推走, 本拍位移 m)``。
+
+    **为什么看"中点"而不是看目标孔**：目标孔是**追**的对象，工具一动它就该动
+    （对准本意如此），看它分不出"正常对准"和"推着工件走"。而两孔**中点**在
+    对准过程中不该动 —— 它只有工件整体平移时才会动。所以中点位移干净地对应
+    "工件被搬动了"。
+
+    **为什么看"两孔中点"而不是单孔**：单孔在相机里会因分辨率/遮挡抖动，且
+    目标孔本来就在被逼近；中点是两者的平均，噪声更小、语义更明确。
+
+    :param mid_now: 本拍两孔中点 (x, y)[m]（基座系）。
+    :param mid_prev: 上一拍中点；``None`` = 还没有上一拍（第一拍不判）。
+    :param mid_prev2: 上上拍中点。用来确认"位移是持续的"（单拍可能是误检）。
+    :returns: ``(判定, 本拍位移 m)``。
+    """
+    if mid_prev is None:
+        return False, 0.0
+    dx = float(mid_now[0]) - float(mid_prev[0])
+    dy = float(mid_now[1]) - float(mid_prev[1])
+    d1 = math.hypot(dx, dy)
+    if d1 >= float(hard_m):
+        return True, d1
+    if d1 < float(tol_m) or mid_prev2 is None:
+        return False, d1
+    px = float(mid_prev[0]) - float(mid_prev2[0])
+    py = float(mid_prev[1]) - float(mid_prev2[1])
+    d2 = math.hypot(px, py)
+    # 第二拍确认：上一拍也挪了（≥ 阈值的一半），且方向一致（点积 > 0）。
+    # 为什么允许方向只"大致一致"：工件被推着走时前后两拍的推动量本来就不同
+    # （工件一让开，工具就少推一点），拿夹角严格相等会漏判。
+    if d2 >= float(tol_m) * 0.5 and (dx * px + dy * py) > 0.0:
+        return True, d1
+    return False, d1
+
+
 def descend_needed_m(
     flange_z_m: float,
     tool_tip_below_flange_m: float,
@@ -555,6 +797,58 @@ def descend_budget_m(needed_m: float) -> float:
     return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M)
 
 
+#: ``hole_entry_verdict`` 的结论。
+ENTRY_OK = "ok"        # 接触深度与几何预期相符 ⇒ 规是从孔里下去的、台肩落面
+ENTRY_HIGH = "high"    # 早早就接触 ⇒ 尖端顶在工件**上表面**上，根本没进孔
+
+#: 判"没进孔"的下压深度比例阈值。
+#: 实测两个锚点（2026-09-28，仿真）：
+#:   · 正常入孔：下压 28.6mm / 需要 31.9mm ⇒ 0.90
+#:   · 顶在平面上：下压 0.90mm / 需要 31.9mm ⇒ 0.03
+#: 两者差着一个台肩高度（30mm），取 0.5 时两侧余量都很大（正常那侧要掉到
+#: 一半才会误判）。**不要**为了"更灵敏"把它往 0.9 推 —— 台肩倒角没完全落面、
+#: 板厚公差都会让正常值在 0.85~0.95 之间晃。
+HOLE_ENTRY_MIN_RATIO: float = 0.5
+
+
+def hole_entry_verdict(
+    descended_m: float,
+    needed_m: float,
+    *,
+    min_ratio: float = HOLE_ENTRY_MIN_RATIO,
+) -> str:
+    """S07 判到接触的那一刻：规是**进了孔**、还是**顶在工件上表面**。
+
+    为什么需要它（2026-09-28 实测，同一命令三跑里坏一跑：DONE / ABORT / DONE）：
+      工件在 S08/S09/S10 阶段会被工具推走（仿真里实测被推 20~52mm）。而 k2 的
+      孔位是 **S05 一次性定下**的（``_step_hover_k`` 取 ``_hole_xy``），工件一动
+      它就失效。于是规落在孔**旁边的平面**上，只下压「悬停间隙」那么点就判到接触。
+
+    不拦的代价，实测完整发生过一次：
+      S08 干转 5 轮（切向增量 0.0、法向增量 ±0.0，**一点没进给**）→ 第 6 轮憋住 →
+      报出来的是「转轴偏差 7.6° 超过阈值 —— 关节7 轴与工具 Z 不共线，
+      先确认工具系是否垂直向下、有没有装转接件」。
+
+      **那句话会把所有人引到错误方向**：真因既不是工具系也不是转接件，是孔位失效。
+      而且为这句误导，白烧了 5 轮无效拧入（≈30 秒）和一次憋劲（有把规顶掉的风险）。
+
+    为什么比 ``needed_m`` 而不是比"尖端离面高度"：正常入孔时接触发生在**台肩落面**
+    那一刻 ≈ ``needed_m``；顶在平面上时接触发生在**尖端刚碰到面**那一刻，而台肩还在
+    尖端上方 30mm。两者差着一个台肩高度，所以用比例判最稳（见上面两个实测锚点）。
+
+    纯函数（R5.1）：判错两个方向都贵 —— 把正常的判成"没进孔"会白扔一整个孔；
+    把"没进孔"判成正常，就是上面那 30 秒 + 一次误导。
+
+    ``needed_m <= 0`` 时**不判**（几何本身已经错了，上面那条自检会报），
+    返回 ``ENTRY_OK`` 不在这里抢着报第二个错。
+    """
+    if needed_m <= 0.0:
+        return ENTRY_OK
+    if float(descended_m) < float(needed_m) * float(min_ratio):
+        return ENTRY_HIGH
+    return ENTRY_OK
+
+
 def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
     """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
 
@@ -584,6 +878,115 @@ def ratchet_cycles(total_deg: float, step_deg: float) -> int:
     return int(math.ceil(total_deg / step_deg))
 
 
+@dataclass(frozen=True)
+class RatchetTravel:
+    """棘轮步长与关节行程的**可用性核算**（S08 开头自检，别等到第 3 轮才炸）。"""
+
+    ok: bool
+    step_deg: float          # 配置的一步转角
+    travel_deg: float        # 关节行程（上限 − 下限）
+    need_deg: float          # 一步棘轮真正需要的行程
+    max_step_deg: float      # 行程允许的最大步长
+    why: str                 # 不 ok 时给人看的说明（ok 时为空串）
+
+
+def ratchet_travel_ok(
+    step_deg: float, limits_rad: Optional[Tuple[float, float]], margin_deg: float
+) -> RatchetTravel:
+    """一步棘轮 ``step_deg`` 放得进关节行程吗？
+
+    **为什么需要**：2026-09-24 实测，``screw_step_deg=180`` 配 Rizon4 的
+    关节 7（±175°，行程 350°）在第 3 轮 ABORT —— 慢转那一步要转 180°、
+    回转那一步还要 180°，两次加起来 360° > 350°，关节**必然**有一头贴限位；
+    更糟的是"让余量"也腾不出地方（要腾就得往反向再让一整步，还是 360°）。
+    日志里那一长串"贴限位 → 让余量 → 还是贴限位"就是这么来的。
+
+    判据取 ``2×step + 2×margin ≤ 行程``：
+    - 2×step：棘轮一个来回（慢转 + 回转）要的行程；
+    - 2×margin：两头都要留余量 —— 贴死限位再让余量时**一点空间都没有**，
+      而实测每轮回转还会比指令少走 4~5°（速度/加减速限制），
+      这点亏欠要靠余量吃，否则 7~8 轮后累计偏出限位、再也转不回来。
+
+    :param limits_rad: ``(lower, upper)`` [rad]；``None`` = 拿不到限位，放行
+        （返回 ``ok=True``，理由写清楚：真机靠驱动器报 JOINT_LIMIT 兜底）。
+    """
+    need = 2.0 * float(step_deg) + 2.0 * float(margin_deg)
+    if limits_rad is None:
+        return RatchetTravel(
+            ok=True, step_deg=float(step_deg), travel_deg=float("inf"),
+            need_deg=need, max_step_deg=float("inf"),
+            why="",
+        )
+    travel = math.degrees(float(limits_rad[1]) - float(limits_rad[0]))
+    max_step = 0.5 * (travel - 2.0 * float(margin_deg))
+    if need <= travel:
+        return RatchetTravel(
+            ok=True, step_deg=float(step_deg), travel_deg=travel,
+            need_deg=need, max_step_deg=max_step, why="",
+        )
+    # 给个能直接照着改的步长：往整十度取整（现场习惯填 120/150/180），
+    # 但不许把步长压到 30° 以下 —— 那样拧进/拧出的轮数会多到失控。
+    suggest = max(30.0, math.floor(max_step / 10.0) * 10.0)
+    why = (
+        f"棘轮一步 {step_deg:.0f}° 放不进关节 7 的行程：一步棘轮要 "
+        f"2×{step_deg:.0f}° + 2×{margin_deg:.0f}°(余量) = {need:.0f}°，"
+        f"而行程只有 {travel:.0f}°（上限 {math.degrees(limits_rad[1]):.0f}° − "
+        f"下限 {math.degrees(limits_rad[0]):.0f}°）。\n"
+        f"        改法：site.yaml 的 gauge_assembly.screw_step_deg 改到 "
+        f"{max_step:.0f}° 以内（例如 {suggest:.0f}°），并保证 "
+        f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍**（"
+        f"累计必须转整数圈，否则规停在螺纹中间、S09 也就少拧出几分之一圈）。"
+    )
+    return RatchetTravel(
+        ok=False, step_deg=float(step_deg), travel_deg=travel,
+        need_deg=need, max_step_deg=max_step, why=why,
+    )
+
+
+#: ``rotation_verdict`` 的结论。
+ROT_OK = "ok"
+ROT_LIMIT = "limit"   # 转了一部分、方向对、但没走完 ⇒ 最可能是贴到关节限位
+ROT_AXIS = "axis"     # 转轴与工具 Z 不共线 ⇒ 以「度」为单位的一切动作都不成立
+ROT_SHORT = "short"   # 其它偏差（方向不对/过冲）⇒ 真有问题，该停
+
+
+def rotation_verdict(
+    delta_deg: float,
+    got_deg: float,
+    mis_deg: float,
+    tol_deg: float,
+) -> str:
+    """绕工具 Z 转 ``delta_deg``、实测转 ``got_deg`` 的验收判据。
+
+    **纯函数**：这条判据 2026-09-24 把整条 S09 判死过一次
+    （"期望 +180.0°、实测 +16.4°，偏差过大" → ABORT），而真因只是
+    **关节7 的行程被前面的棘轮回转顶到了上限**（±175°，而棘轮一步就要 180°）。
+    判错两个方向都贵：
+      * 把"贴限位"判成"short" ⇒ 白扔一整轮（还可能把规撞歪）；
+      * 把"真有问题"判成"limit" ⇒ 该停的没停，继续拧。
+
+    还有一条**曾经漏判**的：只比绝对值时，"让它 +180、它 -170"会算成
+    10° 误差、落在 10% 容差里 ⇒ 判成功，而实际上一个孔都没拧。所以方向必须比。
+    所以它必须能脱离节点被钉住。
+    """
+    if abs(got_deg) < 3.0 and abs(delta_deg) > 10.0:
+        return ROT_LIMIT          # 几乎没动：与下面的"只走了一部分"同因不同余量
+    if mis_deg > tol_deg:
+        return ROT_AXIS
+    if abs(got_deg) >= 3.0 and abs(delta_deg) >= 3.0 and got_deg * delta_deg < 0:
+        # 方向反了。这条以前**漏判**：老判据只比绝对值
+        # （``abs(abs(got) - abs(delta))``），所以"让它 +180、它 -170"会算 10° 误差
+        # 落在 10% 容差里 → 判成功，而实际上一个孔都没拧。
+        return ROT_SHORT
+    slack = max(tol_deg, 0.1 * abs(delta_deg))
+    short = abs(delta_deg) - abs(got_deg)
+    if short > slack:
+        return ROT_LIMIT          # 方向对、只是没走完 ⇒ 被硬止点截住
+    if short < -slack:
+        return ROT_SHORT          # 过冲：转多了，同样不该当成到位
+    return ROT_OK
+
+
 def is_joint_limit_error(text: str) -> bool:
     """S01：把驱动/规划报的字符串认成"撞到关节极限"。
 
@@ -646,6 +1049,7 @@ EVENT_DOC: Dict[Event, str] = {
     Event.ALIGN_OUT: "对位超差",
     Event.CONTACT_OK: "已接触工件面",
     Event.FORCE_OVER_LIMIT: "力超安全上限",
+    Event.HOLE_LOST: "孔位失效（规没进孔）",
     Event.SCREW_DONE: "达到目标圈数",
     Event.SCREW_STOP: "力判据满足（咬住螺纹）",
     Event.GAUGE_DROPPED: "通止规掉了",

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

@@ -26,6 +26,9 @@ import numpy as np
 #: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
 DEFAULT_LIMIT_MARGIN_DEG = 3.0
 
+#: 厂家 A7。``Robot::SendJointPosition`` 的关节向量按 A1..A7 排列，A7 是最后一轴。
+A7_INDEX = 6
+
 _JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
 _ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
 
@@ -178,6 +181,43 @@ def prewind(
     )
 
 
+def a7_limit_target(
+    q: Sequence[float],
+    limits: Optional[Tuple[float, float]],
+    direction: int,
+    margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
+) -> Optional[np.ndarray]:
+    """只把 A7 收到软件限位内侧。前六轴原样拷回，不许在这里改。
+
+    厂家接口是 ``Robot::SendJointPosition``（模式 ``NRT_JOINT_POSITION``）。
+    向量按 A1..A7 排，A7 是下标 ``A7_INDEX``。限位是 ``RobotInfo::q_min/q_max``
+    的那一对，真机上与 URDF ``joint7`` 的 lower/upper 是同一组数。
+
+    ``direction`` 与 ``prewind`` 相同：``+1`` 关节角增大 = 逆时针，目标取上限；
+    ``-1`` 关节角减小 = 顺时针，目标取下限。现场若发现正负反了，改
+    ``site.yaml`` 的 ``extreme_direction``，不要改这里的符号。
+
+    没有限位、方向不是 ±1、或者余量把上下限挤没了，返回 None。
+    调用方必须停，不许猜一个角度发出去。
+    """
+    if direction not in (-1, 1):
+        raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
+    arr = np.asarray(q, dtype=float).reshape(-1)
+    if arr.size != 7:
+        raise ValueError(f"A7 目标要 7 个关节角，收到 {arr.size}")
+    if limits is None:
+        return None
+    lo = float(limits[0])
+    up = float(limits[1])
+    margin = math.radians(float(margin_deg))
+    if not math.isfinite(lo) or not math.isfinite(up) or up - lo <= 2.0 * margin:
+        return None
+    target = (up - margin) if direction > 0 else (lo + margin)
+    out = arr.copy()
+    out[A7_INDEX] = target
+    return out
+
+
 def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
     """从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
 

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

@@ -39,6 +39,8 @@ from arm_application.site_io import (
     joint_prefix_default,
 )
 
+from arm_application.ros_node import TolerantNode
+
 
 
 def _load_ee_cfg() -> dict:
@@ -78,7 +80,7 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
     return pt
 
 
-class GotoEePlane(Node):
+class GotoEePlane(TolerantNode):
     def __init__(self, cfg: dict) -> None:
         super().__init__("goto_ee_plane")
         self.cfg = cfg

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

@@ -33,8 +33,11 @@ from arm_application.handeye_lib import (
     BoardConfig,
     CameraIntrinsics,
     ChessboardStabilizer,
+    T_from_flexiv_tcp,
     calibrate_hand_eye,
+    cam_to_flange_via_tool,
     detect_chessboard,
+    format_matrix_yaml,
     format_transform_yaml,
     is_pose_filled,
     pose_to_Rt,
@@ -50,6 +53,8 @@ from arm_application.site_io import joint_prefix_default
 from arm_msgs.action import FollowJointTrajectory
 from arm_msgs.srv import GetArmState
 
+from arm_application.ros_node import TolerantNode
+
 DEFAULT_JOINTS = [
     'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
 ]
@@ -79,7 +84,7 @@ def _camera_info_to_intrinsics(msg: CameraInfo) -> CameraIntrinsics:
     return CameraIntrinsics(K=K, dist=dist)
 
 
-class HandeyeCalibrate(Node):
+class HandeyeCalibrate(TolerantNode):
     def __init__(self) -> None:
         super().__init__('handeye_calibrate')
 
@@ -111,11 +116,10 @@ class HandeyeCalibrate(Node):
 
         # 棋盘（可自定义）
         # 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
-        # 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
+        # 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
-        # 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
+        # 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
-        # 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
+        self.declare_parameter('board_cols', 11)  # 内角点
-        self.declare_parameter('board_cols', 8)  # 内角点
+        self.declare_parameter('board_rows', 8)
-        self.declare_parameter('board_rows', 11)
         self.declare_parameter('square_size_m', 0.015)
 
         self.declare_parameter('stable_window', 8)
@@ -136,6 +140,11 @@ class HandeyeCalibrate(Node):
         self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
         self.declare_parameter('result_file', '')
         self.declare_parameter('cleanup_images', True)
+        # 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
+        # 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
+        self.declare_parameter('gripper_tool', '')
+        self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
+        self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
 
         prefix = str(self.get_parameter('joint_prefix').value or '')
         base = list(
@@ -150,6 +159,21 @@ class HandeyeCalibrate(Node):
         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.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
+        tcp_xyz = list(
+            self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
+            or [0.0, 0.0, 0.0]
+        )
+        tcp_q = list(
+            self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
+            or [1.0, 0.0, 0.0, 0.0]
+        )
+        self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
+        if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
+            raise RuntimeError(
+                f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
+                '等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
+            )
 
         self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
         self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
@@ -263,10 +287,12 @@ class HandeyeCalibrate(Node):
         self.images_dir.mkdir(parents=True, exist_ok=True)
 
         eye_to_hand = self.mode == 'eye_to_hand'
+        grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
         self.get_logger().info(
             f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
             f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
-            f'方格={self.board.square_size}m，v={self.velocity_scale}'
+            f'方格={self.board.square_size}m，v={self.velocity_scale} '
+            f'gripper_tool={grip}'
         )
         self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
 
@@ -366,12 +392,58 @@ class HandeyeCalibrate(Node):
 
         self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
         R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
-        name = (
+        extra = ''
-            'T_cam_to_base (eye_to_hand)'
+        if eye_to_hand:
-            if eye_to_hand
+            name = 'T_cam_to_base (eye_to_hand)'
-            else 'T_cam_to_gripper (eye_in_hand)'
+            yaml_txt = format_transform_yaml(R, t, name)
-        )
+            if self.gripper_tool:
-        yaml_txt = format_transform_yaml(R, t, name)
+                extra = (
+                    f'gripper_tool: {self.gripper_tool}\n'
+                    + format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
+                )
+                self.get_logger().info(
+                    f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
+                    f'T_4x4 是 cam→基座，不折  t={t.reshape(3)*1000} mm'
+                )
+        elif self.gripper_tool:
+            T_cam_tool = np.eye(4)
+            T_cam_tool[:3, :3] = R
+            T_cam_tool[:3, 3] = t.reshape(3)
+            T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
+            delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
+            tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
+            if tool_len > 0.05 and delta < 1e-4:
+                self.get_logger().error(
+                    'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
+                    '折算没生效。结果作废。'
+                )
+                return 11
+            # 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
+            if (
+                self.T_flange_to_tool[2, 3] > 0.05
+                and float(T_cam_flange[2, 3]) < 0.0
+            ):
+                self.get_logger().error(
+                    f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
+                    '在法兰后方，乘序反了。结果作废。'
+                )
+                return 12
+            R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
+            name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
+            yaml_txt = format_transform_yaml(R, t, name)
+            extra = (
+                f'gripper_tool: {self.gripper_tool}\n'
+                + format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
+                + format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
+            )
+            self.get_logger().info(
+                f'gripper={self.gripper_tool}：'
+                f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
+                f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
+            )
+        else:
+            name = 'T_cam_to_gripper (eye_in_hand)'
+            yaml_txt = format_transform_yaml(R, t, name)
         header = (
             f'# 手眼标定结果\n'
             f'# mode: {self.mode}\n'
@@ -381,8 +453,20 @@ class HandeyeCalibrate(Node):
             f'# board: {self.board.cols}x{self.board.rows} '
             f'square_m={self.board.square_size}\n'
         )
+        if self.gripper_tool and eye_to_hand:
+            header += (
+                f'# gripper_tool: {self.gripper_tool}\n'
+                f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
+                f'# 旧结果备份在 results/backup/\n'
+            )
+        elif self.gripper_tool:
+            header += (
+                f'# gripper_tool: {self.gripper_tool}\n'
+                f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
+                f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
+            )
         self.result_file.parent.mkdir(parents=True, exist_ok=True)
-        self.result_file.write_text(header + yaml_txt, encoding='utf-8')
+        self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
         (self.output_dir / 'captures.json').write_text(
             json.dumps(meta, indent=2), encoding='utf-8'
         )

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

@@ -126,6 +126,39 @@ def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
     return T
 
 
+def cam_to_base_T(
+    T_handeye: np.ndarray, T_ee: np.ndarray, mode: str
+) -> np.ndarray:
+    """相机系 → 基座系的 4×4。
+
+    * ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。
+    * ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把
+      固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。
+    """
+    T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4)
+    tag = str(mode or "").strip().lower()
+    if tag == "eye_to_hand":
+        return T_he
+    if tag != "eye_in_hand":
+        raise ValueError(
+            f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}"
+        )
+    return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he
+
+
+def cam_point_to_base(
+    p_cam: np.ndarray,
+    T_handeye: np.ndarray,
+    T_ee: np.ndarray,
+    mode: str,
+) -> np.ndarray:
+    """工件三维点（相机系）→ 基座系 XYZ。"""
+    T_bc = cam_to_base_T(T_handeye, T_ee, mode)
+    xyz = np.asarray(p_cam, dtype=float).reshape(3)
+    ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
+    return (T_bc @ ph)[:3]
+
+
 def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
     Ri = R.T
     ti = -Ri @ t.reshape(3, 1)
@@ -309,6 +342,38 @@ def calibrate_hand_eye(
     return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
 
 
+def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
+    """Studio ``tcp_location`` → 法兰→工具 4×4。
+
+    Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
+    和本仓库其余地方的 xyzw 不一样。只在这里换一次。
+    """
+    xyz = np.asarray(xyz_m, dtype=float).reshape(3)
+    q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
+    if q.size == 7:
+        qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
+    else:
+        qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
+    R = quat_xyzw_to_R((qx, qy, qz, qw))
+    return Rt_to_T(R, xyz.reshape(3, 1))
+
+
+def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
+    """AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
+
+    ``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
+    OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
+    所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
+
+    乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
+    扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
+    两种都不报错。
+    """
+    T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
+    T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
+    return T_ft @ T_ct
+
+
 def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
     T = Rt_to_T(R, t)
     lines = [
@@ -321,6 +386,14 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
     return '\n'.join(lines) + '\n'
 
 
+def format_matrix_yaml(T: np.ndarray, key: str) -> str:
+    arr = np.asarray(T, dtype=float).reshape(4, 4)
+    lines = [f'{key}:']
+    for row in arr:
+        lines.append('  - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
+    return '\n'.join(lines) + '\n'
+
+
 def is_pose_filled(joints: Sequence[float]) -> bool:
     """若全为零 / NaN 占位则返回 False。"""
     arr = np.asarray(joints, dtype=float)

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

@@ -37,6 +37,8 @@ import rclpy
 from rcl_interfaces.msg import ParameterDescriptor
 from rclpy.node import Node
 
+from arm_application.ros_node import TolerantNode, param_as_str
+
 HELP = """
 ================================================
  手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
@@ -94,14 +96,6 @@ def _setup_matplotlib_font():
     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:
     """通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
 
@@ -128,6 +122,8 @@ class RdkBridge:
             'PATH': f'{Path(py).parent}:/usr/bin:/bin',
             'LD_LIBRARY_PATH': flexiv_lib,
             'LANG': 'C.UTF-8',
+            'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
+            'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
         }
         # 不要继承 conda 的 PYTHONPATH。
         # 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
@@ -240,7 +236,7 @@ class RdkBridge:
             pass
 
 
-class HandeyeTeach(Node):
+class HandeyeTeach(TolerantNode):
     def __init__(self) -> None:
         super().__init__('handeye_teach')
         self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
@@ -250,7 +246,7 @@ class HandeyeTeach(Node):
         self.declare_parameter('backend', 'ros')  # ros | rdk
         self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
         self.declare_parameter('drag_primitive', 'FloatingJoint')  # or FloatingCartesian
-        self._serial = _param_as_str(self, 'camera_serial', '')
+        self._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)
@@ -260,6 +256,9 @@ class HandeyeTeach(Node):
         # 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
         # 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
         self.declare_parameter('target_points', 0)
+        # 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
+        # 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
+        self.declare_parameter('poses_file', '')
 
         self.backend = str(self.get_parameter('backend').value).strip().lower()
         self.robot_sn = str(self.get_parameter('robot_sn').value)
@@ -267,8 +266,12 @@ class HandeyeTeach(Node):
 
         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'
+        _pf = str(self.get_parameter('poses_file').value).strip()
+        self.poses_file = (
+            Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
+        )
         self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
+        self._last_capture_t = 0.0
         self.images_dir.mkdir(parents=True, exist_ok=True)
 
         self._captures: List[dict] = []
@@ -537,6 +540,20 @@ class HandeyeTeach(Node):
             if q is None:
                 self.get_logger().error('读取关节失败')
                 return
+            now = time.monotonic()
+            if now - self._last_capture_t < 0.8:
+                self.get_logger().warn('按太快，忽略（防 Enter 连发）')
+                return
+            if self._captures:
+                last = self._captures[-1]['joints']
+                delta = max(abs(a - b) for a, b in zip(q, last))
+                if delta < 0.015:
+                    self.get_logger().warn(
+                        f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
+                        '先拖到另一个姿态再按 Enter。'
+                    )
+                    return
+            self._last_capture_t = now
 
             idx = len(self._captures)
             stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')

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

@@ -41,6 +41,8 @@ from arm_application.site_io import joint_prefix_default
 from arm_msgs.action import FollowJointTrajectory
 from arm_msgs.srv import GetArmState
 
+from arm_application.ros_node import TolerantNode
+
 try:
     import cv2
 except Exception:  # noqa: BLE001
@@ -110,7 +112,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
     raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
 
 
-class HandeyeWaypoints(Node):
+class HandeyeWaypoints(TolerantNode):
     def __init__(self) -> None:
         super().__init__('handeye_waypoints')
         self.declare_parameter('joint_prefix', joint_prefix_default())

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

@@ -134,6 +134,67 @@ def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
     return None
 
 
+def _keypoint_holes(
+    dets,
+    *,
+    conf_thr: float,
+    min_kconf: float,
+    radius_px: float,
+    min_dist_px: float,
+):
+    """pose 模型直接给出的 k1/k2 关键点当孔位。
+
+    现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。
+    返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None
+    （不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。
+
+    为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆，
+    光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里
+    直接回归出来的，对这几类干扰天然更稳。
+    """
+    if not dets:
+        return None
+    for det in sorted(dets, key=lambda d: d.conf, reverse=True):
+        if float(det.conf) < conf_thr:
+            continue
+        kpts = getattr(det, "kpts", None)
+        if kpts is None:
+            continue
+        kpts = np.asarray(kpts, dtype=float)
+        if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3:
+            continue
+        k1, k2 = kpts[1].copy(), kpts[2].copy()
+        if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()):
+            continue
+        if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf:
+            logger.info(
+                "k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆",
+                float(k1[2]),
+                float(k2[2]),
+                min_kconf,
+            )
+            continue
+        dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
+        if dist < float(min_dist_px):
+            logger.info(
+                "k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆",
+                dist,
+                min_dist_px,
+            )
+            continue
+        r = float(radius_px)
+        # **刻意不按图像 x 交换左右**。
+        # Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的
+        # k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度，
+        # 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同
+        # 的物理孔 —— 那是最难查的一类现场问题。
+        # 下游怎么认这两个点由 `stage2.k1_label` 决定：
+        #   hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2，
+        #   所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。
+        return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r
+    return None
+
+
 def _refine_circle_center(
     gray: np.ndarray,
     cx: float,
@@ -466,6 +527,35 @@ def detect_holes(
                 gate_block=True,
             )
 
+        # ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ----
+        if bool(cfg.get("keypoint_enable", True)):
+            kp = _keypoint_holes(
+                dets,
+                conf_thr=conf,
+                min_kconf=float(cfg.get("keypoint_min_conf", 0.30)),
+                radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)),
+                min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)),
+            )
+            if kp is not None:
+                (k1, k2, r1, r2) = kp
+                hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
+                hole_radii = [float(r1), float(r2)]
+                dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
+                gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
+                draw_img = _render_success(
+                    image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
+                )
+                return {
+                    "status": True,
+                    "msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px",
+                    "hole_points": hole_points,
+                    "hole_radii": hole_radii,
+                    "bbox": list(bbox),
+                    "label": label,
+                    "source": "keypoint",
+                    "draw_img": draw_img,
+                }
+
         # ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
         if bool(cfg.get("insert_pair_enable", True)):
             gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image

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

@@ -25,6 +25,8 @@ from arm_application.site_io import load_site_dict
 from arm_msgs.msg import HoleDetection as HoleDetectionMsg
 from arm_msgs.msg import HoleDetections
 
+from arm_application.ros_node import TolerantNode
+
 
 def _as_bool(val, default: bool = False) -> bool:
     if val is None:
@@ -57,7 +59,7 @@ def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
     return msg
 
 
-class HoleDetectNode(Node):
+class HoleDetectNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("hole_detect")
         self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")

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

@@ -21,7 +21,7 @@ DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
 # 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
 # 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
 # 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
-_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
+_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx"
 
 
 @dataclass
@@ -143,17 +143,18 @@ class HoleDetector:
     def _load_yolo(self):
         if self._detector is not None:
             return self._detector
-        from arm_application.yolo_detector import YoloDetector, resolve_weights
+        from arm_application.yolo_detector import build_detector, resolve_weights
 
         weights = resolve_weights(
             self.model_path,
             backend=str(self.cfg.get("yolo_backend") or "auto"),
         )
-        self._detector = YoloDetector(
+        # build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。
+        self._detector = build_detector(
             weights,
-            device=str(self.device),
             conf=float(self.cfg.get("conf", 0.25)),
             iou=float(self.cfg.get("iou", 0.7)),
+            device=str(self.device),
             imgsz=int(self.cfg.get("imgsz", 640)),
         )
         return self._detector

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

@@ -62,6 +62,15 @@ class KalmanFilter3D:
         self._predict(dt)
         self._t = t
 
+    def zero_velocity(self) -> None:
+        """把速度状态清零、位置保持不动。
+
+        用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不
+        采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成
+        "按旧速度匀速跑掉"，刚好是我们要避免的那件事。
+        """
+        self.x[3:] = 0.0
+
     def update(self, z: np.ndarray) -> None:
         """用一次三维位置观测做量测更新。"""
         z = np.asarray(z, dtype=float).reshape(3)

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

@@ -15,8 +15,10 @@ from geometry_msgs.msg import PoseStamped
 
 from arm_msgs.action import MoveToPose
 
+from arm_application.ros_node import TolerantNode
 
-class MoveToPoseClient(Node):
+
+class MoveToPoseClient(TolerantNode):
     def __init__(self) -> None:
         super().__init__('move_to_pose_client')
         self._client = ActionClient(self, MoveToPose, 'move_to_pose')

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

@@ -54,6 +54,21 @@ class OnnxWorkpieceDetector:
         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])
+        # 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。
+        # 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型
+        # （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框
+        # ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。
+        # 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。
+        # 宁可启动就失败（元规则：静默失效是头号敌人）。
+        outs = self._sess.get_outputs()
+        tail = outs[0].shape[-1] if (outs and outs[0].shape) else None
+        if isinstance(tail, int) and tail != 6:
+            raise RuntimeError(
+                f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是"
+                " ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的"
+                " pose 模型必须交给 pose_detector / build_detector，否则坐标会被"
+                "当成置信度解出垃圾框，而且不会报错。"
+            )
         self.conf = float(conf)
         self.imgsz = int(imgsz)
         # 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。

build/arm_application/build/lib/arm_application/operator_arm_mesh.py

@@ -0,0 +1,339 @@
+"""用非夕给的 Rizon4 外壳网格摆出当前姿态。
+
+网格和安装位姿来自 ``flexiv_description`` 的 ``visual_parameters.yaml``
+与 ``meshes/Rizon4/visual/*.obj``，不另画一套简图。面数按固定步长抽稀，
+只为了界面里能转得动，几何仍是厂家的。
+"""
+
+from __future__ import annotations
+
+import math
+from pathlib import Path
+
+import numpy as np
+import yaml
+
+from arm_application.operator_arm_state import link_frames
+from arm_application.site_io import ros2_root
+
+# 单个连杆原始面数大约 5000。再按 700 抽稀会在壳上留出三角洞，
+# 白壳和灰蓝关节分不出来。这里保留全部面。
+_FACES_PER_LINK = 8000
+_MODELS: dict[str, list[tuple[list[tuple[float, float, float]], tuple]]] | None = None
+
+
+def posed_triangles(q: list[float]) -> list[tuple]:
+    """返回基座系下的三角面 ``(a, b, c, 明暗, rgb)``。
+
+    ``rgb`` 来自同目录 ``.mtl`` 的 ``Kd``。外壳是白的，关节壳体是灰蓝，
+    这是厂家导出的两种材质，不是另涂的颜色。
+    """
+    world, shade, rgb = _posed_arrays(q)
+    return [
+        (
+            (float(world[i, 0, 0]), float(world[i, 0, 1]), float(world[i, 0, 2])),
+            (float(world[i, 1, 0]), float(world[i, 1, 1]), float(world[i, 1, 2])),
+            (float(world[i, 2, 0]), float(world[i, 2, 1]), float(world[i, 2, 2])),
+            float(shade[i]),
+            (float(rgb[i, 0]), float(rgb[i, 1]), float(rgb[i, 2])),
+        )
+        for i in range(shade.shape[0])
+    ]
+
+
+def posed_gl(q: list[float]) -> tuple[bytes, int, tuple[float, float, float], float]:
+    """给显卡的一份网格：交错 ``xyzrgb``、顶点数、中心、半径。
+
+    拖视角只改观察矩阵，不再把四万个面在界面线程里重投影。
+    """
+    world, shade, rgb = _posed_arrays(q)
+    light = (0.55 + 0.45 * shade)[:, None]
+    colored = (rgb * light).astype(np.float32)
+    pos = world.reshape(-1, 3).astype(np.float32)
+    col = np.repeat(colored, 3, axis=0)
+    center = pos.mean(axis=0)
+    radius = float(np.linalg.norm(pos - center, axis=1).max())
+    packed = np.concatenate([pos, col], axis=1).astype(np.float32, copy=False)
+    return (
+        packed.tobytes(),
+        int(pos.shape[0]),
+        (float(center[0]), float(center[1]), float(center[2])),
+        max(radius, 1e-4),
+    )
+
+
+def _posed_arrays(q: list[float]):
+    """每个连杆做一次矩阵乘法，不再对每个顶点走 Python 循环。"""
+    frames = link_frames(q)
+    worlds: list[np.ndarray] = []
+    shades: list[np.ndarray] = []
+    colors: list[np.ndarray] = []
+    for link, parts in _models().items():
+        frame = frames.get(link)
+        if frame is None:
+            continue
+        rot = np.asarray(frame[0], dtype=np.float64)
+        origin = np.asarray(frame[1], dtype=np.float64)
+        for part in parts:
+            local = np.einsum("ij,fvj->fvi", part["erot"], part["verts"]) + part["epos"]
+            world = np.einsum("ij,fvj->fvi", rot, local) + origin
+            ab = world[:, 1] - world[:, 0]
+            ac = world[:, 2] - world[:, 0]
+            normal = np.cross(ab, ac)
+            norm = np.linalg.norm(normal, axis=1)
+            norm = np.where(norm > 0.0, norm, 1.0)
+            facing = np.abs(normal[:, 0] + normal[:, 1]) / (norm * math.sqrt(2.0))
+            worlds.append(world)
+            shades.append(0.28 + 0.72 * facing)
+            colors.append(part["rgb"])
+    if not worlds:
+        empty = np.zeros((0, 3, 3), dtype=np.float64)
+        return empty, np.zeros((0,), dtype=np.float64), np.zeros((0, 3), dtype=np.float64)
+    return np.concatenate(worlds), np.concatenate(shades), np.concatenate(colors)
+
+
+def view_rotation(yaw: float, pitch: float):
+    """先绕竖直轴转，再俯仰。拖拽视角用这一份，不在界面里另写一套。"""
+    return _mul(_rx(pitch), _rz(yaw))
+
+
+def view_scale(radius: float, width: int, height: int) -> tuple[float, float]:
+    """和着色器里的 ``u_sx`` / ``u_sy`` 同一套缩放。宽屏时横向再缩一档。"""
+    fit = 0.85 / max(float(radius), 1e-4)
+    width = max(int(width), 1)
+    height = max(int(height), 1)
+    if width >= height:
+        return fit * height / width, fit
+    return fit, fit * width / height
+
+
+def screen_point(
+    point: tuple[float, float, float],
+    yaw: float,
+    pitch: float,
+    center: tuple[float, float, float],
+    radius: float,
+    width: int,
+    height: int,
+) -> tuple[float, float]:
+    """基座系的点投到控件像素。纵向和着色器里的 ``p.z`` 是同一套，+Z 朝画面上方。"""
+    shifted = (
+        float(point[0]) - float(center[0]),
+        float(point[1]) - float(center[1]),
+        float(point[2]) - float(center[2]),
+    )
+    viewed = _apply(view_rotation(yaw, pitch), shifted)
+    sx, sy = view_scale(radius, width, height)
+    ndc_x = viewed[0] * sx
+    # 着色器写的是 ``p.z * u_sy``。OpenGL 的 y 朝上，这里再换成 Qt 的像素 y。
+    ndc_y = viewed[2] * sy
+    width = max(int(width), 1)
+    height = max(int(height), 1)
+    return (ndc_x * 0.5 + 0.5) * width, (1.0 - (ndc_y * 0.5 + 0.5)) * height
+
+
+def ground_grid(extent: float = 1.0, step: float = 0.25) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]:
+    """地平面格子，z = 0，单位米。以前这张格子在 RViz 里，现在画在工位仿真上。"""
+    if step <= 0 or extent <= 0:
+        raise ValueError("地平面格子的间距和范围都要大于 0")
+    count = int(round(extent / step))
+    lines = []
+    for index in range(-count, count + 1):
+        coord = index * step
+        lines.append(((-extent, coord, 0.0), (extent, coord, 0.0)))
+        lines.append(((coord, -extent, 0.0), (coord, extent, 0.0)))
+    return lines
+
+
+def axis_tip(
+    origin: tuple[float, float, float],
+    rot: tuple,
+    axis: int,
+    length: float,
+) -> tuple[float, float, float]:
+    """坐标系某一轴的端点。``rot`` 把连杆坐标变到基座坐标，列向量就是三根轴。"""
+    basis = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))[axis]
+    direction = _apply(rot, basis)
+    return (
+        origin[0] + direction[0] * length,
+        origin[1] + direction[1] * length,
+        origin[2] + direction[2] * length,
+    )
+
+
+def _place(rot, origin, erot, epos, vertex):
+    local = _add(epos, _apply(erot, vertex))
+    return _add(origin, _apply(rot, local))
+
+
+def _shade(a, b, c) -> float:
+    ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
+    vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
+    nx = uy * vz - uz * vy
+    ny = uz * vx - ux * vz
+    nz = ux * vy - uy * vx
+    norm = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
+    facing = abs(nx + ny) / (norm * math.sqrt(2.0))
+    return 0.28 + 0.72 * facing
+
+
+def _models() -> dict[str, list]:
+    global _MODELS
+    if _MODELS is not None:
+        return _MODELS
+    root = _description_root()
+    visual = yaml.safe_load((root / "config/Rizon4/visual_parameters.yaml").read_text(encoding="utf-8"))
+    params = (visual or {}).get("visual_parameters") or {}
+    loaded: dict[str, list] = {}
+    for link, spec in params.items():
+        shell = ((spec or {}).get("visual") or {})
+        parts = []
+        for kind in ("shell", "ring"):
+            item = shell.get(kind)
+            if not isinstance(item, dict):
+                continue
+            mesh = item.get("mesh") or {}
+            path = root / str(mesh.get("path") or "")
+            if not path.is_file():
+                continue
+            scale = _scale(mesh.get("scale"))
+            origin = item.get("origin") or {}
+            erot = _rpy(
+                float(origin.get("roll") or 0.0),
+                float(origin.get("pitch") or 0.0),
+                float(origin.get("yaw") or 0.0),
+            )
+            epos = (
+                float(origin.get("x") or 0.0),
+                float(origin.get("y") or 0.0),
+                float(origin.get("z") or 0.0),
+            )
+            parts.append(_part_arrays(_load_obj(path, scale, _materials(path)), erot, epos))
+        if parts:
+            loaded[str(link)] = parts
+    if "base_link" not in loaded or "link7" not in loaded:
+        raise FileNotFoundError(f"Rizon4 外壳网格不完整：{root}")
+    _MODELS = loaded
+    return loaded
+
+
+def _part_arrays(faces: list[tuple], erot, epos) -> dict:
+    """把一个零件收成数组，姿态更新时整块相乘。"""
+    if not faces:
+        verts = np.zeros((0, 3, 3), dtype=np.float64)
+        rgb = np.zeros((0, 3), dtype=np.float64)
+    else:
+        verts = np.asarray([[a, b, c] for a, b, c, _color in faces], dtype=np.float64)
+        rgb = np.asarray([color for _a, _b, _c, color in faces], dtype=np.float64)
+    return {
+        "verts": verts,
+        "rgb": rgb,
+        "erot": np.asarray(erot, dtype=np.float64),
+        "epos": np.asarray(epos, dtype=np.float64),
+    }
+
+
+def _description_root() -> Path:
+    root = ros2_root()
+    candidates = (
+        root / "src/flexiv_description",
+        root / "install/flexiv_description/share/flexiv_description",
+    )
+    for item in candidates:
+        if (item / "config/Rizon4/visual_parameters.yaml").is_file():
+            return item
+    raise FileNotFoundError("找不到 flexiv_description 的 Rizon4 外观")
+
+
+def _materials(path: Path) -> dict[str, tuple[float, float, float]]:
+    mtl = path.with_suffix(".mtl")
+    colors: dict[str, tuple[float, float, float]] = {}
+    name = ""
+    if not mtl.is_file():
+        return colors
+    for line in mtl.read_text(encoding="utf-8", errors="replace").splitlines():
+        if line.startswith("newmtl "):
+            name = line.split(None, 1)[1].strip()
+        elif name and line.startswith("Kd "):
+            parts = line.split()
+            if len(parts) >= 4:
+                colors[name] = (float(parts[1]), float(parts[2]), float(parts[3]))
+    return colors
+
+
+def _load_obj(path: Path, scale: tuple[float, float, float], materials: dict) -> list[tuple]:
+    fallback = (0.75, 0.76, 0.78)
+    rgb = fallback
+    vertices: list[tuple[float, float, float]] = []
+    faces: list[tuple[int, int, int, tuple]] = []
+    for line in path.read_text(encoding="utf-8", errors="replace").splitlines():
+        if line.startswith("usemtl "):
+            rgb = materials.get(line.split(None, 1)[1].strip(), fallback)
+        elif line.startswith("v "):
+            _v, x, y, z, *_rest = line.split()
+            vertices.append((float(x) * scale[0], float(y) * scale[1], float(z) * scale[2]))
+        elif line.startswith("f "):
+            idx = []
+            for token in line.split()[1:]:
+                raw = token.split("/")[0]
+                if not raw:
+                    continue
+                number = int(raw)
+                idx.append(number - 1 if number > 0 else number)
+            for offset in range(1, len(idx) - 1):
+                faces.append((idx[0], idx[offset], idx[offset + 1], rgb))
+    if not faces or not vertices:
+        return []
+    step = max(1, len(faces) // _FACES_PER_LINK)
+    chosen = faces[::step]
+    out = []
+    count = len(vertices)
+    for ia, ib, ic, color in chosen:
+        if not (0 <= ia < count and 0 <= ib < count and 0 <= ic < count):
+            continue
+        out.append((vertices[ia], vertices[ib], vertices[ic], color))
+    return out
+
+
+def _scale(raw) -> tuple[float, float, float]:
+    if raw is None:
+        return (1.0, 1.0, 1.0)
+    if isinstance(raw, (int, float)):
+        value = float(raw)
+        return (value, value, value)
+    text = str(raw).replace(",", " ").split()
+    nums = [float(item) for item in text[:3]]
+    while len(nums) < 3:
+        nums.append(nums[-1] if nums else 1.0)
+    return (nums[0], nums[1], nums[2])
+
+
+def _rpy(roll: float, pitch: float, yaw: float):
+    return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
+
+
+def _rx(angle: float):
+    c, s = math.cos(angle), math.sin(angle)
+    return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
+
+
+def _ry(angle: float):
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
+
+
+def _rz(angle: float):
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
+
+
+def _mul(a, b):
+    return tuple(tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)) for i in range(3))
+
+
+def _apply(rot, vec):
+    return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))
+
+
+def _add(a, b):
+    return (a[0] + b[0], a[1] + b[1], a[2] + b[2])

build/arm_application/build/lib/arm_application/operator_arm_state.py

@@ -0,0 +1,595 @@
+"""工位界面用的机械臂状态：只读使能 / Remote，并用关节角画出姿态。
+
+不会调用 Enable，也不会在控制栈或拖拽桥已经占着 RDK 时再连一次。
+``robot.mode()`` 看不出 Manual，必须看 ``operational_status``。
+示教器上的 Auto（Remote）没有远程切换接口，这里只负责把这句话摆在界面上。
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import os
+import re
+import subprocess
+import threading
+import time
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+from arm_application.site_io import listed_arms, ros2_root
+
+# 和 scripts/lib/rdk_status.py 的 MANUAL_HINT 同一件事，界面上始终可见。
+REMOTE_PROMPT = (
+    "请在示教器上打开 Auto（Remote）。"
+    "RDK 没有远程拨这个开关的接口，必须人到现场拨。"
+)
+
+_AXES = (
+    (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),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+)
+_JOINTS = tuple(f"joint{i}" for i in range(1, 8))
+_LOCK = threading.Lock()
+_CACHE: dict[str, dict] = {}
+_STOP = False
+_STARTED = False
+_HOLD = 0
+_HOLD_LOCK = threading.Lock()
+
+
+def _empty(arm: str) -> dict:
+    return {
+        "arm": arm,
+        "level": "warn",
+        "enabled": False,
+        "summary": "还没读到这台臂的状态",
+        "prompt": REMOTE_PROMPT,
+        "prompt_hot": True,
+        "mode": "",
+        "op_status": "",
+        "fault": None,
+        "estop_released": None,
+        "q": None,
+        "points": [],
+        "source": "",
+        "stack_up": False,
+        "stamp": 0.0,
+    }
+
+
+def snapshot(arm: str) -> dict:
+    with _LOCK:
+        row = _CACHE.get(arm)
+        return dict(row) if row else _empty(arm)
+
+
+def present(
+    arm: str,
+    *,
+    stack_up: bool,
+    bridge_up: bool,
+    probe: dict | None,
+    joints: list[float] | None,
+    error: str = "",
+) -> dict:
+    """把一次读数收成界面直接能画的字典。纯函数，不连机器人。"""
+    probe = probe or {}
+    q = _seven(joints) or _seven(probe.get("q"))
+    op_status = str(probe.get("op_status") or "")
+    mode = str(probe.get("mode") or "")
+    manual = "MANUAL" in op_status.upper()
+    if stack_up and q is not None:
+        enabled = True
+        level = "ok"
+        summary = "控制栈在发关节角，这台臂已经接上。"
+        if probe.get("_shared_stack"):
+            summary = "控制栈在发关节角，但看不出是 89 还是 91，两台显示的是同一份。"
+        source = "控制栈 /joint_states"
+    elif stack_up:
+        enabled = False
+        level = "fail"
+        summary = "控制栈在跑，但没有关节数据。通常是还没使能，一直在等 operational。"
+        source = "控制栈"
+    elif bridge_up and not probe:
+        enabled = False
+        level = "warn"
+        summary = "软拖拽桥占着 RDK，界面不再另开一条连接。"
+        source = "拖拽桥"
+        q = None
+    elif probe.get("connected"):
+        enabled = bool(probe.get("operational"))
+        level = str(probe.get("level") or ("ok" if enabled else "fail"))
+        summary = str(probe.get("summary") or ("已使能" if enabled else "未使能"))
+        source = "RDK 直读（未使能，只查询）"
+    else:
+        enabled = False
+        level = "fail" if error else "warn"
+        summary = error or str(probe.get("summary") or probe.get("error") or "还没连上 RDK")
+        source = "RDK 直读" if probe else ""
+    hot = (not enabled) or manual
+    return {
+        "arm": arm,
+        "level": level,
+        "enabled": enabled,
+        "summary": summary,
+        "prompt": REMOTE_PROMPT,
+        "prompt_hot": hot,
+        "mode": mode,
+        "op_status": op_status,
+        "fault": probe.get("fault") if probe else None,
+        "estop_released": probe.get("estop_released") if probe else None,
+        "q": q,
+        "points": fk_points(q) if q else fk_points([0.0] * 7),
+        "pose_live": q is not None,
+        "source": source,
+        "stack_up": stack_up,
+        "stamp": time.time(),
+    }
+
+
+def link_frames(q: list[float]) -> dict[str, tuple]:
+    """各连杆坐标系，和 ``rizon_arm.xacro`` 的关节顺序一致。
+
+    返回 ``base_link`` 与 ``link1``…``link7``。每个值是 ``(旋转, 原点)``，
+    旋转把连杆坐标变到基座坐标。厂家网格就画在这些坐标系里。
+    """
+    kin = _kinematics()
+    rot = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
+    pos = (0.0, 0.0, 0.0)
+    frames: dict[str, tuple] = {"base_link": (rot, pos)}
+    for index, angle in enumerate(q[:7]):
+        spec = kin[_JOINTS[index]]
+        pos = _add(pos, _apply(rot, (spec["x"], spec["y"], spec["z"])))
+        rot = _mul(rot, _rpy(spec["roll"], spec["pitch"], spec["yaw"]))
+        rot = _mul(rot, _axis_rot(_AXES[index], float(angle)))
+        frames[f"link{index + 1}"] = (rot, pos)
+    return frames
+
+
+def fk_points(q: list[float]) -> list[tuple[float, float, float]]:
+    """按 Rizon4 的 default_kinematics 做正运动学，返回基座到法兰的点（米）。
+
+    关节轴与 ``rizon_arm.xacro`` 一致：1/3/5/7 绕 z，2/4/6 绕 y。
+    URDF 顺序是先平移（父坐标系），再 rpy，再绕关节轴转 q。
+    """
+    frames = link_frames(q)
+    points = [frames["base_link"][1]]
+    points.extend(frames[f"link{index}"][1] for index in range(1, 8))
+    _rot, pos = _flange_pose(frames)
+    points.append(pos)
+    return points
+
+
+def display_frames(q: list[float], workpiece: tuple | None = None) -> list[dict]:
+    """界面要画的坐标系，都在基座系里。
+
+    第一套是基座。第二套默认是法兰（旋转与 ``link7`` 相同，原点再加
+    ``link7_to_flange``）。传入工件系时，第二套改画在工件上，不再画法兰。
+    """
+    frames = link_frames(q)
+    base_rot, base_pos = frames["base_link"]
+    shown = [{"name": "基座", "rot": base_rot, "pos": base_pos}]
+    if workpiece is None:
+        rot, pos = _flange_pose(frames)
+        shown.append({"name": "法兰", "rot": rot, "pos": pos})
+        return shown
+    rot, pos = workpiece
+    if not isinstance(rot, tuple) or len(rot) != 3 or not isinstance(pos, tuple) or len(pos) != 3:
+        raise ValueError("工件坐标系要有旋转和原点")
+    shown.append({"name": "工件", "rot": rot, "pos": pos})
+    return shown
+
+
+def workpiece_frame(robot: dict | None) -> tuple | None:
+    """有完整工件系时返回 ``(旋转, 原点)``。没有就返回 None。
+
+    ``workpiece_surface_z_m`` 只是上表面高度，没有原点的 XY，也没有朝向，
+    不能当成坐标系。配了 ``gauge_assembly.workpiece_frame`` 却缺 ``xyz`` 或
+    ``rpy`` 时直接报错，不用单位阵把缺的朝向补上。
+    ``xyz`` 单位米，``rpy`` 单位弧度，都写在基座系，旋转顺序与连杆 ``rpy`` 相同。
+    """
+    if not isinstance(robot, dict):
+        return None
+    gauge = robot.get("gauge_assembly")
+    if not isinstance(gauge, dict) or "workpiece_frame" not in gauge:
+        return None
+    raw = gauge.get("workpiece_frame")
+    if not isinstance(raw, dict):
+        raise ValueError("工件坐标系要写成 xyz 和 rpy，各三个数")
+    xyz = raw.get("xyz")
+    rpy = raw.get("rpy")
+    if not _three_floats(xyz) or not _three_floats(rpy):
+        raise ValueError("工件坐标系要有 xyz 和 rpy，各三个数，单位是米和弧度，写在基座系里")
+    return _rpy(float(rpy[0]), float(rpy[1]), float(rpy[2])), (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+    )
+
+
+def _flange_pose(frames: dict) -> tuple:
+    """法兰位姿。旋转就是 ``link7`` 的旋转，平移来自 ``link7_to_flange``。"""
+    rot, pos = frames["link7"]
+    flange = _kinematics()["link7_to_flange"]
+    pos = _add(pos, _apply(rot, (float(flange["x"]), float(flange["y"]), float(flange["z"]))))
+    return rot, pos
+
+
+def _three_floats(raw: Any) -> bool:
+    if not isinstance(raw, (list, tuple)) or len(raw) != 3:
+        return False
+    return all(isinstance(item, (int, float)) and not isinstance(item, bool) for item in raw)
+
+
+def stack_running() -> bool:
+    return _stack_arm() is not None or _alive("ros2_control_node")
+
+
+def control_holds_rdk() -> bool:
+    """有控制命令在用 RDK 时，状态轮询必须让开。"""
+    with _HOLD_LOCK:
+        return _HOLD > 0
+
+
+def release_for_control() -> None:
+    """停掉正在读状态的 RDK 子进程，把机器人交给控制命令。"""
+    with _HOLD_LOCK:
+        global _HOLD
+        _HOLD += 1
+    _stop_status_probes()
+    deadline = time.time() + 3.0
+    while time.time() < deadline and _status_probe_running():
+        time.sleep(0.05)
+
+
+def resume_after_control() -> None:
+    with _HOLD_LOCK:
+        global _HOLD
+        _HOLD = max(0, _HOLD - 1)
+
+
+def hold_until_idle(pid: int, *, until_stack: bool = False) -> None:
+    """后台脚本还在时继续占着 RDK。进程退出，或控制栈已经起来，就放开。"""
+    def _watch() -> None:
+        while True:
+            if until_stack and stack_running():
+                break
+            if not _pid_alive(pid):
+                break
+            time.sleep(0.3)
+        resume_after_control()
+
+    threading.Thread(target=_watch, daemon=True, name="arm-rdk-hold").start()
+
+
+def motion_blocked(action: str, *, stack_up: bool, op_status: str) -> str:
+    """控制栈没起来时，动臂按钮给一句能执行的原因，而不是把脚本挂在那儿。"""
+    needs_stack = {
+        "pipeline", "stage_track", "stage_locate", "stage_screw",
+        "reset_home", "gauge_run", "handeye_calibrate",
+    }
+    if action not in needs_stack or stack_up:
+        return ""
+    manual = "MANUAL" in str(op_status or "").upper()
+    tail = "示教器现在是 Manual，要先拨到 Auto（Remote）。" if manual else ""
+    return f"控制栈没起来，这个动作动不了臂。{tail}请先在操作页执行「上电控制栈」。"
+
+
+def start() -> None:
+    global _STARTED
+    with _LOCK:
+        if _STARTED:
+            return
+        _STARTED = True
+    threading.Thread(target=_loop, daemon=True, name="arm-state").start()
+
+
+def retain_pose(fresh: dict, previous: dict | None, *, now: float | None = None, hold_s: float = 15.0) -> dict:
+    """这一拍没读到关节角时，短时间沿用上一拍，避免模型被打回零位。
+
+    超过 ``hold_s`` 仍没有新读数就放开，界面回到“没连上”，不把过期姿态一直当成当前。
+    """
+    if fresh.get("q") is not None or not previous or previous.get("q") is None:
+        return fresh
+    moment = time.time() if now is None else now
+    if moment - float(previous.get("stamp") or 0.0) > hold_s:
+        return fresh
+    kept = dict(fresh)
+    kept["q"] = list(previous["q"])
+    kept["points"] = list(previous.get("points") or [])
+    kept["pose_live"] = True
+    note = "关节角沿用上次读数。"
+    summary = str(kept.get("summary") or "")
+    if note not in summary:
+        kept["summary"] = f"{summary} {note}".strip()
+    return kept
+
+
+def _loop() -> None:
+    while not _STOP:
+        if control_holds_rdk():
+            time.sleep(0.2)
+            continue
+        try:
+            arms = _arms()
+            if not arms:
+                time.sleep(2.0)
+                continue
+            stack_arm = _stack_arm()
+            stack_up = stack_arm is not None or _alive("ros2_control_node")
+            bridge_up = _alive("rdk_drag_bridge.py")
+            joints = _read_joints() if stack_up else None
+            if stack_up:
+                targets = [row for row in arms if stack_arm is None or row["id"] == stack_arm]
+                if not targets:
+                    targets = arms
+                shared = stack_arm is None
+                for row in targets:
+                    fresh = present(
+                        row["id"],
+                        stack_up=True,
+                        bridge_up=bridge_up,
+                        probe={"_shared_stack": True} if shared else None,
+                        joints=joints,
+                    )
+                    _store(retain_pose(fresh, snapshot(row["id"])))
+            else:
+                for row in arms:
+                    probe: dict | None = None
+                    error = ""
+                    if control_holds_rdk():
+                        break
+                    if not bridge_up:
+                        probe, error = _probe(row["sn"], row["iface"], row["pc_ip"])
+                    fresh = present(
+                        row["id"],
+                        stack_up=False,
+                        bridge_up=bridge_up,
+                        probe=probe,
+                        joints=None,
+                        error=error,
+                    )
+                    _store(retain_pose(fresh, snapshot(row["id"])))
+        except Exception as exc:  # noqa: BLE001 —— 读状态失败不能把界面线程打死
+            for row in _arms():
+                _store(present(
+                    row["id"], stack_up=False, bridge_up=False, probe=None, joints=None, error=str(exc),
+                ))
+        time.sleep(4.0)
+
+
+def _stop_status_probes() -> None:
+    try:
+        subprocess.run(
+            ["pkill", "-f", "scripts/lib/rdk_status.py"],
+            capture_output=True, text=True, timeout=2,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return
+
+
+def _status_probe_running() -> bool:
+    return "rdk_status.py" in _pgrep("scripts/lib/rdk_status.py")
+
+
+def _pid_alive(pid: int) -> bool:
+    if pid <= 0:
+        return False
+    try:
+        os.kill(pid, 0)
+    except OSError:
+        return False
+    return True
+
+
+def _store(row: dict) -> None:
+    with _LOCK:
+        _CACHE[str(row.get("arm") or "")] = row
+
+
+def _arms() -> list[dict]:
+    path = ros2_root() / "config" / "site.yaml"
+    if not path.is_file():
+        return []
+    data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    return [
+        {"id": row["id"], "sn": row["sn"], "iface": row["iface"], "pc_ip": row["pc_ip"]}
+        for row in listed_arms(data)
+    ]
+
+
+def _stack_arm() -> str | None:
+    text = _pgrep("run_real.sh")
+    ids = {row["id"] for row in _arms()}
+    for match in re.finditer(r"--arm(?:=|\s+)(\S+)", text):
+        token = match.group(1)
+        if token in ids:
+            return token
+    return None
+
+
+def _alive(pattern: str) -> bool:
+    return bool(_pgrep(pattern).strip())
+
+
+def _pgrep(pattern: str) -> str:
+    try:
+        proc = subprocess.run(
+            ["pgrep", "-af", pattern],
+            capture_output=True, text=True, timeout=2,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return ""
+    lines = []
+    for line in proc.stdout.splitlines():
+        if "pgrep" in line:
+            continue
+        lines.append(line)
+    return "\n".join(lines)
+
+
+def _read_joints() -> list[float] | None:
+    try:
+        proc = subprocess.run(
+            ["timeout", "4", "ros2", "topic", "echo", "/joint_states", "--once"],
+            capture_output=True, text=True, timeout=6,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return None
+    return _parse_joint_echo(proc.stdout)
+
+
+def _parse_joint_echo(text: str) -> list[float] | None:
+    for chunk in text.split("---"):
+        if "position:" not in chunk:
+            continue
+        try:
+            data = yaml.safe_load(chunk)
+        except yaml.YAMLError:
+            continue
+        if not isinstance(data, dict):
+            continue
+        names = list(data.get("name") or [])
+        pos = list(data.get("position") or [])
+        if names and len(names) == len(pos):
+            by = {str(name): float(value) for name, value in zip(names, pos)}
+            q = []
+            for joint in _JOINTS:
+                key = next((name for name in by if name == joint or name.endswith(joint)), None)
+                if key is None:
+                    q = []
+                    break
+                q.append(by[key])
+            if len(q) == 7:
+                return q
+        if len(pos) >= 7:
+            try:
+                return [float(v) for v in pos[:7]]
+            except (TypeError, ValueError):
+                return None
+    return None
+
+
+def _probe(sn: str, iface: str, pc_ip: str = "") -> tuple[dict | None, str]:
+    python = os.environ.get("RDK_PYTHON", "").strip() or str(Path.home() / "rdk_py310" / "bin" / "python")
+    script = ros2_root() / "scripts" / "lib" / "rdk_status.py"
+    if not Path(python).is_file() or not script.is_file():
+        return None, "找不到 RDK 解释器或 rdk_status.py，无法读使能状态"
+    cmd = _probe_cmd(python, script, sn, iface, pc_ip)
+    try:
+        proc = subprocess.run(cmd, capture_output=True, text=True, timeout=8)
+    except subprocess.TimeoutExpired:
+        return None, "读 RDK 超时。若控制栈没开，检查网线，并确认示教器已开 Remote。"
+    except OSError as exc:
+        return None, f"读 RDK 失败：{exc}"
+    payload = _json_object(proc.stdout) or _json_object(proc.stderr)
+    if not payload:
+        tail = (proc.stdout + proc.stderr).strip()[-240:]
+        return None, tail or "RDK 没有返回状态"
+    return payload, ""
+
+
+def _probe_cmd(python: str, script: Path, sn: str, iface: str, pc_ip: str) -> list[str]:
+    """RDK 白名单要本机 IPv4。只传网卡名 ``eth0`` 时，内核改名叫 enx… 就永远连不上。"""
+    cmd = [python, str(script), "--sn", sn, "--json"]
+    if iface:
+        cmd += ["--iface", iface]
+    if pc_ip:
+        cmd += ["--pc-ip", pc_ip]
+    return cmd
+
+
+def _json_object(text: str) -> dict:
+    decoder = json.JSONDecoder()
+    for index, char in enumerate(text):
+        if char != "{":
+            continue
+        try:
+            data, _end = decoder.raw_decode(text[index:])
+        except json.JSONDecodeError:
+            continue
+        if isinstance(data, dict) and "connected" in data:
+            return data
+    return {}
+
+
+_KIN: dict[str, dict[str, float]] | None = None
+
+
+def _kinematics() -> dict[str, dict[str, float]]:
+    global _KIN
+    if _KIN is not None:
+        return _KIN
+    root = ros2_root()
+    candidates = (
+        root / "src/flexiv_description/config/Rizon4/default_kinematics.yaml",
+        root / "install/flexiv_description/share/flexiv_description/config/Rizon4/default_kinematics.yaml",
+    )
+    path = next((item for item in candidates if item.is_file()), None)
+    if path is None:
+        raise FileNotFoundError("找不到 Rizon4 default_kinematics.yaml")
+    data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    kin = data.get("kinematics") or {}
+    _KIN = kin
+    return kin
+
+
+def _rpy(roll: float, pitch: float, yaw: float) -> tuple:
+    return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
+
+
+def _rx(angle: float) -> tuple:
+    c, s = math.cos(angle), math.sin(angle)
+    return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
+
+
+def _ry(angle: float) -> tuple:
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
+
+
+def _rz(angle: float) -> tuple:
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
+
+
+def _axis_rot(axis: tuple[float, float, float], angle: float) -> tuple:
+    x, y, z = axis
+    if abs(z) > 0.5:
+        return _rz(angle if z > 0 else -angle)
+    if abs(y) > 0.5:
+        return _ry(angle if y > 0 else -angle)
+    return _rx(angle if x > 0 else -angle)
+
+
+def _mul(a: tuple, b: tuple) -> tuple:
+    return tuple(
+        tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
+        for i in range(3)
+    )
+
+
+def _apply(rot: tuple, vec: tuple[float, float, float]) -> tuple[float, float, float]:
+    return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))  # type: ignore[return-value]
+
+
+def _add(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]:
+    return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
+
+
+def _seven(raw: Any) -> list[float] | None:
+    if not isinstance(raw, list) or len(raw) < 7:
+        return None
+    try:
+        q = [float(v) for v in raw[:7]]
+    except (TypeError, ValueError):
+        return None
+    if any(v != v or abs(v) > 12.6 for v in q):
+        return None
+    return q

build/arm_application/build/lib/arm_application/operator_camera_view.py

@@ -0,0 +1,675 @@
+"""工位程序里的相机预览。
+
+只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
+
+回调里先丢掉当前页没在看的流。彩色按相机原分辨率收进来，不按时钟抽帧，
+也不在收图的线程里做颜色转换。另一条线只画最新的一张，所以帧率跟相机走，
+848 和 1280 都不会为了帧率被缩成 480。深度是伪彩色，仍然抽稀，
+而且没打开深度时不订阅，避免大图挤占彩色。
+手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。
+"""
+
+from __future__ import annotations
+
+import subprocess
+import threading
+import time
+
+import numpy as np
+
+# 彩色上限。D405 是 848×480，D435 是 1280×720，都不超过它，所以不会被缩小。
+_COLOR_WIDTH = 1280
+# 深度上色不是要看的画质。全幅每帧上色会把刷新占满，所以这一路仍抽稀。
+_DEPTH_WIDTH = 480
+# 手眼只在正在看的那一页编码，宽度跟彩色预览一样，不另缩一档。
+_JPEG_WIDTH = 1280
+# 只留给还在用 ``accept_frame`` 的调用。彩色实况不再用它抽帧：
+# 相机大约 30 帧，再按这个间隔丢一次，画面会掉到十几帧。
+_INTERVAL = 1.0 / 30.0
+_DET_HOLD_S = 1.5
+
+_LOCK = threading.Lock()
+_STOP = threading.Event()
+_THREAD: threading.Thread | None = None
+_JPEG: dict[str, bytes] = {}
+_PREVIEW: dict[str, tuple[bytes, int, int]] = {}
+_PREVIEW_WANT: set[str] = set()
+_JPEG_WANT: set[str] = set()
+_NEXT: dict[str, float] = {}
+_DET: dict[str, tuple[float, list[float]]] = {}
+# 没画框的预览。检测一到就在这张上重画，不必等下一帧相机。
+_CLEAN: dict[str, dict] = {}
+# 每路最近一秒的到达时刻，用来算帧率。值是 (宽, 高, 帧/秒)，宽高是相机原图。
+_META: dict[str, tuple[int, int, float]] = {}
+_STAMPS: dict[str, list[float]] = {}
+# 每路最新原图。收图线程只往这里放，画图线程只取最新的一张。
+_RAW: dict[str, dict] = {}
+_SLOTS: dict[str, list] = {}
+_SLOT_AT: dict[str, int] = {}
+_PAINTED: dict[str, int] = {}
+_WAKE = threading.Event()
+_PAINT_THREAD: threading.Thread | None = None
+_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
+
+
+def preview_targets(site: dict) -> list[dict]:
+    """主界面只要左臂腕部相机和共用的手外相机。
+
+    右臂腕部不进这一屏。仿真相机名字以 ``sim`` 开头，也不进。
+    推理框的角色仍是 ``wrist`` / ``overhead``，跟话题对得上。
+    """
+    from arm_application.site_io import listed_arms, overhead_camera
+
+    found: list[dict] = []
+    seen: set[str] = set()
+    left = next((row for row in listed_arms(site) if row.get("side") == "left"), None)
+    name = str((left or {}).get("camera") or "").strip().strip("/")
+    if name and not name.startswith("sim"):
+        cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
+        cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
+        tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
+        seen.add(name)
+        found.append({"id": name, "label": f"左腕 {tag}", "role": "wrist"})
+    cam, _others = overhead_camera(site or {}, None)
+    oname = str((cam or {}).get("name") or "").strip().strip("/")
+    if oname and not oname.startswith("sim") and oname not in seen:
+        tag = str((cam or {}).get("model") or "D435").strip().upper() or "D435"
+        found.append({"id": oname, "label": f"手外 {tag}", "role": "overhead"})
+    return found
+
+
+def sim_preview_targets(site: dict) -> list[dict]:
+    """仿真腕部和仿真手外。画面进主界面，不另开 Gazebo 的相机窗。"""
+    from arm_application.site_io import overhead_camera
+
+    found: list[dict] = []
+    robots = ((site or {}).get("robot") or {}).get("robots") or []
+    cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
+    for robot in robots:
+        if not isinstance(robot, dict):
+            continue
+        if str(robot.get("sn") or "").strip().lower() != "sim":
+            continue
+        cam = robot.get("camera") if isinstance(robot.get("camera"), dict) else {}
+        name = str((cam or {}).get("name") or "").strip().strip("/")
+        if not name:
+            continue
+        cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
+        tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
+        found.append({"id": name, "label": f"仿真左腕 {tag}", "role": "wrist", "source": "sim"})
+        break
+    overhead, _others = overhead_camera(site or {}, {"arm": "sim"})
+    oname = str((overhead or {}).get("name") or "").strip().strip("/")
+    if oname.startswith("sim"):
+        tag = str((overhead or {}).get("model") or "D435").strip().upper() or "D435"
+        found.append({"id": oname, "label": f"仿真手外 {tag}", "role": "overhead", "source": "sim"})
+    return found
+
+
+def stream_targets(site: dict | None = None) -> list[dict]:
+    """真机和仿真都订上。主界面切换看哪一组，没在看的那组回调里直接丢掉。"""
+    if site is None:
+        from arm_application.operator_console import _site
+
+        site = _site()
+    real = preview_targets(site)
+    for item in real:
+        item["source"] = "real"
+    seen = {item["id"] for item in real}
+    return real + [item for item in sim_preview_targets(site) if item["id"] not in seen]
+
+
+def targets() -> list[dict]:
+    """当前站点的主界面真机相机。规则在 ``preview_targets``。"""
+    from arm_application.operator_console import _site
+
+    return preview_targets(_site())
+
+
+def sim_targets() -> list[dict]:
+    from arm_application.operator_console import _site
+
+    return sim_preview_targets(_site())
+
+
+def state() -> dict:
+    with _LOCK:
+        return {
+            "on": _STATE["on"],
+            "error": _STATE["error"],
+            "stamp": _STATE["stamp"],
+            "frames": dict(_STATE["frames"]),
+        }
+
+
+def frame(name: str, kind: str) -> bytes | None:
+    with _LOCK:
+        return _JPEG.get(f"{name}_{kind}")
+
+
+def previews() -> dict[str, tuple[bytes, int, int]]:
+    """当前预览。值是 ``(rgb888, 宽, 高)``，同一帧反复取到的是同一块字节。"""
+    with _LOCK:
+        return dict(_PREVIEW)
+
+
+def preview_meta() -> dict[str, tuple[int, int, float]]:
+    """每路相机原图的宽、高，以及最近一秒的帧率。还没来帧就是空。"""
+    with _LOCK:
+        return dict(_META)
+
+
+def _mark_rate(key: str, width: int, height: int) -> None:
+    """记下这一帧的原图尺寸，并用最近一秒的到达间隔算帧率。"""
+    now = time.monotonic()
+    with _LOCK:
+        stamps = _STAMPS.setdefault(key, [])
+        stamps.append(now)
+        cut = now - 1.0
+        while len(stamps) > 1 and stamps[0] < cut:
+            stamps.pop(0)
+        if len(stamps) >= 2 and stamps[-1] > stamps[0]:
+            fps = (len(stamps) - 1) / (stamps[-1] - stamps[0])
+        else:
+            fps = 0.0
+        _META[key] = (int(width), int(height), float(fps))
+
+
+def watch(preview: set[str], jpeg: set[str] | None = None) -> None:
+    """当前页要哪几路。没点名的流在回调里直接丢掉，不拷图像。
+
+    ``preview`` 走原分辨率 RGB，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。
+    """
+    global _PREVIEW_WANT, _JPEG_WANT
+    jpeg = set(jpeg or ())
+    preview = set(preview)
+    with _LOCK:
+        if preview == _PREVIEW_WANT and jpeg == _JPEG_WANT:
+            return
+        _PREVIEW_WANT = preview
+        _JPEG_WANT = jpeg
+        for key in preview | jpeg:
+            _NEXT.pop(key, None)
+
+
+def wants(key: str) -> tuple[bool, bool]:
+    """返回 ``(要预览, 要 JPEG)``。"""
+    with _LOCK:
+        return key in _PREVIEW_WANT, key in _JPEG_WANT
+
+
+def offer_color(key: str, image: np.ndarray, *, bgr: bool, role: str) -> None:
+    """收一帧彩色原图。没人看就丢掉。有人看就整幅留下，不按时钟抽帧。"""
+    preview, jpeg = wants(key)
+    if not preview and not jpeg:
+        return
+    if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
+        return
+    _offer(key, image, bgr=bgr, role=role, kind="color")
+
+
+def offer_depth(key: str, image: np.ndarray) -> None:
+    """收一帧深度原图。伪彩色仍在画图线程里做，不堵收图。"""
+    preview, _jpeg = wants(key)
+    if not preview:
+        return
+    if image.ndim != 2 or image.shape[0] < 1 or image.shape[1] < 1:
+        return
+    _offer(key, image, bgr=False, role="", kind="depth")
+
+
+def _offer(key: str, image: np.ndarray, *, bgr: bool, role: str, kind: str) -> None:
+    height, width = int(image.shape[0]), int(image.shape[1])
+    with _LOCK:
+        slots = _SLOTS.setdefault(key, [None, None])
+        index = _SLOT_AT.get(key, 0)
+        slot = slots[index]
+        if slot is None or slot.shape != image.shape or slot.dtype != image.dtype:
+            slot = np.empty(image.shape, dtype=image.dtype)
+            slots[index] = slot
+        np.copyto(slot, image)
+        seq = int(_RAW.get(key, {}).get("seq", 0)) + 1
+        _RAW[key] = {
+            "seq": seq,
+            "frame": slot,
+            "bgr": bool(bgr),
+            "role": role,
+            "kind": kind,
+            "w": width,
+            "h": height,
+        }
+        _SLOT_AT[key] = 1 - index
+    _mark_rate(key, width, height)
+    _WAKE.set()
+
+
+def publish_latest() -> None:
+    """把各路还没画的最新原图做成预览。中间帧直接跳过，避免越画越落后。"""
+    with _LOCK:
+        jobs = []
+        for key, item in _RAW.items():
+            if _PAINTED.get(key) == item["seq"]:
+                continue
+            jobs.append((
+                key,
+                int(item["seq"]),
+                np.array(item["frame"], copy=True),
+                bool(item["bgr"]),
+                str(item["role"]),
+                str(item["kind"]),
+                int(item["w"]),
+                int(item["h"]),
+            ))
+    for key, seq, frame, bgr, role, kind, width, height in jobs:
+        try:
+            preview, jpeg = wants(key)
+            if kind == "depth":
+                if preview:
+                    colored = depth_preview(frame)
+                    _store(key, colored, preview=colored, jpeg=False)
+            elif preview or jpeg:
+                limit = _JPEG_WIDTH if jpeg else _COLOR_WIDTH
+                rgb = rgb_preview(frame, bgr=bgr, limit=limit)
+                shown = rgb
+                if preview and int(rgb.shape[1]) > _COLOR_WIDTH:
+                    shown = rgb_preview(rgb, bgr=False, limit=_COLOR_WIDTH)
+                if preview:
+                    _remember_clean(key, shown, role, width, height)
+                    shown = _overlay_detection(shown, role, width, height)
+                _store(key, rgb, preview=shown if preview else None, jpeg=jpeg)
+        except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，下一帧继续
+            with _LOCK:
+                _STATE["error"] = str(exc)
+        finally:
+            with _LOCK:
+                if _PAINTED.get(key, 0) < seq:
+                    _PAINTED[key] = seq
+
+
+def _paint_loop() -> None:
+    while not _STOP.is_set():
+        _WAKE.wait(0.005)
+        _WAKE.clear()
+        if _STOP.is_set():
+            return
+        try:
+            publish_latest()
+        except Exception as exc:  # noqa: BLE001 —— 画图线程不能因为一帧坏掉就退出
+            with _LOCK:
+                _STATE["error"] = str(exc)
+
+
+def accept_frame(key: str) -> bool:
+    """这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。
+
+    彩色实况不再走这里。固定间隔会把相机自己的帧率再砍掉一截。
+    """
+    now = time.monotonic()
+    with _LOCK:
+        if key not in _PREVIEW_WANT and key not in _JPEG_WANT:
+            return False
+        if now < _NEXT.get(key, 0.0):
+            return False
+        _NEXT[key] = now + _INTERVAL
+        return True
+
+
+def _close_external_viewers() -> None:
+    """关掉独立的四窗格查看器。它会把左右红外挂出来，本程序不再用它。"""
+    subprocess.run(["pkill", "-f", "lib/arm_application/realsense_view"], check=False)
+    subprocess.run(["pkill", "-f", "arm_application.realsense_view"], check=False)
+
+
+def paint_detection(
+    rgb: np.ndarray,
+    xyxy: list[float] | tuple,
+    src_width: int,
+    src_height: int,
+) -> np.ndarray:
+    """把推理结果画到相机预览上：绿框、画面中心、跟踪红点、置信度和距离。
+
+    ``xyxy`` 前四项是原图像素的框。够长时第 5 项是置信度，第 7、8 项是跟踪点，
+    第 9 项是距离（米）。空框原样返回。
+    """
+    import cv2
+
+    if rgb.ndim != 3 or src_width <= 0 or src_height <= 0 or len(xyxy) < 4:
+        return rgb
+    scale_x = rgb.shape[1] / float(src_width)
+    scale_y = rgb.shape[0] / float(src_height)
+    x1 = int(round(float(xyxy[0]) * scale_x))
+    y1 = int(round(float(xyxy[1]) * scale_y))
+    x2 = int(round(float(xyxy[2]) * scale_x))
+    y2 = int(round(float(xyxy[3]) * scale_y))
+    height, width = rgb.shape[:2]
+    x1 = max(0, min(width - 1, x1))
+    x2 = max(0, min(width - 1, x2))
+    y1 = max(0, min(height - 1, y1))
+    y2 = max(0, min(height - 1, y2))
+    if x2 <= x1 or y2 <= y1:
+        return rgb
+    out = np.array(rgb, copy=True)
+    thick = max(2, width // 180)
+    cv2.rectangle(out, (x1, y1), (x2, y2), (80, 220, 120), thick)
+    cv2.drawMarker(
+        out,
+        (width // 2, height // 2),
+        (255, 220, 60),
+        cv2.MARKER_CROSS,
+        max(8, width // 24),
+        1,
+    )
+    if len(xyxy) >= 8:
+        px = int(round(float(xyxy[6]) * scale_x))
+        py = int(round(float(xyxy[7]) * scale_y))
+        px = max(0, min(width - 1, px))
+        py = max(0, min(height - 1, py))
+        cv2.circle(out, (px, py), max(3, width // 80), (255, 48, 48), -1)
+    if len(xyxy) >= 5:
+        conf = float(xyxy[4])
+        dist = f"  z={float(xyxy[8]):.3f}m" if len(xyxy) >= 9 else ""
+        cv2.putText(
+            out,
+            f"{conf:.2f}{dist}",
+            (x1, max(14, y1 - 6)),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.45,
+            (80, 220, 120),
+            1,
+            cv2.LINE_AA,
+        )
+    return out
+
+
+def inference_status() -> str:
+    """腕部和手外最近一次推理框。超过保持时间就当没有。"""
+    now = time.monotonic()
+    with _LOCK:
+        items = dict(_DET)
+    parts = []
+    for role, title in (("wrist", "腕部"), ("overhead", "手外")):
+        item = items.get(role)
+        if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 5:
+            parts.append(f"{title} 没有框")
+            continue
+        parts.append(f"{title} 置信 {float(item[1][4]):.2f}")
+    return "推理  " + "    ".join(parts)
+
+
+def _remember_detection(role: str, data: list[float]) -> None:
+    with _LOCK:
+        if len(data) < 4:
+            _DET.pop(role, None)
+        else:
+            _DET[role] = (time.monotonic(), data)
+    _stamp_role(role)
+
+
+def _remember_clean(key: str, rgb: np.ndarray, role: str, src_w: int, src_h: int) -> None:
+    with _LOCK:
+        _CLEAN[key] = {
+            "rgb": np.array(rgb, copy=True),
+            "role": role,
+            "src_w": int(src_w),
+            "src_h": int(src_h),
+        }
+
+
+def _overlay_detection(rgb: np.ndarray, role: str, src_width: int, src_height: int) -> np.ndarray:
+    now = time.monotonic()
+    with _LOCK:
+        item = _DET.get(role)
+    if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 4:
+        return rgb
+    return paint_detection(rgb, item[1], src_width, src_height)
+
+
+def _stamp_role(role: str) -> None:
+    """检测更新后，把框画回这一路相机已经在显示的画面上。"""
+    now = time.monotonic()
+    with _LOCK:
+        frames = [(key, dict(meta)) for key, meta in _CLEAN.items() if meta.get("role") == role]
+        item = _DET.get(role)
+    fresh = item is not None and now - item[0] <= _DET_HOLD_S and len(item[1]) >= 4
+    for key, meta in frames:
+        shown = meta["rgb"]
+        if fresh:
+            shown = paint_detection(shown, item[1], meta["src_w"], meta["src_h"])
+        with _LOCK:
+            _PREVIEW[key] = (np.ascontiguousarray(shown).tobytes(), int(shown.shape[1]), int(shown.shape[0]))
+            _STATE["frames"][key] = True
+            _STATE["stamp"] = time.time()
+
+
+def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _COLOR_WIDTH) -> np.ndarray:
+    """缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。"""
+    import cv2
+
+    if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
+        raise ValueError("彩色预览要的是高×宽×3")
+    view = image
+    width = int(view.shape[1])
+    if width > limit:
+        height = max(1, int(round(int(view.shape[0]) * limit / float(width))))
+        view = cv2.resize(view, (limit, height), interpolation=cv2.INTER_AREA)
+    if bgr:
+        return cv2.cvtColor(view, cv2.COLOR_BGR2RGB)
+    if not view.flags["OWNDATA"] or not view.flags["C_CONTIGUOUS"]:
+        return np.ascontiguousarray(view)
+    return view
+
+
+def depth_preview(depth: np.ndarray, limit: int = _DEPTH_WIDTH) -> np.ndarray:
+    """uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。"""
+    import cv2
+
+    if depth.ndim != 2 or depth.shape[0] < 1 or depth.shape[1] < 1:
+        raise ValueError("深度预览要的是高×宽")
+    step = max(1, int(depth.shape[1]) // limit)
+    small = depth[::step, ::step]
+    valid = small > 0
+    gray = np.zeros(small.shape, np.uint8)
+    if bool(valid.any()):
+        sample = small[valid]
+        if sample.size > 8000:
+            sample = sample[:: max(1, sample.size // 8000)]
+        lo, hi = np.percentile(sample.astype(np.float32, copy=False), [5, 95])
+        hi = max(float(hi), float(lo) + 1.0)
+        norm = np.clip((small.astype(np.float32) - float(lo)) / (hi - float(lo)), 0.0, 1.0)
+        gray = (norm * 255.0).astype(np.uint8)
+        gray[~valid] = 0
+    colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
+    colored[~valid] = 0
+    return rgb_preview(colored, bgr=True, limit=limit)
+
+
+def _jpeg_bgr(rgb: np.ndarray) -> bytes | None:
+    import cv2
+
+    if rgb.size == 0:
+        return None
+    ok, buf = cv2.imencode(
+        ".jpg",
+        cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR),
+        [int(cv2.IMWRITE_JPEG_QUALITY), 70],
+    )
+    if not ok:
+        return None
+    return buf.tobytes()
+
+
+def _as_array(data, dtype, count: int) -> np.ndarray:
+    """优先直接看消息缓冲。缓冲不支持时才拷一份。"""
+    try:
+        arr = np.frombuffer(data, dtype=dtype, count=count)
+    except (TypeError, BufferError, ValueError):
+        arr = np.frombuffer(bytes(data), dtype=dtype, count=count)
+    if arr.size < count:
+        raise ValueError("图像字节数不够")
+    return arr
+
+
+def _color_view(msg) -> np.ndarray | None:
+    if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
+        return None
+    need = int(msg.height) * int(msg.width) * 3
+    arr = _as_array(msg.data, np.uint8, need)
+    return arr.reshape(int(msg.height), int(msg.width), 3)
+
+
+def _depth_view(msg) -> np.ndarray | None:
+    if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
+        return None
+    need = int(msg.height) * int(msg.width)
+    arr = _as_array(msg.data, np.uint16, need)
+    return arr.reshape(int(msg.height), int(msg.width))
+
+
+def _store(key: str, rgb: np.ndarray, *, preview: np.ndarray | None, jpeg: bool) -> None:
+    encoded = _jpeg_bgr(rgb) if jpeg else None
+    with _LOCK:
+        if preview is not None:
+            _PREVIEW[key] = (preview.tobytes(), int(preview.shape[1]), int(preview.shape[0]))
+        if encoded:
+            _JPEG[key] = encoded
+        if preview is not None or encoded:
+            _STATE["frames"][key] = True
+            _STATE["stamp"] = time.time()
+            _STATE["error"] = ""
+
+
+def _loop(cams: list[dict]) -> None:
+    import rclpy
+    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
+
+    class _Node(Node):
+        def __init__(self) -> None:
+            super().__init__("operator_camera_view")
+            det_qos = QoSProfile(
+                history=HistoryPolicy.KEEP_LAST,
+                depth=5,
+                reliability=ReliabilityPolicy.RELIABLE,
+            )
+            self.create_subscription(
+                Float32MultiArray, "workpiece/detection", self._det("wrist"), det_qos
+            )
+            self.create_subscription(
+                Float32MultiArray,
+                "workpiece/detection_overhead",
+                self._det("overhead"),
+                det_qos,
+            )
+            self._depth_subs: dict[str, object] = {}
+            for cam in cams:
+                name = cam["id"]
+                role = str(cam.get("role") or "wrist")
+                self.create_subscription(
+                    Image,
+                    f"/{name}/color/image_raw",
+                    self._color(name, role),
+                    qos_profile_sensor_data,
+                )
+
+        def sync_depth(self) -> None:
+            """深度没打开就不订。订上之后每一帧都会整幅拷进进程，彩色帧率会被拖下去。"""
+            with _LOCK:
+                want = {
+                    cam["id"]
+                    for cam in cams
+                    if f"{cam['id']}_depth" in _PREVIEW_WANT
+                }
+            for cam in cams:
+                name = cam["id"]
+                if name in want and name not in self._depth_subs:
+                    self._depth_subs[name] = self.create_subscription(
+                        Image,
+                        f"/{name}/aligned_depth_to_color/image_raw",
+                        self._depth(name),
+                        qos_profile_sensor_data,
+                    )
+                elif name not in want and name in self._depth_subs:
+                    self.destroy_subscription(self._depth_subs.pop(name))
+
+        def _det(self, role: str):
+            def _cb(msg: Float32MultiArray) -> None:
+                _remember_detection(role, [float(v) for v in msg.data])
+            return _cb
+
+        def _color(self, name: str, role: str):
+            key = f"{name}_color"
+
+            def _cb(msg: Image) -> None:
+                # 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
+                try:
+                    view = _color_view(msg)
+                    if view is None:
+                        return
+                    offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
+                except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
+                    with _LOCK:
+                        _STATE["error"] = str(exc)
+            return _cb
+
+        def _depth(self, name: str):
+            key = f"{name}_depth"
+
+            def _cb(msg: Image) -> None:
+                try:
+                    view = _depth_view(msg)
+                    if view is None:
+                        return
+                    offer_depth(key, view)
+                except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
+                    with _LOCK:
+                        _STATE["error"] = str(exc)
+            return _cb
+
+    rclpy.init()
+    node = _Node()
+    try:
+        while not _STOP.is_set():
+            node.sync_depth()
+            rclpy.spin_once(node, timeout_sec=0.01)
+    except Exception as exc:
+        with _LOCK:
+            _STATE["error"] = str(exc)
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+        with _LOCK:
+            _STATE["on"] = False
+
+
+def start() -> str:
+    global _THREAD, _PAINT_THREAD
+    cams = stream_targets()
+    if not cams:
+        with _LOCK:
+            _STATE["error"] = "site 里没有可预览的真机相机"
+        return _STATE["error"]
+    _close_external_viewers()
+    with _LOCK:
+        if _STATE["on"]:
+            return "相机预览已经在程序里"
+        _STATE["on"] = True
+        _STATE["error"] = ""
+    _STOP.clear()
+    _WAKE.clear()
+    _THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
+    _THREAD.start()
+    _PAINT_THREAD = threading.Thread(target=_paint_loop, daemon=True, name="cam-paint")
+    _PAINT_THREAD.start()
+    names = "、".join(cam["label"] for cam in cams)
+    return f"预览订了 {names} 的彩色原图，深度点开才订，不含左右红外。"
+
+
+def stop() -> None:
+    _STOP.set()
+    _WAKE.set()
+    for thread in (_THREAD, _PAINT_THREAD):
+        if thread is not None:
+            thread.join(timeout=1.5)

build/arm_application/build/lib/arm_application/operator_console.py

@@ -12,6 +12,7 @@ from __future__ import annotations
 import json
 import os
 import re
+import signal
 import subprocess
 import threading
 import time
@@ -23,7 +24,7 @@ from urllib.parse import urlparse
 import yaml
 
 from arm_application import operator_tactile_view as tactile_view
-from arm_application.site_io import eye_to_hand_result_name, overhead_camera
+from arm_application.site_io import eye_to_hand_result_name, listed_arms, overhead_camera, usb_expectations
 
 HOST = "127.0.0.1"
 PORT = 8766
@@ -36,11 +37,60 @@ else:
     ROOT = Path.home() / "ros2"
 
 LOG_DIR = Path("/tmp/orisys_console")
+
+# 动臂和软拖拽都要这台臂，不能一边下发位置一边再开拖拽。
+# 相机、夹爪、触觉不碰这条，也不该排在它后面等。
+_ROBOT_LANES = frozenset({"arm", "teach"})
+_ARM_ACTIONS = frozenset({
+    "pipeline", "stage_track", "stage_locate", "stage_screw",
+    "reset_home", "gauge_run", "real_up", "handeye_calibrate",
+})
+_LANE_WAIT = {
+    "grip": "夹爪上一条还没到位",
+    "camera": "相机命令还没返回",
+    "tactile": "触觉命令还没返回",
+    "arm": "这台臂上一条运动还没结束",
+    "teach": "软拖拽还没返回",
+}
+
+
+def command_lane(action: str) -> str:
+    """每类命令一条通道。同一条还没返回才拒绝，不让相机去等夹爪。"""
+    if action == "stop_commands":
+        return "stop"
+    if action in ("camera_d405", "camera_d435"):
+        return "camera"
+    if action.startswith("tactile"):
+        return "tactile"
+    if action.startswith("gripper"):
+        return "grip"
+    if action == "handeye_teach":
+        return "teach"
+    if action in _ARM_ACTIONS:
+        return "arm"
+    return action
+
+
+def command_blocked_message(running: set[str], lane: str) -> str:
+    """这条通道，或同一台臂上的另一条运动，还没结束时返回要显示的句子。"""
+    if lane == "stop":
+        return ""
+    if lane in running:
+        return _LANE_WAIT.get(lane, "这一条命令还没返回")
+    if lane in _ROBOT_LANES:
+        other = next((name for name in _ROBOT_LANES if name != lane and name in running), "")
+        if other == "arm":
+            return "这台臂还在动，软拖拽先不开"
+        if other == "teach":
+            return "软拖拽占着这台臂，运动先不开"
+    return ""
 ASSETS = Path(__file__).resolve().parent
 _LOCK = threading.Lock()
 _TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
 _HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
 _PROCS: dict[str, int] = {}
+_LIVE: dict[str, subprocess.Popen] = {}
+_PROC_LOCK = threading.Lock()
 
 
 def _push_force(side: str, sample: dict) -> None:
@@ -76,29 +126,26 @@ def _site() -> dict:
 
 
 def _arms() -> list[dict]:
-    robots = (_site().get("robot") or {}).get("robots") or []
+    """真机臂来自 site.yaml。没有清单就空着，不补本机的 89/91。"""
-    out = []
+    return [
-    for robot in robots:
+        {
-        if not isinstance(robot, dict):
+            "id": row["id"],
-            continue
+            "label": row["label"],
-        sn = str(robot.get("sn") or "")
+            "sn": row["sn"],
-        arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
+            "tactile": row["tactile"],
-        if not arm_no:
+            "camera": row["camera"],
-            continue
+            "gripper": row["gripper"],
-        out.append({
+            "rotary": row["rotary"],
-            "id": arm_no,
+        }
-            "label": str(robot.get("label") or f"臂{arm_no}"),
+        for row in listed_arms(_site())
-            "sn": sn,
-            "tactile": bool(robot.get("tactile")),
-            "camera": str((robot.get("camera") or {}).get("name") or ""),
-            "gripper": str((robot.get("gripper") or {}).get("model") or ""),
-        })
-    return out or [
-        {"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
-        {"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
     ]
 
 
+def _default_arm() -> str:
+    rows = _arms()
+    return str(rows[0]["id"]) if rows else ""
+
+
 def _usb() -> list[dict]:
     found = {"d405": [], "d435": [], "orisys": []}
     want = {
@@ -119,10 +166,11 @@ def _usb() -> list[dict]:
             continue
         speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
         found[key].append({"port": dev.name, "speed": speed})
+    expect = usb_expectations(_site())
     return [
-        {"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
+        {"id": "d405", "label": "腕部 D405", "expect": expect["d405"], "items": found["d405"]},
-        {"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
+        {"id": "d435", "label": "手外 D435", "expect": expect["d435"], "items": found["d435"]},
-        {"id": "orisys", "label": "视触觉", "expect": 2, "items": found["orisys"]},
+        {"id": "orisys", "label": "视触觉", "expect": expect["orisys"], "items": found["orisys"]},
     ]
 
 
@@ -214,13 +262,16 @@ def _poll_tactile() -> None:
 
 
 def _robot(arm: str) -> dict:
+    from arm_application.site_io import arm_short_id
+
     robots = (_site().get("robot") or {}).get("robots") or []
     for robot in robots:
         if not isinstance(robot, dict):
             continue
         sn = str(robot.get("sn") or "")
-        short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
+        if sn.lower() == "sim":
-        if short == arm:
+            continue
+        if arm_short_id(sn) == arm:
             return robot
     return {}
 
@@ -305,12 +356,30 @@ def _handeye(arm: str) -> dict:
         or shared.get("handeye_waypoints_eye_to_hand")
         or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
     ))
+    wrist = _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {})
+    wrist_name = ""
+    if isinstance(robot.get("camera"), dict):
+        wrist_name = str(robot["camera"].get("name") or "").strip().strip("/")
+    wrist["preview_camera"] = wrist_name
+    wrist["preview_label"] = "腕部相机"
+    outside = _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {})
+    outside["preview_camera"] = cam_name.strip().strip("/")
+    outside["preview_label"] = "手外相机"
     return {
-        "eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
+        "eye_in_hand": wrist,
-        "eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
+        "eye_to_hand": outside,
     }
 
 
+def _by_arm(reader) -> dict:
+    return {row["id"]: reader(row["id"]) for row in _arms()}
+
+
+def _gripper_live() -> dict:
+    from arm_application.operator_gripper_state import snapshot as grip_snapshot
+    return _by_arm(grip_snapshot)
+
+
 def status() -> dict:
     with _LOCK:
         tactile = {
@@ -322,20 +391,23 @@ def status() -> dict:
             "left": list(_HIST["left"]),
             "right": list(_HIST["right"]),
         }
+    from arm_application.operator_arm_state import snapshot as arm_snapshot
     return {
         "arms": _arms(),
+        "arm_live": _by_arm(arm_snapshot),
+        "gripper_live": _gripper_live(),
         "usb": _usb(),
         "services": _services(),
         "grippers": _grippers(),
         "tactile": tactile,
         "history": history,
         "view": tactile_view.state(),
-        "handeye": {"89": _handeye("89"), "91": _handeye("91")},
+        "handeye": _by_arm(_handeye),
         "now": time.time(),
     }
 
 
-# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
+# 会动真机的动作必须 confirm。臂号必须是 site.yaml 里某台真机的末两位。
 _ACTIONS = {
     "real_up": {
         "title": "上电控制栈",
@@ -377,21 +449,21 @@ _ACTIONS = {
         "script": "run_gripper.sh",
         "args": ["--arm", "{arm}", "--cli", "probe"],
         "moves": False,
-        "blurb": "读夹爪是否在线，不动手指。",
+        "blurb": "读夹爪是否在线，不动手指。驱动没在跑时先拉起，不回零。",
     },
     "gripper_open": {
         "title": "夹爪张开",
         "script": "run_gripper.sh",
         "args": ["--arm", "{arm}", "--cli", "open"],
         "moves": True,
-        "blurb": "张开当前臂夹爪。",
+        "blurb": "张开当前臂夹爪。驱动没在跑时先拉起，不回零。",
     },
     "gripper_close": {
         "title": "夹爪闭合",
         "script": "run_gripper.sh",
         "args": ["--arm", "{arm}", "--cli", "close"],
         "moves": True,
-        "blurb": "闭合当前臂夹爪。",
+        "blurb": "闭合当前臂夹爪。驱动没在跑时先拉起，不回零。",
     },
     "gripper_rotate": {
         "title": "右爪转 90°",
@@ -401,11 +473,11 @@ _ACTIONS = {
         "blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
     },
     "pipeline": {
-        "title": "整体三段",
+        "title": "跟踪并对孔",
         "script": "run_all.sh",
         "args": ["--arm", "{arm}"],
         "moves": True,
-        "blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。",
+        "blurb": "跟踪和对孔一次跑完。搜孔轻压已取消，这一步不含旋入。会动臂。",
     },
     "stage_track": {
         "title": "只测跟踪",
@@ -421,12 +493,12 @@ _ACTIONS = {
         "moves": True,
         "blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
     },
-    "stage_search": {
+    "stage_screw": {
-        "title": "只测搜孔",
+        "title": "只测旋转装配",
         "script": "run_stage3_search.sh",
-        "args": ["--arm", "{arm}"],
+        "args": ["--arm", "{arm}", "--screw"],
         "moves": True,
-        "blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。",
+        "blurb": "按现场配置的拧入深度和螺距把螺纹规旋进去。会动臂。臂应已在孔口。",
     },
     "reset_home": {
         "title": "复位到 home",
@@ -462,9 +534,9 @@ _ACTIONS = {
     "handeye_teach": {
         "title": "示教采点",
         "script": "run_handeye.sh",
-        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"],
+        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"],
         "moves": True,
-        "blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
+        "blurb": "软拖拽示教。点数和是否接着采由手眼页上的控件决定。要先停控制栈，会动臂。",
     },
     "handeye_merge": {
         "title": "合并示教点",
@@ -507,6 +579,63 @@ def catalog() -> dict:
     }
 
 
+def _remember(action: str, proc: subprocess.Popen) -> None:
+    with _PROC_LOCK:
+        _LIVE[action] = proc
+        _PROCS[action] = proc.pid
+
+
+def _forget(action: str) -> None:
+    with _PROC_LOCK:
+        _LIVE.pop(action, None)
+
+
+def _pid_alive(pid: int) -> bool:
+    if pid <= 0:
+        return False
+    try:
+        os.kill(pid, 0)
+    except OSError:
+        return False
+    return True
+
+
+def stop_started() -> dict:
+    """停掉本界面拉起的命令。只向登记过的进程组发信号，不扫全机。"""
+    with _PROC_LOCK:
+        items = list(_LIVE.items())
+    if not items:
+        return {"ok": True, "message": "没有正在跑的命令"}
+    names: list[str] = []
+    for action, proc in items:
+        pid = int(getattr(proc, "pid", 0) or 0)
+        if not _pid_alive(pid):
+            _forget(action)
+            continue
+        try:
+            os.killpg(os.getpgid(pid), signal.SIGTERM)
+        except ProcessLookupError:
+            _forget(action)
+            continue
+        except OSError as exc:
+            return {"ok": False, "message": f"停不了 {action}：{exc}"}
+        names.append(action)
+    time.sleep(0.3)
+    for action, proc in items:
+        pid = int(getattr(proc, "pid", 0) or 0)
+        if not _pid_alive(pid):
+            _forget(action)
+            continue
+        try:
+            os.killpg(os.getpgid(pid), signal.SIGKILL)
+        except OSError:
+            pass
+        _forget(action)
+    if not names:
+        return {"ok": True, "message": "没有正在跑的命令"}
+    return {"ok": True, "message": "已停止：" + "、".join(names)}
+
+
 def _stop_tactile() -> str:
     subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
     subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
@@ -514,7 +643,39 @@ def _stop_tactile() -> str:
     return "已发送停止视触觉节点"
 
 
-def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict:
+def render_action_args(
+    template: list[str], arm: str, mode: str, extras: dict | None = None,
+) -> list[str]:
+    """把动作模板里的臂号、模式、点数换成这次界面上的值。"""
+    extra = extras or {}
+    needs_points = any("{points}" in str(token) for token in template)
+    points_text = ""
+    if needs_points:
+        try:
+            points = int(extra.get("points", 12))
+        except (TypeError, ValueError):
+            raise ValueError("目标点数不是整数") from None
+        if points < 4 or points > 30:
+            raise ValueError("目标点数只接受 4–30")
+        points_text = str(points)
+    repl = {"{arm}": arm, "{mode}": mode, "{points}": points_text}
+    out: list[str] = []
+    for token in template:
+        text = str(token)
+        for key, value in repl.items():
+            text = text.replace(key, value)
+        out.append(text)
+    if extra.get("append"):
+        if "teach" not in out:
+            raise ValueError("只有示教采点能接着采")
+        out.append("--append")
+    return out
+
+
+def run_action(
+    action: str, arm: str, confirm: bool, mode: str = "eye_in_hand",
+    extras: dict | None = None,
+) -> dict:
     if action == "tactile_down":
         tactile_view.stop()
         return {"ok": True, "message": _stop_tactile()}
@@ -528,8 +689,8 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
     spec = _ACTIONS.get(action)
     if spec is None:
         return {"ok": False, "message": "没有这个动作"}
-    if arm not in ("89", "91"):
+    if arm not in {row["id"] for row in _arms()}:
-        return {"ok": False, "message": "臂号只接受 89 或 91"}
+        return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
     if mode not in ("eye_in_hand", "eye_to_hand"):
         return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
     if spec["moves"] and not confirm:
@@ -537,40 +698,86 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
     script = ROOT / "scripts" / spec["script"]
     if not script.is_file():
         return {"ok": False, "message": f"找不到 {script}"}
-    args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
+    from arm_application.operator_arm_state import (
+        hold_until_idle,
+        motion_blocked,
+        release_for_control,
+        resume_after_control,
+        snapshot as arm_snapshot,
+        stack_running,
+    )
+    blocked = motion_blocked(
+        action,
+        stack_up=stack_running(),
+        op_status=str((arm_snapshot(arm) or {}).get("op_status") or ""),
+    )
+    if blocked:
+        return {"ok": False, "message": blocked}
+    uses_rdk = action in {
+        "real_up", "handeye_teach", "pipeline", "stage_track", "stage_locate",
+        "stage_screw", "reset_home", "gauge_run", "handeye_calibrate",
+    }
+    if uses_rdk:
+        release_for_control()
+    from arm_application.operator_gripper_state import hold_serial
+    grip_port = action in {
+        "gripper_probe", "gripper_open", "gripper_close", "gripper_rotate", "gripper_up",
+    }
+    try:
+        args = render_action_args(list(spec["args"]), arm, mode, extras)
+    except ValueError as exc:
+        return {"ok": False, "message": str(exc)}
     LOG_DIR.mkdir(parents=True, exist_ok=True)
     log_path = LOG_DIR / f"{action}_{arm}.log"
     shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
-    if spec.get("sync"):
+    try:
-        try:
+        if spec.get("sync"):
-            proc = subprocess.run(
+            proc = subprocess.Popen(
                 ["bash", str(script), *args],
                 cwd=str(ROOT),
-                capture_output=True,
+                stdout=subprocess.PIPE,
+                stderr=subprocess.STDOUT,
                 text=True,
-                timeout=float(spec.get("timeout") or 40),
+                start_new_session=True,
             )
-        except subprocess.TimeoutExpired:
+            _remember(action, proc)
-            return {"ok": False, "message": "命令超时还没返回", "detail": shown}
+            try:
-        detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+                detail, _ = proc.communicate(timeout=float(spec.get("timeout") or 40))
-        log_path.write_text(detail)
+            except subprocess.TimeoutExpired:
-        ok = proc.returncode == 0
+                try:
-        return {
+                    os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
-            "ok": ok,
+                except OSError:
-            "message": shown if ok else f"退出码 {proc.returncode}",
+                    pass
-            "detail": detail[-4000:],
+                return {"ok": False, "message": "命令超时还没返回", "detail": shown}
-            "log": str(log_path),
+            finally:
-        }
+                _forget(action)
-    log_f = log_path.open("ab")
+            detail = (detail or "").strip()
-    proc = subprocess.Popen(
+            log_path.write_text(detail)
-        ["bash", str(script), *args],
+            ok = proc.returncode == 0
-        cwd=str(ROOT),
+            return {
-        stdout=log_f,
+                "ok": ok,
-        stderr=subprocess.STDOUT,
+                "message": shown if ok else f"退出码 {proc.returncode}",
-        start_new_session=True,
+                "detail": detail[-4000:],
-    )
+                "log": str(log_path),
-    _PROCS[action] = proc.pid
+            }
-    return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
+        log_f = log_path.open("ab")
+        proc = subprocess.Popen(
+            ["bash", str(script), *args],
+            cwd=str(ROOT),
+            stdout=log_f,
+            stderr=subprocess.STDOUT,
+            start_new_session=True,
+        )
+        _remember(action, proc)
+        if grip_port:
+            hold_serial(proc.pid)
+        if uses_rdk:
+            hold_until_idle(proc.pid, until_stack=(action == "real_up"))
+            uses_rdk = False
+        return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
+    finally:
+        if uses_rdk:
+            resume_after_control()
 
 
 def _bounded(name: str, raw, low: float, high: float) -> float:
@@ -585,12 +792,12 @@ def _bounded(name: str, raw, low: float, high: float) -> float:
 
 def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
     """按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
-    if arm not in ("89", "91"):
+    if arm not in {row["id"] for row in _arms()}:
-        return {"ok": False, "message": "臂号只接受 89 或 91"}
+        return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
     if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
         return {"ok": False, "message": "没有这种夹爪调节"}
-    if kind != "pos" and arm != "91":
+    if kind != "pos" and not any(row["id"] == arm and row.get("rotary") for row in _arms()):
-        return {"ok": False, "message": "左臂平行爪没有旋转轴"}
+        return {"ok": False, "message": "这台夹爪没有旋转轴"}
     try:
         if kind == "pos":
             pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
@@ -615,7 +822,17 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
                 summary = f"转 {turns:.3f} 圈  力矩 {force}%  速度 {speed}%"
     except ValueError as exc:
         return {"ok": False, "message": str(exc)}
+    from arm_application.operator_gripper_state import pause_serial, resume_serial
+
     script = ROOT / "scripts" / "run_gripper.sh"
+    pause_serial()
+    try:
+        return _run_gripper_argv(script, arm, force, speed, cli, summary)
+    finally:
+        resume_serial()
+
+
+def _run_gripper_argv(script, arm: str, force: int, speed: int, cli: list[str], summary: str) -> dict:
     argv = [
         "bash", str(script),
         "--arm", arm,
@@ -624,19 +841,40 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
         "--cli", *cli,
     ]
     shown = " ".join(argv)
+    proc = subprocess.Popen(
+        argv, cwd=str(ROOT), stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
+        text=True, start_new_session=True,
+    )
+    _remember(f"gripper_{arm}", proc)
     try:
-        proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
+        try:
-    except subprocess.TimeoutExpired:
+            detail, _ = proc.communicate(timeout=90)
-        return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
+        except subprocess.TimeoutExpired:
-    detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+            try:
+                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
+            except OSError:
+                pass
+            return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
+    finally:
+        _forget(f"gripper_{arm}")
+    detail = (detail or "").strip()
     ok = proc.returncode == 0
     return {
         "ok": ok,
-        "message": summary if ok else f"夹爪退出码 {proc.returncode}",
+        "message": summary if ok else _grip_failure_message(proc.returncode, detail),
         "detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
     }
 
 
+def _grip_failure_message(code: int, detail: str) -> str:
+    """退出码旁边带上脚本打出来的原因，别只剩一个数字。"""
+    for line in reversed(detail.splitlines()):
+        text = line.strip()
+        if text.startswith("ERROR:") or text.startswith("无法"):
+            return text
+    return f"夹爪退出码 {code}"
+
+
 class Handler(BaseHTTPRequestHandler):
     def log_message(self, fmt: str, *args) -> None:
         return
@@ -700,12 +938,12 @@ class Handler(BaseHTTPRequestHandler):
             return
         if path == "/api/tactile/view":
             action = "tactile_view_on" if body.get("on") else "tactile_view_off"
-            result = run_action(action, "89", False)
+            result = run_action(action, _default_arm(), False)
             self._json(200 if result.get("ok") else 400, result)
             return
         result = run_action(
             str(body.get("action") or ""),
-            str(body.get("arm") or "89"),
+            str(body.get("arm") or _default_arm()),
             bool(body.get("confirm")),
             str(body.get("mode") or "eye_in_hand"),
         )
@@ -715,6 +953,10 @@ class Handler(BaseHTTPRequestHandler):
 def main() -> None:
     import atexit
     atexit.register(tactile_view.stop)
+    from arm_application.operator_arm_state import start as start_arm_state
+    from arm_application.operator_gripper_state import start as start_gripper_state
+    start_arm_state()
+    start_gripper_state()
     threading.Thread(target=_poll_tactile, daemon=True).start()
     server = ThreadingHTTPServer((HOST, PORT), Handler)
     print(f"Orisys 工位界面  http://{HOST}:{PORT}", flush=True)

build/arm_application/build/lib/arm_application/operator_gripper_mesh.py

@@ -0,0 +1,138 @@
+"""大寰夹爪的一比一外形。
+
+左臂按下载包 ``PGEA-3D-Model-and-2D-Drawing-4`` 里的
+``PGEA-100-26-W-F`` 尺寸图。图签就是这个型号，总长 138、宽 55、厚 29，
+安装端抱闸约 20，主体 78，手指伸出约 40，闭合缝 1.2。
+现场装的是 ``PGEA-100-20-W``，这包里没有 100-20 的图，开口仍用
+``site.yaml`` 的 20 mm，不用图上的 26。
+
+同包另一张完整的带抱闸图是 ``PGEA-100-40-W-F``（手指更长，张开约 40.8）。
+右臂是 ``RGI-100-30``，这包里没有旋转爪的图，仍按手册外形。
+"""
+
+from __future__ import annotations
+
+import math
+
+# 米。来源见模块说明。长指向指尖，宽是开合方向，厚是图上的 29。
+PGEA_SIZE = (0.138, 0.055, 0.029)
+PGEA_STROKE_MM = 20.0
+PGEA_CLOSED_GAP_M = 0.0012
+PGEA_FINGER_M = 0.040
+RGI_SIZE = (0.158, 0.0755, 0.047)
+RGI_STROKE_MM = 30.0
+
+# 深色底上要能分开：银壳体、蓝抱闸、橙手指、绿导轨。
+# 手指以前是 0.10 的黑，乘上未连接时的 0.55 就沉进背景里。
+_BODY = (0.86, 0.88, 0.91)
+_BRAKE = (0.16, 0.42, 0.90)
+_FINGER = (0.95, 0.46, 0.10)
+_RAIL = (0.18, 0.78, 0.28)
+
+
+def triangles(
+    rotary: bool,
+    permille: float | None,
+    rotation_deg: float | None,
+    stroke_mm: float | None,
+) -> tuple[list[tuple], str]:
+    """返回 ``(三角面, 说明)``。三角面是 ``(a, b, c, 明暗占位, rgb)``，明暗由界面再乘。"""
+    if rotary:
+        length, width, thick = RGI_SIZE
+        stroke = (float(stroke_mm) if stroke_mm else RGI_STROKE_MM) / 1000.0
+        note = (
+            f"RGI-100-30  外形 {length*1000:.0f}×{width*1000:.1f}×{thick*1000:.0f} mm"
+            f"  行程 {stroke*1000:.0f} mm。这包图纸里没有旋转爪，外形仍按手册。"
+        )
+        body_end = 0.118
+        brake = None
+    else:
+        length, width, thick = PGEA_SIZE
+        stroke = (float(stroke_mm) if stroke_mm else PGEA_STROKE_MM) / 1000.0
+        note = (
+            f"PGEA-100-26-W-F 尺寸图  外形 {length*1000:.0f}×{width*1000:.0f}×{thick*1000:.0f} mm"
+            f"  现场行程 {stroke*1000:.0f} mm。包里没有 100-20 的图，开口不用图上的 26。"
+        )
+        body_end = length - PGEA_FINGER_M
+        brake = (0.0, 0.020)
+    gap = stroke * (0.5 if permille is None else max(0.0, min(1.0, float(permille) / 1000.0)))
+    if not rotary:
+        gap += PGEA_CLOSED_GAP_M
+    parts = _solid(length, width, thick, body_end, gap, brake, rotary)
+    if rotary and rotation_deg:
+        parts = _spin_fingers(parts, math.radians(float(rotation_deg)), body_end)
+    out = []
+    for a, b, c, rgb in parts:
+        out.append((a, b, c, _face_shade(a, b, c), rgb))
+    if permille is None:
+        note = "没有读数。下面是半开，不是当前开口。 " + note
+    return out, note
+
+
+def _solid(length, width, thick, body_end, gap, brake, rotary: bool):
+    hx, hy = thick / 2.0, width / 2.0
+    faces = []
+    z0 = 0.0
+    if brake is not None:
+        z0 = brake[1]
+        faces.extend(_box(0.0, brake[1], -hy, hy, -hx, hx, _BRAKE))
+    faces.extend(_box(z0, body_end, -hy, hy, -hx, hx, _BODY))
+    if rotary:
+        finger_t = min(0.010, hy * 0.4)
+        finger_h = thick * 0.72
+    else:
+        # 尺寸图端视图：指厚 6.5，指高 16，都落在 55×29 的外形里面。
+        finger_t = 0.0065
+        finger_h = 0.016
+        rail = 0.003
+        faces.extend(_box(z0, body_end, -hy * 0.62, -hy * 0.62 + rail, hx - rail, hx, _RAIL))
+        faces.extend(_box(z0, body_end, hy * 0.62 - rail, hy * 0.62, hx - rail, hx, _RAIL))
+    half = gap / 2.0
+    faces.extend(_box(body_end, length, -(half + finger_t), -half, -finger_h / 2, finger_h / 2, _FINGER))
+    faces.extend(_box(body_end, length, half, half + finger_t, -finger_h / 2, finger_h / 2, _FINGER))
+    return faces
+
+
+def _spin_fingers(parts, angle: float, body_end: float):
+    c, s = math.cos(angle), math.sin(angle)
+    spun = []
+    for a, b, c0, rgb in parts:
+        if a[0] >= body_end - 1e-6 and b[0] >= body_end - 1e-6 and c0[0] >= body_end - 1e-6:
+            a, b, c0 = (_rot_z(a, c, s), _rot_z(b, c, s), _rot_z(c0, c, s))
+        spun.append((a, b, c0, rgb))
+    return spun
+
+
+def _face_shade(a, b, c) -> float:
+    """顶面亮、底面暗。零件颜色已经分开，明暗再把面分出来。"""
+    ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
+    vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
+    nx = uy * vz - uz * vy
+    ny = uz * vx - ux * vz
+    nz = ux * vy - uy * vx
+    length = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
+    dot = (nx * 0.15 + ny * -0.25 + nz * 0.95) / length
+    return 0.42 + 0.58 * max(0.0, min(1.0, dot))
+
+
+def _rot_z(point, c, s):
+    x, y, z = point
+    return (x, y * c - z * s, y * s + z * c)
+
+
+def _box(x0, x1, y0, y1, z0, z1, rgb) -> list[tuple]:
+    """轴向盒子。x 指向指尖，y 是开合方向，z 是厚度。"""
+    p = (
+        (x0, y0, z0), (x1, y0, z0), (x1, y1, z0), (x0, y1, z0),
+        (x0, y0, z1), (x1, y0, z1), (x1, y1, z1), (x0, y1, z1),
+    )
+    quads = (
+        (0, 1, 2, 3), (4, 7, 6, 5),
+        (0, 4, 5, 1), (3, 2, 6, 7),
+        (0, 3, 7, 4), (1, 5, 6, 2),
+    )
+    out = []
+    for a, b, c, d in quads:
+        out.append((p[a], p[b], p[c], rgb))
+        out.append((p[a], p[c], p[d], rgb))
+    return out

build/arm_application/build/lib/arm_application/operator_gripper_state.py

@@ -0,0 +1,436 @@
+"""工位界面用的夹爪状态：只读开口和转角，并交给界面画出来。
+
+夹爪节点已经占着串口时，只订它发布的话题，不再自己打开串口。
+没有节点时才短连一次。连上之后只读寄存器，不写初始化，避免手指回零把工件松开。
+"""
+
+from __future__ import annotations
+
+import os
+import subprocess
+import threading
+import time
+from typing import Any
+
+import yaml
+
+from arm_application.site_io import listed_arms, ros2_root
+
+_GRIP = {0: "运动中", 1: "已到位", 2: "已夹住", 3: "已掉落"}
+_ROT = {0: "转动中", 1: "已到位", 2: "受阻", 3: "曾经受阻"}
+
+_LOCK = threading.Lock()
+_CACHE: dict[str, dict] = {}
+_STARTED = False
+_PORT_HOLD = 0
+_PORT_BUSY = 0
+_PORT_LOCK = threading.Lock()
+
+
+def snapshot(arm: str) -> dict:
+    with _LOCK:
+        row = _CACHE.get(arm)
+        return dict(row) if row else _empty(arm)
+
+
+def _empty(arm: str) -> dict:
+    row = next((item for item in _arms() if item["id"] == arm), None)
+    return present(
+        arm,
+        rotary=bool(row and row["rotary"]),
+        stroke_mm=float(row["stroke_mm"]) if row else 20.0,
+        reading=None,
+        error="还没读到夹爪",
+    )
+
+
+def present(
+    arm: str,
+    *,
+    rotary: bool,
+    stroke_mm: float,
+    reading: dict | None,
+    error: str = "",
+) -> dict:
+    """把一次读数收成界面能画的字典。不打开串口。"""
+    stroke = float(stroke_mm) if stroke_mm and stroke_mm > 0 else 20.0
+    row = reading or {}
+    permille = _unit(row.get("permille"), 0, 1000)
+    connected = bool(row.get("connected")) and permille is not None
+    opening = None if permille is None else permille / 1000.0 * stroke
+    rotation = _finite(row.get("rotation_deg")) if rotary else 0.0
+    grip_state = row.get("grip_state")
+    rot_state = row.get("rotation_state")
+    if not connected:
+        summary = error or str(row.get("error_msg") or "夹爪没连上")
+    else:
+        bits = [_GRIP.get(int(grip_state), "状态未知") if _finite(grip_state) is not None else "状态未知"]
+        bits.append(f"开口 {opening:.1f} mm" if opening is not None else "")
+        if rotary and rotation is not None:
+            bits.append(f"转角 {rotation:.1f}°")
+            if _finite(rot_state) is not None:
+                bits.append(_ROT.get(int(rot_state), ""))
+        summary = "  ".join(bit for bit in bits if bit)
+    return {
+        "arm": arm,
+        "rotary": rotary,
+        "connected": connected,
+        "initialized": bool(row.get("initialized")) if connected else False,
+        "permille": permille,
+        "opening_mm": opening,
+        "stroke_mm": stroke,
+        "rotation_deg": rotation if rotary else None,
+        "force": _finite(row.get("force")),
+        "speed": _finite(row.get("speed")),
+        "grip_state": int(grip_state) if _finite(grip_state) is not None else None,
+        "grip_text": _GRIP.get(int(grip_state), "—") if _finite(grip_state) is not None else "—",
+        "rotation_text": _ROT.get(int(rot_state), "—") if rotary and _finite(rot_state) is not None else "—",
+        "moving": bool(row.get("moving")) if connected else False,
+        "gripped": bool(row.get("gripped")) or (_finite(grip_state) == 2) if connected else False,
+        "summary": summary,
+        "error": "" if connected else (error or str(row.get("error_msg") or "")),
+        "source": str(row.get("source") or ""),
+        "pose_live": connected,
+        "stamp": time.time(),
+    }
+
+
+def retain_reading(
+    fresh: dict,
+    previous: dict | None,
+    *,
+    now: float | None = None,
+    hold_s: float = 15.0,
+) -> dict:
+    """这一拍没读到开口时，短时间沿用上一拍。
+
+    和机械臂同一件事：话题 echo 偶发超时会把刚读到的开口清成“没连上”，
+    模型和顶栏就停在那里。超过 ``hold_s`` 仍没有新读数才放开。
+    """
+    if fresh.get("connected") or not previous or not previous.get("connected"):
+        return fresh
+    moment = time.time() if now is None else now
+    if moment - float(previous.get("stamp") or 0.0) > hold_s:
+        return fresh
+    kept = dict(fresh)
+    for key in (
+        "connected", "initialized", "permille", "opening_mm", "stroke_mm",
+        "rotation_deg", "force", "speed", "grip_state", "grip_text",
+        "rotation_text", "moving", "gripped", "pose_live", "rotary", "source",
+    ):
+        if key in previous:
+            kept[key] = previous[key]
+    kept["summary"] = str(previous.get("summary") or fresh.get("summary") or "")
+    note = "沿用上次读数。"
+    if note not in kept["summary"]:
+        kept["summary"] = f"{kept['summary']} {note}".strip()
+    return kept
+
+
+def pause_serial() -> None:
+    """控制命令要自己开串口，或把驱动拉起来。先拦住状态线程，等它把口放开。
+
+    一次短读要过好几个寄存器，1.5 秒经常还没结束。提前返回会让新驱动
+    去抢还被占着的口，然后以「需要 24 V」失败。
+    """
+    global _PORT_HOLD
+    with _PORT_LOCK:
+        _PORT_HOLD += 1
+    deadline = time.time() + 6.0
+    while time.time() < deadline:
+        with _PORT_LOCK:
+            busy = _PORT_BUSY
+        if not busy:
+            return
+        time.sleep(0.05)
+
+
+def hold_serial(pid: int) -> None:
+    """后台夹爪脚本还活着时，状态线程不要去开串口。"""
+    pause_serial()
+
+    def _watch() -> None:
+        while pid > 0:
+            try:
+                os.kill(pid, 0)
+            except OSError:
+                break
+            time.sleep(0.2)
+        resume_serial()
+
+    threading.Thread(target=_watch, daemon=True, name="gripper-port-hold").start()
+
+
+def resume_serial() -> None:
+    global _PORT_HOLD
+    with _PORT_LOCK:
+        _PORT_HOLD = max(0, _PORT_HOLD - 1)
+
+
+def serial_paused() -> bool:
+    with _PORT_LOCK:
+        return _PORT_HOLD > 0
+
+
+def start() -> None:
+    global _STARTED
+    with _LOCK:
+        if _STARTED:
+            return
+        _STARTED = True
+    threading.Thread(target=_loop, daemon=True, name="gripper-state").start()
+
+
+def _loop() -> None:
+    while True:
+        try:
+            arms = _arms()
+            nodes = _nodes()
+            for arm in arms:
+                reading, error = _read_one(arm, nodes)
+                fresh = present(
+                    arm["id"],
+                    rotary=arm["rotary"],
+                    stroke_mm=arm["stroke_mm"],
+                    reading=reading,
+                    error=error,
+                )
+                _store(retain_reading(fresh, snapshot(arm["id"])))
+        except Exception as exc:  # noqa: BLE001 —— 读夹爪失败不能拖死界面
+            for row in _arms():
+                _store(present(
+                    row["id"], rotary=bool(row.get("rotary")), stroke_mm=float(row.get("stroke_mm") or 20.0),
+                    reading=None, error=str(exc),
+                ))
+        time.sleep(2.5)
+
+
+def _store(row: dict) -> None:
+    with _LOCK:
+        _CACHE[str(row.get("arm") or "")] = row
+
+
+def _arms() -> list[dict]:
+    path = ros2_root() / "config" / "site.yaml"
+    if not path.is_file():
+        return []
+    data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    shared = data.get("gripper") if isinstance(data.get("gripper"), dict) else {}
+    baud = int(shared.get("baud") or 115200)
+    slave = int(shared.get("slave_id") or 1)
+    rts = str(shared.get("rs485_rts") or "")
+    out = []
+    for row in listed_arms(data):
+        item = dict(row)
+        item["baud"] = baud
+        item["slave"] = slave
+        item["rts"] = rts
+        out.append(item)
+    return out
+
+
+def _nodes() -> dict[str, bool]:
+    text = _pgrep("dh_pgea_node|dual_gripper")
+    return {
+        "pgea": "dh_pgea_node" in text or "lib/arm_application/dh_pgea" in text,
+        "dual": "dual_gripper" in text,
+    }
+
+
+def _read_one(arm: dict, nodes: dict[str, bool]) -> tuple[dict | None, str]:
+    if nodes.get("dual") and arm.get("side") in ("left", "right"):
+        topic = f"/dual_gripper_driver/{arm['side']}/status"
+        parsed = _parse_status_echo(_echo(topic))
+        if parsed is not None:
+            parsed["source"] = f"话题 {topic}"
+            return parsed, ""
+        return None, "夹爪节点在跑，但这侧话题没有数据"
+    if not arm.get("rotary") and nodes.get("pgea"):
+        parsed = _parse_states_echo(_echo("/gripper/states"))
+        if parsed is not None:
+            parsed["source"] = "话题 /gripper/states"
+            return parsed, ""
+        return None, "平行爪节点在跑，但 /gripper/states 没有数据"
+    if not arm.get("port"):
+        return None, "site.yaml 里这台夹爪没有串口"
+    if serial_paused():
+        return None, "控制命令正在用串口"
+    return _read_serial(arm)
+
+
+def _read_serial(arm: dict) -> tuple[dict | None, str]:
+    """短连串口只读。auto_initialize 必须关掉，否则旋转爪一连上就会找零。"""
+    global _PORT_BUSY
+    if serial_paused():
+        return None, "控制命令正在用串口"
+    with _PORT_LOCK:
+        _PORT_BUSY += 1
+    try:
+        return _read_serial_open(arm)
+    finally:
+        with _PORT_LOCK:
+            _PORT_BUSY = max(0, _PORT_BUSY - 1)
+
+
+def _read_serial_open(arm: dict) -> tuple[dict | None, str]:
+    try:
+        if arm["rotary"]:
+            from arm_application.dual_gripper import RotaryGripper, RotaryGripperConfig
+            grip = RotaryGripper(RotaryGripperConfig(
+                port=arm["port"],
+                slave_id=int(arm["slave"]),
+                baudrate=int(arm["baud"]),
+                timeout=0.25,
+                auto_initialize=False,
+            ))
+            grip.connect()
+            try:
+                status = grip.get_status()
+            finally:
+                grip.disconnect()
+            if not status.connected:
+                return None, status.error_msg or "旋转爪没有应答"
+            return {
+                "connected": True,
+                "initialized": bool(status.initialized),
+                "permille": float(status.finger_position_per_mille),
+                "rotation_deg": float(status.rotation_deg),
+                "force": float(status.force),
+                "speed": float(status.velocity),
+                "grip_state": int(status.gripper_state),
+                "rotation_state": int(status.rotation_state),
+                "moving": bool(status.is_moving),
+                "gripped": bool(status.is_gripped),
+                "error_msg": status.error_msg or "",
+                "source": f"串口 {arm['port']} 只读",
+            }, ""
+        from arm_application.dh_pgea import DhPgeaError, DhPgeaGripper
+        grip = DhPgeaGripper(
+            port=arm["port"],
+            baudrate=int(arm["baud"]),
+            slave_id=int(arm["slave"]),
+            stroke_mm=float(arm["stroke_mm"]),
+            rs485_rts=str(arm["rts"]),
+        )
+        grip.open()
+        try:
+            state = grip.get_run_states(retries=1)
+        finally:
+            grip.close()
+        return {
+            "connected": True,
+            "initialized": bool(state.initialized),
+            "permille": float(state.position),
+            "rotation_deg": 0.0,
+            "force": float(state.target_force),
+            "speed": None,
+            "grip_state": int(state.grip_state),
+            "rotation_state": None,
+            "moving": int(state.grip_state) == 0,
+            "gripped": int(state.grip_state) == 2,
+            "error_msg": "",
+            "source": f"串口 {arm['port']} 只读",
+        }, ""
+    except Exception as exc:  # noqa: BLE001 —— 串口被节点占着、没上电，都要变成一句人话
+        text = str(exc).strip() or exc.__class__.__name__
+        if "DhPgeaError" in exc.__class__.__name__ or "Permission" in text or "busy" in text.lower():
+            return None, text
+        return None, text
+
+
+def _echo(topic: str) -> str:
+    try:
+        proc = subprocess.run(
+            ["timeout", "3", "ros2", "topic", "echo", topic, "--once"],
+            capture_output=True, text=True, timeout=5,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return ""
+    return proc.stdout
+
+
+def _parse_status_echo(text: str) -> dict | None:
+    for chunk in text.split("---"):
+        if "finger_position_per_mille:" not in chunk and "connected:" not in chunk:
+            continue
+        try:
+            data = yaml.safe_load(chunk)
+        except yaml.YAMLError:
+            continue
+        if not isinstance(data, dict) or "connected" not in data:
+            continue
+        return {
+            "connected": bool(data.get("connected")),
+            "initialized": bool(data.get("initialized")),
+            "permille": data.get("finger_position_per_mille"),
+            "rotation_deg": data.get("rotation_deg"),
+            "force": data.get("force"),
+            "speed": data.get("velocity"),
+            "grip_state": data.get("gripper_state"),
+            "rotation_state": data.get("rotation_state"),
+            "moving": bool(data.get("is_moving")),
+            "gripped": bool(data.get("is_gripped")),
+            "error_msg": str(data.get("error_msg") or ""),
+        }
+    return None
+
+
+def _parse_states_echo(text: str) -> dict | None:
+    for chunk in text.split("---"):
+        if "data:" not in chunk:
+            continue
+        try:
+            data = yaml.safe_load(chunk)
+        except yaml.YAMLError:
+            continue
+        if not isinstance(data, dict):
+            continue
+        raw = data.get("data")
+        if not isinstance(raw, list) or len(raw) < 5:
+            continue
+        try:
+            init, grip, pos, _target, force = (float(v) for v in raw[:5])
+        except (TypeError, ValueError):
+            continue
+        return {
+            "connected": True,
+            "initialized": init >= 0.5,
+            "permille": pos,
+            "rotation_deg": 0.0,
+            "force": force,
+            "speed": None,
+            "grip_state": int(grip),
+            "rotation_state": None,
+            "moving": int(grip) == 0,
+            "gripped": int(grip) == 2,
+            "error_msg": "",
+        }
+    return None
+
+
+def _pgrep(pattern: str) -> str:
+    try:
+        proc = subprocess.run(
+            ["pgrep", "-af", pattern],
+            capture_output=True, text=True, timeout=2,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return ""
+    return "\n".join(line for line in proc.stdout.splitlines() if "pgrep" not in line)
+
+
+def _finite(raw: Any) -> float | None:
+    try:
+        value = float(raw)
+    except (TypeError, ValueError):
+        return None
+    if value != value:
+        return None
+    return value
+
+
+def _unit(raw: Any, low: float, high: float) -> float | None:
+    value = _finite(raw)
+    if value is None:
+        return None
+    return max(low, min(high, value))

build/arm_application/build/lib/arm_application/operator_handeye_board.py

@@ -0,0 +1,91 @@
+"""手眼页的棋盘预览。只在已有彩色画面上画角点，不打开相机，也不动臂。"""
+
+from __future__ import annotations
+
+import threading
+import time
+
+import numpy as np
+
+from arm_application.handeye_lib import BoardConfig, detect_chessboard
+
+
+def coverage_fraction(corners: np.ndarray, width: int, height: int) -> float:
+    """角点外接框占画面的比例。宽或高为 0 时返回 0。"""
+    if width <= 0 or height <= 0:
+        return 0.0
+    pts = np.asarray(corners, dtype=float).reshape(-1, 2)
+    if pts.size == 0:
+        return 0.0
+    area = float(pts[:, 0].max() - pts[:, 0].min()) * float(pts[:, 1].max() - pts[:, 1].min())
+    return area / float(width * height)
+
+
+def coverage_words(fraction: float) -> str:
+    """30%–70% 是示教时棋盘该占的画面。再小对不准，再大姿态拉不开。"""
+    if fraction < 0.30:
+        return "偏小，再靠近一点"
+    if fraction > 0.70:
+        return "偏大，再离远一点"
+    return "大小合适"
+
+
+def annotate_board(jpeg: bytes, cols: int, rows: int) -> tuple[bytes, str]:
+    """返回（可能画了角点的 JPEG，一行中文状态）。检测失败时 JPEG 原样返回。"""
+    import cv2
+
+    if cols <= 0 or rows <= 0:
+        return jpeg, "配置里没有这块板的内角点"
+    buf = np.frombuffer(jpeg, dtype=np.uint8)
+    image = cv2.imdecode(buf, cv2.IMREAD_COLOR)
+    if image is None:
+        return jpeg, "这一帧解不出来"
+    found = detect_chessboard(image, BoardConfig(int(cols), int(rows), 1.0))
+    if found is None:
+        return jpeg, "画面里没有这块板"
+    corners, pattern = found
+    drawn = image.copy()
+    cv2.drawChessboardCorners(drawn, pattern, corners, True)
+    fraction = coverage_fraction(corners, drawn.shape[1], drawn.shape[0])
+    ok, out = cv2.imencode(".jpg", drawn, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
+    blob = out.tobytes() if ok else jpeg
+    percent = int(round(fraction * 100))
+    return blob, f"棋盘已找到，约占画面 {percent}%。{coverage_words(fraction)}"
+
+
+class BoardWatch:
+    """后台画角点。界面线程只提交最新一帧，检测再慢也不卡住窗口。"""
+
+    def __init__(self) -> None:
+        self._lock = threading.Lock()
+        self._pending: tuple | None = None
+        self._result = {"token": 0, "jpeg": b"", "text": ""}
+        self._stop = threading.Event()
+        self._thread = threading.Thread(target=self._loop, daemon=True, name="handeye-board")
+        self._thread.start()
+
+    def submit(self, token: int, jpeg: bytes, cols: int, rows: int) -> None:
+        with self._lock:
+            if self._result["token"] == token and not self._pending:
+                return
+            self._pending = (token, jpeg, int(cols), int(rows))
+
+    def latest(self) -> tuple[bytes, str, int]:
+        with self._lock:
+            return self._result["jpeg"], self._result["text"], int(self._result["token"])
+
+    def _loop(self) -> None:
+        while not self._stop.is_set():
+            with self._lock:
+                job = self._pending
+                self._pending = None
+            if job is None:
+                time.sleep(0.05)
+                continue
+            token, jpeg, cols, rows = job
+            try:
+                blob, text = annotate_board(jpeg, cols, rows)
+            except Exception as exc:  # noqa: BLE001 —— 检测失败不能拖死预览
+                blob, text = jpeg, f"棋盘检测失败：{exc}"
+            with self._lock:
+                self._result = {"token": token, "jpeg": blob, "text": text}

build/arm_application/build/lib/arm_application/operator_teach.py

@@ -8,6 +8,8 @@
 
 from __future__ import annotations
 
+import re
+import subprocess
 import threading
 from datetime import datetime, timezone
 from pathlib import Path
@@ -17,13 +19,17 @@ import yaml
 _LOCK = threading.Lock()
 _BRIDGE = None
 _SN = ""
+_YIELDED = False
+_ARM_CONTROLLER = "rizon_arm_controller"
 
 
 def _file(arm: str) -> Path:
     from arm_application.operator_console import ROOT
+    from arm_application.site_io import listed_arms, load_site_dict
 
-    if arm not in ("89", "91"):
+    known = {row["id"] for row in listed_arms(load_site_dict())}
-        raise ValueError("臂号只接受 89 或 91")
+    if arm not in known:
+        raise ValueError(f"臂号 {arm} 不在 site.yaml 里")
     return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
 
 
@@ -36,9 +42,14 @@ def _bridge_for(sn: str):
         return _BRIDGE
     close()
     from arm_application.handeye_teach import RdkBridge
+    from arm_application.operator_arm_state import release_for_control, resume_after_control
 
-    _BRIDGE = RdkBridge(sn)
+    release_for_control()
-    _SN = sn
+    try:
+        _BRIDGE = RdkBridge(sn)
+        _SN = sn
+    finally:
+        resume_after_control()
     return _BRIDGE
 
 
@@ -55,19 +66,95 @@ def close() -> None:
         pass
 
 
+def _switch_arm_controller(activate: bool) -> str:
+    """停或恢复关节轨迹控制器。硬件还在，只是不再下发关节位置。
+
+    控制栈在 ``NRT_JOINT_POSITION`` 里持续 ``SendJointPosition``，
+    模式一旦被改走，写循环两秒内又切回去。软拖拽要低阻抗，必须先停这条控制器。
+    """
+    if activate:
+        payload = (
+            "{activate_controllers: ['%s'], deactivate_controllers: [], "
+            "strictness: 2, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
+            % _ARM_CONTROLLER
+        )
+    else:
+        payload = (
+            "{activate_controllers: [], deactivate_controllers: ['%s'], "
+            "strictness: 1, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
+            % _ARM_CONTROLLER
+        )
+    try:
+        proc = subprocess.run(
+            [
+                "ros2", "service", "call",
+                "/controller_manager/switch_controller",
+                "controller_manager_msgs/srv/SwitchController",
+                payload,
+            ],
+            capture_output=True,
+            text=True,
+            timeout=15,
+        )
+    except FileNotFoundError:
+        return "找不到 ros2，停不了关节轨迹控制器"
+    except subprocess.TimeoutExpired:
+        return "关节轨迹控制器没回来" if activate else "停关节轨迹控制器超时"
+    text = ((proc.stdout or "") + (proc.stderr or "")).strip()
+    if proc.returncode == 0 and re.search(r"ok\s*[:=]\s*true", text, re.IGNORECASE):
+        return ""
+    tail = text.splitlines()[-1] if text else f"退出码 {proc.returncode}"
+    verb = "恢复" if activate else "停下"
+    return f"没能{verb}关节轨迹控制器：{tail}"
+
+
+def _yield_position_stream() -> str:
+    """控制栈没起来就什么都不做。起来了就先把位置控制器让开。"""
+    global _YIELDED
+    from arm_application.operator_arm_state import stack_running
+
+    if not stack_running():
+        return ""
+    err = _switch_arm_controller(False)
+    if err:
+        return err + "。位置环还在推，软拖拽切不过去。"
+    _YIELDED = True
+    return ""
+
+
+def _restore_position_stream() -> str:
+    global _YIELDED
+    if not _YIELDED:
+        return ""
+    err = _switch_arm_controller(True)
+    _YIELDED = False
+    return err
+
+
 def drag_on(sn: str) -> dict:
     with _LOCK:
         try:
+            yielded = _yield_position_stream()
+            if yielded:
+                return {"ok": False, "message": yielded}
             bridge = _bridge_for(sn)
             result = bridge.drag_on(mode="soft")
         except Exception as exc:
-            return {"ok": False, "message": str(exc)}
+            restore = _restore_position_stream()
-    if not result.get("ok"):
+            message = str(exc)
-        return {"ok": False, "message": str(result.get("error") or result)}
+            if restore:
-    return {
+                message = f"{message}；{restore}"
-        "ok": True,
+            return {"ok": False, "message": message}
-        "message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
+        if not result.get("ok"):
-    }
+            restore = _restore_position_stream()
+            message = str(result.get("error") or result)
+            if restore:
+                message = f"{message}；{restore}"
+            return {"ok": False, "message": message}
+        note = "软拖拽已开启。轻推机械臂，再记点。"
+        if _YIELDED:
+            note += "关节轨迹控制器已停下，关掉拖拽后会回来。"
+        return {"ok": True, "message": note}
 
 
 def drag_off(sn: str) -> dict:
@@ -76,9 +163,16 @@ def drag_off(sn: str) -> dict:
             bridge = _bridge_for(sn)
             result = bridge.drag_off()
         except Exception as exc:
-            return {"ok": False, "message": str(exc)}
+            restore = _restore_position_stream()
+            message = str(exc)
+            if restore:
+                message = f"{message}；{restore}"
+            return {"ok": False, "message": message}
     if not result.get("ok"):
         return {"ok": False, "message": str(result.get("error") or result)}
+    restore = _restore_position_stream()
+    if restore:
+        return {"ok": False, "message": f"软拖拽已关闭，但{restore}"}
     return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"}
 
 

build/arm_application/build/lib/arm_application/overhead_oneshot.py

@@ -45,6 +45,11 @@ from arm_application.path_profile import (  # 伺服和测试从本模块再导
 
 # 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
 D435_MIN_PROGRESS_FRAC = 0.40
+# SwitchMode 返回不等于已经进 primitive。连试仍失败必须让一阶段立刻中止，
+# 不许空等 150s 装成"还在粗定位"（2026-10-16 17:27）。
+D435_MOVEL_RETRY = 3
+D435_MOVEL_RETRY_S = 0.3
+D435_MOVEL_GAVE_UP = "厂家 MoveL 连试 3 次没走成"
 
 
 def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
@@ -154,16 +159,27 @@ def d405_commands_arm(
 ) -> bool:
     """D405 这一帧的点能不能下发给左臂。
 
-    推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
+    推理从启动就开着。没有高置信度目标、或 D435 厂家 MoveL 还没结束、
-    停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
+    法兰还没连续停稳时，这帧**不下发**（输出截断，不许拿去控臂）。
-    也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
+    厂家移动结束并且法兰停稳（``settled``，时长是 ``d435_handback_sec``）
-    ``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
+    之后才放行，伺服用它追工件框的矩形中心。框心对准停稳后才进精拍下臂。
+    ``move_only``：只测 D435 位移，D405 点一律不下发。
     """
     if move_only:
         return False
-    if not using_d435:
+    if using_d435:
-        return True
+        return bool(arrived) and bool(settled)
-    return bool(arrived) and bool(settled)
+    return True
+
+
+def retract_goal_z(z_now: float, z_target: float) -> float:
+    """收臂目标 Z：已经高于目标就钉在当前高度，不许为了追 5 mm 往下补。
+
+    2026-10-16：贴面 MoveL 之后 JTC 还握着悬停关节，收臂 hop1 计划 +5 mm
+    实际抬了 18–25 mm；hop2 把「抬过头」当成误差 ``Δz=−13 mm`` 往回压，
+    看起来就是进孔再弹出。抬过了 = 成功。
+    """
+    return max(float(z_now), float(z_target))
 
 
 def oneshot_needs_verticalize(

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

@@ -11,31 +11,116 @@
 from __future__ import annotations
 
 import json
+import math
 import os
 import signal
 import subprocess
 import time
-from typing import Optional
+from pathlib import Path
+from typing import Optional, Tuple
 
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PointStamped
-from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
 from std_msgs.msg import Bool, String
+from trajectory_msgs.msg import JointTrajectory
 
+from arm_application.gauge_kin import (
+    a7_limit_target,
+    match_limits,
+    parse_joint_limits,
+)
+from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
+from arm_application.site_io import load_site_dict
+from arm_application.stage2_locate_node import _pt
 from arm_application.stage3_search_node import Stage3Search
 
 
-#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
+#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
 #:
-#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
+#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
-#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
+#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
-#: 小幅下降（只需 5cm 以内的行程）。
+#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
 #:
 #: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
 #: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
 #: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
 TRACK_CLEARANCE_M = 0.05
+#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
+TRACK_LOCK_GAP_M = 0.040
+
+
+def init_z_refusal(
+    z_m: float,
+    tool_len_m: float,
+    surface_z_m: float,
+    gap_m: float,
+    z_min: float,
+    z_max: float,
+) -> str:
+    """初始法兰高度能不能用。空串表示可以；非空就是拒绝原因，不许悄悄抬高。"""
+    z = float(z_m)
+    if z < float(z_min) - 1e-6 or z > float(z_max) + 1e-6:
+        return (
+            f"初始法兰 Z {z:.3f}m 超出工作盒 [{float(z_min):.3f}, {float(z_max):.3f}]，"
+            "拒绝开跑"
+        )
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    if tool <= 0.0 or surf <= 0.0:
+        return "写了初始高度，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
+    clear = z - tool - surf
+    if clear < float(gap_m) - 0.001:
+        return (
+            f"初始法兰 Z {z:.3f}m 会让工具尖进入工件："
+            f"尖端 {z - tool:.3f}，工件面 {surf:.3f}，"
+            f"间隙 {clear * 1000:.1f}mm，要求 ≥ {float(gap_m) * 1000:.1f}mm。"
+            "拒绝开跑，不会悄悄抬高。"
+        )
+    return ""
+
+
+def hold_at_insert_z(final, *, skip3: bool) -> bool:
+    """贴面已经按 ``final_gap_mm`` 停下时，不再进三阶段。
+
+    三阶段开局会抬到悬高上方测触觉，再落回 ``hole_hover_z_m``。
+    没有接触传感器。到位高度就是这次的贴面间隙，不另设一个 Z。
+    """
+    if skip3:
+        return True
+    if not isinstance(final, dict):
+        return False
+    return bool(final.get("insert_held"))
+
+
+def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
+    """一阶段锁高。
+
+    悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
+    对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
+    否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
+    """
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    hover = float(hover_z)
+    if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
+        return hover
+    return hover + TRACK_CLEARANCE_M
+
+
+def stage1_log_fatal(log_path: str) -> str:
+    """一阶段日志里已经写明失败时，不许再空等锁定。空串 = 还没到这一步。"""
+    try:
+        text = Path(log_path).read_text(encoding="utf-8", errors="replace")
+    except OSError:
+        return ""
+    if D435_MOVEL_GAVE_UP in text:
+        return (
+            f"D435 {D435_MOVEL_GAVE_UP}（RDK 控制模式没切到 primitive），"
+            f"一阶段中止。不是视野/标定。看 {log_path}"
+        )
+    return ""
 
 
 def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
@@ -52,6 +137,9 @@ def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) ->
     以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
     因为缺依赖早退了"被读成"算法不收敛"，白查半天。
     """
+    fatal = stage1_log_fatal(log_path)
+    if fatal:
+        return fatal
     if points_seen <= 0:
         return (
             f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
@@ -79,11 +167,26 @@ class AssemblyPipeline(Stage3Search):
         self.declare_parameter("skip_stage1", False)
         self.declare_parameter("skip_stage2", False)
         self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth
+        self.declare_parameter("d435_move_only", False)
+        self.declare_parameter("init_z_m", -1.0)  # >0：启动后先到这个法兰高度
         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._robot_description: Optional[str] = None
+        # robot_state_publisher 用 TRANSIENT_LOCAL 只发一次。默认 VOLATILE
+        # 订阅在它之后才起来，3s 内一条都收不到，A7 会因“没有限位”拒绝旋转。
+        self.create_subscription(
+            String,
+            "/robot_description",
+            self._on_robot_description,
+            QoSProfile(
+                depth=1,
+                durability=DurabilityPolicy.TRANSIENT_LOCAL,
+                reliability=ReliabilityPolicy.RELIABLE,
+            ),
+        )
         self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
         # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
         # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
@@ -113,9 +216,11 @@ class AssemblyPipeline(Stage3Search):
         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.d435_move_only = bool(self.get_parameter("d435_move_only").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"
+            f"skip1={skip1} skip2={skip2} skip3={skip3} "
+            f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
         )
 
     def _state(self, s: str) -> None:
@@ -125,6 +230,113 @@ class AssemblyPipeline(Stage3Search):
         self._state_pub.publish(msg)
         self._pipe_pub.publish(msg)
 
+    def _on_robot_description(self, msg: String) -> None:
+        self._robot_description = str(msg.data)
+
+    def _a7_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]:
+        """A7 软件限位。来自 ``/robot_description`` 的 joint7，与厂家 ``q_min/q_max`` 同一对。"""
+        t0 = time.monotonic()
+        while self._robot_description is None and time.monotonic() - t0 < timeout_s:
+            self._spin(0.05)
+        if not self._robot_description:
+            self.get_logger().error("拿不到 /robot_description，A7 没有限位，拒绝旋转")
+            return None
+        got = match_limits(
+            parse_joint_limits(self._robot_description), [self.joint_names[-1]]
+        )
+        if got is None:
+            self.get_logger().error(
+                f"/robot_description 里没有 {self.joint_names[-1]} 的限位，拒绝旋转"
+            )
+        return got
+
+    def _send_a7_only(self, a7_rad: float, speed_deg_s: float, tag: str) -> bool:
+        """只下发 A7。前六轴用这一拍的当前角，厂家 ``SendJointPosition`` 收到的也是这一组。"""
+        q, Tee = self._wait_state(5.0)
+        if q is None or Tee is None or int(q.size) != 7:
+            self.get_logger().error(f"{tag}: 拿不到 7 轴关节，不下发")
+            return False
+        z0 = float(Tee[2, 3])
+        q_hold = q.copy()
+        q_hold[6] = float(a7_rad)
+        delta_deg = abs(math.degrees(float(q_hold[6] - q[6])))
+        dur = max(0.8, delta_deg / max(float(speed_deg_s), 1.0))
+        zeros = np.zeros(q.size)
+        traj = JointTrajectory()
+        traj.header.stamp.sec = 0
+        traj.header.stamp.nanosec = 0
+        traj.joint_names = list(self.joint_names)
+        traj.points = [_pt(q, zeros, 0.2), _pt(q_hold, zeros, 0.2 + dur)]
+        self.get_logger().info(
+            f"{tag}: A7 {math.degrees(float(q[6])):+.1f}° → "
+            f"{math.degrees(float(q_hold[6])):+.1f}°，"
+            f"{speed_deg_s:.0f}°/s，前六轴不动，法兰 Z={z0:.4f}m"
+        )
+        self._send_action(traj, wait_s=dur + 8.0)
+        q2, Tee2 = self._wait_state(5.0)
+        if q2 is None or Tee2 is None:
+            self.get_logger().error(f"{tag}: 走完读不到关节")
+            return False
+        others = float(np.max(np.abs(q2[:6] - q[:6])))
+        dz = abs(float(Tee2[2, 3]) - z0)
+        a7_err = abs(float(q2[6] - q_hold[6]))
+        self.get_logger().info(
+            f"{tag}: 前六轴最大偏差 {others:.4f} rad，"
+            f"法兰 ΔZ={dz * 1000:.1f}mm，A7 残差 {math.degrees(a7_err):.1f}°"
+        )
+        if others > 0.02:
+            self.get_logger().error(f"{tag}: 前六轴动了 {others:.3f} rad，中止")
+            return False
+        if dz > 0.003:
+            self.get_logger().error(f"{tag}: 法兰 Z 变了 {dz * 1000:.1f}mm，中止")
+            return False
+        if a7_err > 0.05:
+            self.get_logger().error(
+                f"{tag}: A7 还差 {math.degrees(a7_err):.1f}°，没到限位内侧"
+            )
+            return False
+        self._pin(q2)
+        return True
+
+    def _run_a7_seat(self) -> bool:
+        """贴面后咬螺纹：A7 先逆时针到限位，再顺时针到限位。不改 Z。"""
+        limits = self._a7_limits()
+        if limits is None:
+            return False
+        ga = load_site_dict().get("gauge_assembly") or {}
+        try:
+            margin = float(ga["extreme_margin_deg"])
+            speed = float(ga["extreme_speed_deg_s"])
+            ccw = int(ga["extreme_direction"])
+        except (KeyError, TypeError, ValueError) as exc:
+            self.get_logger().error(
+                f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转"
+            )
+            return False
+        if ccw not in (-1, 1):
+            self.get_logger().error(
+                f"extreme_direction 只能是 +1/-1，收到 {ccw}，拒绝旋转"
+            )
+            return False
+        # +1 = 关节角增大 = 逆时针。第一步逆时针出孔让出牙，第二步顺时针进孔咬合。
+        steps = (
+            (ccw, "A7 逆时针至极限（出孔）"),
+            (-ccw, "A7 顺时针至极限（进孔）"),
+        )
+        for direction, tag in steps:
+            q, _Tee = self._wait_state(5.0)
+            if q is None:
+                self.get_logger().error(f"{tag}: 没有当前关节")
+                return False
+            target = a7_limit_target(q, limits, direction, margin)
+            if target is None:
+                self.get_logger().error(f"{tag}: 算不出 A7 限位目标")
+                return False
+            if not self._send_a7_only(float(target[6]), speed, tag):
+                return False
+        self.get_logger().info("A7 两步完成：逆时针到限位，再顺时针到限位。法兰高度未改。")
+        return True
+
     def _on_lock(self, msg: Bool) -> None:
         if bool(msg.data):
             self._locked = True
@@ -163,33 +375,64 @@ class AssemblyPipeline(Stage3Search):
         self._kill_old_stage1()
 
     def _track_z(self) -> float:
-        """跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
+        """一阶段锁高。写了 ``init_z_m`` 就用它，否则见 :func:`track_lock_z`。"""
-        return float(self.hover_z) + TRACK_CLEARANCE_M
+        z = float(self.get_parameter("init_z_m").value)
+        if z > 0.0:
+            return z
+        return track_lock_z(
+            float(self.hover_z),
+            float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
+            float(getattr(self, "surface_z_m", 0.0) or 0.0),
+        )
 
-    def _ensure_clearance(self) -> None:
+    def _ensure_clearance(self) -> bool:
-        """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
+        """没写初始高度：只在低于锁高时抬上去。写了：升或降到那个法兰 Z，不安全就拒绝。"""
+        user = float(self.get_parameter("init_z_m").value) > 0.0
         q, Tee = self._wait_state(4.0)
         if q is None or Tee is None:
-            return
+            if user:
+                self.get_logger().error("设了 --init-z，但拿不到当前位姿，拒绝开跑")
+                return False
+            return True
         tz = self._track_z()
+        if user:
+            why = init_z_refusal(
+                tz,
+                float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
+                float(getattr(self, "surface_z_m", 0.0) or 0.0),
+                float(self.final_gap_m),
+                float(self.ws_min[2]),
+                float(self.ws_max[2]),
+            )
+            if why:
+                self.get_logger().error(why)
+                return False
         z = float(Tee[2, 3])
-        if z >= tz - 0.008:
+        if user:
-            return
+            if abs(z - tz) <= 0.008:
+                self.get_logger().info(f"已在初始高度 Z={z:.3f}m（目标 {tz:.3f}）")
+                return True
+        elif z >= tz - 0.008:
+            return True
         self._state("pre_lift")
-        self.get_logger().info(
+        self.get_logger().info(f"先到初始高度：Z {z:.3f} → {tz:.3f}m")
-            f"先离开工件/孔口：Z {z:.3f} → {tz:.3f}m（跟踪高=hover+50mm）"
+        ok, q, Tee = self._goto(
-        )
-        _, 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)
+        z2 = float(Tee[2, 3])
+        if user and (not ok or abs(z2 - tz) > 0.008):
+            self.get_logger().error(f"初始高度没到位：Z={z2:.3f}，目标 {tz:.3f}")
+            return False
+        return True
 
     def _run_stage1(self) -> int:
         """拉起 perception+servo，等到 /visual_servo/locked 后冻结并拆跟踪。"""
-        self._ensure_clearance()
+        if not self._ensure_clearance():
+            return 2
         self._state("stage1_track")
         self._kill_old_stage1()
         self._locked = False
@@ -205,12 +448,17 @@ class AssemblyPipeline(Stage3Search):
             "enable_control:=true",
             "publish_handeye_tf:=false",
             f"ee_z_m:={lock_z:.3f}",
-            "lock_hold_sec:=0.6",
             f"use_sim_time:={str(use_sim).lower()}",
+            f"d435_move_only:={str(self.d435_move_only).lower()}",
         ]
-        self.get_logger().info(
+        if self.d435_move_only:
-            f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
+            self.get_logger().info(
-        )
+                f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
+            )
+        else:
+            self.get_logger().info(
+                f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
+            )
         self._s1 = subprocess.Popen(
             cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
         )
@@ -221,6 +469,11 @@ class AssemblyPipeline(Stage3Search):
             self._spin(0.1)
             if self._locked:
                 break
+            fatal = stage1_log_fatal(log_path)
+            if fatal:
+                self._stop_stage1()
+                self.get_logger().error(fatal)
+                return 1
             if self._s1.poll() is not None:
                 self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
                 return 1
@@ -230,7 +483,12 @@ class AssemblyPipeline(Stage3Search):
                 stage1_timeout_reason(self._point_msgs, timeout, log_path)
             )
             return 1
-        self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
+        if self.d435_move_only:
+            self.get_logger().info(
+                "D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
+            )
+        else:
+            self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
         self._stop_stage1()
         time.sleep(0.35)
         q = self._q()
@@ -242,6 +500,9 @@ class AssemblyPipeline(Stage3Search):
         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 self.d435_move_only:
+            skip2 = True
+            skip3 = True
         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
@@ -252,7 +513,8 @@ class AssemblyPipeline(Stage3Search):
                 if code != 0:
                     return code
             else:
-                self._ensure_clearance()
+                if not self._ensure_clearance():
+                    return 2
                 self._state("skip_stage1")
 
             q, Tee = self._wait_state(10.0)
@@ -267,19 +529,54 @@ class AssemblyPipeline(Stage3Search):
                 if code != 0:
                     self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
                     return code
-                self.get_logger().info(
+                stay = hold_at_insert_z(final, skip3=skip3)
-                    f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
+                if stay and final.get("insert_held"):
-                    + (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
+                    self.get_logger().info(
-                )
+                        f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — "
+                        f"插孔停在法兰 Z={final.get('flange_z_m')}m，"
+                        f"规尖离面 {final.get('tip_gap_mm')}mm"
+                        f"（目标 {final.get('commanded_gap_mm')}mm，来自 final_gap_mm）。"
+                        "没有接触传感器，不另设 Z，不抬回悬高。"
+                    )
+                else:
+                    self.get_logger().info(
+                        f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
+                        + (
+                            " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
+                            if skip3
+                            else " — 立刻下压搜孔并轻压（不拧入）"
+                        )
+                    )
             else:
                 self._state("skip_stage2")
+                stay = hold_at_insert_z(final, skip3=skip3)
 
-            if skip3:
+            if stay:
                 self._state("skip_stage3")
-                self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
+                if self.d435_move_only:
+                    self.get_logger().info(
+                        "D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
+                        "15s 后本进程退出，臂保持不动"
+                    )
+                elif isinstance(final, dict) and final.get("insert_held"):
+                    if q is not None:
+                        self._pin(q)
+                    self.get_logger().info(
+                        "插孔已停在贴面高度，不进搜孔、不抬回悬高。"
+                        "接着只转 A7：先逆时针到限位，再顺时针到限位。"
+                    )
+                    if not self._run_a7_seat():
+                        self.get_logger().error(
+                            "A7 旋转没走完，臂停在当前关节。不抬 Z。"
+                        )
+                else:
+                    self.get_logger().info(
+                        "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
+                        "15s 后本进程退出，臂保持不动"
+                    )
                 if final:
                     self._publish_result(final)
-                self._linger_hold(final if final else {"ok": True}, seconds=0.8)
+                self._linger_hold(final if final else {"ok": True}, seconds=15.0)
                 payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
                 try:
                     payload["stage2"] = json.loads(

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

@@ -21,6 +21,11 @@ class PlaneFeature:
     xyz: np.ndarray  # (3,) 相机坐标系下的立体点云质心
     n_inliers: int
     note: str
+    # 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。
+    # 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的
+    # 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的
+    # 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。
+    borrowed: bool = False
 
 
 def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:

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

@@ -32,16 +32,21 @@ from rclpy.qos import qos_profile_sensor_data
 from sensor_msgs.msg import CameraInfo, Image
 from std_msgs.msg import Float32MultiArray
 
+from arm_application.ros_node import TolerantNode, param_as_str
 
-def _param_as_str(node: Node, name: str, default: str = '') -> str:
+# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"：
-    """字符串参数，同时兼容命令行传数字的情况（例如序列号）。"""
+# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。
-    node.declare_parameter(
+# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）：
-        name, default, ParameterDescriptor(dynamic_typing=True)
+#   D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途
-    )
+#   `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失）
-    val = node.get_parameter(name).value
+#   之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦
-    if val is None or val == '':
+#   接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**，
-        return ''
+#   表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。
-    return str(val).strip()
+USB_RESEAT_HINT = (
+    '拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— '
+    '只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。'
+    '看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。'
+)
 
 
 def _fill_camera_info(
@@ -68,23 +73,17 @@ def _fill_camera_info(
 
 
 def _site_yaml_path() -> Path:
-    env = str(os.environ.get("SITE_CONFIG") or "").strip()
+    from arm_application.site_io import site_path
-    if env:
+
-        p = Path(env).expanduser()
+    return site_path()
-        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):
+class RealsenseColorPub(TolerantNode):
     def __init__(self) -> None:
         super().__init__('realsense_color_pub')
         self.declare_parameter('camera_name', 'd405')  # 话题命名空间
         self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
-        self._serial = _param_as_str(self, 'camera_serial', '')
+        self._serial = param_as_str(self, 'camera_serial', '')
         self.declare_parameter('width', 1280)
         self.declare_parameter('height', 720)
         self.declare_parameter('fps', 15)
@@ -177,6 +176,10 @@ class RealsenseColorPub(Node):
         self._overlay: Optional[list] = None
         self._overlay_stamp = 0.0
         self._capture_started = False
+        self._cap_lock = threading.Lock()
+        self._cap_event = threading.Event()
+        self._pending = None
+        self._last_frame_mono = 0.0
         self._usb_desc = ''
         self._last_ok = None
         self._site_yaml = _site_yaml_path()
@@ -310,23 +313,26 @@ class RealsenseColorPub(Node):
         self._info_pub.publish(info)
 
         if depth_u16 is not None:
-            dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
+            self._publish_depth(depth_u16, stamp)
-            dmsg = Image()
+
-            dmsg.header.stamp = stamp
+    def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
-            dmsg.header.frame_id = self._frame_id
+        dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
-            dmsg.height = dh
+        dmsg = Image()
-            dmsg.width = dw
+        dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
-            dmsg.encoding = '16UC1'
+        dmsg.header.frame_id = self._frame_id
-            dmsg.is_bigendian = 0
+        dmsg.height = dh
-            dmsg.step = dw * 2
+        dmsg.width = dw
-            dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
+        dmsg.encoding = '16UC1'
-            self._depth_pub.publish(dmsg)
+        dmsg.is_bigendian = 0
-            if not self._logged_depth:
+        dmsg.step = dw * 2
-                self._logged_depth = True
+        dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
-                self.get_logger().info(
+        self._depth_pub.publish(dmsg)
-                    f'首个对齐深度 {dw}x{dh} → '
+        if not self._logged_depth:
-                    f'/{self._name}/aligned_depth_to_color/image_raw'
+            self._logged_depth = True
-                )
+            self.get_logger().info(
+                f'首个对齐深度 {dw}x{dh} → '
+                f'/{self._name}/aligned_depth_to_color/image_raw'
+            )
 
     def _start_pipeline(self, reconnect: bool = False) -> None:
         try:
@@ -348,8 +354,8 @@ class RealsenseColorPub(Node):
         if not devices:
             raise RuntimeError(
                 '没有连接 RealSense（多次重试后 USB 仍为空）。'
-                '请把 D405 插到**本机**的 USB3（蓝色）口，拔插一次，'
+                '拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；'
-                '并关闭 realsense-viewer / 192.168.0.108 上占用该相机的采集端。'
+                + USB_RESEAT_HINT
             )
 
         found = self._pick_device(devices, rs)
@@ -374,9 +380,12 @@ class RealsenseColorPub(Node):
             f'requested {self._width}x{self._height}@{self._fps}Hz'
         )
         if usb2:
+            # 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
+            # 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
+            # （SuperSpeed 没协商上）。
             self.get_logger().warn(
-                f'{cam_name} 接在 USB {usb_desc} 上 —— 深度/IR 将保持 '
+                f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 '
-                '640x480@15。把 D405 插到 USB3（蓝色）口可获得 848@30。'
+                f'640x480@15Hz。' + USB_RESEAT_HINT
             )
             self._width, self._height, self._fps = 640, 480, 15
 
@@ -455,7 +464,9 @@ class RealsenseColorPub(Node):
                         trial.enable_stream(
                             rs.stream.infrared, 2, w, h, rs.format.y8, fps
                         )
-                    profile = pipe.start(trial)
+                    # 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
+                    # 场景一动红外对不齐，彩色帧就在这里被丢掉。
+                    profile = pipe.start(trial, self._on_sdk_frame)
                     have_depth = with_depth
                     have_infra = infra_mode != 'none'
                     self._infra_mode = infra_mode
@@ -501,11 +512,18 @@ class RealsenseColorPub(Node):
                 break
         if profile is None:
             raise RuntimeError(
-                f'D405 没有可用的彩色配置。最后的错误：{last_err}'
+                f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
             )
 
         self._pipe = pipe
         self._profile = profile
+        try:
+            sensor = profile.get_device().first_color_sensor()
+            if sensor.supports(rs.option.frames_queue_size):
+                # 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
+                sensor.set_option(rs.option.frames_queue_size, 1)
+        except Exception:  # noqa: BLE001
+            pass
         self._have_depth = have_depth
         self._have_infra = have_infra
         self._logged_depth = False
@@ -607,7 +625,7 @@ class RealsenseColorPub(Node):
 
         if not getattr(self, '_capture_started', False):
             self._capture_started = True
-            self._thread = threading.Thread(target=self._capture_loop, daemon=True)
+            self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish")
             self._thread.start()
 
     def _enable_color_ae_fast(self, profile, rs) -> None:
@@ -700,18 +718,60 @@ class RealsenseColorPub(Node):
             )
             return False
 
-    def _capture_loop(self) -> None:
+    def _on_sdk_frame(self, frame) -> None:
+        """SDK 回调。只拷走彩色并握住这一组帧，立刻返回。
+
+        对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取，
+        动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。
+        """
+        try:
+            frames = frame.as_frameset()
+            color = frames.get_color_frame()
+            if not color:
+                return
+            bgr = np.array(np.asanyarray(color.get_data()), copy=True)
+            frames.keep()
+            with self._cap_lock:
+                self._pending = (bgr, frames)
+            self._last_frame_mono = time.monotonic()
+            self._cap_event.set()
+        except Exception as exc:  # noqa: BLE001
+            try:
+                self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0)
+            except Exception:  # noqa: BLE001
+                pass
+
+    def _publish_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
+            if not self._cap_event.wait(0.2):
+                silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0
+                if silent:
+                    usb = getattr(self, '_usb_desc', '?')
+                    self.get_logger().error(
+                        f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— '
+                        '请给 USB 线缆留点余量，别让机械臂把它拽掉。'
+                    )
+                    self._last_frame_mono = 0.0
+                    got = False
+                    self._reconnect()
+                continue
+            self._cap_event.clear()
+            with self._cap_lock:
+                item = self._pending
+                self._pending = None
+            if item is None:
+                continue
+            bgr, frames = item
             try:
-                t_wait0 = time.monotonic()
+                t_pub = time.monotonic()
-                frames = self._pipe.wait_for_frames(timeout_ms=2000)
+                # 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。
-                self._t_wait = time.monotonic() - t_wait0
+                self._publish_frame(bgr, None)
+                self._t_pub = time.monotonic() - t_pub
                 ir_l = (
                     frames.get_infrared_frame(1)
                     if self._have_infra and self._infra_mode in ('both', 'left')
@@ -722,44 +782,45 @@ class RealsenseColorPub(Node):
                     if self._have_infra and self._infra_mode in ('both', 'right')
                     else None
                 )
+                aligned = frames
                 if self._align is not None:
                     t_al = time.monotonic()
-                    frames = self._align.process(frames)
+                    aligned = self._align.process(frames)
                     self._t_align = time.monotonic() - t_al
+                depth = aligned.get_depth_frame()
                 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:
+                if dropped:
                     usb = getattr(self, '_usb_desc', '?')
                     self.get_logger().error(
                         f'相机数据流中断（USB={usb}）：{text[:120]}。'
                         '正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
                     )
                     got = False
-                    misses = 0
+                    self._last_frame_mono = 0.0
                     self._reconnect()
-                    continue
+                else:
-                if misses == 1:
+                    self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
-                    self.get_logger().warn(f'wait_for_frames：{exc}')
                 continue
-            if not color:
-                misses += 1
-                continue
-            misses = 0
-            bgr = np.asanyarray(color.get_data())
-            depth = frames.get_depth_frame()
             depth_u16 = np.asanyarray(depth.get_data()) if depth else None
             if depth_u16 is not None and self._depth_to_mm != 1.0:
                 # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
                 depth_u16 = np.clip(
                     depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
                 ).astype(np.uint16)
+            if depth_u16 is not None:
+                try:
+                    self._publish_depth(depth_u16)
+                except Exception as exc:  # noqa: BLE001
+                    if not self._running:
+                        break
+                    self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
             self._frame_i += 1
             self._fps_meter(color, depth, depth_u16)
             # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
@@ -792,7 +853,6 @@ class RealsenseColorPub(Node):
                     )
             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:

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

@@ -27,6 +27,7 @@ from sensor_msgs.msg import Image
 from std_msgs.msg import Float32MultiArray
 
 from arm_application.realsense_color_pub import _site_yaml_path
+from arm_application.ros_node import TolerantNode
 
 
 def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
@@ -56,15 +57,22 @@ def _mono_to_bgr(mono: np.ndarray) -> np.ndarray:
     return np.ascontiguousarray(np.stack([mono, mono, mono], axis=-1))
 
 
-class RealsenseView(Node):
+class RealsenseView(TolerantNode):
     def __init__(self) -> None:
         super().__init__('realsense_view')
         self.declare_parameter('camera_name', 'd405')
         self.declare_parameter('overlay_hold_sec', 0.10)
+        self.declare_parameter('overlay_detection', True)
+        # D405 用 workpiece/detection；D435 用 workpiece/detection_overhead（像素坐标系不同）。
+        self.declare_parameter('detection_topic', 'workpiece/detection')
         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._overlay_detection = bool(self.get_parameter('overlay_detection').value)
+        self._det_topic = str(self.get_parameter('detection_topic').value or '').strip() or (
+            'workpiece/detection'
+        )
         self._hud = str(self.get_parameter('hud_label').value or '').strip()
 
         self._color: Optional[Image] = None
@@ -101,9 +109,10 @@ class RealsenseView(Node):
             depth=5,
             reliability=ReliabilityPolicy.RELIABLE,
         )
-        self.create_subscription(
+        if self._overlay_detection:
-            Float32MultiArray, 'workpiece/detection', self._on_det, det_qos
+            self.create_subscription(
-        )
+                Float32MultiArray, self._det_topic, self._on_det, det_qos
+            )
         # stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示
         self.create_subscription(
             Image, '/stage2/overlay', self._on_stage2_overlay, qos_profile_sensor_data
@@ -124,6 +133,7 @@ class RealsenseView(Node):
         self.create_timer(1.0 / hz, self._push)
         self.get_logger().info(
             f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 '
+            f'检测={self._det_topic if self._overlay_detection else "关"} '
             f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。'
         )
 
@@ -186,6 +196,9 @@ class RealsenseView(Node):
             self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
             return
         if "overlay_hold_sec" in cam:
+            # 顶视检测频率低，保留 launch 里设的较长 hold，别被 site 的 0.1s 冲掉。
+            if "overhead" in self._det_topic:
+                return
             hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
             if abs(hold - self._hold) > 1e-6:
                 self.get_logger().info(

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

@@ -12,6 +12,8 @@ from rclpy.node import Node
 from arm_application.site_io import joint_prefix_default
 from arm_msgs.action import FollowJointTrajectory
 
+from arm_application.ros_node import TolerantNode
+
 DEFAULT_JOINTS = [
     'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
 ]
@@ -25,7 +27,7 @@ DEFAULT_POSITIONS = [
 ]
 
 
-class RunJointTrajectory(Node):
+class RunJointTrajectory(TolerantNode):
     def __init__(self) -> None:
         super().__init__('run_joint_trajectory')
         self.declare_parameter('joint_prefix', joint_prefix_default())

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

@@ -80,3 +80,137 @@ def ee_frame_default() -> str:
     sn = os.environ.get("SITE_ROBOT_SN", "").strip()
     return f"{sn}_flange" if sn else ""
 
+
+def overhead_camera(
+    site: dict[str, Any], robot: dict[str, Any] | None = None
+) -> tuple[dict[str, Any], list[str]]:
+    """手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。
+
+    手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时
+    相机固定在工位外，不随臂动）。
+
+    **为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方——
+    S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的
+    后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。
+    所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。
+
+    收敛规则（确定性的三级）：
+      1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）；
+      2. 都没有 ``arm``（= 双臂共用的那台）时回落到它；
+      3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。
+    """
+    cams = [
+        cfg
+        for cfg in (site.get("cameras") or {}).values()
+        if isinstance(cfg, dict)
+        and str(cfg.get("role") or "").strip().lower().startswith("overhead")
+    ]
+    if not cams:
+        return {}, []
+    arm = str((robot or {}).get("arm") or "").strip()
+    mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
+    hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
+    chosen = dict(hits[0])
+    others = [str(c.get("name") or "?") for c in hits[1:]]
+    return chosen, others
+
+
+def eye_to_hand_result_name(short: str, camera_name: str) -> str:
+    """眼在手外结果文件的**文件名**（不含目录）。
+
+    外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用，
+    所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼
+    （launch / export_site_env / S03 找文件都走这一处）。
+    """
+    short = str(short or "").strip()
+    cam = str(camera_name or "").strip().strip("/")
+    if short and cam:
+        return f"{short}_{cam}_eye_to_hand.yaml"
+    if cam:
+        return f"{cam}_eye_to_hand.yaml"
+    if short:
+        return f"{short}_eye_to_hand.yaml"
+    return "eye_to_hand.yaml"
+
+
+def resolve_eye_to_hand_file(
+    root: Path,
+    *,
+    explicit: str = "",
+    camera_name: str = "",
+) -> Path | None:
+    """找手外相机的手眼文件。没有就返回 None。
+
+    路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在
+    "换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
+
+    候选顺序：
+      1. ``explicit`` —— launch 按 site.yaml 派生的确切路径
+         （``<臂号>_<相机>_eye_to_hand.yaml``）；
+      2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）；
+      3. ``eye_to_hand.yaml`` —— 再兜底。
+    """
+    candidates: list[Path] = []
+    if explicit:
+        p = Path(explicit).expanduser()
+        candidates.append(p if p.is_absolute() else root / p)
+    cam = str(camera_name or "").strip().strip("/")
+    if cam:
+        candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml")
+    candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml")
+    for cand in candidates:
+        if cand.is_file():
+            return cand
+    return None
+
+
+TRACK_CAMERA_ROLES = ("wrist", "overhead")
+
+
+def track_camera_role(site: dict[str, Any] | None) -> str:
+    """一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。
+
+    写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。
+    缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。
+    """
+    perc = (site or {}).get("perception")
+    perc = perc if isinstance(perc, dict) else {}
+    raw = perc.get("track_camera")
+    if raw is None or str(raw).strip() == "":
+        return "wrist"
+    role = str(raw).strip().lower()
+    if role not in TRACK_CAMERA_ROLES:
+        raise ValueError(
+            f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。"
+            "写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。"
+        )
+    return role
+
+
+def track_handeye_mode(role: str) -> str:
+    """跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。"""
+    if str(role or "").strip().lower() == "overhead":
+        return "eye_to_hand"
+    return "eye_in_hand"
+
+
+def handeye_static_tf_frames(
+    *,
+    mode: str,
+    ee_frame: str,
+    base_frame: str,
+    cam_frame: str,
+) -> tuple[str, str]:
+    """手眼静态 TF 的 ``(父系, 子系)``。
+
+    两种装法的父子关系**不同**，不能混：
+      * ``eye_in_hand``：法兰 ← 相机（相机跟着臂动）
+      * ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动）
+
+    把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF ——
+    定位总能到附近、永远差一截，而人只会去怀疑标定精度。
+    """
+    if str(mode or "").strip().lower() == "eye_to_hand":
+        return str(base_frame), str(cam_frame)
+    return str(ee_frame), str(cam_frame)
+

build/arm_application/build/lib/arm_application/stage3_search_node.py

@@ -104,6 +104,18 @@ def tactile_timeout_reason(pairs: Sequence[Any], timeout_s: float) -> str:
         )
     return f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）"
 
+
+def should_lift_air_baseline(
+    z_now_m: float, z_air_m: float, *, tactile_live: bool
+) -> bool:
+    """空载抬升只在触觉已经在发、并且当前低于空载高度时做。
+
+    触觉没数据时先抬到 hover+50mm 再报断流，等于插孔做到一半把臂送回锁高。
+    """
+    if not tactile_live:
+        return False
+    return float(z_now_m) < float(z_air_m) - 0.008
+
 def spiral_points(
     cx: float, cy: float, pitch_mm: float, step_mm: float, r_max_mm: float
 ) -> List[Tuple[float, float]]:
@@ -514,7 +526,8 @@ class Stage3Search(Stage2Locate):
             tgt[2] += dz_m
             self._goto(q, Tee, tgt, verticalize=True, tol_m=0.001,
                        tol_tilt=self.vert_tol_rad * 3.0, tag="retract",
-                       v=0.03, max_hops=3, settle_s=0.2, min_dur=0.6)
+                       v=0.03, max_hops=2, settle_s=0.2, min_dur=0.6,
+                       hold_xy=True, z_up_only=True)
         except Exception:
             pass
 
@@ -581,9 +594,21 @@ class Stage3Search(Stage2Locate):
         result: Dict[str, Any] = {"ok": False, "stage": "stage3", "target_hole": self.target_hole}
         try:
             # 基线必须悬空测。二阶段结束在 hover≈触面+1mm，带载会把阈值抬飞。
+            # 触觉没在发就原地中止，不许先抬到锁高再报断流。
             z_now = float(Tee[2, 3])
             z_air = float(self.hover_z) + 0.05
-            if z_now < z_air - 0.008:
+            self._spin(0.05)
+            tac_live = bool(
+                live_tactile(time.monotonic(), (self._tac_l, self._tac_r))
+            )
+            if not tac_live:
+                raise SearchAbort(
+                    tactile_timeout_reason(
+                        (self._tac_l, self._tac_r), float(self.tac_timeout)
+                    ),
+                    code=7,
+                )
+            if should_lift_air_baseline(z_now, z_air, tactile_live=True):
                 self._state("air_baseline")
                 self.get_logger().info(
                     f"抬到 {z_air:.3f}m 测触觉空载基线（当前 Z={z_now:.3f}）"

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

@@ -22,8 +22,10 @@ from arm_application.site_io import load_site_dict, site_path
 from arm_msgs.msg import TactileStatus
 from sensor_msgs.msg import Image
 
+from arm_application.ros_node import TolerantNode
 
-class TactileNode(Node):
+
+class TactileNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("tactile")
         self.declare_parameter("publish_hz", 20.0)

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

@@ -176,6 +176,51 @@ def staging_dir(root: Union[str, Path]) -> Path:
     return Path(root) / STAGING_DIRNAME
 
 
+def infer_group(
+    root: Union[str, Path],
+    source: Union[str, Path],
+    fallback: str = "yolo",
+) -> str:
+    """转换产物该落到**哪个分组**：默认沿用源文件所在的分组。
+
+    为什么不能一律落到 ``yolo``（2026-09-28 真机踩到，代价是一次白转）
+    ----------------------------------------------------------------
+    现场把 ``models/hole_insert/detect_best_1.onnx`` 转 engine，工具默认
+    ``--group yolo``，于是产物落到 ``models/yolo/detect_best_1.engine`` ——
+    而运行时找 engine 的规则是**同级同名**（``yolo_detector.resolve_weights``：
+    ``p.with_suffix(".engine")``，即 site.yaml 指的 ``.onnx`` 旁边）。两边
+    一分家，engine 就等于不存在：现象是"启动即报缺 engine"，而文件明明躺着。
+    这正是本项目最忌讳的**静默失效** —— 工具没报错，位置也"合理"。
+
+    规则：
+      - 源文件已在权重根目录下的某个分组里（例如 ``models/hole_insert/x.onnx``）
+        → 沿用 ``hole_insert``。这样"就地转换"永远落在原地，运行时必然找得到；
+      - 源文件在 ``staging/``、权重根目录之外或根目录正下方 → ``fallback``
+        （默认 ``yolo``）：那才是"新来的模型，还没决定分组"的正常情形。
+
+    Args:
+        root: 权重根目录（``models/``）。
+        source: 输入文件。
+        fallback: 无法沿用时的分组名。
+
+    Returns:
+        分组名（不带路径）。
+    """
+    r = Path(root)
+    try:
+        rel = Path(source).expanduser().resolve().relative_to(r.resolve())
+    except ValueError:
+        # 不在权重根目录下（/tmp、外部磁盘、staging 之类）→ 没有"原地"可言
+        return fallback
+    if len(rel.parts) < 2:
+        # 直接躺在 models/ 正下方：没有分组信息，用默认分组
+        return fallback
+    group = rel.parts[0]
+    if is_staging(r / group) or group.startswith("."):
+        return fallback
+    return group
+
+
 def staging_files(root: Union[str, Path]) -> List[Path]:
     """staging 里的待处理文件。
 

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

@@ -43,6 +43,9 @@ class Detection:
     conf: float
     cls_id: int
     cls_name: str
+    # pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。
+    # 检测模型恒为 None —— 下游必须先判 None，别假设它有。
+    kpts: Optional[np.ndarray] = None
 
     @property
     def cx(self) -> float:
@@ -301,12 +304,51 @@ class YoloDetector:
         return seq, dets, (time.monotonic() - t0) * 1000.0
 
 
+def build_detector(
+    weights: Union[str, Path],
+    *,
+    backend: str = "auto",
+    conf: float = 0.25,
+    iou: float = 0.45,
+    device: str = "",
+    imgsz: int = 640,
+):
+    """按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。
+
+    为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见
+    ``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型
+    是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。
+
+    返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。
+    """
+    path = Path(weights).expanduser()
+    from .pose_detector import PoseDetector, is_pose_model
+
+    if is_pose_model(path):
+        logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name)
+        return PoseDetector(
+            path,
+            conf=conf,
+            iou=iou,
+            device=device or "0",
+            imgsz=imgsz,
+        )
+    return YoloDetector(
+        path,
+        conf=conf,
+        iou=iou,
+        device=device,
+        imgsz=imgsz,
+    )
+
+
 def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
-    """spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。"""
+    """spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。"""
     try:
         path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
-        det = YoloDetector(
+        det = build_detector(
             path,
+            backend=cfg.get("backend") or "auto",
             conf=float(cfg.get("conf") or 0.25),
             iou=float(cfg.get("iou") or 0.45),
             device=str(cfg.get("device") or "0"),
@@ -336,7 +378,13 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
             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))
+                (
+                    d.xyxy.astype(np.float32),
+                    float(d.conf),
+                    int(d.cls_id),
+                    str(d.cls_name),
+                    None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32),
+                )
                 for d in dets
             ]
             _put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
@@ -399,15 +447,20 @@ class YoloInferProcess:
     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:
+    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool:
         """把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
-        排空到队列里最新的帧，所以不会跑任何过期数据。"""
+        排空到队列里最新的帧，所以不会跑任何过期数据。
+
+        返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」
+        而空等，把 D405 也卡住。
+        """
         if not self.alive:
             raise RuntimeError("YOLO 工作进程已死亡")
         try:
             self._q_in.put((int(seq), image_bgr), timeout=0.010)
+            return True
         except Exception:
-            pass
+            return False
 
     def wait_result(self, timeout: float = 0.045):
         """返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
@@ -444,15 +497,20 @@ class YoloInferProcess:
             return None
         seq, packed = int(msg[1]), msg[2]
         gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
-        dets = [
+        dets = []
-            Detection(
+        for row in packed:
-                xyxy=np.asarray(xyxy, dtype=float),
+            # 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。
-                conf=float(conf),
+            xyxy, conf, cid, name = row[0], row[1], row[2], row[3]
-                cls_id=int(cid),
+            kpts = row[4] if len(row) > 4 else None
-                cls_name=str(name),
+            dets.append(
+                Detection(
+                    xyxy=np.asarray(xyxy, dtype=float),
+                    conf=float(conf),
+                    cls_id=int(cid),
+                    cls_name=str(name),
+                    kpts=None if kpts is None else np.asarray(kpts, dtype=float),
+                )
             )
-            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):

build/arm_application/setup.py

@@ -1 +1 @@
-../../src/arm_application/setup.py
+/home/cc/ros2/src/arm_application/setup.py

build/flexiv_hardware/CMakeFiles/flexiv_hardware.dir/compiler_depend.internal

@@ -22,283 +22,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/include/class_loader/class_loader/visibility_control.hpp
  /home/cc/anaconda3/envs/ros_flexiv/include/console_bridge/console.h
  /home/cc/anaconda3/envs/ros_flexiv/include/console_bridge/console_bridge_export.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Cholesky
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Core
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Dense
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Eigen
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Eigenvalues
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Geometry
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Householder
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/IterativeLinearSolvers
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Jacobi
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/LU
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/OrderingMethods
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/QR
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SVD
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Sparse
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseCholesky
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseCore
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseLU
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseQR
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Version
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/LDLT.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/LLT.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArithmeticSequence.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Array.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArrayBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArrayWrapper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Assign.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/AssignEvaluator.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/BandMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Block.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CommaInitializer.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ConditionEstimator.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CoreEvaluators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CoreIterators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseBinaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseNullaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseTernaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseUnaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseUnaryView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseStorage.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DeviceWrapper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Diagonal.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DiagonalMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DiagonalProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Dot.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/EigenBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Fill.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/FindCoeff.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Fuzzy.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GeneralProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GenericPacketMath.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GlobalFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/IO.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/IndexedView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/InnerProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Inverse.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Map.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MapBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MathFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MathFunctionsImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Matrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MatrixBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NestByValue.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NoAlias.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NumTraits.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PartialReduxEvaluator.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PermutationMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PlainObjectBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Product.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ProductEvaluators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Random.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/RandomImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/RealView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Redux.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Ref.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Replicate.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Reshaped.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ReturnByValue.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Reverse.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Select.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SelfAdjointView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SelfCwiseBinaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SkewSymmetricMatrix3.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Solve.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SolveTriangular.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SolverBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/StableNorm.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/StlIterators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Stride.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Swap.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Transpose.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Transpositions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/TriangularMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/VectorBlock.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/VectorwiseOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Visitor.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/BFloat16.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/ConjHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/Half.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/Settings.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/Complex.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/MathFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/Reductions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/TypeCasting.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/AssignmentFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/BinaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/NullaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/StlFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/TernaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/UnaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralBlockPanelKernel.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/Parallelizer.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointMatrixVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointRank2Update.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularMatrixMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularMatrixVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularSolverMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularSolverVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Assert.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/BlasUtil.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Constants.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/DisableStupidWarnings.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/EmulateArray.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ForwardDeclarations.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/IndexedViewHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/IntegralConstant.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/MKL_support.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Macros.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Memory.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Meta.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/MoreMeta.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ReenableStupidWarnings.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ReshapedHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Serializer.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/StaticAssert.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/SymbolicIndex.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/XprHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/ComplexEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/ComplexSchur.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/EigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/HessenbergDecomposition.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/RealQZ.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/RealSchur.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/Tridiagonalization.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/AlignedBox.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/AngleAxis.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/EulerAngles.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Homogeneous.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Hyperplane.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/OrthoMethods.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/ParametrizedLine.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Quaternion.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Rotation2D.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/RotationBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Scaling.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Transform.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Translation.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Umeyama.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/arch/Geometry_SIMD.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/BlockHouseholder.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/Householder.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/HouseholderSequence.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Jacobi/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Jacobi/Jacobi.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/Determinant.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/FullPivLU.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/InverseImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/PartialPivLU.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/arch/InverseSize4.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Amd.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Eigen_Colamd.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Ordering.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/ColPivHouseholderQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/CompleteOrthogonalDecomposition.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/FullPivHouseholderQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/HouseholderQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/BDCSVD.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/JacobiSVD.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/SVDBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/UpperBidiagonalization.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/SimplicialCholesky.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/AmbiVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/CompressedStorage.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseAssign.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseBlock.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseColEtree.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCompressedBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDenseProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDiagonalProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDot.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseFuzzy.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMap.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMatrixBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseRedux.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseRef.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSelfAdjointView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSolverBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseTranspose.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseTriangularView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseUtil.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/TriangularSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLUImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Memory.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Structs.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Utils.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_column_bmod.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_column_dfs.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_panel_bmod.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_panel_dfs.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_pivotL.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_pruneL.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_relax_snode.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseQR/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseQR/SparseQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/Image.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/Kernel.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/RealSvd2x2.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ArrayCwiseBinaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ArrayCwiseUnaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/BlockMethods.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/CommonCwiseBinaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/CommonCwiseUnaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/IndexedViewMethods.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/MatrixCwiseBinaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ReshapedMethods.inc
  /home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/handle.hpp
  /home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/hardware_info.hpp
  /home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/macros.hpp
@@ -787,12 +510,10 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cctype
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cerrno
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/chrono
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/climits
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/clocale
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cmath
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/codecvt
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/compare
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/complex
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/concepts
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/condition_variable
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/csignal
@@ -872,7 +593,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/utility
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/variant
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/vector
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/version
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/atomic_word.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/basic_file.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/c++allocator.h
@@ -889,17 +609,11 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/messages_members.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/os_defines.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/time_members.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/emmintrin.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/limits.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/mm_malloc.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/mmintrin.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdalign.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdarg.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdbool.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stddef.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdint.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/syslimits.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/xmmintrin.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/alloca.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/arpa/inet.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/asm-generic/bitsperlong.h
@@ -933,15 +647,12 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/iscanonical.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/libc-header-start.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/libm-simd-decl-stubs.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/local_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/locale.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/long-double.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/math-vector.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls-helper-functions.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls-narrow.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix1_lim.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix2_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix_opt.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/pthread_stack_min-dynamic.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/pthreadtypes-arch.h
@@ -1014,14 +725,12 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/types/wint_t.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/typesizes.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/uintn-identity.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/uio_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/unistd_ext.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/waitflags.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/waitstatus.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wchar.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wctype-wchar.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wordsize.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/xopen_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/ctype.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/endian.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/errno.h
@@ -1031,10 +740,8 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/gnu/stubs.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/ifaddrs.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/libintl.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/limits.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/close_range.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/errno.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/limits.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/posix_types.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/stddef.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/locale.h
@@ -1063,7 +770,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/flexiv_install/include/flexiv/rdk/data.hpp
  /home/cc/flexiv_install/include/flexiv/rdk/mode.hpp
  /home/cc/flexiv_install/include/flexiv/rdk/robot.hpp
- /home/cc/flexiv_install/include/flexiv/rdk/utility.hpp
  /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
  /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/visibility_control.h
 

build/flexiv_hardware/CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o.d

@@ -237,10 +237,16 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/basic_file.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/c++io.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/fstream.tcc \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/map \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_tree.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/ext/aligned_buffer.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/node_handle.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_map.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multimap.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/erase_if.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/variant \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/enable_special_members.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/monostate.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/ext/aligned_buffer.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/thread \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/std_thread.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/unique_ptr.h \
@@ -328,11 +334,8 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/std_function.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/set \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_tree.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/node_handle.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_set.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multiset.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/erase_if.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/unordered_map \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/unordered_map.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/hashtable.h \
@@ -344,9 +347,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/list \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_list.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/list.tcc \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/map \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_map.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multimap.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rcl/rcl/guard_condition.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rcl/rcl/allocator.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rcutils/rcutils/allocator.h \

build/flexiv_hardware/cmake_args.last

@@ -1 +1 @@
-['-DCMAKE_BUILD_TYPE=Release']
+['-DAMENT_CMAKE_SYMLINK_INSTALL=1']

config/site.example.yaml

@@ -10,8 +10,8 @@
 # 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
 #       现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
 #
-# 说明：仓库里的文件用相对仓库根的路径（例如 models/yolo/...）。
+# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
-#       不要改回某个用户的绝对家目录，换台电脑就会找不到文件。
+#       部署时请改成实际用户的 home 目录。
 # =============================================================================
 
 # =============================================================================
@@ -39,7 +39,7 @@ robot:
       # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
       # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
       tactile: true
-      # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
+      # 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
       tool_name: "tool10120938"
       camera:
         name: d405_left          # 话题 /d405_left/... 与 91 分开
@@ -61,17 +61,17 @@ robot:
         #     （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
         # 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
         auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_89.yaml"
+      named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
       paths:
-        handeye_file: "handeye/results/89_eye_in_hand.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+        handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/89"
+        handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
       handeye:
         # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
         # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
         # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
         gripper_tool: "tool10120938"
-        gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
+        gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
         gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
       # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
       # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
@@ -179,9 +179,11 @@ robot:
         #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
         #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
         #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
-        # 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
+        # 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
-        # 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
+        #   [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
-        tool_tip_below_flange_m: 0.2245
+        #   示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
+        # 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
+        tool_tip_below_flange_m: 0.226587
     # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
     # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
     #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -211,7 +213,7 @@ robot:
         # 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
         # T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
         #（"手眼标定结果缺失"），S04 跟踪起不来。
-        handeye_file: "handeye/results/sim_eye_in_hand.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/sim_eye_in_hand.yaml"
       # 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
       #
       # 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
@@ -305,11 +307,11 @@ robot:
         port: "/dev/ttyUSB1"
         stroke_mm: 30.0
         auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_91.yaml"
+      named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
       paths:
-        handeye_file: "handeye/results/91_eye_in_hand.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
+        handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/91"
+        handeye_workdir: "/home/USER/ros2/handeye/tmp/91"
       # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
       # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
       #   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
@@ -330,7 +332,7 @@ robot:
 
 paths:
   # >>> YOLO 权重（标准放法，不必改代码）<<<
-  #   1) 把 .onnx / .pt 放到 models/yolo/
+  #   1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
   #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
   #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
   #        bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
@@ -344,18 +346,18 @@ paths:
   #    ultralytics 解不了 pose 的 TensorRT engine，运行时走
   #    arm_application/pose_detector.py 的自解码（见该模块头）。
   #    仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
-  yolo_weights: "models/yolo/gongjian0929.onnx"
+  yolo_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
   # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
   # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
   # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
   # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
   # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
-  hole_weights: "models/yolo/gongjian0929.onnx"
+  hole_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
   # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
-  handeye_file: "handeye/results/89_eye_in_hand.yaml"
+  handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
-  handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+  handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
-  handeye_calibrate: "handeye/calibrate.yaml"
+  handeye_calibrate: "/home/USER/ros2/handeye/calibrate.yaml"
-  handeye_workdir: "handeye/tmp/89"
+  handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
   # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
   fastdds_profile: ""
 
@@ -595,9 +597,8 @@ servo:
   lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
   # 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
   lock_still_sec: 3.0
-  # 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
+  # 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
-  # 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
+  d435_handback_sec: 2.0
-  d435_handback_sec: 0.5
   # 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
   wrist_align_sec: 0.5
   lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -679,10 +680,10 @@ gripper:
 hole_detect:
   backend: "two_stage"   # two_stage | yolo | hough
   # backend=yolo 时用。
-  weights: "models/yolo/gongjian0929.onnx"
+  weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
   # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
   # 所以和上面的跟踪权重是同一份。
-  workpiece_weights: "models/yolo/gongjian0929.onnx"
+  workpiece_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
   # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
   # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
   # 亮环 + Hough 那套老路径。
@@ -690,7 +691,7 @@ hole_detect:
   keypoint_min_conf: 0.30
   keypoint_min_dist_px: 15.0
   keypoint_hole_radius_px: 12.0   # 关键点没有半径；反投影时按这个半径取孔周深度环
-  config: "src/arm_application/config/hole_detect.yaml"
+  config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
   conf: 0.25
   iou: 0.7
   imgsz: 640
@@ -768,7 +769,7 @@ hole_detect:
     # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
     # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
     # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
-    coarse_max_offset_m: 0.12   # 目标离夹爪轴线超过此值就不像粗居中量 [m]
+    coarse_max_offset_m: 0.08   # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
     coarse_border_px: 2         # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
     # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
     align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
@@ -783,11 +784,15 @@ hole_detect:
     # 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
     # 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
     once_ff_look_per_m: 0.0
-    final_gap_mm: 2.0        # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
+    final_gap_mm: 2.0        # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
-                             # 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
+                             # 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
+    hole_entry_mm: 1.0       # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
+                             # 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
+                             # 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
+                             # 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
-                             # 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
+                             # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
     final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
     # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
     #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -800,7 +805,7 @@ hole_detect:
     #     ⇒ 读数往**小**错一次就永远到不了目标。
     #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
     #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
-    # 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
+    # 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
     #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
     #   + 任一步 _goto 走不到位就停。
     #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -812,9 +817,11 @@ hole_detect:
     max_depth_m: 0.60
     linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
 
-  # ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
+  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
-  # 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
+  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
-  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89                # 搜孔+落座
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心不插入
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # 螺纹规旋入
   stage3:
     # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
     #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -841,12 +848,9 @@ hole_detect:
     drop_frames: 2           # 跌落需持续的帧数（防毛刺）
     center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
     center_scan_mm: 0.3
-    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入。旋转装配不读这项
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
     insert_retry: 3
-    screw_pitch_mm: 0.0      # 搜孔轻压不旋。旋转装配不读这项
+    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
-    # 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
-    screw_stage_depth_mm: 6.0
-    screw_stage_pitch_mm: 1.0
     search_speed: 0.03       # 搜孔笛卡尔速度 [m/s]
     settle_sec: 0.25         # 每个微步后的静止时间
     tactile_timeout_s: 3.0   # 触觉话题断流多久中止
@@ -932,8 +936,16 @@ gauge_assembly:
   # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
-  extreme_speed_deg_s: 20.0  # 关节角速度 [deg/s]，慢一点防冲击
+  extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。规装配预绕，以及插孔后首轮
-  extreme_direction: 1       # +1=逆时针 / -1=顺时针（改符号，别去改代码）
+                             # 顺时针进孔，都用这个数。首轮逆时针出孔、空回各轮的
+                             # 正反转不读它，走接口上限 0.90 rad/s。
+  unwind_open_permille: 400  # 第三步松爪只开到这个千分比。0=合，1000=全开。
+                             # 2026-10-19 从 200 改成 400，只张开这个量。
+  unwind_lift_mm: 3.0       # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。
+                             # 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm，下降 -4.70mm，差 0.16mm。
+                             # 空回到限位后，下降量用这次实测的抬高，不另写一个数。
+                             # 超过 5mm 拒绝。
+  extreme_direction: -1      # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
   extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                              # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
 
@@ -1102,7 +1114,7 @@ tactile:
   # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
   model: "dd03"
   # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
-  config_file: "devices/sensors/orisys/models/dd03/config.json"
+  config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json"
   # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
   # 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
   left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
@@ -1129,8 +1141,7 @@ go_home:
 
 network:
   # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
-  #   MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enxXXXXXXXXXXXX 之间变，
+  #   MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enxXXXXXXXXXXXX）。
-  #   NetworkManager 按 MAC 绑定，不按名字。
   #   本机 <PC_IP>  ↔  <左臂SN>(左89)=.101  /  <右臂SN>(右91)=.100
   #   NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
   # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。

config/site.yaml

@@ -23,7 +23,7 @@ robot:
       # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
       # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
       tactile: true
-      # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
+      # 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
       tool_name: "tool10120938"
       camera:
         name: d405_left          # 话题 /d405_left/... 与 91 分开
@@ -45,17 +45,17 @@ robot:
         #     （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
         # 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
         auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_89.yaml"
+      named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_89.yaml"
       paths:
-        handeye_file: "handeye/results/89_eye_in_hand.yaml"
+        handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+        handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/89"
+        handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
       handeye:
         # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
         # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
         # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
         gripper_tool: "tool10120938"
-        gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
+        gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
         gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
       # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
       # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
@@ -163,9 +163,11 @@ robot:
         #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
         #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-063989
         #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
-        # 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
+        # 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
-        # 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
+        #   [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
-        tool_tip_below_flange_m: 0.2245
+        #   示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
+        # 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
+        tool_tip_below_flange_m: 0.226587
     # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
     # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
     #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -195,7 +197,7 @@ robot:
         # 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
         # T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
         #（"手眼标定结果缺失"），S04 跟踪起不来。
-        handeye_file: "handeye/results/sim_eye_in_hand.yaml"
+        handeye_file: "/home/cc/ros2/handeye/results/sim_eye_in_hand.yaml"
       # 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
       #
       # 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
@@ -289,11 +291,11 @@ robot:
         port: "/dev/ttyUSB1"
         stroke_mm: 30.0
         auto_init: false
-      named_poses_file: "src/arm_control/config/named_poses_91.yaml"
+      named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_91.yaml"
       paths:
-        handeye_file: "handeye/results/91_eye_in_hand.yaml"
+        handeye_file: "/home/cc/ros2/handeye/results/91_eye_in_hand.yaml"
-        handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
+        handeye_waypoints: "/home/cc/ros2/handeye/poses/91_eye_in_hand.yaml"
-        handeye_workdir: "handeye/tmp/91"
+        handeye_workdir: "/home/cc/ros2/handeye/tmp/91"
       # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
       # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
       #   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
@@ -314,7 +316,7 @@ robot:
 
 paths:
   # >>> YOLO 权重（标准放法，不必改代码）<<<
-  #   1) 把 .onnx / .pt 放到 models/yolo/
+  #   1) 把 .onnx / .pt 放到 /home/cc/ros2/models/yolo/
   #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
   #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
   #        bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
@@ -328,18 +330,18 @@ paths:
   #    ultralytics 解不了 pose 的 TensorRT engine，运行时走
   #    arm_application/pose_detector.py 的自解码（见该模块头）。
   #    仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
-  yolo_weights: "models/yolo/gongjian0929.onnx"
+  yolo_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
   # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
   # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
   # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
   # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
   # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
-  hole_weights: "models/yolo/gongjian0929.onnx"
+  hole_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
   # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
-  handeye_file: "handeye/results/89_eye_in_hand.yaml"
+  handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
-  handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
+  handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
-  handeye_calibrate: "handeye/calibrate.yaml"
+  handeye_calibrate: "/home/cc/ros2/handeye/calibrate.yaml"
-  handeye_workdir: "handeye/tmp/89"
+  handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
   # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
   fastdds_profile: ""
 
@@ -579,9 +581,8 @@ servo:
   lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
   # 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
   lock_still_sec: 3.0
-  # 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
+  # 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
-  # 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
+  d435_handback_sec: 2.0
-  d435_handback_sec: 0.5
   # 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
   wrist_align_sec: 0.5
   lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -663,10 +664,10 @@ gripper:
 hole_detect:
   backend: "two_stage"   # two_stage | yolo | hough
   # backend=yolo 时用。
-  weights: "models/yolo/gongjian0929.onnx"
+  weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
   # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
   # 所以和上面的跟踪权重是同一份。
-  workpiece_weights: "models/yolo/gongjian0929.onnx"
+  workpiece_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
   # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
   # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
   # 亮环 + Hough 那套老路径。
@@ -674,7 +675,7 @@ hole_detect:
   keypoint_min_conf: 0.30
   keypoint_min_dist_px: 15.0
   keypoint_hole_radius_px: 12.0   # 关键点没有半径；反投影时按这个半径取孔周深度环
-  config: "src/arm_application/config/hole_detect.yaml"
+  config: "/home/cc/ros2/src/arm_application/config/hole_detect.yaml"
   conf: 0.25
   iou: 0.7
   imgsz: 640
@@ -752,7 +753,7 @@ hole_detect:
     # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
     # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
     # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
-    coarse_max_offset_m: 0.12   # 目标离夹爪轴线超过此值就不像粗居中量 [m]
+    coarse_max_offset_m: 0.08   # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
     coarse_border_px: 2         # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
     # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
     align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
@@ -767,11 +768,15 @@ hole_detect:
     # 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
     # 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
     once_ff_look_per_m: 0.0
-    final_gap_mm: 2.0        # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
+    final_gap_mm: 2.0        # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
-                             # 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
+                             # 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
+    hole_entry_mm: 1.0       # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
+                             # 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
+                             # 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
+                             # 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
-                             # 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
+                             # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
     final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
     # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
     #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -784,7 +789,7 @@ hole_detect:
     #     ⇒ 读数往**小**错一次就永远到不了目标。
     #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
     #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
-    # 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
+    # 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
     #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
     #   + 任一步 _goto 走不到位就停。
     #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -796,9 +801,11 @@ hole_detect:
     max_depth_m: 0.60
     linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
 
-  # ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
+  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
-  # 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
+  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
-  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89                # 搜孔+落座
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心不插入
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # 螺纹规旋入
   stage3:
     # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
     #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -825,12 +832,9 @@ hole_detect:
     drop_frames: 2           # 跌落需持续的帧数（防毛刺）
     center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
     center_scan_mm: 0.3
-    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入。旋转装配不读这项
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
     insert_retry: 3
-    screw_pitch_mm: 0.0      # 搜孔轻压不旋。旋转装配不读这项
+    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
-    # 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
-    screw_stage_depth_mm: 6.0
-    screw_stage_pitch_mm: 1.0
     search_speed: 0.03       # 搜孔笛卡尔速度 [m/s]
     settle_sec: 0.25         # 每个微步后的静止时间
     tactile_timeout_s: 3.0   # 触觉话题断流多久中止
@@ -916,8 +920,16 @@ gauge_assembly:
   # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
-  extreme_speed_deg_s: 20.0  # 关节角速度 [deg/s]，慢一点防冲击
+  extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。规装配预绕，以及插孔后首轮
-  extreme_direction: 1       # +1=逆时针 / -1=顺时针（改符号，别去改代码）
+                             # 顺时针进孔，都用这个数。首轮逆时针出孔、空回各轮的
+                             # 正反转不读它，走接口上限 0.90 rad/s。
+  unwind_open_permille: 400  # 第三步松爪只开到这个千分比。0=合，1000=全开。
+                             # 2026-10-19 从 200 改成 400，只张开这个量。
+  unwind_lift_mm: 3.0       # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。
+                             # 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm，下降 -4.70mm，差 0.16mm。
+                             # 空回到限位后，下降量用这次实测的抬高，不另写一个数。
+                             # 超过 5mm 拒绝。
+  extreme_direction: -1      # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
   extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                              # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
 
@@ -1086,7 +1098,7 @@ tactile:
   # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
   model: "dd03"
   # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
-  config_file: "devices/sensors/orisys/models/dd03/config.json"
+  config_file: "/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json"
   # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
   # 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
   left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
@@ -1113,8 +1125,7 @@ go_home:
 
 network:
   # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
-  #   MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enx6c1ff7d06f17 之间变，
+  #   MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。
-  #   NetworkManager 按 MAC 绑定，不按名字。
   #   本机 192.168.2.10  ↔  063989(左89)=.101  /  063991(右91)=.100
   #   NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
   # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。

docs/GRIPPER.md

@@ -67,4 +67,19 @@ ros2 service call /dual_gripper_driver/right/execute arm_msgs/srv/GripperExecute
 
 USB 未插齐时节点仍可起来（`allow_offline:=true`），对应侧 `status.error_msg` 会写连接失败。
 
+## `/gripper/states` 的离线帧
+
+`data` 是 6 个元素：
+`[initialized, grip_state, position, target_pos, target_force, online]`。
+
+`online=0` 表示这一帧**没读到总线**，前 5 个是上一次读到的值，不是现在的状态。老节点只发 5 个元素（那时它只在读成功时才发），5 元素帧按有效帧处理。
+
+读 `0x0201` 无应答时节点照样发离线帧，`/gripper/joint_states` **不发**：关节状态没有离线位，宁可不发，也不要把上一次的开口当成现在的。
+
+流水线 `_wait_gripper` 只认有效帧；超时没等到有效帧时报「读不到状态（只有离线帧，Modbus 无应答），不是夹爪没张开」，不能再报成「夹爪没到位」——2026-10-20 读 `0x0201` 一连 264 次无应答，实测在张开后停住、微抬没有下发，日志却只写「未到位」。
+
+串口 `EIO`（USB 掉线）抛 `DhPgeaBusError`，节点先关串口再重开（1 秒最多一次），不再整进程带 traceback 退出。没有应答（`DhPgeaError`）先继续重试；连续 `BUS_REOPEN_AFTER_FAILURES=30` 拍都读不到也重开一次——2026-10-20 那次就是写有回显、读一直无应答，重启节点立刻恢复，说明坏的是串口会话。
+
+发送不再调用 `Serial.flush()`（底层是 `tcdrain`）。USB 串口不收数据时这个调用不看写超时。2026-10-20 10:40 合爪命令已经发布，节点被它堵住 120s 才打出 `ctrl pos=0`，流水线 8s 时状态还是 `grip=1 pos=400`，这一轮就停了。现在发送缓冲在 `timeout_s`（0.25s）内排不空就失败返回；流水线对同一次开合再发一次命令。
+
 用户需在 `dialout`：`sudo usermod -aG dialout $USER`。

docs/HANDBOOK.md

@@ -34,7 +34,7 @@
 
 ### 1.3 一次完整任务分几步
 
-代码里跟踪和对孔串成一条命令。搜孔轻压已取消，`run_all.sh` 不会再进入那一步，也不能用参数打开。旋入是单独的 `run_stage3_search.sh --screw`。
+代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完：
 
 ```
 run_all.sh
@@ -46,13 +46,18 @@ run_all.sh
   │                  YOLO 找工件外框 → Hough 找左右两个孔心 →
   │                  深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
   │                  移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
+  │
+  └─ stage3_search   柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
+                     再十字微扫定心，最后在力限内下压（可选按螺距旋入）
 ```
 
-跟踪和对孔在同一个进程里交接。盯日志：
+三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。
+盯日志就能知道跑到哪了 —— 三行关键输出：
 
 ```
 [pipeline] === stage1_track ===      → 在等跟踪锁定
 [pipeline] === stage2_locate ===     → 在拍照对孔
+[pipeline] === stage3_search ===     → 触面 → ★ 入孔 → 轻压到位
 ```
 
 日常入口（三个终端，**必须同一个 `--arm`**）：
@@ -60,7 +65,7 @@ run_all.sh
 ```bash
 bash ~/ros2/scripts/run_real.sh --arm 89                    # T1 控制栈，一直开着
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，一直开着
-bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 跟踪 + 对孔（不含搜孔、不含旋入）
+bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
 bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
 ```
 
@@ -68,15 +73,16 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
 `D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
 的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
 这一段运动是进程内厂家 `MoveL`（不杀 `run_real.sh`）：当前厂家 `tcp_pose` 加
-ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后法兰 XY 连续停稳
+ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这一点变到基座系后，只下发厂家 `MoveL`。赶路期间 D405 推理开着，
-`servo.d435_handback_sec`（0.5 s）才交回 D405。赶路期间不下发 D405（两套控制互抢会晃）。
+控制点截断。法兰连续停稳 `servo.d435_handback_sec`（2 s）后才打开 D405，
-停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
+追工件框的矩形中心。框心对准并停稳后才精拍 k1/k2、下臂。
 启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
 下臂贴面用 `--final-gap`（规尖离面，毫米；site 默认 `final_gap_mm` 2）。笛卡尔锁 XY
 时只改 Z：法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。
-会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
+`--final-gap` ≤ 0、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
+往孔里进只走 `stage2.hole_entry_mm`（正值，上限 8 mm）。
 不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
-框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。
+框心对准连续 `servo.wrist_align_sec`（0.5 s）才 `/visual_servo/locked`，然后才精拍下臂。
 本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2，
 把框心到目标孔一次走完（步长上限 40mm）。现行实验 `hole_once_lock_xy_descend: true`：
 走完后不补拍，一条厂家 MoveL 收到贴面。`once_ff_look_per_m: 0` 时 MoveL 只降 Z
@@ -84,7 +90,10 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后
 不是 ROS 绝对坐标。保存点 `37b85aa` 是观察高 15mm 再斜线补 XY。看 `/tmp/stage2_result.json`。
 `main` 保存点仍是多走多拍。
 孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
-`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
+`tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
+贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器。
+`stage2.hole_entry_mm`（现为 1）> 0 时，插孔目标改成规尖在工件上表面以下这么深（孔口凹槽），再转 A7；为 0 时仍停在 `final_gap_mm`。进孔超过 8 mm（板厚 12 mm）直接拒绝。2026-10-19 12:20 写成 4 mm 时，臂在面下 1.6 mm 被挡住。
+然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`（接口最快），再顺时针转到另一侧，这一段用 `extreme_speed_deg_s`（现为 10 °/s）。顺时针到位记为「首轮咬合结束」。接着下面这一整段固定重复 4 轮（常量 `UNWIND_ROUNDS`，不进 `site.yaml`）：`run_gripper.sh --arm <臂号> --cli pos` 加上 `unwind_open_permille`（现为 400，不全开），再用一条厂家 `MoveL` 只抬 TCP 的 Z `unwind_lift_mm`（现为 3，即 0.3 cm；XY 和姿态不动，不拆成关节小跳）。这条和随后的下降都带 `zoneRadius=ZFine`、`targetTolerLevel=1`：厂家默认 `Z50` 和等级 3 会在还差约 0.5 mm 时就报到位，下降和记住的上抬对不上，合爪就不会发。A7 再逆时针回到限位。然后下降刚才这一轮实测的那一段（不另写一个下降量，也不用上一轮的数），`--cli close`。合爪目标是位置 0；固件在位置先到时写 grip=1（到位），力先到才写 grip=2（夹住）。手指已经在合拢端就继续，还停在张开位则不转。再顺时针转到限位。日志打 `空回第i/4轮 开始` 和 `完成`，带本轮上抬、下降的毫米数。某一轮没走完就停在当前关节，后面的轮次不再做。首轮逆时针出孔，以及空回每一轮的逆时针、顺时针，请求接口上限 0.90 rad/s；两端速度为 0 的样条峰速约是平均的 1.5 倍，所以这几段按平均约 34 °/s 排程，避免时间到了还差几十度。首轮顺时针进孔不在这几段里，速度是上面的 10 °/s。前六轴不动。顺时针是进孔，逆时针是出孔。夹爪或这一段 Z 对不上，就停在当前关节。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 
 ```bash
@@ -107,8 +116,8 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
 
 | 能力 | 状态 | 怎么自己确认 |
 |------|------|--------------|
-| 跟踪 → 对孔 | ✅ 真机跑通过。搜孔轻压已取消，这条命令不再下压搜孔 | `run_all.sh --arm 89` |
+| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` |
-| 按螺距旋入（螺纹规） | ✅ 单独阶段。不带 `--screw` 的旧搜孔命令会直接拒绝 | `run_stage3_search.sh --arm 89 --screw` |
+| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
 | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
 | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
 | D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
@@ -505,7 +514,7 @@ bash scripts/run_unit_tests.sh -k gauge              # 只跑名字带 gauge 的
 | **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
 | **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
 | **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
-| **stage1/2** | 跟踪 / 对孔，见 1.3。搜孔轻压已取消 |
+| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
 | **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
 | **基线** | 触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值 |
 | **R1.1 / M1 …** | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 里的规则编号，文档和提交信息里直接引用 |

docs/HOLE_DETECT.md

@@ -57,7 +57,8 @@ hole_detect:
 
 ## 三阶段串联（推荐日常入口）
 
-一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。
+一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
+（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
 
 ```bash
 # 终端 1 —— 控制栈（一直开着）
@@ -66,12 +67,12 @@ bash ~/ros2/scripts/run_real.sh --arm 89
 # 终端 2 —— 相机预览（一直开着，看跟踪画面）
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
 
-# 终端 3 —— 跟踪 + 对孔（到这里停，不搜孔）
+# 终端 3 —— 串联（搜到孔后轻压保持，不拧）
-bash ~/ros2/scripts/run_all.sh --arm 89                  # 跟踪 + 对孔
+bash ~/ros2/scripts/run_all.sh --arm 89                  # 跟踪 + 对孔 + 搜孔轻压
 bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2        # 换 k2
 bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1    # 臂已在工件上方，跳过跟踪
-# 旋入不写在这条命令里：
+# 若这一次就要拧入（一般留给后续阶段）：
-bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
 ```
 
 `run_pipeline.sh` 是 `run_all.sh` 的别名。
@@ -84,9 +85,9 @@ bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
 1. `[pipeline] === stage1_track ===` → 等跟踪锁定
 2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
 3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
-对孔结束后臂停在当前高度。搜孔轻压不再接着跑。
+4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
 
-急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看
+急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
 `/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
 
 高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
@@ -148,10 +149,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8            # 锁 XY，规
 | `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
 | `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |
 
-## 旋转装配（原三阶段节点；搜孔轻压入口已取消）
+## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）
-
-不带 `--screw` 时 `run_stage3_search.sh` 直接退出，不会触面，也不会螺旋搜孔。
-下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。
 
 前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
 
@@ -169,9 +167,12 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8            # 锁 XY，规
 7. **hold** — 锁定关节，发布结果
 
 ```bash
-# 前置：T1 run_real.sh --arm 89；对孔已完成（臂保持不动）
+# 前置：T1 run_real.sh --arm 89；二阶段已完成（臂保持不动）
-# 不带 --screw 会直接退出，不会搜孔。
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89                 # 搜孔+落座
-bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心，不插入
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # M6 螺纹规旋入
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5      # 临时改搜索半径
+bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap       # 搜前用 D405 复拍对准一次
 ```
 
 **力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且

handeye/poses/89_eye_in_hand.yaml

@@ -8,49 +8,59 @@
 
 /**:
   ros__parameters:
-    num_poses: 21
+    num_poses: 26
-    # 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
+    # 2026-10-16 重新示教 26 点（旧 21 点在 poses/backup/）；A5 11×8，tool10120938
     waypoint_positions: [
       # pose 0
-      -0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
+      -0.211261, -0.571428, 0.247301, 1.552793, -0.119748, 0.614676, -0.023410,
       # pose 1
-      -0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
+      -0.169531, -0.647538, 0.404777, 1.568624, -0.268686, 0.700171, 0.395947,
       # pose 2
-      -0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
+      -0.759204, -0.846289, 0.735872, 1.387307, -0.539553, 0.576374, -0.060241,
       # pose 3
-      -1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
+      -0.925104, -1.394748, 1.532423, 0.826872, -0.686369, -0.074315, -0.145523,
       # pose 4
-      -0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
+      -1.004515, -1.491910, 1.596406, 1.245659, -0.864236, 0.212990, 0.460589,
       # pose 5
-      -0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
+      -0.373052, -1.441561, 1.624783, 0.325602, -1.361228, -0.404116, 0.511430,
       # pose 6
-      -0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
+      -0.281212, -1.452318, 1.627866, 0.517637, -1.412135, -0.420614, 0.217023,
       # pose 7
-      -0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
+      -0.184868, -1.473244, 1.670229, 0.827772, -1.522330, -0.529014, -0.212158,
       # pose 8
-      -0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
+      -0.246577, -1.532828, 1.679171, 1.204498, -1.697638, -0.607779, -0.517785,
       # pose 9
-      -0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
+      -0.463435, -1.549157, 1.650964, 1.948746, -1.841124, -0.638016, -0.842437,
       # pose 10
-      -0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
+      -1.573539, -0.359801, 1.261534, 2.423352, -0.405671, 1.620875, -0.045014,
       # pose 11
-      -0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
+      -1.549088, -0.710364, 1.347227, 2.111776, -0.606133, 1.116345, -0.567788,
       # pose 12
-      -0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
+      -1.512218, -0.983202, 1.445454, 1.808168, -0.602205, 0.699420, -0.803771,
       # pose 13
-      -0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
+      -1.023248, -1.342205, 1.465909, 1.092911, -0.843418, 0.099672, 1.287399,
       # pose 14
-      -1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
+      -0.870257, -1.473283, 1.657987, 0.831271, -1.034030, -0.144608, 1.263203,
       # pose 15
-      -0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
+      -0.384368, -1.417984, 1.403442, 2.075614, -1.952106, -0.394139, 1.022592,
       # pose 16
-      -0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
+      -0.320712, -1.513967, 1.508547, 1.652983, -1.854378, -0.409816, 1.024834,
       # pose 17
-      -0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
+      -0.046066, -1.630857, 1.639221, 1.105078, -1.971509, -0.601654, 1.076452,
       # pose 18
-      -1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
+      -0.183628, -1.588663, 1.989621, 0.918793, -1.873979, -0.629910, 0.092012,
       # pose 19
-      -1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
+      -1.300709, -1.159500, 1.611790, 1.761984, -0.963027, 0.391619, -0.468980,
       # pose 20
-      -1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
+      -0.750548, -1.221231, 1.566283, 1.128755, -1.163176, -0.102841, 0.958420,
+      # pose 21
+      -0.874185, -1.297230, 1.655940, 1.303348, -1.334815, -0.047425, 2.047690,
+      # pose 22
+      -0.671314, -1.510973, 1.813908, 1.235398, -1.778996, -0.462958, 2.899902,
+      # pose 23
+      -0.710795, -1.522124, 1.653705, 1.316372, -1.574330, 0.086268, 1.453877,
+      # pose 24
+      -0.533642, -1.545775, 1.837151, 1.467081, -1.463448, -0.161183, 0.387563,
+      # pose 25
+      -0.850451, -1.523689, 1.982673, 1.082314, -1.771622, -0.354609, 0.583659,
     ]

handeye/poses/89_eye_to_hand.yaml

@@ -7,48 +7,38 @@
 
 /**:
   ros__parameters:
-    num_poses: 21
+    num_poses: 16
     waypoint_positions: [
       # pose 0
-      -0.798667, -0.611079, 0.054912, 2.246988, 0.049199, 1.389219, 0.196135,
+      -0.850450, -1.523690, 1.982676, 1.082314, -1.771616, -0.354603, 0.583665,
       # pose 1
-      -0.787304, -0.531383, 0.240108, 2.183440, 0.367026, 1.370311, 0.472489,
+      -2.098137, -0.606148, 1.213612, 2.290328, -0.795715, 0.618633, 0.213293,
       # pose 2
-      -1.615896, -0.687070, 1.187600, 2.243693, -0.162789, 1.650045, 1.026121,
+      -2.115870, -0.711154, 1.204674, 2.128285, -0.937034, 0.514766, -0.356714,
       # pose 3
-      -1.787431, -0.668004, 1.408195, 2.025431, -0.064718, 0.565415, 1.324635,
+      -2.017820, -0.798105, 1.516945, 2.002096, -0.392371, -0.035888, 0.657789,
       # pose 4
-      -2.024343, -0.863793, 1.349640, 2.359469, -0.720629, 0.084467, 0.821412,
+      -2.021874, -0.805058, 1.533201, 2.001407, -0.399671, -0.045924, 0.653138,
       # pose 5
-      -1.884100, -1.117584, 1.183093, 2.127937, -1.258013, 0.151934, 0.985555,
+      -2.135665, -1.020864, 1.346306, 2.334297, -1.268378, 0.406336, 0.897832,
       # pose 6
-      -1.866465, -1.194137, 1.143263, 1.872811, -1.320028, 0.280719, 0.777663,
+      -1.793944, -1.143422, 1.244557, 1.942177, -1.315611, 0.473616, 1.619695,
       # pose 7
-      -1.729402, -1.139685, 1.127574, 1.913544, -1.329630, -0.194162, 1.383834,
+      -2.075950, -1.215149, 1.322761, 2.142359, -1.372597, 0.398777, 1.008202,
       # pose 8
-      -1.611937, -1.158158, 1.064049, 1.918313, -1.480676, -0.392994, 1.833894,
+      -2.153500, -1.215892, 1.345656, 2.306931, -1.412319, 0.415145, 0.997235,
       # pose 9
-      -2.252978, -1.387014, 1.686186, 2.101298, -1.333349, -0.111994, 0.036817,
+      -2.127777, -1.256654, 1.370562, 2.009440, -1.369584, 0.219094, -0.132912,
       # pose 10
-      -2.179722, -1.297659, 1.497171, 2.167675, -1.272158, 0.100548, 0.673603,
+      -2.138686, -1.270867, 1.591854, 2.150744, -0.898729, 0.079835, 0.840791,
       # pose 11
-      -1.921897, -1.543024, 1.000863, 1.897273, -1.455972, 1.437388, -0.140968,
+      -2.063605, -1.300562, 1.582835, 2.080463, -0.904827, 0.243915, 1.110180,
       # pose 12
-      -1.884059, -1.063347, 1.439248, 2.253200, -1.164963, -0.423306, 0.907197,
+      -1.867772, -1.316320, 1.376713, 1.888199, -1.274029, 0.284609, 1.405712,
       # pose 13
-      -1.987022, -1.235459, 1.354108, 2.370862, -1.354044, 0.065378, 0.923079,
+      -1.829092, -1.355347, 1.315742, 1.808403, -1.413826, 0.338349, 1.459711,
       # pose 14
-      -1.770869, -1.333861, 1.241996, 1.800723, -1.395819, -0.255590, 1.389660,
+      -2.192461, -1.374082, 1.330037, 2.154137, -1.528020, 0.279283, 0.657702,
       # pose 15
-      -1.761759, -1.369860, 1.213137, 1.791639, -1.467956, -0.170537, 1.473557,
+      -2.137444, -1.468886, 1.525289, 2.348419, -1.025385, 1.011855, 1.148855,
-      # pose 16
-      -1.504294, -1.346227, 1.206550, 1.660186, -1.320782, -0.393676, 1.657215,
-      # pose 17
-      -1.846546, -1.476930, 0.946255, 1.910615, -2.435429, 1.535168, 2.151544,
-      # pose 18
-      -1.700067, -1.452760, 0.909035, 1.758222, -2.103930, 1.445249, 2.174743,
-      # pose 19
-      -2.173713, -0.983647, 1.399849, 2.384939, -1.379509, -0.216471, 0.253561,
-      # pose 20
-      -2.160187, -0.967904, 1.376288, 2.451713, -1.393524, -0.212278, 0.473606,
     ]

handeye/poses/backup/89_eye_in_hand_21pts_before_20261016.yaml

@@ -0,0 +1,56 @@
+# 手眼标定示教的关节位姿（7 关节 × N）。由 calibrate + waypoints 共用。
+#
+# 在这里编辑，或者：
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#
+# /** 这样任意节点名都能收到这些参数。
+
+/**:
+  ros__parameters:
+    num_poses: 21
+    # 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
+    waypoint_positions: [
+      # pose 0
+      -0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
+      # pose 1
+      -0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
+      # pose 2
+      -0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
+      # pose 3
+      -1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
+      # pose 4
+      -0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
+      # pose 5
+      -0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
+      # pose 6
+      -0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
+      # pose 7
+      -0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
+      # pose 8
+      -0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
+      # pose 9
+      -0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
+      # pose 10
+      -0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
+      # pose 11
+      -0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
+      # pose 12
+      -0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
+      # pose 13
+      -0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
+      # pose 14
+      -1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
+      # pose 15
+      -0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
+      # pose 16
+      -0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
+      # pose 17
+      -0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
+      # pose 18
+      -1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
+      # pose 19
+      -1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
+      # pose 20
+      -1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
+    ]

handeye/poses/backup/89_eye_to_hand_24pts_before_20261016.yaml

@@ -0,0 +1,60 @@
+# 眼在手外示教关节（7 关节 × N）。板夹在 tool10120938 上，顶置 D435 看它。
+#
+# 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。
+#
+#   bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
+#   bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
+
+/**:
+  ros__parameters:
+    num_poses: 24
+    waypoint_positions: [
+      # pose 0
+      -1.241563, -0.359631, 0.346153, 2.172565, -0.179027, 0.929920, -0.778876,
+      # pose 1
+      -1.388832, -0.554698, 0.416075, 1.897617, -0.358341, 0.827495, -0.762302,
+      # pose 2
+      -1.436048, -0.573046, 0.461762, 1.870967, -0.387696, 0.813090, -0.762917,
+      # pose 3
+      -1.614491, -0.519365, 0.693991, 2.156193, -0.499113, 0.968762, 0.073287,
+      # pose 4
+      -2.020087, -0.869088, 1.059166, 2.155478, -1.198473, 0.740567, 0.478840,
+      # pose 5
+      -2.082145, -1.122210, 1.203011, 1.950272, -1.269847, 0.434138, -0.641975,
+      # pose 6
+      -1.983153, -0.886082, 0.999329, 2.324821, -1.272773, 0.754127, 0.313613,
+      # pose 7
+      -2.021479, -0.886609, 1.055679, 2.337716, -1.282773, 0.669319, -0.028355,
+      # pose 8
+      -2.022792, -0.947211, 1.118194, 2.127275, -1.211995, 0.527528, -0.169900,
+      # pose 9
+      -2.004766, -1.065820, 1.183799, 1.901084, -1.217488, 0.380285, -0.312733,
+      # pose 10
+      -1.942863, -1.219617, 1.256733, 1.653113, -1.222302, 0.261818, -0.420706,
+      # pose 11
+      -2.039558, -1.082441, 1.189833, 1.996967, -1.241252, 0.521591, 0.526563,
+      # pose 12
+      -2.098074, -1.006236, 1.538183, 2.118382, -0.737422, 0.019903, 0.648802,
+      # pose 13
+      -2.039609, -1.159222, 1.488501, 1.908579, -1.168793, -0.140451, 0.034889,
+      # pose 14
+      -1.880783, -1.104601, 1.579382, 2.036427, -0.665049, 0.254105, 1.518504,
+      # pose 15
+      -2.015530, -1.535468, 1.711220, 1.836908, -1.707450, -0.400234, -0.708683,
+      # pose 16
+      -1.997672, -1.626362, 1.529443, 2.200206, -1.291013, 0.398664, 1.954208,
+      # pose 17
+      -1.981875, -1.620651, 1.545443, 2.373140, -1.167365, 0.396487, 2.012547,
+      # pose 18
+      -2.179415, -1.511968, 1.578832, 2.401706, -1.326480, -0.051353, 1.137181,
+      # pose 19
+      -2.173463, -1.530413, 1.575534, 2.173673, -1.289673, -0.090117, 0.867214,
+      # pose 20
+      -2.096314, -1.569352, 1.579820, 1.942057, -1.458598, -0.057003, 0.572953,
+      # pose 21
+      -2.129376, -1.566787, 1.903585, 1.962858, -1.454137, -0.335990, -0.148441,
+      # pose 22
+      -2.020280, -1.527165, 1.887130, 1.728957, -1.491867, -0.370585, -0.277940,
+      # pose 23
+      -2.140817, -1.607357, 1.617236, 2.153850, -1.571057, -0.147540, 0.770535,
+    ]

handeye/results/89_d435_eye_to_hand.yaml

@@ -1,22 +1,22 @@
 # 手眼标定结果
 # mode: eye_to_hand
 # camera_source: d435
-# views: 24
+# views: 15
-# utc: 2026-10-12T05:20:30.137123+00:00
+# utc: 2026-10-16T03:02:49.214344+00:00
 # board: 11x8 square_m=0.02
 # gripper_tool: tool10120938
 # T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
 # 旧结果备份在 results/backup/
 gripper_tool: tool10120938
 T_flange_to_tool:
-  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [1.00000000, 0.00000000, 0.00000000, 0.00075900]
-  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 1.00000000, 0.00000000, -0.00008800]
-  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22627400]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
 # T_cam_to_base (eye_to_hand)
-translation_m: [0.37094529, -0.52805928, 0.73745585]
+translation_m: [0.38078822, -0.52208142, 0.70359964]
 T_4x4:
-  - [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
+  - [-0.03086969, 0.99833255, -0.04877684, 0.38078822]
-  - [0.99991380, 0.01024832, 0.00820703, -0.52805928]
+  - [0.99951823, 0.03067553, -0.00472440, -0.52208142]
-  - [0.00862304, -0.04123150, -0.99911241, 0.73745585]
+  - [-0.00322027, -0.04889918, -0.99879853, 0.70359964]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/89_eye_in_hand.yaml

@@ -1,28 +1,27 @@
 # 手眼标定结果
 # mode: eye_in_hand
 # camera_source: d405_left
-# views: 21
+# views: 25
-# utc: 2026-10-12T03:29:51.243487+00:00
+# utc: 2026-10-16T02:29:07.572249+00:00
 # board: 11x8 square_m=0.015
 # gripper_tool: tool10120938
 # T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
 # T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
-# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
 gripper_tool: tool10120938
 T_flange_to_tool:
-  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [1.00000000, 0.00000000, 0.00000000, 0.00075900]
-  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 1.00000000, 0.00000000, -0.00008800]
-  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22627400]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
 T_cam_to_tool:
-  - [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
+  - [0.01117708, -0.86715374, 0.49791511, -0.06192365]
-  - [0.99932767, -0.03620511, 0.00577974, -0.01523625]
+  - [0.99992647, 0.01203574, -0.00148504, -0.00762539]
-  - [0.01281276, 0.49256977, 0.87017863, -0.09761517]
+  - [-0.00470501, 0.49789509, 0.86722450, -0.09779711]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
 # T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
-translation_m: [-0.06011995, -0.00853625, 0.12688483]
+translation_m: [-0.06116465, -0.00771339, 0.12847689]
 T_4x4:
-  - [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
+  - [0.01117708, -0.86715374, 0.49791511, -0.06116465]
-  - [0.99932767, -0.03620511, 0.00577974, -0.00853625]
+  - [0.99992647, 0.01203574, -0.00148504, -0.00771339]
-  - [0.01281276, 0.49256977, 0.87017863, 0.12688483]
+  - [-0.00470501, 0.49789509, 0.86722450, 0.12847689]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/backup/89_d435_eye_to_hand_tool10120938_oldtcp_20261012.yaml

@@ -0,0 +1,22 @@
+# 手眼标定结果
+# mode: eye_to_hand
+# camera_source: d435
+# views: 24
+# utc: 2026-10-12T05:20:30.137123+00:00
+# board: 11x8 square_m=0.02
+# gripper_tool: tool10120938
+# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
+# 旧结果备份在 results/backup/
+gripper_tool: tool10120938
+T_flange_to_tool:
+  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+# T_cam_to_base (eye_to_hand)
+translation_m: [0.37094529, -0.52805928, 0.73745585]
+T_4x4:
+  - [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
+  - [0.99991380, 0.01024832, 0.00820703, -0.52805928]
+  - [0.00862304, -0.04123150, -0.99911241, 0.73745585]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/backup/89_eye_in_hand_tool10120938_20261012.yaml

@@ -0,0 +1,28 @@
+# 手眼标定结果
+# mode: eye_in_hand
+# camera_source: d405_left
+# views: 21
+# utc: 2026-10-12T03:29:51.243487+00:00
+# board: 11x8 square_m=0.015
+# gripper_tool: tool10120938
+# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
+# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
+# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
+gripper_tool: tool10120938
+T_flange_to_tool:
+  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+T_cam_to_tool:
+  - [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
+  - [0.99932767, -0.03620511, 0.00577974, -0.01523625]
+  - [0.01281276, 0.49256977, 0.87017863, -0.09761517]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+# T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
+translation_m: [-0.06011995, -0.00853625, 0.12688483]
+T_4x4:
+  - [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
+  - [0.99932767, -0.03620511, 0.00577974, -0.00853625]
+  - [0.01281276, 0.49256977, 0.87017863, 0.12688483]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

install/arm_application/lib/arm_application/assembly_pipeline

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','assembly_pipeline'
 import re
 import sys

install/arm_application/lib/arm_application/demo_named_poses

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','demo_named_poses'
 import re
 import sys

install/arm_application/lib/arm_application/dh_pgea

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea'
 import re
 import sys

install/arm_application/lib/arm_application/dh_pgea_cli

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea_cli'
 import re
 import sys

install/arm_application/lib/arm_application/dual_gripper

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper'
 import re
 import sys

install/arm_application/lib/arm_application/dual_gripper_cli

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper_cli'
 import re
 import sys

install/arm_application/lib/arm_application/gauge_assembly

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','gauge_assembly'
 import re
 import sys

install/arm_application/lib/arm_application/goto_ee_plane

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','goto_ee_plane'
 import re
 import sys

install/arm_application/lib/arm_application/handeye_calibrate

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_calibrate'
 import re
 import sys

install/arm_application/lib/arm_application/handeye_teach

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_teach'
 import re
 import sys

install/arm_application/lib/arm_application/handeye_waypoints

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_waypoints'
 import re
 import sys

install/arm_application/lib/arm_application/hole_detect

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','hole_detect'
 import re
 import sys

install/arm_application/lib/arm_application/move_to_pose_client

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','move_to_pose_client'
 import re
 import sys

install/arm_application/lib/arm_application/perception_node

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','perception_node'
 import re
 import sys

install/arm_application/lib/arm_application/realsense_color_pub

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_color_pub'
 import re
 import sys

install/arm_application/lib/arm_application/realsense_view

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_view'
 import re
 import sys

install/arm_application/lib/arm_application/run_joint_trajectory

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','run_joint_trajectory'
 import re
 import sys

install/arm_application/lib/arm_application/servo_node

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','servo_node'
 import re
 import sys

install/arm_application/lib/arm_application/stage2_locate

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage2_locate'
 import re
 import sys

install/arm_application/lib/arm_application/stage3_search

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage3_search'
 import re
 import sys

install/arm_application/lib/arm_application/tactile

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile'
 import re
 import sys

install/arm_application/lib/arm_application/tactile_cli

@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
 # EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile_cli'
 import re
 import sys

install/arm_application/lib/python3.11/site-packages/arm-application.egg-link

@@ -0,0 +1,2 @@
+/home/cc/ros2/build/arm_application
+.

install/arm_application/lib/python3.11/site-packages/arm_application/__init__.py

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

install/arm_application/lib/python3.11/site-packages/arm_application/box_tracker.py

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

install/arm_application/lib/python3.11/site-packages/arm_application/demo_named_poses.py

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